Permalink
Browse files

bugfix in universal controller

  • Loading branch information...
1 parent 7556d1e commit d53ca618b75b86073386d6429507df88266f518b alexey committed Mar 26, 2012
View
4 youbot_common/youbot_description/include/youbot_universal_control.h
@@ -82,20 +82,20 @@ class YouBotUniversalController : public pr2_controller_interface::Controller
std::vector<control_toolbox::Pid> pids;
ros::Time lastTime;
std::vector <double> setPoints;
+ std::vector <double> filteredVelocity;
ros::Subscriber positionCommandSubscriber;
ros::Subscriber velocityCommandSubscriber;
ros::Subscriber torqueCommandSubscriber;
void updateJointPosition(double setPoint, pr2_mechanism_model::JointState* joint_state_, control_toolbox::Pid* pid_controller_, const ros::Duration& dt);
void updateJointTorque(double setPoint, pr2_mechanism_model::JointState* joint_state_, control_toolbox::Pid* pid_controller_, const ros::Duration& dt);
- void updateJointVelocity(double setPoint, pr2_mechanism_model::JointState* joint_state_, control_toolbox::Pid* pid_controller_, const ros::Duration& dt);
+ void updateJointVelocity(double setPoint, pr2_mechanism_model::JointState* joint_state_, control_toolbox::Pid* pid_controller_, const ros::Duration& dt, const int& jointIndex);
void positionCommand(const brics_actuator::JointPositions &jointPositions);
void velocityCommand(const brics_actuator::JointVelocities &jointVelocities);
void torqueCommand(const brics_actuator::JointTorques &jointTorques);
- double filteredVelocity;
};
} // namespace
View
11 youbot_common/youbot_description/src/youbot_universal_control.cpp
@@ -123,6 +123,7 @@ bool YouBotUniversalController::init(pr2_mechanism_model::RobotState *robotPtr,
// Initializing targetVelocities vector
setPoints.resize(joints.size());
+ filteredVelocity.resize(joints.size());
// Sets up pid controllers for all of the joints from yaml file
std::string gainsNS;
@@ -180,7 +181,7 @@ void YouBotUniversalController::update()
}
case YouBotUniversalController::VELOCITY :
{
- updateJointVelocity(setPoints[i], joints[i], &pids[i], dt);
+ updateJointVelocity(setPoints[i], joints[i], &pids[i], dt, i);
break;
}
case YouBotUniversalController::TORQUE :
@@ -199,17 +200,17 @@ void YouBotUniversalController::update()
}
-void YouBotUniversalController::updateJointVelocity(double setPoint, pr2_mechanism_model::JointState* joint_state_, control_toolbox::Pid* pid_controller_, const ros::Duration& dt)
+void YouBotUniversalController::updateJointVelocity(double setPoint, pr2_mechanism_model::JointState* joint_state_, control_toolbox::Pid* pid_controller_, const ros::Duration& dt, const int& jointIndex)
{
double error(0);
assert(joint_state_->joint_);
double command_ = setPoint;
double velocity_ = joint_state_->velocity_;
const double T = 1;
- filteredVelocity = filteredVelocity + (velocity_ - filteredVelocity) * dt.toSec()/(dt.toSec()+T);
- error = filteredVelocity - command_;
- ROS_DEBUG("Current velocity: %f, filtered velocity: %f, Error: %f\n",velocity_, filteredVelocity, error);
+ filteredVelocity[jointIndex] = filteredVelocity[jointIndex] + (velocity_ - filteredVelocity[jointIndex]) * dt.toSec()/(dt.toSec()+T);
+ error = filteredVelocity[jointIndex] - command_;
+ ROS_DEBUG("Current velocity: %f, filtered velocity: %f, Error: %f\n",velocity_, filteredVelocity[jointIndex], error);
double commanded_effort = pid_controller_ -> updatePid(error, dt);
joint_state_->commanded_effort_ = commanded_effort;

0 comments on commit d53ca61

Please sign in to comment.