2

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.

Shubham Vyas
  • 163
  • 10
  • 2
    I noticed that the joints in this urdf refer to links that are defined later. Usually links are defined before they are mentioned in joints. I'm not sure that's a problem -- just mentioning it since it is unusual. You might try defining the links first just to make sure that's not the cause. – Sherm Dec 01 '20 at 16:27
  • Thanks for the idea. I did change the urdf and tried again but it still has the same issues. The parameter group of nan still exists. – Shubham Vyas Dec 01 '20 at 16:43
  • 2
    OK, I've asked the guy who wrote the Parameter stuff to take a look. – Sherm Dec 01 '20 at 21:36

1 Answers1

2

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"))
joemasterjohn
  • 133
  • 5
  • 1
    Ah. I see. Thanks for answering! I'm indeed trying to simulate a free-floating system (satellite with robot arm) and hence use the pseudo-links (with zero mass/inertia) to add reaction wheel actuators to the base body. Thus, I cannot weld/attach the base to the world frame. The aim is to have a free-floating base while being able to apply torques to the base using internal reaction wheels modeled as joints. Looks like the pseudo-link approach is not a good way for this. I'll try adding a torque source system connected to the MultiBodyPlant to apply torque to the base. – Shubham Vyas Dec 02 '20 at 09:10
  • 1
    I believe you can achieve what you are trying to model by having a single floating pseudo-link (base_main) and creating 3 bodies (perhaps cylinders with no collision or visual geometry with non-zero mass and non-zero inertia) and attaching them with revolute joints to base_main along the three principal axes. Then those joints serve to apply torques to the free floating arm and you will have more accurate momentum calculations than if you were to not model the inertial properties of the reaction wheels. – joemasterjohn Dec 02 '20 at 20:08
  • Indeed I considered that approach initially. However, since I'm only focused on torques on the base I tried to simplify by avoiding the velocity/accelerations of the cylindrical reaction wheel. Furthermore, fixing/welding the base to the world gives me rotational flexibility but I am also looking for translational freedom for the base. Hence, I am now trying using a *floating* joint in the urdf to have full 6DoF flexibility while welding the base_main to the world. – Shubham Vyas Dec 03 '20 at 10:27
  • Seems link floating type joints are supported (no documentation about them and the transmission raises an error in AddModelFromFile() as ```ERROR: Transmission specifies joint Joint_base_floating which does not exist. ```. Hence, to achieve full 6DoF flexibility with torque control I plan to then use this [answer](https://stackoverflow.com/questions/61271126/applying-an-external-force-to-an-object-in-pydrake) and add a force system on which I can apply constraints. – Shubham Vyas Dec 03 '20 at 10:38