Как добавить ограничения конечных эффекторов к вершинам и краям GCS? ⇐ Python

[Anonymous]

Я пытался добавить некоторые ограничения на конечный эффектор на некоторых участках траектории при использовании GCSTrajectoryOptimization.
Вот пример задачи:
Допустим, мне удалось схватить чашку и поставить ее на полку. Теперь я хотел бы убедиться, что его содержимое не вытечет во время движения! Для этого мне нужно добавить такие ограничения (если я добавлял их в задачу ИК):

Код: Выделить всё

R_C_desired = RotationMatrix.MakeZRotation(0)
ik.AddOrientationConstraint(
frameB=EE_frame,
R_AbarA=R_C_desired,
frameA=W_frame,
theta_bound=acceptable_tilt
)


Но я не уверен, как я могу применить такое ограничение к узлам и краям моей GCS, чтобы движение соответствовало таким ограничениям.

1. Вот первый наивный подход, который я попробовал

Я попробовал определить переменные для q, а затем передать их через эту функцию:

Код: Выделить всё

def set_positions(q):
plant_double = MultibodyPlant(time_step=0.0)
parser = Parser(plant_double)

scenario = """
directives:
- add_model:
name: iiwa
file: package://drake_models/iiwa_description/sdf/iiwa7_no_collision.sdf
default_joint_positions:
iiwa_joint_1: [0]
iiwa_joint_2: [0.6]
iiwa_joint_3: [0]
iiwa_joint_4: [-1.75]
iiwa_joint_5: [0]
iiwa_joint_6: [ 1.0]
iiwa_joint_7: [0]
- add_weld:
parent: world
child: iiwa::iiwa_link_0
X_PC:
translation: [0, 0.8, 0.8]
rotation: !Rpy { deg: [0, 0, 0]}
"""

iiwa_model = parser.AddModelsFromString(scenario, ".dmd.yaml")
plant_double.Finalize()

plant = plant_double.ToScalarType[Expression]()

context = plant.CreateDefaultContext()
plant.SetPositions(context, iiwa_model[0], q)
return context, plant

А потом сделал следующее:

Код: Выделить всё

context, plant = set_positions(q)
ee_frame = plant.GetFrameByName("iiwa_link_7")
T_e = ee_frame.CalcPoseInWorld(context)

Чтобы получить положение кадра EE в символической форме, а затем добавить свои ограничения с помощью: vertix.AddConstraint() и Edge.AddConstraint().
Все совершенно замирает, учитывая, насколько большим и невыпуклым оказывается это выражение.

2. Второй подход, который я попробовал
Я понимаю, что стандартный подход заключается в перемещении ограничений с этапа оптимизации траектории на этап выпуклой декомпозиции. Я попробовал это при использовании компоновщика IRIS, создав IK с ограничениями, а затем добавив ограниченную программу в IrisOptions через options.prog_with_additional_constraints:

Код: Выделить всё

options = IrisOptions()
prog, ik = generate_ik_prog(plant, region_definition)
options.prog_with_additional_constraints = prog

Этот подход иногда работает, но он очень медленный и часто не сходится. Другая проблема этого подхода заключается в том, что из-за медленной работы IRIS (даже без вышеуказанных ограничений) делать это на лету непрактично. Это означает, что мне нужно заранее знать все мои ограничения и создать множество подграфов с различными ограничениями, которые будут использоваться при планировании.
Мой вопрос
Каков правильный способ добавления ограничений конечных эффекторов к вершинам и краям GCS?

Подробнее здесь: https://stackoverflow.com/questions/790 ... s-of-a-gcs