Как реализовать совместные ограничения для дискретного моделирования многотельной установки - PullRequest
Новая
       11

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

3 голосов
/ 05 мая 2020

Я реализую симуляцию двухзвенного акробота с использованием pydrake и хотел бы установить ограничения на соединения, чтобы предотвратить столкновение нижней ссылки с верхней. Я добавил совместные ограничения в URDF и анализирую этот URDF для создания объекта acrobot multibodyplant. Я использовал функции для успешной проверки того, что совместный предел анализируется (position_lower_limits (), position_upper_limits ()), но моя симуляция не реагирует на эти ограничения. Я бы очень признателен за помощь в определении, почему это может быть.

Я создаю и моделирую многотельную установку acrobot следующим образом: 

plt_is_interactive = SetupMatplotlibBackend()
builder = DiagramBuilder()

# instantiate the pendulum and the scene graph
double_pendulum, scene_graph = AddMultibodyPlantSceneGraph(
    builder,
    time_step=0.01 # discrete update period 
)

# parse the urdf 
urdf_path = 'double_pendulum_no_shoulder.urdf'
Parser(double_pendulum).AddModelFromFile(urdf_path)
double_pendulum.Finalize()

#Simulation Set-Up

# Set the constant torque 
torque_system = builder.AddSystem(ConstantVectorSource([[0.]]))
builder.Connect(torque_system.get_output_port(0), double_pendulum.get_input_port(3))
visualizer = builder.AddSystem(
    PlanarSceneGraphVisualizer(scene_graph, xlim=[-10., 10.], ylim=[-10., 10.], show=plt_is_interactive))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
                visualizer.get_input_port(0))

diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = diagram.GetMutableSubsystemContext(double_pendulum, context)
double_pendulum.SetPositionsAndVelocities(plant_context, [2, 0, 0, 0])

# Simulate
duration = 10.0 if get_ipython() else 0.1 # sets a shorter duration during testing
context.SetTime(0.0)

AdvanceToAndVisualize(simulator, visualizer, duration)

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

<limit lower="-2.35" upper="2.35"/>

Ответы [ 3 ]

3 голосов
/ 05 мая 2020

Я просто попытался воспроизвести, и был также удивлен, что это не работает.
Вот воспроизводимое воспроизведение, которое нарушает нижний предел сустава в локте, который установлен на 0,0. 1003 * Он также распечатывает предел из соединения, подтверждая, что синтаксический анализ сработал. https://www.dropbox.com/s/2m12ws0g88t5uei/joint_limits_test.py?dl=0 https://www.dropbox.com/s/fdjb4uv4m2i17i8/double_pendulum.urdf?dl=0

1 голос
/ 06 мая 2020

Мы только что разговаривали с @Russ Tedrake. Как я уже упоминал, это «угловой» случай для MultibodyPlant, который, к сожалению, не справляется хорошо, см. Проблему Дрейка # 12775 .

«Угловой» случай здесь заключается в том, что инерция вращения плеча относительно оси локтя равна нулю. Это потому, что верхняя часть руки моделируется как точечная масса, сосредоточенная на оси сустава.

Быстрое решение, с которым мы можем справиться сегодня, - это моделирование плеча в виде стержня solid, который имеет ненулевые моменты инерции относительно оси сустава.

То есть, в вашем URDF вам нужно будет обновить значения инерции в <inertia>, чтобы они соответствовали стержню длиной 1,0 и массой 1,0 (в вашем случае) относительно его центра масс.

Для вашего случая я проверил что следующее изменение работает: 

<origin xyz="0 0 -0.5" rpy="0 0 0" />
<inertia ixx="0.08333333" ixy="0" ixz="0" iyy="0.08333333" iyz="0" izz="0"/>

Что эффективно заменяет вашу точечную массу тонким стержнем длиной 1,0 и массой 1,0.

1 голос
/ 05 мая 2020

... но моя симуляция не соответствует этим ограничениям

что именно вы имеете в виду ?.

Во-первых, вы должны знать, что наши совместные ограничения являются «мягкими», что означает, что они не являются ограничениями, а больше похожи на жесткие пружины. MultibodyPlant автоматически вычисляет жесткость этих пружин для вас, чтобы гарантировать стабильность наших решателей.

Это функция временного шага. Чем меньше временной шаг, тем жестче пределы соединения; они сходятся к жесткому пределу квадратично с шагом по времени.

При этом я вижу, что ваш временной шаг довольно большой (10 мс). Возможно, ваши пределы слишком мягкие, и это то, что вы наблюдаете в результатах моделирования. сначала нужно время, чтобы проверить эту гипотезу. Вы должны наблюдать сходимость квадратичных c с шагом по времени.

...