bkloster

Hello all,
I'm using many one-DOF angular joints for my robots. The real robot's joints (servos) send their current angle to their controller, so we need this kind of feedback in the simulation as well. Is there a member variable / function I can access on a PhysicsJoint to read the current angle

At the moment, I use the orientations of the connected entities to calculate the angle but there are some problems with that (like always returning positive angles, regardless of the position of the joint).

Thanks,
Ben


Re: Microsoft Robotics - Simulation Current joint angle

bkloster

Code Snippet

private double GetJointAngle(PhysicsJoint joint)
{
// Extract some variables
EntityJointConnector con1, con2;
con1 = joint.State.Connectors[0];
con2 = joint.State.Connectors[1];
Entity seg1, seg2;
seg1 = (Entity) con1.Entity;
seg2 = (Entity) con2.Entity;
Vector3 jointAxis = con1.JointAxis;
Quaternion q1 = seg1.State.Pose.Orientation;
Quaternion q2 = seg2.State.Pose.Orientation;

// Calculate angle (unsigned)
double phi = 2 * Math.Acos(
QuaternionDotProduct(Quaternion.Normalize(q1),
Quaternion.Normalize(q2)));

// Calculate parallelepipedial product of the orientation of the entities and the
// joint axis
Quaternion inverse = Quaternion.Normalize(InvertQuaternion(q1));
Quaternion vectorAsQuat = new Quaternion(con1.JointAxis.X,
con1.JointAxis.Y,
con1.JointAxis.Z,
0);
Quaternion rightproduct = Quaternion.Multiply(vectorAsQuat, inverse);
Quaternion rotatedVectorAsQuat = Quaternion.Multiply(Quaternion.Normalize(q1),
rightproduct);
Vector3 jointAxisInGlobal = new Vector3(rotatedVectorAsQuat.X,
rotatedVectorAsQuat.Y,
rotatedVectorAsQuat.Z);
Vector3 seg1Rot = Quaternion.ToAxisAngle(seg1.State.Pose.Orientation).Axis;
Vector3 seg2Rot = Quaternion.ToAxisAngle(seg2.State.Pose.Orientation).Axis;
double sign = Vector3.Dot(jointAxisInGlobal, Vector3.Cross(seg2Rot, seg1Rot));
phi = phi * Math.Sign(sign);

return phi;
}


Maybe someone finds it useful. Of course, it only works for hinge joints (one rotational degree of freedom). But beware, it's not thoroughly tested.




Re: Microsoft Robotics - Simulation Current joint angle

Alan Oursland

Thank you, bkloster. This was very helpful.




Re: Microsoft Robotics - Simulation Current joint angle

bkloster

The above contains an error. It sometimes messes up the sign of the angle when the joint's axis or the entities' orientation have near-zero elements. This should work:


/// <summary>
/// Extracts the current angle of a hinge joint
/// </summary>
/// <param name="joint">The joint, should be a hinge joint (1 rotational DOF)</param>
/// <returns>The current angle of the joint in radians</returns>
private float GetJointAngle(PhysicsJoint joint)
{
// Extract some variables
EntityJointConnector con1 = joint.State.Connectors[0];
EntityJointConnector con2 = joint.State.Connectors[1];
Entity seg1 = (Entity)con1.Entity;
Entity seg2 = (Entity)con2.Entity;
Quaternion q1 = seg1.State.Pose.Orientation;
Quaternion q2 = seg2.State.Pose.Orientation;
Vector3 jointAxis = con1.JointAxis;


// Calculate angle (unsigned)
float phi = GetAngleBetweenQuaternions(q1, q2);

// Calculate parallelepipedial product of the orientation of the entities and the
// joint axis to determine the sign of the angle
Vector3 jointAxisInGlobal = RotateVectorByQuaternion(con1.JointAxis, q1);
Vector3 seg1Rot = RotateVectorByQuaternion(con1.JointNormal, q1);
Vector3 seg2Rot = RotateVectorByQuaternion(con2.JointNormal, q2);
int sign = Math.Sign(GetParallelEpipedialProduct(jointAxisInGlobal, seg2Rot, seg1Rot));
phi = phi * sign;
return phi;
}

#region Quaternion and Vector Fun
/// <summary>
/// Rotates a vector by the rotation defined by the Quaternion
/// </summary>
/// <param name="v">The vector to be rotated</param>
/// <param name="q">The quaternion that defines the rotation</param>
/// <returns>The rotated vector</returns>
private Vector3 RotateVectorByQuaternion(Vector3 v, Quaternion q)
{
Quaternion inverse = Quaternion.Normalize(InvertQuaternion(q));
Quaternion vectorAsQuat = new Quaternion(v.X, v.Y, v.Z, 0);
Quaternion rightproduct = Quaternion.Multiply(vectorAsQuat, inverse);
Quaternion rotatedVectorAsQuat = Quaternion.Multiply(Quaternion.Normalize(q), rightproduct);
Vector3 result = new Vector3(rotatedVectorAsQuat.X,
rotatedVectorAsQuat.Y,
rotatedVectorAsQuat.Z);
return result;
}

/// <summary>
/// Parallelepipedial product of the three vectors
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <param name="v3"></param>
/// <returns></returns>
private float GetParallelEpipedialProduct(Vector3 v1, Vector3 v2, Vector3 v3)
{
return Vector3.Dot(v1, Vector3.Cross(v2, v3));
}

private float GetAngleBetweenQuaternions(Quaternion q1, Quaternion q2)
{
float dotProduct = QuaternionDotProduct(Quaternion.Normalize(q1),
Quaternion.Normalize(q2));
float phi = (float)(2 * Math.Acos(dotProduct));
return phi;
}

private float QuaternionDotProduct(Quaternion q1, Quaternion q2)
{
float product = q1.W * q2.W + q1.X * q2.X + q1.Y * q2.Y + q1.Z * q2.Z;
if (product > 1)
{
return 1;
}
else if (product < -1)
{
return -1;
}
return product;
}

private Quaternion InvertQuaternion(Quaternion q)
{
return new Quaternion(-q.X, -q.Y, -q.Z, q.W);
}
#endregion