drake - 通过镜像内部函数 CalcNormalAndTangentContactJacobians() 寻找接触雅可比矩阵

Question

我试图找到定义为 M(q) q_ddot + C(q, q_dot) = G(q) + B(q) u + J'(q) lambda 的接触雅可比矩阵。

我尝试将函数 CalcNormalAndTangentContactJacobians() 镜像为 multibody_plant.cc link但这些值并没有加起来。

以下是我设置系统的方式(为了尽可能简单的设置):

我有一个通过旋转关节安装到世界的圆柱体和一个可以通过虚拟棱柱关节在 x-y 工厂中自由移动的球。我运行模拟，以恒定扭矩驱动关节与球接触并将其推开。我通过模拟记录系统的状态，并在有联系时找到一个实例并查看该特定实例。

我通过 SetPositionsAndVelocities 和 SetActuationInArray 将 MultibodyPlant(MBP) 设置为完全模拟中的状态，并获取相关变量 M、Cv、tauG、B、 q_ddot、tauExt 和 contactResults。

现在我已准备好查找接触 Jacobian。 (根据@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)

通过我得到的打印输出查看结果:

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]]

这里是正确的接触雅可比行列式供引用(我通过定义推导接触雅可比得到这些):

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]

差异非常小(对于这个非常简单的设置)，所以也许它不是完全错误的，但残差的规模仍然令人担忧。请注意，第一项是错误的。在更复杂的系统中，所有旋转关节都有实验观察的残留。

我真的不明白是什么导致了这个残差，这个残差在一个更复杂的系统中要大得多，其规模相当于没有接触项的残差(即使只有一个关节，我可以添加更多的图如果有人感兴趣的话)。

作为引用，这里是整个 0.5 秒模拟的粗略残差:

我尝试过但也没有成功的其他事情:

• query_object.ComputeSignedDistancePairClosestPoints(geometryA_id, geometryB_id)

使用 SignedDistancePair 而不是 PenetrationAsPointPair

• 使用相对帧

  p_GbCb = signed_distance_pair.p_BCbp_GaCa = signed_distance_pair.p_ACaJv_v_BCa_W = plant.CalcJacobianTranslationalVelocity(plant_context, wrt, frameB, p_GbCb, frameA, frame_W);

• 使用 JacobianWrtVariable.kQDot 代替 JacobianWrtVariable.kV

• 使用 inspector.GetPoseInFrame 确保姿势在我们认为它应该在的框架中作为引用 this file

最后一点:

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);

---

解决方案:

感谢@Alejandro，我终于弄明白了这个问题:接触点是p_WCa 和p_WCb 之间的中间点。在 CalcJacobianTranslationalVelocity 中使用 p_WCa 和 p_WCb 将产生如上所述的小残差。这可能是由于物理引擎的构建方式。虽然我没有进一步的见解来解释如何在引擎盖下计算数值，但在这里我想分享两种找到接触雅可比行列式的方法，它们产生相同的结果，得出接触动力学方程中的零残差:

方法一，用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

方法 2:使用 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, \

残差
最后是上述方法 1 和方法 2 的残差。 1e-15 本质上是零 :)

Answer 1

你的问题好像出在两个地方；

问题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总是说实话。请注意，根据 CalcJacobianTranslationalVelocity() 的文档，位置向量必须在frame_B上表示。我可以从文档 p_BoBi_B 中的字母符号表示法很快看出，它告诉我位置向量是从 B 的原点 Bo 到第 i 个点 Bi，在框架 B 中表示.

这非常重要，我相信您的错误的原因。在您的例子中，您传递的是 p_WCa，即从 Wo(世界原点)到 Ca 点的位置，在世界中表示。一个完全不同的对象。只是运气好(2D 中的特定配置)，其他项似乎没问题。

所以，解决方案。得到A(B)在W中的位姿，X_WA。然后执行:p_ACa = X_WA.inverse() * p_WCa。现在 p_ACa(= p_ACa_A，如果您遵循我们的字母组合表示法)，就是您需要的，从 Ao 到 Ca 的位置向量，用 A 表示。对于 body B 也是如此。

问题2

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

.rotation 是错误的。您需要的是 RigidTransform 对象，即:

X_WA = plant.EvalBodyPoseInWorld(plant_context, bodyA)

除非有充分的理由，否则请不要转换为 matrix。此外，根据我们的会标符号 X 保留给 RigidTransform 对象，而 R 保留给 RotationMatrix 对象。

这就是我在第一次通过时没有检测到这一点的原因。

问题3

当使用有符号距离时，与以前类似的错误。

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

不应使用 .rotation()。避免这些问题的一种方法是使用良好的表示法。我建议使用我们的会标符号，它几乎可以告诉您只需通过模式匹配就可以做什么。

也就是把上面写成:

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

否则 p_GaCa_A = X_AGa.rotation().multiply(p_GaCa)，这不是同一个对象。

总结

我们可以只坚持一个实现吗？用 PenetraionAsPointPair 说？请注意，不幸的是，PenetraionAsPointPair 和 SignedDistancePair 具有不同的 API，它们不彼此并行。也就是说，PenetraionAsPointPair 报告 p_WCa(检查器不需要)，SignedDistancePair 报告p_GaCa(检查员需要解释X_AGa)。

希望这能解决您的问题。