-
Notifications
You must be signed in to change notification settings - Fork 308
controller_interface
wmeeusse edited this page Nov 9, 2012
·
23 revisions
We want to write a PR2 controller that tracks position. Since the PR2 takes effort commands, we need to inherit the `JointEffortInterface`.
class PR2PositionController : public Controller<JointEffortCommandInterface> { public: bool init(JointEffortCommandInterface* hw, ros::NodeHandle &n) { # get joint name from the parameter server std::string my_joint; if (!n.param('joint', my_joint)){ ROS_ERROR("Could not find joint name"); return false; } # get the joint object to use in the realtime loop joint_ = hw->getJointCommandEffort(my_joint); return true; } void update(const ros::Time& time) { double error = setpoint_ - joint_.getJointState().getPosition(); joint_cmd_.setCommand(error*gain_); } void starting(const ros::Time& time) { } void stopping(const ros::Time& time) { } private: JointEffortCommand joint_; static const double gain_ = 1.25; static const double setpoint_ = 3.00; }