Set and Get position of gazebo model, using ROS plugin.

edit

Hi,

I will write a short example how to listen to a ROS topic, and control the joint regarding the position you get from the topic, the control will be via a PID controller. Here is some info about PID controllers.

You will change the value of the joint target position in the Callback function, and compute the effort and apply it on the joint in the OnUpdate() function.

Then you can just create a ROS subscriber and init the PID in your Load() function, something like:

void ROSPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
    {

      int argc = 0;
      char** argv = NULL;
      ros::init(argc,argv,"ROS plugin");
      this->rosnode = new ros::NodeHandle();

      // Store the pointer to the model
      this->model = _parent;

      // Store the joint you want to control
      this->joint = this->model->GetJoint("joint_name");

      // initialize the pid ( common::Pid ), 
      // you have to see for yourself which values works best for you
      this->jointPID (10, 0, 0, 10, -10);

      // subscribing to the rost topic, and calling the callback function
      this->ROS_subscriber = this->rosnode->subscribe("/ROS_topic", 10, &ROSPlugin::ROSCallback, this);

      // Listen to the update event. This event is broadcast every simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateStart(boost::bind(&DataGlovePlugin::OnUpdate, this));

    }

The callback function from the ROS topic:

void ROSPlugin::ROSCallback(const package_name::MsgType& msg)
{
     // you update the position of where you want the joint to move
     this->joint_target_pos = msg.pos;
}

The OnUpdate() function:

void ROSPlugin::OnUpdate()
{
    // compute the steptime for the PID
    common::Time currTime = this->model->GetWorld()->GetSimTime();
    common::Time stepTime = currTime - this->prevUpdateTime;
 this->prevUpdateTime = currTime;

   // set the current position of the joint, and the target position, 
    // and the maximum effort limit
    double pos_target = this->joint_target_pos;
    double pos_curr = this->joint->GetAngle(0).Radian();
    double max_cmd = this->joint_max_effort;

    // calculate the error between the current position and the target one
    double pos_err = pos_curr - pos_target;

    // compute the effort via the PID, which you will apply on the joint
    double effort_cmd = this->jointPID.Update(pos_err, stepTime);

    // check if the effort is larger than the maximum permitted one
    effort_cmd = effort_cmd > max_cmd ? max_cmd :
                (effort_cmd < -max_cmd ? -max_cmd : effort_cmd);

    // apply the force on the joint
    this->joint->SetForce(0, effort_cmd);

}

If you have problems implementing the idea let me know.

Cheers, Andrei