0

In the examples/quadrotor/ example, a custom QuadrotorPlant is specified and its output is passed into QuadrotorGeometry where the QuadrotorPlant state is packaged into FramePoseVector for the SceneGraph to visualize.

The relevant code segment in QuadrotorGeometry that does this:

...
  builder->Connect(
      quadrotor_geometry->get_output_port(0),
      scene_graph->get_source_pose_port(quadrotor_geometry->source_id_));
...

void QuadrotorGeometry::OutputGeometryPose(
    const systems::Context<double>& context,
    geometry::FramePoseVector<double>* poses) const {
  DRAKE_DEMAND(frame_id_.is_valid());

  const auto& state = get_input_port(0).Eval(context);
  math::RigidTransformd pose(
      math::RollPitchYawd(state.segment<3>(3)),
      state.head<3>());

  *poses = {{frame_id_, pose.GetAsIsometry3()}};
}

In my case, I have a floating based multibody system (think a quadrotor with a pendulum attached) of which I've created a custom plant (LeafSystem). The minimal coordinates for such a system would be 4 (quaternion) + 3 (x,y,z) + 1 (joint angle) = 7. If I were to follow the QuadrotorGeometry example, I believe I would need to specify the full RigidTransformd for the quadrotor and the full RigidTransformd of the pendulum.

Question

Is it possible to set up the visualization / specify the pose such that I only need to specify the 7 (pose of quadrotor + joint angle) state minimal coordinates and have the internal MultibodyPlant handle the computation of each individual body's (quadrotor and pendulum) full RigidTransform which can then be passed to the SceneGraph for visualization?

I believe this was possible with the "attic-ed" (which I take to mean "to be deprecated") RigidBodyTree, which was accomplished in examples/compass_gait

  lcm::DrakeLcm lcm;
  auto publisher = builder.AddSystem<systems::DrakeVisualizer>(*tree, &lcm);
  publisher->set_name("publisher");

  builder.Connect(compass_gait->get_floating_base_state_output_port(),
                  publisher->get_input_port(0));

Where get_floating_base_state_output_port() was outputting the CompassGait state with only 7 states (3 rpy + 3 xyz + 1 hip angle).

What is the MultibodyPlant, SceneGraph equivalent of this?

Update (Using MultibodyPositionToGeometryPose from Russ's deleted answer

I created the following function which, attempts to create a MultibodyPlant from the given model_file and connects the given plant pose_output_port through MultibodyPositionToGeometryPose.

The pose_output_port I'm using is the 4(quaternion) + 3(xyz) + 1(joint angle) minimal state.

void add_plant_visuals(
        systems::DiagramBuilder<double>* builder,
        geometry::SceneGraph<double>* scene_graph,
        const std::string model_file,
        const systems::OutputPort<double>& pose_output_port)
{
    multibody::MultibodyPlant<double> mbp;
    multibody::Parser parser(&mbp, scene_graph);
    auto model_id = parser.AddModelFromFile(model_file);
    mbp.Finalize();

    auto source_id = *mbp.get_source_id();

    auto multibody_position_to_geometry_pose = builder->AddSystem<systems::rendering::MultibodyPositionToGeometryPose<double>>(mbp);
    builder->Connect(pose_output_port,
            multibody_position_to_geometry_pose->get_input_port());
    builder->Connect(
            multibody_position_to_geometry_pose->get_output_port(),
            scene_graph->get_source_pose_port(source_id));

    geometry::ConnectDrakeVisualizer(builder, *scene_graph);
}

The above fails with the following exception

abort: Failure at multibody/plant/multibody_plant.cc:2015 in get_geometry_poses_output_port(): condition 'geometry_source_is_registered()' failed.
Rufus
  • 5,111
  • 4
  • 28
  • 45
  • Adding link provided by Russ in a previous answer whcih I thought was useful: https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1rendering_1_1_multibody_position_to_geometry_pose.html – Rufus Jun 17 '19 at 03:39
  • BTW I recognized that the doxygen for `MultibodyPositionToGeometryPose` had not rendered correctly -- the missing documentation called out the necessity of making sure the `MBP` stays alive. A PR is about to merge to fix that. – Sean Curtis Jun 19 '19 at 19:01

2 Answers2

1

So, there's a lot in here. I have a suspicion there's a simple answer, but we may have to converge on it.

First, my assumptions:

  1. You've got an "internal" MultibodyPlant (MBP). Presumably, you also have a context for it, allowing you to perform meaningful state-dependent calculations.
  2. Furthermore, I presume the MBP was responsible for registering the geometry (probably happened when you parsed it).
  3. Your LeafSystem will directly connect to the SceneGraph to provide poses.
  4. Given your state, you routinely set the state in the MBP's context to do that evaluation.

Option 1 (Edited):

In your custom LeafSystem, create the FramePoseVector output port, create the calc callback for it, and inside that callback, simply invoke the Eval() of the pose output port of the internal MBP that your LeafSystem own (having previously set the state in your locally owned Context for the MBP and passing in the pointer to the FramePoseVector that your LeafSystem's callback was provided with).

Essentially (in a very coarse way):

MySystem::MySystem() {
  this->DeclareAbstractOutputPort("geometry_pose",
      &MySystem::OutputGeometryPose);
}

void MySystem::OutputGeometryPose(
    const Context& context, FramePoseVector* poses) const {
  mbp_context_.get_mutable_continuous_state()
    .SetFromVector(my_state_vector);
  mbp_.get_geometry_poses_output_port().Eval(mpb_context_, poses);
}

Option 2:

Rather than implementing a LeafSystem that has an internal plant, you could have a Diagram that contains an MBP and exports the MBP's FramePoseVector output directly through the diagram to connect.

Sean Curtis
  • 1,425
  • 7
  • 8
  • Thx for the reply! I've updated my question based on the deleted reply by Russ (not sure why it was deleted). Regarding your assumptions, my current set up is quite similar to the quadrotor example. The MBP is only there for loading the model for visualizing. All state-dependent calculations are done separate form that MBP in my custom LeafSystem. I believe I'm stuck on 3 and 4 where I'm not exactly sure how I can connect the output of my LeafSystem to the SceneGraph to provide the pose. – Rufus Jun 18 '19 at 09:42
  • I suppose the straightforward way would be to have my LeafSystem output each link (frame)'s pose via `*poses = {{frame_id_, pose.GetAsIsometry3()}}`. However, I would like to avoid the unnecessary step of calculating each frame's pose and have the MBP do it for me. I was wondering if I can simply specify the minimal coordinates (floating base + joint angle) and have MBP automatically find out the `poses`. – Rufus Jun 18 '19 at 09:50
  • To clarify, my LeafSystem of which I implemented the dynamics of my plant does not contain a MultibodyPlant, similar to how the `QuadrotorPlant` does not contain an MBP. – Rufus Jun 18 '19 at 09:58
1

This answer addresses, specifically, your edit where you are attempting to use the MultibodyPositionToGeometryPose approach. It doesn't address the larger design issues.

Your problem is that the MultibodyPositiontToGeometryPose system takes a reference to an MBP and keeps a reference to that same MBP. That means the MBP must be alive and well for at least as long as the MPTGP is. However, in your code snippet, your MBP is local to the add_plant_visuals() function so it is destroyed as soon as the function is over.

You'll need to create something that is persisted and owned by someone else.

(This is tightly related to my option 2 - now edited for improved clarity.)

Sean Curtis
  • 1,425
  • 7
  • 8