При использовании многотельной установки (MBP) для установления ограничений динамики для многотельной системы с контактом мы смотрим на уравнение динамики
M (q) q_ddot + C (q, q_dot) = G ( q) + B (q) u + J '(q) lambda
, где q - состояния системы, * _dot и * _ddot обозначают * производную по времени и двойную производную соответственно; M (q) - инерционная матрица, C (q, q_dot) состоит из терминов для эффекта Кориолиса, центростремительного и гироскопического c, G (q) - гравитационная сила, B (q) - матрица исполнительного механизма. , u - вход (крутящий момент / сила), J '(q) - транспонированный контактный якобиан, а лямбда - сила контакта в контактной системе координат.
есть ли способ получить контактные термины Якоби J (q) с завода, или пользователи могут реализовать его с результатами контактов?
Обновление вопроса:
Я пытался проверить результаты, полученные на основе якобиана, полученного вручную, и обнаружил некоторые несоответствия, которых я не понимаю.
Во-первых, я сконструировал очень простую систему: связь, прикрепленная к миру через шарнир вращения, и сфера, которая может катиться в плоскости xy через шарниры c фиктивных призм. Звено называется левым проксимальным. Контактный якобиан легко получить вручную с помощью точки контакта, возвращенной из contactResults
, с помощью которой проверяется динамика контакта.
Итак, вот как я нахожу контактный якобиан из функций Дрейка:
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
верно (в основном потому, что он находится на объекте с призматикой c стыков), но Jva
не соответствует ожидаемому. Я считаю, что все входные данные описаны в документации , а напечатанные значения p_GbCb выглядят разумно (по крайней мере, норма соответствует радиусу). Однако, поскольку значения не отмечены, должно быть что-то, что мне не хватает ...