通过镜像内部函数 CalcNormalAndTangentContactJacobians() 寻找接触雅可比矩阵
Finding Contact Jacobian by mirroring internal function CalcNormalAndTangentContactJacobians()
我试图找到定义为 M(q) q_ddot + C(q, q_dot) = G(q) + B(q) u + J'( q) 拉姆达。
我尝试像 multibody_plant.cclink 中那样镜像函数 CalcNormalAndTangentContactJacobians()，但这些值没有相加。
以下是我设置系统的方式（为了尽可能简单的设置）：
我有一个通过旋转关节安装到世界的圆柱体和一个可以通过虚拟棱柱关节在 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_BCb
p_GaCa = signed_distance_pair.p_ACa
Jv_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
方法二：用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 本质上是零 :)
你的问题好像出在两个地方；
问题 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()，位置向量必须表示在frame_B上。我可以从文档p_BoBi_B中的字母符号表示法很快看出，它告诉我位置向量是从B的原点Bo到i-th点Bi，表示在帧B.
这很重要，我相信你的错误的原因。在你的例子中，你传递的是 p_WCa，从 Wo（世界原点）到 Ca 点的位置，在世界中表示。一个完全不同的object。运气好（2D中的特定配置），其他条款似乎还可以。
所以，解决方案。得到A(B)在W中的pose，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 object，即：
X_WA = plant.EvalBodyPoseInWorld(plant_context, bodyA)
除非有充分的理由，否则请不要转换为 matrix。此外，根据我们的字母组合符号 X 保留给 RigidTransform objects 而 R 保留给 RotationMatrix objects.
这就是我第一次没有检测到这一点的原因。
问题 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)，不一样object。
总结
我们可以只坚持一个实现吗？用 PenetraionAsPointPair 说？请注意，不幸的是，PenetraionAsPointPair 和 SignedDistancePair 具有 不同的 API，而 不 彼此并行。也就是说，PenetraionAsPointPair报告p_WCa（检查员不需要），SignedDistancePair报告p_GaCa（检查员需要占X_AGa).
希望这能解决您的问题。
我试图找到定义为 M(q) q_ddot + C(q, q_dot) = G(q) + B(q) u + J'( q) 拉姆达。
我尝试像 multibody_plant.cclink 中那样镜像函数 CalcNormalAndTangentContactJacobians()，但这些值没有相加。
以下是我设置系统的方式（为了尽可能简单的设置）：
我有一个通过旋转关节安装到世界的圆柱体和一个可以通过虚拟棱柱关节在 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)使用使用相对帧
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
SignedDistancePair代替PenetrationAsPointPair
从最后一点开始：
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
方法二：用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 本质上是零 :)
你的问题好像出在两个地方；
问题 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()，位置向量必须表示在frame_B上。我可以从文档p_BoBi_B中的字母符号表示法很快看出，它告诉我位置向量是从B的原点Bo到i-th点Bi，表示在帧B.
这很重要，我相信你的错误的原因。在你的例子中，你传递的是 p_WCa，从 Wo（世界原点）到 Ca 点的位置，在世界中表示。一个完全不同的object。运气好（2D中的特定配置），其他条款似乎还可以。
所以，解决方案。得到A(B)在W中的pose，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 object，即：
X_WA = plant.EvalBodyPoseInWorld(plant_context, bodyA)
除非有充分的理由，否则请不要转换为 matrix。此外，根据我们的字母组合符号 X 保留给 RigidTransform objects 而 R 保留给 RotationMatrix objects.
这就是我第一次没有检测到这一点的原因。
问题 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)，不一样object。
总结
我们可以只坚持一个实现吗？用 PenetraionAsPointPair 说？请注意，不幸的是，PenetraionAsPointPair 和 SignedDistancePair 具有 不同的 API，而 不 彼此并行。也就是说，PenetraionAsPointPair报告p_WCa（检查员不需要），SignedDistancePair报告p_GaCa（检查员需要占X_AGa).
希望这能解决您的问题。