Dodatkowa treść z nans w kontekście MultiBodyPlant powodująca błąd podczas symulacji

Dec 01 2020

Po zaimportowaniu pliku urdf i utworzeniu MultiBodyPlant widzę dodatkową grupę parametrów z nans, kiedy drukuję kontekst. Pełny drukowany kontekst można zobaczyć tutaj . Grupa parametrów, której nie mogę wyjaśnić / zrozumieć, to ta:

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

Jeśli uzyskam topologię zakładu za pomocą: pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].write_svg("robot_topology.svg")

Widzę topologię zakładu zgodnie z oczekiwaniami podczas tworzenia urdf (z dodatkowym WorldBody wewnątrz WorldBodyInstance ). Obraz topologii można zobaczyć tutaj .

Wydaje się, że ta dodatkowa treść powoduje następujący błąd podczas symulacji w czasie ciągłym (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.

Próbowałem usunąć pierwsze 3 pseudo-łącza, ale pierwsza grupa parametrów w Context nadal zawiera nans. Wszystkie inne grupy parametrów poprawnie odpowiadają korpusom / stawom w pliku urdf.

Czy ten błąd może dotyczyć sposobu zapisywania pliku urdf?

Plik urdf można znaleźć tutaj . Kod użyty do wydrukowania kontekstu:

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)

Jeśli uruchomię symulację w czasie dyskretnym (time_step = 0,001), to symulacja przebiega pomyślnie, ale nie widzę żadnych zmian w wizualizatorze meshcat ani w drukowanym kontekście po symulacji po zastosowaniu momentu obrotowego połączenia do złącza base_roll za pomocą następujących linii:

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)

W związku z tym zarówno symulacje czasu ciągłego, jak i dyskretnego wydają się zawodzić (prawdopodobnie) z powodu nans, których nie mogę wyjaśnić na podstawie modelu.

Odpowiedzi

2 joemasterjohn Dec 02 2020 at 06:15

Do Koncepcja nieliczby że widzisz w grupie parametrów odpowiadają na ciele świata i choć wyglądają podejrzanie Nie sądzę, że są źródłem problemu. Ciało świata nie ma prawidłowego zestawu parametrów inercyjnych (dlatego są one ustawione na NaN) i jest traktowane w kodzie jako przypadek specjalny. Jak napisano w obecnej implementacji, API parametrów ciała jest wszechobecne dla każdego ciała. Każda treść ma zestaw parametrów w kontekście, niezależnie od tego, czy są one prawidłowe. To może być punkt sporny w przypadku organów specjalnych („świata”), więc otworzyłem kwestię do omówienia.

Problem, który masz teraz, polega na tym, że twoje base_mainłącze (a tym samym cały robot) jest swobodnie unoszącym się ciałem. Nie możesz mieć swobodnie pływającego drzewa ogniw o zerowej masie połączonych przegubem z innym swobodnie pływającym drzewem ogniw o niezerowej masie, ponieważ jakikolwiek moment obrotowy przyłożony na łączącym je połączeniu ( Joint_base_yaww twoim przypadku) spowodowałby nieskończone przyspieszenie na nadwoziach wewnętrznych. Aby rozwiązać ten problem: