Zusätzlicher Körper mit Nans im Kontext einer MultiBodyPlant, der während der Simulation Fehler verursacht

Question

Nach dem Importieren einer urdf-Datei und dem Erstellen eines MultiBodyPlant wird beim Drucken des Kontexts eine zusätzliche Parametergruppe mit nans angezeigt . Der vollständige gedruckte Kontext zu sehen ist hier . Die Parametergruppe, die ich nicht erklären / verstehen kann, ist diese:

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

Wenn ich die Pflanzentopologie erhalte mit: pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].write_svg("robot_topology.svg")

Ich sehe die Topologie der Anlage so, wie ich es beim Erstellen der URL erwarte (mit einem zusätzlichen WorldBody in WorldBodyInstance ). Das Topologiebild ist hier zu sehen .

Dieser zusätzliche Körper scheint während einer zeitkontinuierlichen Simulation den folgenden Fehler zu verursachen (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.

Ich habe versucht, die ersten 3 Pseudo-Links zu entfernen, aber die erste Parametergruppe im Kontext ist immer noch mit nans. Alle anderen Parametergruppen entsprechen korrekt den Körpern / Gelenken in der urdf-Datei.

Könnte dieser Fehler darin liegen, wie eine Urdf-Datei geschrieben wird?

Die urdf-Datei finden Sie hier . Der zum Drucken des Kontexts verwendete Code:

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)

Wenn ich eine zeitdiskrete Simulation durchführe (time_step = 0,001), wird die Simulation erfolgreich ausgeführt, aber ich sehe keine Änderung am Meshcat-Visualizer oder an der gedruckten Kontext-Nachsimulation, nachdem ein Gelenkdrehmoment auf die base_roll- Verbindung angewendet wurde. Verwenden Sie dazu die folgenden Zeilen:

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)

Daher scheinen sowohl die zeitkontinuierliche als auch die zeitdiskrete Simulation (wahrscheinlich) aufgrund der Nans zu versagen, die ich aus dem Modell nicht erklären kann.

Answer 1

Die NaNs , die Sie in der Parametergruppe sehen, entsprechen dem Weltkörper , und obwohl sie verdächtig aussehen, glaube ich nicht, dass sie die Wurzel Ihres Problems sind. Der Weltkörper hat keinen gültigen Satz von Trägheitsparametern (daher sind sie auf NaN gesetzt) ​​und wird im Code als Sonderfall behandelt. Wie in der aktuellen Implementierung beschrieben, ist die Body-Parameter-API für jeden Body allgegenwärtig. Jeder Körper hat eine Reihe von Parametern im Kontext, unabhängig davon, ob sie gültig sind. Dies könnte ein Streitpunkt für spezielle Gremien ("Welt") sein, daher habe ich ein Thema zur Diskussion gestellt.

Das Problem, das Sie jetzt haben, ist, dass Ihre base_mainVerbindung (und damit Ihr gesamter Roboter) ein frei schwebender Körper ist. Es ist nicht gestattet, einen frei schwebenden Baum von Nullmassengliedern mit einem Gelenk an einem anderen frei schwebenden Baum von Gliedern mit einer Masse ungleich Null zu befestigen, da jedes Drehmoment, das an dem sie verbindenden Gelenk ( Joint_base_yawin Ihrem Fall) aufgebracht wird, eine unendliche Beschleunigung verursachen würde auf den Innenbordkörpern. Um dies zu lösen:

• (Option 1): Fügen Sie eine feste Verbindung zwischen base_mainund worldin Ihrer URDF-Datei hinzu.

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

• (Option 2): Schweißen Sie den base_mainKörperrahmen des Links an den Weltrahmen im Code.

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