Incorrect integration of generalized coordinates for BallJoint and FreeJoint

[jslee02]

BallJoint has 3-dimensional generalized coordinates and uses exponential mapping to represent its rotation matrix as

R = exp([q])          (1)

where R is 3-by-3 rotation matrix, exp() is exponential map, q is 3-dimensional vector, and [q] is a skew-symmetric matrix of q.

We currently integrate the generalized coordinates for Euler integration in the following manner

q(k + 1) = q(k) + dq(k) * h          (2)

where dq(k) is the time derivative at k-th time step and h is time step of integration. Then the next step of rotation matrix can be obtained as

R(k + 1) = exp([q(k + 1)])
         = exp([q(k) + dq(k) * h]).          (3)

However, rotation matrix should be integrated in geometric manner as

R(k + 1) = R(k) * exp([dq(k) * h]).          (4)

Equation (3) and (4) are not equivalent unless the multiplication operator is commutative for exp([q(k)]) and exp([dq(k) * h]) which means

exp([q(k)]) * exp([dq(k) * h]) = exp([dq(k) * h]) * exp([q(k)])          (5)

To satisfy equation (5), [q(k)] and [dq(k) * h] should be commutative for multiplication operator by the definition of exponential map of matrix. This is possible when the direction of q(k) and dq(k) are same that barley happens in most dynamic simulation.

FreeJoint uses exponential map to represent its rotation so it also has same issue with BallJoint.

We are planning to fix this bug at DART 4.0.

[jslee02]

70698dd is just workaround. We need to fix this in right way at some point.

[ana-GT]

Hi, I am using Dart on branch release-3.0 and am having a problem with FreeJoint:

If I use the current version of updateTransform for FreeJoint, my simulation does not work properly when the object has a rotation.

Example: For a freeJoint J, if I set q = (1.57, 0, 0, 2.1, -0.6, 1.4)
I would expect to see J.mT (the local transform of the joint) to be:

1, 0, 0, 2.1
0, 0, -1, -0.6
0, 1, 0, 1.4
0, 0, 0, 1

However, using the current version of updateTransform, mT gives me this:

1, 0, 0, 2.1
0, 0, -1, -1.27
0, 1, 0, 0.509
0, 0, 0, 1

This messes up the translation part. If I use the old code (from commit 93cff20 of Dec 14), I get the former, expected matrix.
May this be a bug on the patch?

Notes:

1. Current updateTransform function:

void FreeJoint::updateTransform() {
// TODO(JS): This is workaround for Issue #122.
mT_Joint = math::expMap(get_q());
mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse();
assert(math::verifyTransform(mT));
}

1. Old updateTransform (that works for me):
   void FreeJoint::updateTransform() {
   Eigen::Vector3d q1(mCoordinate[0].get_q(),
   mCoordinate[1].get_q(),
   mCoordinate[2].get_q());
   Eigen::Vector3d q2(mCoordinate[3].get_q(),
   mCoordinate[4].get_q(),
   mCoordinate[5].get_q());

   mT = mT_ParentBodyToJoint
   * Eigen::Translation3d(q2)
   * math::expAngular(q1)
   * mT_ChildBodyToJoint.inverse();

   assert(math::verifyTransform(mT));
   }

[jslee02]

The original configuration space for rigid body is SE(3) (Special Eucliean Group) and we can use Eigen::Isometry3d to represent it. However, we use real vector to represent configuration of skeleton so SE(3) also should be parametrized to 6-dimensional vector. Problem is that the ways of parametrizing SE(3) to 6-dimensional vector is not unique.

For example:
f1(q) = Trans(q2) * ExpRot(q1)
f2(q) = Trans(q2) * EulerXYZ(q1)
f3(q) = Exp(q)
where q1 is 3-dimensional vector of upper part of q, q2 is 3-dimensional vector of lower part of q.

Previously, FreeJoint used f1 ("Old updateTransform" on your notes) but now f3 ("Current updateTransform function" on your notes). I guess your expecting prametrization function is f1 or f2. So the issue you mentioned is not bug but it just comes from using different parametrizing function.

Using parametrized 6-dimensional vector for SE(3) has some issues not only the multiple parametrizing ways but also singularity especially when we use Euler angles. So I'm planning to use SE(3) directly for the configuration of FreeJoint.

By the way, until we use SE(3) for FreeJoint configuration, I guess we don't need to change from f1 to f3 unless there is a reason. I'll double check if there isn't other issue when we roll back to use f1 again.