Corps supplémentaire avec nans dans le contexte d'un MultiBodyPlant provoquant une erreur lors de la simulation

Question

Après avoir importé un fichier urdf et créé un MultiBodyPlant, je vois un groupe de paramètres supplémentaire avec nans lorsque j'imprime le contexte. Le contexte imprimé complet peut être vu ici . Le groupe de paramètres que je ne peux pas expliquer / comprendre est celui-ci:

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

Si j'obtiens la topologie de l'installation en utilisant: pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].write_svg("robot_topology.svg")

Je vois la topologie de la plante comme je m'y attendais en créant l'urdf (avec un WorldBody supplémentaire dans WorldBodyInstance ). L'image de topologie peut être vue ici .

Ce corps supplémentaire semble être à l'origine de l'erreur suivante lors d'une simulation en temps continu (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.

J'ai essayé de supprimer les 3 premiers pseudo-liens mais le premier groupe de paramètres dans le contexte est toujours avec nans. Tous les autres groupes de paramètres correspondent correctement aux corps / joints du fichier urdf.

Cette erreur pourrait-elle provenir de la façon dont un fichier urdf est écrit?

Le fichier urdf peut être trouvé ici . Le code utilisé pour imprimer le contexte:

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)

Si j'exécute une simulation en temps discret (time_step = 0,001), la simulation s'exécute avec succès mais je ne vois aucun changement sur le visualiseur meshcat ou le contexte imprimé après la simulation après avoir appliqué un couple de joint à l' articulation base_roll en utilisant les lignes suivantes:

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)

Par conséquent, les simulations en temps continu et en temps discret semblent échouer (probablement) en raison des nans que je ne peux pas expliquer à partir du modèle.

Answer 1

Les NaN que vous voyez dans le groupe de paramètres correspondent au corps du monde, et bien qu'ils semblent suspects, je ne pense pas qu'ils soient à l'origine de votre problème. Le corps du monde n'a pas un ensemble valide de paramètres inertiels (ils sont donc définis sur NaN) et est traité comme un cas particulier dans le code. Comme il est écrit dans l'implémentation actuelle, l'API de paramètre de corps est omniprésente pour chaque corps. Chaque corps a un ensemble de paramètres dans le contexte, qu'ils soient valides ou non. Cela pourrait être un point de discorde pour les organismes spéciaux («monde»), j'ai donc ouvert une question à discuter.

Le problème que vous rencontrez maintenant est que votre base_mainlien (et donc tout votre robot) est un corps flottant librement. Vous n'êtes pas autorisé à avoir un arbre flottant libre de liens de masse nulle attachés avec une articulation à un autre arbre flottant de liens avec une masse non nulle, car tout couple appliqué à l'articulation les reliant ( Joint_base_yawdans votre cas) provoquerait une accélération infinie sur les corps intérieurs. Pour résoudre ce problème:

• (Option 1): Ajoutez une articulation fixe entre base_mainet worlddans votre fichier URDF.

  <joint name="base_main" type="fixed">
    <parent link="world"/>
    <child link="base_main"/>
  </joint>
  

• (Option 2): Souder le base_maincadre du corps du lien au cadre du monde dans le code.

  plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base_main"))