通过多体工厂获取雅可比行列式

Question

在使用多体工厂 (MBP) 为具有接触的多体系统建立动力学约束时，我们查看动力学方程

M(q) q_ddot + C(q, q_dot) = G(q) + B(q) u + J'(q) lambda

其中q是系统状态，*_dot和*_ddot表示*'s time derivative and double derivative, respectively; M(q) is the inertial matrix, C(q, q_dot) consists of the terms for Coriolis, centripetal, and gyroscopic effect, G(q) is the gravitational force, B(q) is the actuator matrix, u is the input (torque/force), J'(q)是接触雅可比转置，lambda是接触系中的接触力。

有没有办法从植物中获取接触雅可比项J(q)，或者用户是否可以通过接触结果来实现它？

问题更新：

我一直在尝试验证手推雅可比行列式的返回结果，但有一些我不明白的不匹配。

首先，我构建了一个非常简单的系统：一个 link 通过旋转关节固定在世界上，以及一个可以通过虚拟棱柱关节在 x-y 平面上滚动的球体。 link 被命名为左近端。接触雅可比行列式很容易通过从 contactResults 返回的接触点手动导出，使用它可以验证接触动力学。

现在，这是我从 drake 函数中找到 contact 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与预期不符。我相信所有输入都如 the documentation 中所述，并且 p_GbCb 的打印值看起来合理（至少范数与半径匹配）。然而，由于这些值没有核对，所以我一定遗漏了一些东西......

Answer 1

我认为推荐的工作流程是从 SceneGraph 输出端口获取 QueryObject。在这个 QueryObject 上，您可以在 MultibodyPlant 上调用 ComputeSignedDistancePairClosestPoints(), to find the closest points on each pair of bodies. The distance between these two points is what we call φ(q) in the notes. Then you can call the Jacobian methods 来获取 φ 相对于 q 的梯度。

Answer 2

如果你知道点在哪里，就可以很容易地使用像 CalcJacobianTranslationalVelocity() 这样的方法从 MBP 中得到 Jacobian。

您可能必须查看联系结果才能找到要点；我不确定。

Answer 3

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

正如@RussTedrake 提到的，您可以使用 ComputeSignedDistancePairClosestPoints 到 return 所有最近点对。然后你可以检查SignedDistancePair.distance，如果距离不大于0，那么你可以使用returned SignedDistancePair中的见证点作为接触点（需要注意的是你需要使用SceneGraphInspector::GetPoseInFrame(signed_distance_pair.id_A)将几何框架中的见证点转换为body框架），你可以参考这个code。注意当q改变时，这种方法中的接触点也会改变，甚至接触点的数量也会改变（当q改变时接触点变成不接触，反之亦然versa），因此 J 和 lambda 的大小会改变。这种大小变化可能会导致轨迹优化出现问题，因为 lambda 可能是您的决策变量，而您希望决策变量具有固定大小。
如果你在做轨迹优化，想像"the robot heel is in contact with the ground"那样施加约束，那么我建议将接触点指定为机器人的脚跟。一旦知道机器人脚跟在脚架中的位置（来自 URDF/SDF），就可以使用 CalcJacobianTranslationalVelocity 计算雅可比行列式。并且还施加一个运动学约束，即脚后跟在地面上（你可以参考 PositionConstraint 添加这个运动学约束）。通过固定 body 框架中的接触点，您将获得 J 和 lambda 的固定大小，如果 lambda 是您的决策变量，这对优化很重要。

通过多体工厂获取雅可比行列式

Getting contact Jacobian through multibody plant

drake