Учитывая многотельный объект, мне нужно найти матрицу, которая преобразует лямбду внешних сил в обобщенные силы. Т.е. Phi в следующем уравнении (взято из Эффективно решаемой квадратичной программы для стабилизации динамической локомоции Скотта Куиндерма, Фрэнка Перментера и Расса Тедрейка).
Я предполагаю, что мне нужно сначала добавить соответствующие силы к соответствующим телам в многотельном заводе с помощью
left_foot_contact_force = plant.AddForceElement(plant.GetModelInstanceByName("l_foot"))
right_foot_contact_force = plant.AddForceElement(plant.GetModelInstanceByName("r_foot"))
Однако я не совсем уверен, как поступить, чтобы получить матрицу Phi. Я подозревал, что мне нужно будет использовать python-3.x drake
1 ответ
Если λ - это контактная сила, приложенная в точке Q, прикрепленной к телу B, выраженная в мировой системе отсчета, то эта матрица Φ является матрицей Якоби. Вы можете вычислить матрицу Якоби, используя CalcJaclocity
J = plant.CalcJacobianTranslationalVelocity(context, JacobianWrtVariable.kV, plant.GetBodyByName("l_foot"), p_BQ, plant.world_frame(), plant.world_frame())
Где p_BQ - положение точки Q, измеренное и выраженное в раме ступни.
Если λ - вес кромки фрикционного конуса в точке Q (а именно, контактная сила равна f = ∑ᵢ λᵢeᵢ, где eᵢ - i-я кромка фрикционного конуса в точке Q, измеренная и выраженная в мировой системе отсчета), то матрица Φ имеет вид Φᵀ = Jᵀ * E, где E = [e₁, ..., eₘ] (А именно, если предположить, что в точке Q имеется m ребер линеаризованного конуса трения, и мы соедините эти края конуса трения по горизонтали, чтобы получить матрицу E).
Здесь я показал, как вычислить Φ для одной точки. Если у вас есть несколько точек контакта, вы можете вычислить Φ для каждой точки отдельно, а затем объединить их вместе.