我正在尝试计算一个步行机器人的运动方程，以在数学程序中约束它的运动。
M (q) q_ddot G + C (q, q_dot) = (q) + B (q) u + J ' (q)λ
由于我们使用了不同的决策变量，因此我们的实现基于这个预先存在的操纵器方程约束。
我们面临的问题是雅可比矩阵和其他矩阵的大小不匹配;雅可比矩阵有31行，其余的有30行。这与plant返回的向量之间的不匹配相同。获取位置和植物。getvelocity(可以通过MapQDotToVelocity修复)。
解决这个大小不匹配的建议方法是什么?
用来计算雅可比矩阵的代码:
def Contact_Jacobian(claw_id):
"""Returns the Contact Jacobian"""
query_object = scene_graph_ad.get_query_output_port().Eval(scene_graph_context_ad)
inspector = query_object.inspector()
geometryA_id = Worldbody_id
geometryB_id = claw_id
signed_distance_pair = query_object.ComputeSignedDistancePairClosestPoints(geometryA_id, geometryB_id)
X_AGa = inspector.GetPoseInFrame(signed_distance_pair.id_A)
p_GaCa = signed_distance_pair.p_ACa
p_ACa = X_AGa.cast[AutoDiffXd]().multiply(p_GaCa)
X_BGb = inspector.GetPoseInFrame(signed_distance_pair.id_B)
p_GbCb = signed_distance_pair.p_BCb
p_BCb = X_BGb.cast[AutoDiffXd]().multiply(p_GbCb)
frame_A_id = inspector.GetFrameId(signed_distance_pair.id_A)
frame_B_id = inspector.GetFrameId(signed_distance_pair.id_B)
frameA = plant_ad.GetBodyFromFrameId(frame_A_id).body_frame()
frameB = plant_ad.GetBodyFromFrameId(frame_B_id).body_frame()
frame_W = plant_ad.world_frame()
wrt = JacobianWrtVariable.kQDot
Jv_V_WCa = plant_ad.CalcJacobianTranslationalVelocity(context_ad, wrt, frameA, p_ACa, frame_W, frame_W)
Jv_V_WCb = plant_ad.CalcJacobianTranslationalVelocity(context_ad, wrt, frameB, p_BCb, frame_W, frame_W)
return Jv_V_WCa, Jv_V_WCb
为了说清楚，q和v之间的维度不同的原因是因为你有一个四元数浮点基数(7位;6速度)。
你调用的雅可比矩阵方法可以返回关于q导的雅可比矩阵或者关于v的雅可比矩阵;正如你所说，这是不一样的。
是否有可能所有需要改变的代码是你的wrt变量的值?如果没有，那么我需要知道什么是实际失败(可能是错误消息)，以帮助更多。