Finding Contact Jacobian by mirroring internal function CalcNormalAndTangentContactJacobians()

Question

I'm trying to find contact Jacobian defined as M(q) q_ddot + C(q, q_dot) = G(q) + B(q) u + J'(q) lambda.

I tried to mirror the function CalcNormalAndTangentContactJacobians() as in multibody_plant.cclink but the values are not adding up.

Here is how I setup the system (for the simplest setup possible):

I have a cylinder mounted to world through a revolute joint and a ball that can freely move in the x-y plant through dummy prismatic joints. I run the simulation that drives the joint at constant torque to make contact with the ball and push it off. I log the states of the system through simulation and find an instance when there is contact and look at that particular instance.

I set the MultibodyPlant(MBP) to the states as exactly in the simulation by SetPositionsAndVelocities and SetActuationInArray, and get the relative variables M, Cv, tauG, B, q_ddot, tauExt, and contactResults.

Now I'm ready to find the contact Jacobian. (Updated in reflection to the two suggestions from @Alejandro)

# CalcNormalAndTangentContactJacobians

Jn = np.zeros((num_contact, plant.num_velocities()))
Jt = np.zeros((2*num_contact, plant.num_velocities()))

if num_contact > 0:
    frame_W = plant.world_frame()
    for i in range(num_contact):

        penetration_point_pairs = query_object.ComputePointPairPenetration()

        penetration_point_pair = penetration_point_pairs[0]

        geometryA_id = penetration_point_pair.id_A;
        geometryB_id = penetration_point_pair.id_B;

        linkA = get_name_by_geometryId(plant, geometryA_id)
        bodyA = plant.GetBodyByName(linkA)
        linkB = get_name_by_geometryId(plant, geometryB_id)
        bodyB = plant.GetBodyByName(linkB)

        nhat_BA_W = penetration_point_pair.nhat_BA_W;
        p_WCa = penetration_point_pair.p_WCa;
        p_WCb = penetration_point_pair.p_WCb;

        p_ACa = plant.EvalBodyPoseInWorld(plant_context, bodyA).inverse().multiply(p_WCa)
        p_BCb = plant.EvalBodyPoseInWorld(plant_context, bodyB).inverse().multiply(p_WCb)


        wrt = JacobianWrtVariable.kV
        Jv_WAc = plant.CalcJacobianTranslationalVelocity(plant_context, wrt, \
                                                         bodyA.body_frame(), p_ACa, frame_W, frame_W);
        Jv_WBc = plant.CalcJacobianTranslationalVelocity(plant_context, wrt, \
                                                         bodyB.body_frame(), p_BCb, frame_W, frame_W);


        Jn[i,:] = nhat_BA_W.reshape((1,-1)).dot(Jv_WAc - Jv_WBc);

        R_WC = ComputeBasisFromAxis(2, nhat_BA_W)

        that1_W = R_WC[:,0]
        that2_W = R_WC[:,1]

        Jt[0 ,:] = that1_W.transpose().dot(Jv_WBc - Jv_WAc)
        Jt[1,:] = that2_W.transpose().dot(Jv_WBc - Jv_WAc)

        ft = R_WC[:,0:2].T.dot(contact_force) 
        fn = -R_WC[:,2].dot(contact_force)

        ## Equivalent to:
        #contact_terms = (Jv_WBc - Jv_WAc).T.dot(contact_force)
        contact_terms = Jn.T.dot(fn) + Jt.T.dot(ft).flatten()


        print("===========================")
        print("without contact terms:")
        print(M.dot(v_dot) + Cv - tauG - tauExt - B.dot(u) )
        print("expected contact forces: ")
        print(plant.get_generalized_contact_forces_output_port(model).Eval(plant_context))
        print("contact terms:")
        print(contact_terms.flatten())
        print("residule")
        print(M.dot(v_dot) + Cv - tauG - tauExt - B.dot(u) - contact_terms.flatten())
        print("l2 norm of residual:")
        print(np.linalg.norm(M.dot(v_dot) + Cv - tauG - tauExt - B.dot(u) - contact_terms.flatten()))
        print("Jn:")
        print(Jn)
        print("Jt:")
        print(Jt)

