Apply force to a joint using plugins

edit

Hi ! I just made a plugin so I might be able to help! I supposed you saw this link : Gazebo_plugin_tutorial the way i would do it is to call a function from the gazebo API in the function onUpdate() instead of a service.

re use the same class body as in the tutorial gazebo, but add it the function OnUpdate, which is called anytime gazebo update.

The plugin I did was to control the angle of the joints but I found the function setForce in the API which should be what you are looking for.

#include <gazebo/gazebo.hh>

    namespace gazebo
    {
      class WorldPluginTutorial : public WorldPlugin
      {
        public: WorldPluginTutorial() : WorldPlugin()
                {
                  printf("Hello World!\n");
                  gazebo::physics::ModelPtr this->model ;
                }

        public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
                {
                    this->model = world->GetModel("my_robot");
                }
        public : void OnUpdate(const common::UpdateInfo & /*_info*/)
     {
      // Apply a small linear velocity to the model.
      //this->model->SetLinearVel(math::Vector3(0, 0, 0)); // just in case needed

      // pose of the footprint
      gazebo::math::Pose mapose(this->vect3_pose_model, this->rot_pose_model);
      this->model->SetLinkWorldPose(mapose,"WheelFR_link");
    //pose of every joint 
     this.vecteur_str = ["head","base","joint3","whatever"];
      for (int i = 0 ; i < this->vecteur_str.size(); i++)
      {
         //what I used to control the joints 
        //this->model->GetJoint(vecteur_str[i])->SetAngle(0, vecteur_effort[i]);  
        // This is what you want
        this->model->GetJoint(vecteur_str[i])->SetForce(unsigned int _index, double _effort) 
      }
    }
  };
  GZ_REGISTER_WORLD_PLUGIN(WorldPluginTutorial)
}

If you need the "subscriber/publisher" functions of ROS, just instantiates them in the onLoad() as in any other ROS node.

PS: maybe you'll have more answers on answers.gazebo