changed the calculation of force/torque on parent link for ODEJoint::GetForceTorque()

[osrf-migration]

Original report (archived issue) by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

This was initially submitted as pull request #2110 by @vorndamme though it did not include a test, so it was moved to this issue while a test is written.

Original description:

Changed calculation of parent link force/torque. Force on child is calculated in world frame (was child frame before), where the parent force/torque can simply be the negative of the child force/torque (before, parent force/torque was set to the negative child force/torque, which was specified in child link frame and was added to the overall torque in parent link frame). Now all torques are calculated in world frame, summed up and transformed into child and parent frame in the end.

patch:

diff -r e8fb1728441128d8401f098f022865839de5d4d4 -r d989c13ef2a4f7d9c551ba09872eb3ce5b658c48 gazebo/physics/ode/ODEJoint.cc
--- a/gazebo/physics/ode/ODEJoint.cc
+++ b/gazebo/physics/ode/ODEJoint.cc
@@ -677,10 +677,10 @@
     physics::JointWrench wrenchAppliedWorld;
     if (this->HasType(physics::Base::HINGE_JOINT))
     {
-      // rotate force into child link frame
-      // GetLocalAxis is the axis specified in parent link frame!!!
+      // get torque on child in world frame, torque on parent acts in opposite
+      // direction
       wrenchAppliedWorld.body2Torque =
-        this->GetForce(0u) * this->GetLocalAxis(0u);
+        this->GetForce(0u) * this->GetGlobalAxis(0u);

       // gzerr << "body2Torque [" << wrenchAppliedWorld.body2Torque
       //       << "] axis [" << this->GetLocalAxis(0u)
@@ -690,9 +690,10 @@
     }
     else if (this->HasType(physics::Base::SLIDER_JOINT))
     {
-      // rotate force into child link frame
+      // get force on child in world frame, force on parent acts in opposite
+      // direction
       wrenchAppliedWorld.body2Force =
-        this->GetForce(0u) * this->GetLocalAxis(0u);
+        this->GetForce(0u) * this->GetGlobalAxis(0u);
       wrenchAppliedWorld.body1Force = -wrenchAppliedWorld.body2Force;
     }
     else if (this->HasType(physics::Base::FIXED_JOINT))
@@ -705,6 +706,8 @@
       gzerr << "force torque for joint type [" << this->GetType()
             << "] not implemented, returns false results!!\n";
     }
+    // add wrench in world frame before transform into child and parent frame
+    this->wrench = this->wrench + wrenchAppliedWorld;

     // convert wrench from child cg location to child link frame
     if (this->childLink)
@@ -848,7 +851,6 @@
           this->wrench.body1Torque);
       }
     }
-    this->wrench = this->wrench - wrenchAppliedWorld;
   }
   else
   {

[osrf-migration]

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

---

• set version to "all"

[osrf-migration]

Original comment by Joseph Coombe (Bitbucket: Joseph Coombe).

---

Is this a bug and, if so, how serious?

I'm working on a Gazebo Plugin that controls a joint using force-feedback, and I'm curious if/how this might affect the controller.

Thanks!

[osrf-migration]

Original comment by Jack Liu (Bitbucket: jacknlliu).

---

Is this related with issue #2456 ?