Joints in ODE & Gazebo

Anchor

Set Anchor
- In gazebo: void ODEHingeJoint::SetAnchor(unsigned int /*index*/, const math::Vector3 &_anchor)
  - _anchor is the coordinate of anchor (joint)
- In ode: dJointSetHingeAnchor (dxJointHinge *joint, dReal x, dReal y, dReal z)
  - Implementation: setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
  - $anchor[i] = body[i].R \cdot ([x;y;z] - body[i].pos)$
  - hingeComputeInitialRelativeRotation (joint);
  - qrel = rotation of body 2 from body 1's perspective
  - $qrel = q_1^* \cdot q_2$
Get Anchor
- In ode extern "C" void dJointGetHingeAnchor (dxJointHinge *joint, dVector3 result)
  - Implementation: getAnchor (joint,result,joint->anchor1);

Axis

Set Axis

In gazebo: ODEHingeJoint::SetAxis(unsigned int _index, const math::Vector3 &_axis)

   // ODE needs global axis
  math::Quaternion axisFrame = this->GetAxisFrame(0); // equivalent to this->GetWorldPose().rot;
  math::Vector3 globalAxis = axisFrame.RotateVector(_axis);	 if (this->jointId)
    dJointSetHingeAxis(this->jointId, globalAxis.x, globalAxis.y, globalAxis.z);

In ode:
- Impl: setAxes (joint,x,y,z,joint->axis1,joint->axis2);
  - $axis1 = body[0].R \cdot normalize([x;y;z])$
  - $axis2 = body[1].R \cdot normalize([x;y;z])$
- hingeComputeInitialRelativeRotation (joint);

Get Axis
- In gazebo: math::Vector3 ODEHingeJoint::GetGlobalAxis(unsigned int /*_index*/) const
  - Impl: dJointGetHingeAxis(this->jointId, result);
- In ode: void dJointGetHingeAxis (dxJointHinge *joint, dVector3 result)
  - Impl: getAxis (joint,result,joint->axis1);
  - $axis = body[0].R \cdot axis1$

Angle

Get Angle

In gazebo: ODEHingeJoint::GetAngleImpl(unsigned int /*index*/) const
- result = dJointGetHingeAngle(this->jointId);
In ode:
- dReal ang = getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1, joint->qrel);
GetHingeAngle Function
- dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis, dQuaternion q_initial)
- get qrel = relative rotation between the two bodies
  - $q_{current} = q_1^* \cdot q_2$
  - $qrel = q_{current} \cdot q_{initial}^*$
- then getHingeAngleFromRelativeQuat (qrel,axis)

static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis)

$q = [s,; \vec{v}] = [\cos (\theta/2) ,; \sin(\theta/2) \cdot \vec{u}]$
we can only get $\left|\sin(\theta/2)\right| = \left|v\right|$
implementation:

 static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis)
 {
   // the angle between the two bodies is extracted from the quaternion that
   // represents the relative rotation between them. recall that a quaternion
   // q is:
   //    [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
   // where s is a scalar and v is a 3-vector. u is a unit length axis and
   // theta is a rotation along that axis. we can get theta/2 by:
   //    theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
   // but we can't get sin(theta/2) directly, only its absolute value, i.e.
   //    |v| = |sin(theta/2)| * |u|
   //        = |sin(theta/2)|
   // using this value will have a strange effect. recall that there are two
   // quaternion representations of a given rotation, q and -q. typically as
   // a body rotates along the axis it will go through a complete cycle using
   // one representation and then the next cycle will use the other
   // representation. this corresponds to u pointing in the direction of the
   // hinge axis and then in the opposite direction. the result is that theta
   // will appear to go "backwards" every other cycle. here is a fix: if 
   // points "away" from the direction of the hinge (motor) axis (i.e. more
   // than 90 degrees) then use -q instead of q. this represents the same
   // rotation, but results in the cos(theta/2) value being sign inverted.
 
   // extract the angle from the quaternion. cost2 = cos(theta/2),
   // sint2 = |sin(theta/2)|
   dReal cost2 = qrel[0];
   dReal sint2 = dSqrt (qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]);
   dReal theta = (dDOT(qrel+1,axis) >= 0) ?	// @@@ padding assumption
     (2 * dAtan2(sint2,cost2)) :		// if u points in direction of axis
     (2 * dAtan2(sint2,-cost2));		// if u points in opposite direction
 
   // the angle we get will be between 0..2*pi, but we want to return angles
   // between -pi..pi
   if (theta > M_PI) theta -= 2*M_PI;
 
   // the angle we've just extracted has the wrong sign
   theta = -theta
 
   return theta;
 }

Get Velocity

In gazebo: ODEHingeJoint::GetVelocity(unsigned int /*index*/) const
- calls dJointGetHingeAngleRate(this->jointId)

In ode:

implementation

 dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1);
 dReal rate = dDOT(axis,joint->node[0].body->avel);
 if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);

Torque

Add force
- In gazebo: ODEHingeJoint::SetForceImpl(unsigned int, double effort)
  - calls dJointAddHingeTorque(this->jointId, effort);
- In ode:
  - extern "C" void dJointAddHingeTorque (dxJointHinge *joint, dReal torque)
  - torque size: effort
  - torque axis: getAxis (joint,axis,joint->axis1);
  - axis = axis * torque
  - add torque to body 0:
    - dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]);
  - add torque to body 1:
    - dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]);
- Implementation: dBodyAddTorque(body, fx, fy, fz)
  - body->tacc += [fx, fy, fz]

gnoliyil/hinge-joints-in-ode-gazebo.md

Select an option

No results found

Select an option

No results found

Joints in ODE & Gazebo

Anchor

Axis

Angle

Torque