Дополнительное тело с nans в контексте MultiBodyPlant, вызывающее ошибку при моделировании

Dec 01 2020

После импорта файла urdf и создания MultiBodyPlant я вижу дополнительную группу параметров с nans при печати контекста. Полный печатный контекст можно увидеть здесь . Группа параметров, которую я не могу объяснить / понять, такова:

18 numeric parameter groups with
   10 parameters
     nan nan nan nan nan nan nan nan nan nan

Если я получу топологию завода, используя: pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].write_svg("robot_topology.svg")

Я вижу топологию объекта, как и ожидал, при создании urdf (с дополнительным WorldBody внутри WorldBodyInstance ). Изображение топологии можно увидеть здесь .

Это лишнее тело, кажется, вызывает следующую ошибку во время моделирования в непрерывном времени (time_step = 0.0):

RuntimeError: Encountered singular articulated body hinge inertia for body node index 1. Please ensure that this body has non-zero inertia along all axes of motion.

Я попытался удалить первые 3 псевдосвязи, но первая группа параметров в контексте все еще имеет nans. Все остальные группы параметров правильно соответствуют телам / суставам в файле urdf.

Может ли эта ошибка быть связана с записью файла urdf?

Файл urdf можно найти здесь . Код, используемый для печати контекста:

builder = DiagramBuilder()

plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
Parser(plant, scene_graph).AddModelFromFile(SCpath)
# Remove gravity
plantGravityField = plant.gravity_field()
plantGravityField.set_gravity_vector([0,0,0])
plant.Finalize()

# Adds the MeshcatVisualizer and wires it to the SceneGraph.
meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, delete_prefix_on_load=True)

diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromRoot(context)
print(plant_context)

Если я запускаю симуляцию с дискретным временем (time_step = 0.001), то симуляция выполняется успешно, но я не вижу никаких изменений в визуализаторе meshcat или печатном контексте пост-симуляции после приложения крутящего момента соединения к соединению base_roll с использованием следующих строк:

jointAcutation =  np.zeros((plant.num_actuators(),1))
jointAcutation[0] = 10
plant.get_actuation_input_port().FixValue(plant_context, jointAcutation)

simulator = Simulator(diagram, context)

meshcat.load()

meshcat.start_recording()
simulator.AdvanceTo(30.0)
meshcat.stop_recording()
meshcat.publish_recording()
print(plant_context)

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

Ответы

2 joemasterjohn Dec 02 2020 at 06:15

В NaNs , что вы видите в группе параметров соответствуют мировому телу, и , хотя они выглядят подозрительно , я не думаю , что они являются причиной вашей проблемы. Мировое тело не имеет действительного набора инерциальных параметров (таким образом, для них установлено значение NaN), и оно обрабатывается в коде как особый случай. Как написано в текущей реализации, API параметров тела применяется повсеместно для каждого тела. У каждого тела есть набор параметров в контексте, независимо от того, действительны ли они. Это может быть предметом спора по поводу особых тел («мира»), поэтому я открыл вопрос для обсуждения.

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