Looking at the results through printouts I get:

expected contact forces: 
[  1.12529061   5.90160213 -10.10437394]
contact terms I got:
[  1.12532645   5.90160213 -10.10437394]
residule
[-3.58441669e-05 -8.88178420e-16  1.77635684e-15]
l2 norm of residual:
3.5844166926812804e-05
Jn:
[[ 0.09613277  0.5110166  -0.85957043]]
Jt:
[[-4.99995256e-03  8.59570734e-01  5.11016784e-01]
 [ 8.09411844e-05  4.30262131e-04 -7.23735008e-04]]

Here is the correct contact Jacobian for reference(I get these by deriving the contact Jacobian via definition):

Expected Jn:
[[ 0.09613277  0.5110166  -0.85957043]]
Expected Jt:
[[-4.60458428e-03  8.59570734e-01  5.11016784e-01]
 [ 8.09411844e-05  4.30262131e-04 -7.23735008e-04]]
residuals using these: 
[ 4.44089210e-16 -8.88178420e-16  1.77635684e-15]

The difference is quite small (for this very simple setup) so maybe it's not completely wrong but the scale of residual is still concerning. Notice that the first term is wrong. In a more complex system, all revolute joints have residules from experimental observation.

I really don't understand what's causing this residual, and this residual is much much larger in a more complex system to a scale equivalent to residuals without contact terms(even when there is just one more joint, I can add more plots if anyone's interested).

For reference, here's the residuals over the coarse of the entire 0.5 sec simulation:

Other things I have tried that also didn't work out:

Using SignedDistancePair instead of PenetrationAsPointPair by query_object.ComputeSignedDistancePairClosestPoints(geometryA_id, geometryB_id)
Using relative frames

p_GbCb = signed_distance_pair.p_BCb p_GaCa = signed_distance_pair.p_ACa Jv_v_BCa_W = plant.CalcJacobianTranslationalVelocity(plant_context, wrt, frameB, p_GbCb, frameA, frame_W);
Using JacobianWrtVariable.kQDot instead of JacobianWrtVariable.kV
Using inspector.GetPoseInFrame to make sure the pose is in the frame we think it should be in as referencing this file

from last point:

p_GaCa = signed_distance_pair.p_ACa
p_ACa = inspector.GetPoseInFrame(signed_distance_pair.id_A).rotation().matrix().dot(p_GaCa)
p_GbCb = signed_distance_pair.p_BCb
p_BCb = inspector.GetPoseInFrame(signed_distance_pair.id_B).rotation().matrix().dot(p_GbCb)
Jv_v_BCa_W = plant.CalcJacobianTranslationalVelocity(plant_context, wrt, frameA, p_ACa, frameB, frame_W);

Solution:

Thanks to @Alejandro, I finally figured out the issue: contact point is the middle point between p_WCa and p_WCb. Using p_WCa and p_WCb in CalcJacobianTranslationalVelocity will result in a small residual as described above. This is perhaps due to how the physics engine is built. While I don't have further insights in explaining how numerical values are calculated under the hood, here I would like to share two ways of finding the contact Jacobian that produce identical results that concludes zero residual in the contact dynamics equation:

Method 1, with PointPairPenetration

penetration_point_pairs = query_object.ComputePointPairPenetration()
penetration_point_pair = penetration_point_pairs[0]

geometryA_id = penetration_point_pair.id_A;
geometryB_id = penetration_point_pair.id_B;

linkA = get_name_by_geometryId(plant, geometryA_id)
bodyA = plant.GetBodyByName(linkA)
linkB = get_name_by_geometryId(plant, geometryB_id)
bodyB = plant.GetBodyByName(linkB)

nhat_BA_W = penetration_point_pair.nhat_BA_W;
p_WCa = penetration_point_pair.p_WCa;
p_WCb = penetration_point_pair.p_WCb;

X_WA = plant.EvalBodyPoseInWorld(plant_context, bodyA)
X_WB = plant.EvalBodyPoseInWorld(plant_context, bodyB)
p_ACa = X_WA.inverse().multiply((p_WCa+p_WCb)/2)
p_BCb = X_WB.inverse().multiply((p_WCa+p_WCb)/2)

wrt = JacobianWrtVariable.kV
Jv_WAc = plant.CalcJacobianTranslationalVelocity(plant_context, wrt, \
                                                 bodyA.body_frame(), p_ACa, frame_W, frame_W);
Jv_WBc = plant.CalcJacobianTranslationalVelocity(plant_context, wrt, \
                                                 bodyB.body_frame(), p_BCb, frame_W, frame_W);

Jv = Jv_WBc - Jv_WAc

Method 2: with SignedDistancePairClosestPoints

signed_distance_pair = query_object.ComputeSignedDistancePairClosestPoints(geometryA_id, geometryB_id)
inspector = query_object.inspector()
linkA = get_name_by_geometryId(plant, geometryA_id)
bodyA = plant.GetBodyByName(linkA)
linkB = get_name_by_geometryId(plant, geometryB_id)
bodyB = plant.GetBodyByName(linkB)

nhat_BA_W = signed_distance_pair.nhat_BA_W;
p_GaCa = signed_distance_pair.p_ACa;
p_GbCb = signed_distance_pair.p_BCb;

X_AGa = inspector.GetPoseInFrame(signed_distance_pair.id_A) #Reports the pose of the geometry G with the given id in its registered frame F X_FG.
X_BGb = inspector.GetPoseInFrame(signed_distance_pair.id_B)

p_ACa = X_AGa.multiply(p_GaCa)
p_BCb = X_BGb.multiply(p_GbCb)

X_WA = plant.EvalBodyPoseInWorld(plant_context, bodyA)
X_WB = plant.EvalBodyPoseInWorld(plant_context, bodyB)
p_WCa = X_WA.multiply(p_ACa)
p_WCb = X_WB.multiply(p_BCb)

p_ACa = X_WA.inverse().multiply((p_WCa+p_WCb)/2)
p_BCb = X_WB.inverse().multiply((p_WCa+p_WCb)/2)

Jv_WAc = plant.CalcJacobianTranslationalVelocity(plant_context, wrt, \

Residuals
Finally here's how the residuals look with aforementioned method1 and method2. 1e-15 is essentially zero :)

Alejandro · Accepted Answer · 2020-05-14T03:26:30.267

2

Your problem seems to be in two places;

Problem 1

Jv_WAc = plant.CalcJacobianTranslationalVelocity(plant_context, wrt, \
                                                 bodyA.body_frame(), p_WCa, \
                                                 frame_W, frame_W);
Jv_WBc = plant.CalcJacobianTranslationalVelocity(plant_context, wrt, \
                                                 bodyB.body_frame(), p_WCb, \
                                                 frame_W, frame_W);

Monogram notation always tells the truth. Notice that, per documentation for CalcJacobianTranslationalVelocity(), the position vectors must be expressed on frame_B. I can tell very quickly from the monogram notation in the documentation p_BoBi_B, which tells me the position vector is from B's origin Bo to the i-th point Bi, expressed in frame B.

This is very important, and the reason for your bug I believe. In your case, you are passing p_WCa, the position from Wo (world's origin) to point Ca, expressed in the world. A completely different object. Just by luck (the particular configuration in 2D), the other terms seem ok.

So, the solution. Get the pose of A (B) in W, X_WA. Then do: p_ACa = X_WA.inverse() * p_WCa. Now p_ACa (= p_ACa_A if you follow our monogram notation), is what you need, the position vector from Ao to Ca, expressed in A. Similarly for body B.

Problem 2

X_WA = plant.EvalBodyPoseInWorld(plant_context, bodyA).rotation().matrix()

.rotation is wrong. What you need is the RigidTransform object, i.e.:

X_WA = plant.EvalBodyPoseInWorld(plant_context, bodyA)

Please, do not convert to matrix unless there is a good reason for it. Also, per our monogram notation X is reserved for RigidTransform objects while R is reserved for RotationMatrix objects.

That is the reason I did not detect this on the first pass.

Problem 3

When using signed distances, similar bug as before.

p_ACa = inspector.GetPoseInFrame(signed_distance_pair.id_A).rotation().matrix().dot(p_GaCa)

should not use .rotation(). A way to avoid these problems is by using good notation. I recommend using our monogram notation which pretty much tells you what to do simply by pattern matching.

That is, write the above as:

X_AGa = inspector.GetPoseInFrame(signed_distance_pair.id_A)
p_ACa = X_AGa.multiply(p_GaCa)

otherwise p_GaCa_A = X_AGa.rotation().multiply(p_GaCa), which is not the same object.

Summary

Could we just stick to a single implementation? say with PenetraionAsPointPair? notice that, unfortunately, PenetraionAsPointPair and SignedDistancePair have different APIs that do not parallel each other. That is, while PenetraionAsPointPair report p_WCa (inspector not needed), SignedDistancePair reports p_GaCa (inspector needed to account for X_AGa).

I hope this solves your problem.

edited May 14 '20 at 03:26

answered May 13 '20 at 12:59

Alejandro

1,064
1
8
22

Thanks for pointing that out!! So I tried `X_WA = plant.EvalBodyPoseInWorld(plant_context, bodyA).rotation().matrix()` , `p_ACa = X_WA.T.dot(p_WCa)`, and the same for B. Using them for `CalcJacobianTranslationalVelocity()` still has a nonzero residual. Looking more into details, `Jn` now appears correct and one of the entries of `Jt` is correct, too but `Jt[1,:]` is still not the same as expected. – zliu May 13 '20 at 13:37
1

I believe that still is wrong, because you need the inverse of X_WA. So, more like `p_ACa = X_WA.inverse().multiply(p_WCa) `. I would avoid using `.T` if there is no good reason to use the original objects, which in this case encodes more meaning for rigid transforms. – Alejandro May 13 '20 at 15:12
The inverse of a rotation matrix should be identical to its transpose? However, I did try to use .inverse() just to confirm that they're indeed the same and the results are identical. – zliu May 13 '20 at 16:47
Also confirming `inverse().multiply(p)` returns the same results as directly using the rotation matrix. But thanks for pointing out that it's good practice to use the original objects. – zliu May 13 '20 at 19:50
1

no, X_WA is not a rotation, is a RigidTransform object. It's inverse is not the transpose of the matrix. That is why I suggested not to use `.T`. If you did use `.inverse()`, then I don't know what else from your code could be wrong. Do you have something self contained I could run? (with instruction on how to run it please) – Alejandro May 13 '20 at 21:52
1

I just found out a second bug. Please see my edited response (Problem 1 + Problem 2) now. – Alejandro May 13 '20 at 21:57
First of all thank you so so much for looking into it! I tried `p_ACa = plant.EvalBodyPoseInWorld(plant_context, bodyA).inverse().multiply(p_WCa)` and same for B and using `p_ACa` for `CalcJacobianTranslationalVelocity` and am observing identical results as above, although I do agree that I need the homogeneous transformation instead of just rotation. This is perhaps because the affected link has its origin fixed at world origin through the revolute joint. The problem is, there is still the residuals. – zliu May 13 '20 at 22:58
I updated the findings in the last comment to the OP. – zliu May 13 '20 at 23:03
that last snippet also has a bug. PTAL. – Alejandro May 14 '20 at 03:26
1

For examples these small I usually write down the math by hand and print everything out. At some point something is not what I expect and that helps me find the problem. No shortcuts unfortunately to analyze numeric of things. Things to keep in mind: 1) X_WA is the **pose** of A in W, which is different from 2) R_WA the rotation matrix of A in W. So be very careful when you call `.rotation()`, it might not be what you want. – Alejandro May 14 '20 at 03:29
Thank you so so so much for looking in depth into this problem and all your suggestions! I finally figured out the issue. instead of using `p_WCa` and `p_WCb` I needed the middle point between the two `(p_WCa + p_WCb)/2` for the dynamics equation to check off. Adding detailed findings to the OP shortly. – zliu May 14 '20 at 14:54