Tubuh ekstra dengan nans dalam Konteks MultiBodyPlant menyebabkan Kesalahan Saat Simulasi

Dec 01 2020

Setelah mengimpor file urdf dan membuat MultiBodyPlant, saya melihat grup parameter tambahan dengan nans ketika saya mencetak konteksnya. Konteks lengkap tercetak dapat dilihat di sini . Grup parameter yang tidak dapat saya jelaskan / pahami adalah yang ini:

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

Jika saya mendapatkan topologi tumbuhan menggunakan: pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].write_svg("robot_topology.svg")

Saya melihat topologi tanaman seperti yang saya harapkan saat membuat urdf (dengan tambahan WorldBody di dalam WorldBodyInstance ). Gambar topologi dapat dilihat disini .

Badan ekstra ini tampaknya menyebabkan kesalahan berikut selama simulasi waktu kontinu (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.

Saya mencoba menghapus 3 pseudo-link pertama tetapi grup parameter pertama dalam Konteks masih dengan nans. Semua grup parameter lainnya sesuai dengan badan / sendi di file urdf.

Mungkinkah kesalahan ini terjadi pada cara file urdf ditulis?

File urdf dapat ditemukan di sini . Kode yang digunakan untuk mencetak konteks:

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)

Jika saya menjalankan simulasi waktu-diskrit (time_step = 0,001), maka simulasi berjalan dengan sukses tetapi saya tidak melihat perubahan pada visualizer meshcat atau konteks pasca-simulasi tercetak setelah menerapkan torsi gabungan ke sambungan base_roll menggunakan baris berikut:

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)

Oleh karena itu, simulasi waktu kontinu dan waktu diskrit tampaknya gagal (mungkin) karena nans yang tidak dapat saya jelaskan dari model.

Jawaban

2 joemasterjohn Dec 02 2020 at 06:15

The NaN bahwa Anda melihat pada kelompok parameter sesuai dengan badan dunia, dan meskipun mereka terlihat mencurigakan Saya tidak berpikir mereka adalah akar masalah Anda. Badan dunia tidak memiliki sekumpulan parameter inersia yang valid (sehingga disetel ke NaN), dan ditangani sebagai kasus khusus dalam kode. Seperti yang tertulis dalam implementasi saat ini, API parameter tubuh ada di mana-mana untuk setiap tubuh. Setiap badan memiliki sekumpulan parameter dalam konteks, terlepas dari valid atau tidaknya parameter tersebut. Ini mungkin menjadi titik perdebatan untuk badan-badan khusus ('dunia'), jadi saya telah membuka sebuah masalah untuk didiskusikan.

Masalah yang Anda hadapi sekarang adalah karena base_maintautan Anda (dan dengan demikian seluruh robot Anda) adalah benda mengambang bebas. Anda tidak diperbolehkan memiliki pohon mengambang bebas dari tautan bermassa nol yang terpasang dengan sambungan ke pohon tautan mengambang bebas lainnya dengan massa bukan nol karena torsi apa pun yang diterapkan pada sambungan yang menghubungkannya ( Joint_base_yawdalam kasus Anda) akan menyebabkan percepatan tak terbatas di tubuh bagian dalam. Untuk mengatasi ini: