Home

Stability

This is a future project that hasn't really even been written yet

Writing a new controller

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;
}

Setting up a new robot

Defining a hardware interface

Suppose we have a robot with 2 joints: A & B. Joint A is position controlled, and Joint B is Velocity controlled.

class MyRobotHW : public JointEffortCommandInterface, public JointCommandPositionInterface
{
public:
  RobotHW() { }

  const std::vector<std::string> getJointNames()
  {
    std::vector<std::string> names;
    names.push_back("A");
    names.push_back("B");
    return names;
  }

  double& getPositionCommand(const std::string& name)
  {
    if (name == "A")
      return joint_A_pos_command_;
    else
      throw Error(name + "is not a Position Joint");
  }

  double& getEffortCommand(const std::string& name)
  {
    if (name == "B")
      return joint_B_pos_command_;
    else
      throw Error(name + "Is not a Velocity Joint");
  }

  double getPosition(const std::string& name)
  {
    if (name == "A")
      return pos[0];
    else if(name == "B")
      return pos[1];
    else
      return Error(name + " is not known");
  }

double getVelocity(const std::string& name)
  {
    if (name == "A")
      return vel[0];
    else if(name == "B")
      return vel[1];
    else
      return Error(name + " is not known");
  }


double getEffort(const std::string& name)
  {
    if (name == "A")
      return eff[0];
    else if(name == "B")
      return eff[1];
    else
      return Error(name + " is not known");
  }

private:
  double joint_A_pos_command_;
  double joint_B_vel_command_;

  double pos[2];
  double vel[2];
  double eff[2];
}