drake - Does scene_graph.get_source_pose_port() mix up the order of the inputs?

Question

Currently, I am trying to visualize multiple robots in drake. This means that dynamics will not be taken into account of and that MBP will not be a system. MBP is only used to add urdf models into the world and assist for scene graph.

---

How I removed dynamics and made it purely a visualizer is as follows:

I sent robot joint positions to

builder.AddSystem<MultibodyPositionToGeometryPose<double>>(plant);

and then connected this to

scene_graph.get_source_pose_port(plant.get_source_id().value()));

Part of the code is below:

// Two robots are in the simulations. Their position states will be muxed into one
// in order to send to MultibodyPositionToGeometryPose

// For now, robotA's position states will come from a constant zero vector, 
// while robotB's will come from a receiver. 

std::vector<int> mux_sizes = {  14,27 };

auto plant_states_mux = 
        builder.AddSystem<systems::Multiplexer>(mux_sizes);    
auto robots_to_pose =
        builder.AddSystem<MultibodyPositionToGeometryPose<double>>(plant);

VectorX<double> constant_vector = VectorX<double>::Zero(14);
auto constant_zero_source =
      builder.template AddSystem<systems::ConstantVectorSource<double>>(
      constant_vector);

builder.Connect(constant_zero_source->get_output_port(),
                plant_states_mux->get_input_port(robotA_id));
builder.Connect(robotB_state_receiver->get_position_measured_output_port(),
                plant_states_mux->get_input_port(robotB_id));
builder.Connect(plant_states_mux->get_output_port(0), 
                robots_to_pose->get_input_port());
builder.Connect(robots_to_pose->get_output_port(), 
                scene_graph.get_source_pose_port(plant.get_source_id().value()));

drake::geometry::ConnectDrakeVisualizer(&builder, scene_graph);

Up until the MultibodyPositionToGeometryPose port, I was able to check that the joint positions were in the correct order. This was done by looking at the function "CalcGeometryPose" and outputting the plant_context_ values on to the terminal.

However, when I run the simulation and send joint position to robot_B, it moves the wrong joints. Additionally, robot_A also moves its joint even though I did not send it any states.

I found out that this was due to reordering of the robot ports (either inside

plant_.get_geometry_poses_output_port().Calc(*plant_context_, output);

or in scene graph)

The ports reorder like this: 7 DOF for every robot's base position (4 quaternion, 3 xyz) is listed first. Then the joints are ordered randomly afterwards.

EX:

robotA qw, robotA qx, robotA qy, robotA qz, robotA x, robotA y, robotA z,

robotB qw, robotB qx, robotB qy, robotB qz, robotB x, robotB y, robotB z,

robotA joint 1, robotA joint 2, robotB joint 1, robotB joint 2, robotA joint 3 ... etc

So, I have thought that sending a state to robotA joint 1 would come right after robotA z position. However, the reordering makes it so that it is robotB qw (for this example).

I have tried to reorder the port wiring to match the joints. But when I do so, the ports switch orders.

---

My question is, does scenegraph or MultibodyPositionToGeometryPose reorder position state locations? (perhaps for efficiency or something?)

If so, is there a way to prevent this from happening?

Thank you

标签： drake

Answer 1

I haven't looked in detail to your code, but using a multiplexer essentially assumes that all state of robot 2 follows immediately and contiguously the one of robot 1. A fine assumption, but it is not what MBP does. That assumption is not true in general. However, MBP does offer APIs that state some specific ordering.

If you do want the state of a single robot in a vector form, probably best to use model instances. For instance, you can do GetPositionsAndVelocities(cotext, model_instace) which essentially does the multiplexer work above, but we the caveat that MBP does know how states are stored.

Another recommended option is to use the documented public APIs. In general if we document something, we mean it (unless of course there is a bug, but we try). So I recommend you look at things like Joint::position_start() and related methods to find out how things are indexed and possibly build your own map of how you'd like things to be indexed.

Finally, for free bodies, you can say MBP::SetFreeBodyPose() to set a pose (see Body::is_free() and related methods in Body:).

However, it looks like you got rid of MBP (which is the one that knows these mappings) and only care about viz? maybe you could use two MultibodyPositionToGeometryPose, one for each robot?

Hopefully it helps.