CODEWITHSUNDEEP
drake
Shubham Vyas
Shubham Vyas

Reputation: 165

Extra body with nans in Context of a MultiBodyPlant causing Error While Simulation

After importing an urdf file and creating a MultiBodyPlant I see an extra parameter group with nans when I print the context. The full printed context can be seen here. The parameter group which I cannot explain/understand is this one:

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

If I get the plant topology using: pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].write_svg("robot_topology.svg")

I see the topology of the plant as I expect while making the urdf (with an extra WorldBody inside WorldBodyInstance). The topology image can be seen here.

This extra body seems to be causing the following error during a continuous-time simulation (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.

I tried removing the first 3 pseudo-links but the first parameter group in the Context is still with nans. All the other parameter groups correspond correctly to the bodies/joints in the urdf file.

Could this error be in how a urdf file is written?

The urdf file can be found here. The code used to print context:

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)

If I run a discrete-time simulation (time_step=0.001), then the simulation runs successfully but I see no change on the meshcat visualizer or the printed context post-simulation after applying a joint torque to the base_roll joint using the following lines:

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)

Hence, both the continuous-time and discrete-time simulations seem to be failing (probably) due to the nans which I cannot explain from the model.

Upvotes: 2

Views: 150

Answers (1)

joemasterjohn
joemasterjohn

Reputation: 143

The NaNs that you're seeing in the parameter group correspond to the world body, and although they look suspicious I don't think they are the root of your problem. The world body doesn't have a valid set of inertial parameters (thus they're set to NaN), and is handled as a special case in the code. As it is written in the current implementation, the body parameter API is ubiquitous for every body. Each body has a set of parameters in the context, regardless of whether they are valid. This might be a point of contention for special bodies ('world'), so I've opened up an issue to discuss.

The problem you're having now is because your base_main link (and thus your entire robot) is a free floating body. You're not allowed to have a free floating tree of zero mass links attached with a joint to another free floating tree of links with non-zero mass because any torque applied at the joint connecting them (Joint_base_yaw in your case) would cause an infinite acceleration on the inboard bodies. To solve this:

  • (Option 1): Add a fixed joint between base_main and world in your URDF file.

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

  • (Option 2): Weld the base_main link's body frame to the world frame in the code.

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

Upvotes: 2

Related Questions