Gazebosim 2.2.2. Getting joint forces; getforcetorque "Warning [ODEJoint.cc:1105] GetForceTorque: forgot to set <provide_feedback>?"

Question

I am trying to read the forces of some joints in gazebosim, but after i upgraded to version 2.2.2 from 1.9 it doesn't work anymore.

The SDF file: false

     <link name='left_foot'>
       <pose>0 0.255502204764 0.04 0 0 0</pose>
       <inertial>
         <pose>0 0 0 0 0 0</pose>
         <inertia>
           <ixx>0.0075</ixx>
           <ixy>0</ixy>
           <ixz>0</ixz>
           <iyy>0.00</iyy>
           <iyz>0</iyz>
           <izz>0.00</izz>
         </inertia>
         <mass>2.25</mass>
       </inertial>
       <collision name='left_foot_collision'>
         <geometry>
           <box>
             <size>.1 .2 .08</size>
           </box>
         </geometry>

       <surface>
         <bounce>
           <restitution_coefficient> 0.0 </restitution_coefficient> 
           <threshold> 1000000000 </threshold>
         </bounce>
       </surface>

       </collision>
       <visual name='left_foot_visual'>
         <geometry>
           <box>
             <size>.1 .2 .08</size>
           </box>
         </geometry>
       </visual>
     </link>

     <link name='leftlower_leg'>
       <pose>0 0.294832856462 0.221086146856 6.01787954503 0 0</pose>
       <inertial>
         <pose>0 0 0 0 0 0</pose>
         <inertia>
           <ixx>0.0225</ixx>
           <ixy>0</ixy>
           <ixz>0</ixz>
           <iyy>0.0225</iyy>
           <iyz>0</iyz>
           <izz>0.0225</izz>
         </inertia>
         <mass>3.</mass>
       </inertial>
       <collision name='leftlower_leg_collision'>
         <geometry>
           <box>
             <size>.1 .1 .3</size>
           </box>
         </geometry>

       <surface>
         <bounce>
           <restitution_coefficient> 0.0 </restitution_coefficient> 
           <threshold> 1000000000 </threshold>
         </bounce>
       </surface> 

       </collision>
       <visual name='leftlower_leg_visual'>
         <geometry>
           <box>
             <size>.1 .1 .3</size>
           </box>
         </geometry>
       </visual>
     </link>

     <link name='leftupper_leg'>
       <pose>0 0.241130401436 0.542882746013 6.76700690559 0 0</pose>
       <inertial>
         <pose>0 0 0 0 0 0</pose>
         <inertia>
           <ixx>0.0533</ixx>
           <ixy>0</ixy>
           <ixz>0</ixz>
           <iyy>0.0533</iyy>
           <iyz>0</iyz>
           <izz>0.0533</izz>
         </inertia>
         <mass>4.</mass>
       </inertial>
       <collision name='leftupper_leg_collision'>
         <geometry>
           <box>
             <size>.1 .1 .4</size>
           </box>
         </geometry>

       <surface>
         <bounce>
           <restitution_coefficient> 0.0 </restitution_coefficient> 
           <threshold> 1000000000 </threshold>
         </bounce>
       </surface>

       </collision>
       <visual name='leftupper_leg_visual'>
         <geometry>
           <box>
             <size>.1 .1 .4</size>
           </box>
         </geometry>
       </visual>
     </link>


     <link name='right_foot'>
       <pose>0 0.0 0.04 0 0 0</pose>
       <inertial>
         <pose>0 0 0 0 0 0</pose>
         <inertia>
           <ixx>0.0075</ixx>
           <ixy>0</ixy>
           <ixz>0</ixz>
           <iyy>0.0</iyy>
           <iyz>0</iyz>
           <izz>0.0</izz>
         </inertia>
         <mass>2.25</mass>
       </inertial>
       <collision name='right_foot_collision'>
         <geometry>
           <box>
             <size>.1 .2 .08</size>
           </box>
         </geometry>

       <surface>
         <bounce>
           <restitution_coefficient> 0.0 </restitution_coefficient> 
           <threshold> 1000000000 </threshold>
         </bounce>
       </surface>

       </collision>
       <visual name='right_foot_visual'>
         <geometry>
           <box>
             <size>.5 .2 .08</size>
           </box>
         </geometry>
       </visual>
     </link>

     <link name='rightlower_leg'>
       <pose>0 0.0892702896712 0.200543831788 2.50415869759 0 0</pose>
       <inertial>
         <pose>0 0 0 0 0 0</pose>
         <inertia>
           <ixx>0.0225</ixx>
           <ixy>0</ixy>
           <ixz>0</ixz>
           <iyy>0.0225</iyy>
           <iyz>0</iyz>
           <izz>0.0225</izz>
         </inertia>
         <mass>3.</mass>
       </inertial>
       <collision name='rightlower_leg_collision'>
         <geometry>
           <box>
             <size>.1 .1 .3</size>
           </box>
         </geometry>

       <surface>
         <bounce>
           <restitution_coefficient> 0.0 </restitution_coefficient> 
           <threshold> 1000000000 </threshold>
         </bounce>
       </surface>

       </collision>
       <visual name='rightlower_leg_visual'>
         <geometry>
           <box>
             <size>.1 .1 .3</size>
           </box>
         </geometry>
       </visual>
     </link>

     <link name='rightupper_leg'>
       <pose>0 0.163318937026 0.520507576335 3.21777453298 0 0</pose>
       <inertial>
         <pose>0 0 0 0 0 0</pose>
         <inertia>
           <ixx>0.0533</ixx>
           <ixy>0</ixy>
           <ixz>0</ixz>
           <iyy>0.0533</iyy>
           <iyz>0</iyz>
           <izz>0.0533</izz>
         </inertia>
         <mass>4.</mass>
       </inertial>
       <collision name='rightupper_leg_collision'>
         <geometry>
           <box>
             <size>.1 .1 .4</size>
           </box>
         </geometry>

       <surface>
         <bounce>
           <restitution_coefficient> 0.0 </restitution_coefficient> 
           <threshold> 1000000000 </threshold>
         </bounce>
       </surface>

       </collision>
       <visual name='rightupper_leg_visual'>
         <geometry>
           <box>
             <size>.1 .1 .4</size>
           </box>
         </geometry>
       </visual>
     </link>

  <joint type="revolute" name="right_ankle_hinge">
    <physics>
          <ode>
        <provide_feedback>true</provide_feedback>
          </ode>
    </physics>
    <pose>0 0.0 0.15 0 0 0</pose>
    <child>rightlower_leg</child>
    <parent>right_foot</parent>
    <axis>
     <xyz>1 0 0</xyz>
    </axis>
  </joint>

  <joint type="revolute" name="right_knee_hinge">
    <physics>
          <ode>
        <provide_feedback>true</provide_feedback>
          </ode>
    </physics>
    <pose>0 0 0.2 0 0 0</pose>
    <child>rightupper_leg</child>
    <parent>rightlower_leg</parent>
    <axis>
     <xyz>1 0 0</xyz>
    </axis>
  </joint>

  <joint type="revolute" name="hip_hinge">
    <physics>
          <ode>
        <provide_feedback>true</provide_feedback>
          </ode>
    </physics>
    <pose>0 0 0.2 0 0 0</pose>
    <child>leftupper_leg</child>
    <parent>rightupper_leg</parent>
    <axis>
     <xyz>1 0 0</xyz>
    </axis>
  </joint>

  <joint type="revolute" name="left_knee_hinge">
    <physics>
          <ode>
        <provide_feedback>true</provide_feedback>
          </ode>
    </physics>
    <pose>0 0 0.15 0 0 0</pose>
    <child>leftlower_leg</child>
    <parent>leftupper_leg</parent>
    <axis>
     <xyz>1 0 0</xyz>
    </axis>
  </joint>

  <joint type="revolute" name="left_ankle_hinge">
    <physics>
          <ode>
        <provide_feedback>true</provide_feedback>
          </ode>
    </physics>
    <pose>0 0 0.04 0 0 0</pose>
    <child>left_foot</child>
    <parent>leftlower_leg</parent>
    <axis>
     <xyz>1 0 0</xyz>
    </axis>
  </joint>



    <plugin name="set_joints" filename="build/libmy_robot.so"/>
  </model>
</sdf>

And the plugin: #include #include #include //#include //denne er added for at bruge getsimtime #include #include #include // sleep function you added yourself #include //These two lines you added to use the function getforcetorque #include //These two lines you added to use the function getforcetorque #include #include

int a=0;
int b;
double angle;
double currenttime;
double PI = 3.141592654;
double pi=3.14159265359;



namespace gazebo
{  

  class SetJoints : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) 
    {
     // Store the pointer to the model
     this->model = _parent;

     //this->j2_controller = new physics::JointController(model);



     this->jointleftankle_ = this->model->GetJoint("left_ankle_hinge");
     this->jointrightankle_ = this->model->GetJoint("right_ankle_hinge");
     this->jointleftknee_ = this->model->GetJoint("left_knee_hinge");
     this->jointrightknee_ = this->model->GetJoint("right_knee_hinge");
     this->jointhip_ = this->model->GetJoint("hip_hinge");

     //j2_controller->SetJointPosition(this->model->GetJoint("hip_hinge"), 0.0);


     // Listen to the update event. This event is broadcast every
     // simulation iteration.






     this->updateConnection = event::Events::ConnectWorldUpdateBegin(
         boost::bind(&SetJoints::OnUpdate, this, _1));
    }

    // Called by the world update start event
    public: void OnUpdate(const common::UpdateInfo & /*_info*/)
    {

     common::Time current_time = this->model->GetWorld()->GetSimTime();//her henter du simtime


     double initialrightanklepos=0.93336237079397688;
     double initialrightkneepos=0.71361583539276363;
     double initialhippos=3.5492323726052324;
     double initialleftkneepos=0.82166896624278996-PI/2.;
     double initialleftanklepos=+(initialrightanklepos+initialrightkneepos+initialhippos+initialleftkneepos)-1.*PI;




     double dt    = current_time.Double()
                  - this->last_update_time_.Double();





     physics::JointWrench wrenchleftankle = this->jointleftankle_->GetForceTorque(0);
     physics::JointWrench wrenchrightankle = this->jointrightankle_->GetForceTorque(0);
     //physics::JointWrench wrenchleftknee = this->jointleftknee_->GetForceTorque(0);
     //physics::JointWrench wrenchrightknee = this->jointrightknee_->GetForceTorque(0);
     //physics::JointWrench wrenchhip = this->jointhip_->GetForceTorque(0);

     //double test1 = wrenchrightankle.body1Force.z;
     //double test2 = wrenchleftankle.body2Force.z;
     //printf("right ankle z:%lf\t left ankle z:%lf\n",test1, test2);



     this->last_update_time_ = current_time;


     a++;


    }

    common::Time last_update_time_;

    // Pointer to the model
    private: physics::ModelPtr model;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    //private: physics::JointController * j2_controller;

    //common::Time last_update_time_; Denne er ikke nødvendig.

    //ting til at gemme joint_ i
    physics::JointPtr jointleftankle_;
    physics::JointPtr jointrightankle_;
    physics::JointPtr jointleftknee_;
    physics::JointPtr jointrightknee_;
    physics::JointPtr jointhip_;
    //physics::JointPtr joint;

  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(SetJoints)
}

I get the error:

Warning [ODEJoint.cc:1105] GetForceTorque: forgot to set ?

printed in the terminal.

Answer 1

I found the error:

<joint type="revolute" name="left_ankle_hinge">
    <physics>
         <ode>
       <provide_feedback>true</provide_feedback>
         </ode>
    </physics>
    <pose>0 0 0.04 0 0 0</pose>
    <child>left_foot</child>
    <parent>leftlower_leg</parent>
    <axis>
    <xyz>1 0 0</xyz>
    </axis>
  </joint>

should be changed to:

<joint type="revolute" name="left_ankle_hinge">
    <physics>
          <provide_feedback>true</provide_feedback>
    </physics>
    <pose>0 0 0.04 0 0 0</pose>
    <child>left_foot</child>
    <parent>leftlower_leg</parent>
    <axis>
     <xyz>1 0 0</xyz>
    </axis>
  </joint>

Answer 2

For me, accepted answer did not work (Gazebo 11). I was not able to specify provide_feedback tag in SDF. However I found a solution for my case:

<gazebo reference="joint_name">
    <provideFeedback>true</provideFeedback>
</gazebo>

I post this for others, whom the first solution did not help.