0

I am trying to add multiple robot instances and visualising their motions. I tried a couple of ways to do this and I ran into errors/issues. They are as follows:

I tried adding another model instance after the system is created. 

parsers::urdf::AddModelInstanceFromUrdfFileToWorld(
      FindResourceOrThrow("path/CompassGait.urdf"),
      multibody::joints::kRollPitchYaw, tree.get());
parsers::urdf::AddModelInstanceFromUrdfFileToWorld(
      FindResourceOrThrow("path/quadrotor.urdf"),
      multibody::joints::kRollPitchYaw, tree.get());

As expected, there are two robots which are visible in the visualiser and there are 26 output ports in the system. But i am unable to visualise the required motion by the quadrotor. It seems to be following the x,y,z and roll pitch and yaw derivatives given as an input for the compass gait's output port. Is this an expected behaviour? I experience a similar thing when I add 2 compass gait models and try to make them follow the same path. Even though I give the ports 14-27 the same inputs as i give to 0-13. The second robot is stuck near the origin while the first one moves fine as expected without any issues.

I needed some help or maybe some examples where I can get a better idea about visualising the motion for multiple robots.

sutej kulgod
  • 44
  • 5

1 Answers1

1

[Updated] Please see note at the bottom.

drake::systems::DrakeVisualizer (which I assume you're using to publish your visualization messages) was designed to be connected to the state_output_port of drake::systems::RigidBodyPlant. According to the documentation for RigidBodyPlant,

The state vector, x, consists of generalized positions followed by generalized velocities.

That is, the generalized positions for all model instances come before the generalized velocities. When working with RigidBodyPlant and DrakeVisualizer this ordering is handled automatically.

From your question, however, I gather that you have separate, specialized systems for your quadrotor and compass-gait models (as per their respective examples). Each of these systems outputs its state as [q, v], where q is the generalized position vector and v is the generalized velocity vector. In that case you will need to use drake::systems::Demultiplexer and drake::systems::Multiplexer to split the outputs of the quadrotor and compass-gait systems and reassemble them in the required order:

+---------------+   +-------------+   q     +-------------+
|               |   |             +-------->+             |
|  Compass-gait +-->+Demultiplexer|         |             |
|               |   |             +-----+ v |             |
+---------------+   +-------------+     |   |             |
                                     +----->+             |   +-----------------+
                                     |  |   |             |   |                 |
                                     |  |   | Multiplexer +-->+ DrakeVisualizer |
                                    q|  |   |             |   |                 |
                                     |  +-->+             |   +-----------------+
+---------------+   +-------------+  |      |             |
|               |   |             +--+      |             |
|     Quadrotor +-->+Demultiplexer|         |             |
|               |   |             +---+---->+             |
+---------------+   +-------------+   v     +-------------+

Note: RigidBodyPlant and associated classes are being replaced by drake::multibody::MultibodyPlant and drake::geometry::SceneGraph. See run_quadrotor_lqr.cc for an example of using these new tools with a specialized plant.

Andrés Valenzuela
  • 88
  • 5