通过多体植物获取接触雅可比

Question

在使用多体对象(MBP)建立具有接触的多体系统的动力学约束时，我们考虑了动力学方程。

M(q) q_ddot + C(q，q_dot) = G(q) + B(q) u+ J'(q)λ

其中Q是系统的状态，*_dot和*_ddot分别表示*的时间导数和双导数，M(q)是惯性矩阵，C(q，q_dot)由科里奥利、向心力和陀螺效应的项组成，G(q)是引力，B(q)是执行器矩阵，u是输入(扭矩/力)，J'(q)是接触雅可比转座，lambda是接触框架中的接触力。

是否有方法从工厂获得接触雅可比项J(q)，还是由用户根据接触结果来实现它？

问题最新情况：

我一直试图验证从手工派生的Jacobian返回的结果，并且有一些不正确的匹配，我不明白。

首先，我构建了一个非常简单的系统:一个通过旋转关节固定到世界的链接，以及一个通过虚拟棱柱关节在x-y平面上滚动的球体。链接名为左近端。该接触雅可比易于手工推导，接触点由contactResults返回，并对接触动力学进行了验证。

下面是我如何从drake函数中找到联系人jacobian：

geometryA_id = left_proximal_geometry_id
geometryB_id = object_geometry_id
signed_distance_pair = query_object.ComputeSignedDistancePairClosestPoints(geometryA_id, geometryB_id)
print(signed_distance_pair.distance)

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

frame_A_id = inspector.GetFrameId(signed_distance_pair.id_A)
frame_B_id = inspector.GetFrameId(signed_distance_pair.id_B)
frameA = plant.GetBodyFromFrameId(frame_A_id).body_frame()
frameB = plant.GetBodyFromFrameId(frame_B_id).body_frame()

frame_W = plant.world_frame()
p_GbCb = signed_distance_pair.p_BCb
p_GaCa = signed_distance_pair.p_ACa

wrt = JacobianWrtVariable.kQDot

Jv_v_BCa_W = plant.CalcJacobianTranslationalVelocity(plant_context, wrt, frameB, p_GbCb, frameA, frame_W);


# alternative, compute Jacobian matrix by individually find Jv for A and B w.r.t. world
Jva = plant.CalcJacobianTranslationalVelocity(
    plant_context,
    JacobianWrtVariable(0),
    frameA,
    p_GaCa,
    frame_W,
    frame_W
)
Jvb = plant.CalcJacobianTranslationalVelocity(
    plant_context,
    JacobianWrtVariable(0),
    frameB,
    p_GbCb,
    frame_W,
    frame_W
)
Jv_alternative = Jvb - Jva

从数值结果来看，Jvb是正确的(主要是因为它在带有棱柱节点的物体上)，但Jva与预期的不匹配。我相信所有的输入都如文献资料所描述，p_GbCb的打印值看起来是合理的(至少范数与半径相匹配)。不过，既然这些值是不确定的，那一定是我错过了什么.

Answer 1

我认为推荐的工作流是从QueryObject输出端口获得一个SceneGraph。在这个QueryObject上，你可以调用ComputeSignedDistancePairClosestPoints()，在每对身体上找到最近的点。这两点之间的距离是我们在注释中所称的φ(q)。然后，您可以调用Jacobian方法 on MultibodyPlant，获取φ相对于q的梯度。

Answer 2

如果您知道点在哪里，那么使用CalcJacobianTranslationalVelocity()之类的方法从MBP中获取Jacobian是很容易的。

很可能你要看接触结果才能找到点，我不确定。

Answer 3

接触雅可比取决于接触点在哪里。有两种设置接触点的方法

1. 正如@RussTedrake所提到的，您可以使用ComputeSignedDistancePairClosestPoints返回所有对最近的点。然后可以检查SignedDistancePair.distance，如果距离不大于0，则可以使用返回的SignedDistancePair中的见证点作为接触点(请注意，您需要使用SceneGraphInspector::GetPoseInFrame(signed_distance_pair.id_A)将几何框架中的见证点转换为车身框架)，您可以引用该代码。注意，当q发生变化时，这种方法中的接触点也会发生变化，甚至连接触点的数量也会发生变化(当q发生变化时，接触点不再是接触点，反之亦然)，因此J和lambda的大小也会发生变化。这种大小的变化可能会导致轨迹优化问题，因为lambda很可能是您的决策变量，并且您希望您的决策变量具有固定的大小。
2. 如果你在进行轨迹优化，你想要施加“机器人脚跟与地面接触”这样的约束，那么我建议将接触点指定为机器人的脚跟。一旦您知道了机器人脚跟在脚架中的位置(来自URDF/SDF)，您就可以使用CalcJacobianTranslationalVelocity计算雅可比了。并施加一个运动约束，脚跟在地面上(你可以参考PositionConstraint添加这个运动约束)。通过固定车身框架中的接触点，您有一个固定大小的J和lambda，如果lambda是您的决策变量，那么它们在优化中非常重要。