Получение контакта с Якобианом через многотельный завод

Question

При использовании многотельной установки (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 выглядят разумно (по крайней мере, норма соответствует радиусу). Однако, поскольку значения не отмечены, должно быть что-то, что мне не хватает ...

Answer 1

Если вы знаете, где находятся точки, легко получить якобиан из MBP, используя такие методы, как CalcJacobianTranslationalVelocity () .

Вероятно, вам придется посмотреть результаты контактов, чтобы найти точки; Я не уверен в этом.

Answer 2

Якобиан контакта зависит от того, где находится точка контакта. Есть два способа установить точку контакта

1. Как упоминалось в @RussTedrake, вы можете использовать ComputeSignedDistancePairClosestPoints для возврата всех пар ближайших точек. Затем вы можете проверить SignedDistancePair.distance, если расстояние не больше 0, тогда вы можете использовать точки-свидетели в возвращенном SignedDistancePair в качестве точек контакта (предостережение в том, что вам необходимо преобразовать точку-свидетеля в геометрической рамке к корпусу, используя SceneGraphInspector::GetPoseInFrame(signed_distance_pair.id_A)), вы можете обратиться к этому коду . Обратите внимание, что при изменении q точки контакта в этом подходе также изменятся, и даже количество точек контакта может измениться (точка, которая была в контакте, перестает контактировать при изменении q, или наоборот), следовательно размер J и lambda изменится. Это изменение размера может вызвать проблемы при оптимизации траектории, поскольку lambda, вероятно, является вашей переменной решения, и вы хотите, чтобы переменные решения имели фиксированный размер.
2. Если вы выполняете оптимизацию траектории и хотите наложить ограничение типа «пятка робота соприкасается с землей», тогда я бы предложил обозначить точку контакта, чтобы она была пяткой робота. Как только вы узнаете положение пятки робота в раме ступни (из URDF / SDF), вы можете вычислить якобиан, используя CalcJacobianTranslationalVelocity. А также наложите ограничение kinemati c, при котором пятка находится на земле (вы можете обратиться к PositionConstraint при добавлении этого ограничения kinemati c). Зафиксировав точку контакта в корпусе корпуса, вы получите фиксированный размер J и lambda, которые важны для оптимизации, если lambda - ваша переменная решения.

Answer 3

Я думаю, что рекомендуемый рабочий процесс - получить QueryObject из выходного порта SceneGraph. На этом QueryObject вы можете позвонить ComputeSignedDistancePairClosestPoints(), чтобы найти ближайшие точки на каждой паре тел. Расстояние между этими двумя точками - это то, что в примечаниях мы называем φ (q). Затем вы можете вызвать методы Якоби на MultibodyPlant, чтобы получить градиенты φ относительно q.