Commits

John Hsu  committed 8e110a0

a patch to potential ODEJoint::GetForceTorque segfault on startup

  • Participants
  • Parent commits aef04df
  • Branches get_force_torque_patch

Comments (0)

Files changed (1)

File gazebo/physics/ode/ODEJoint.cc

   // f2, t2 are the force torque measured on parent body's cg
   // f1, t1 are the force torque measured on child body's cg
   dJointFeedback* fb = this->GetFeedback();
-  wrench.body1Force.Set(fb->f1[0], fb->f1[1], fb->f1[2]);
-  wrench.body1Torque.Set(fb->t1[0], fb->t1[1], fb->t1[2]);
-  wrench.body2Force.Set(fb->f2[0], fb->f2[1], fb->f2[2]);
-  wrench.body2Torque.Set(fb->t2[0], fb->t2[1], fb->t2[2]);
+  if (fb)
+  {
+    wrench.body1Force.Set(fb->f1[0], fb->f1[1], fb->f1[2]);
+    wrench.body1Torque.Set(fb->t1[0], fb->t1[1], fb->t1[2]);
+    wrench.body2Force.Set(fb->f2[0], fb->f2[1], fb->f2[2]);
+    wrench.body2Torque.Set(fb->t2[0], fb->t2[1], fb->t2[2]);
 
-  {
-    // convert torque from about child CG to joint anchor location
-    // cg position specified in child link frame
-    math::Vector3 cgPos = this->childLink->GetInertial()->GetPose().pos;
-    // moment arm rotated into world frame (given feedback is in world frame)
-    math::Vector3 childMomentArm =
-      this->childLink->GetWorldPose().rot.RotateVector(
-      this->anchorPos - cgPos);
+    if (this->childLink)
+    {
+      // convert torque from about child CG to joint anchor location
+      // cg position specified in child link frame
+      math::Vector3 cgPos = this->childLink->GetInertial()->GetPose().pos;
+      // moment arm rotated into world frame (given feedback is in world frame)
+      math::Vector3 childMomentArm =
+        this->childLink->GetWorldPose().rot.RotateVector(
+        this->anchorPos - cgPos);
 
-    // gzerr << "anchor [" << anchorPos
-    //       << "] iarm[" << this->childLink->GetInertial()->GetPose().pos
-    //       << "] childMomentArm[" << childMomentArm
-    //       << "] f1[" << wrench.body1Force
-    //       << "] t1[" << wrench.body1Torque
-    //       << "] fxp[" << wrench.body1Force.Cross(childMomentArm)
-    //       << "]\n";
+      // gzerr << "anchor [" << anchorPos
+      //       << "] iarm[" << this->childLink->GetInertial()->GetPose().pos
+      //       << "] childMomentArm[" << childMomentArm
+      //       << "] f1[" << wrench.body1Force
+      //       << "] t1[" << wrench.body1Torque
+      //       << "] fxp[" << wrench.body1Force.Cross(childMomentArm)
+      //       << "]\n";
 
-    wrench.body1Torque += wrench.body1Force.Cross(childMomentArm);
-  }
+      wrench.body1Torque += wrench.body1Force.Cross(childMomentArm);
+    }
 
-  // convert torque from about parent CG to joint anchor location
-  if (this->parentLink)
-  {
-    // parent cg specified in child link frame
-    math::Vector3 cgPos = ((this->parentLink->GetInertial()->GetPose() +
-                          this->parentLink->GetWorldPose()) -
-                          this->childLink->GetWorldPose()).pos;
+    // convert torque from about parent CG to joint anchor location
+    if (this->parentLink)
+    {
+      // parent cg specified in child link frame
+      math::Vector3 cgPos = ((this->parentLink->GetInertial()->GetPose() +
+                            this->parentLink->GetWorldPose()) -
+                            this->childLink->GetWorldPose()).pos;
 
-    // rotate moement arms into world frame
-    math::Vector3 parentMomentArm =
-      this->childLink->GetWorldPose().rot.RotateVector(
-      this->anchorPos - cgPos);
+      // rotate moement arms into world frame
+      math::Vector3 parentMomentArm =
+        this->childLink->GetWorldPose().rot.RotateVector(
+        this->anchorPos - cgPos);
 
-    wrench.body2Torque -= wrench.body2Force.Cross(parentMomentArm);
+      wrench.body2Torque -= wrench.body2Force.Cross(parentMomentArm);
 
-    // A good check is that
-    // the computed body2Torque shoud in fact be opposite of body1Torque
+      // A good check is that
+      // the computed body2Torque shoud in fact be opposite of body1Torque
+    }
+    else
+    {
+      // convert torque from about child CG to joint anchor location
+      // or simply use equal opposite force as body1 wrench
+      wrench.body2Force = -wrench.body1Force;
+      wrench.body2Torque = -wrench.body1Torque;
+    }
   }
   else
   {
-    // convert torque from about child CG to joint anchor location
-    // or simply use equal opposite force as body1 wrench
-    wrench.body2Force = -wrench.body1Force;
-    wrench.body2Torque = -wrench.body1Torque;
+    // forgot to set provide_feedback?
+    gzwarn << "GetForceTorque: forget to set <provide_feedback>?\n";
   }
 
   return wrench;