0

在examples/quadrotor/示例中，QuadrotorPlant指定了一个自定义并将其输出传递到QuadrotorGeometry将QuadrotorPlant状态打包到FramePoseVector的位置SceneGraph以进行可视化。

其中的相关代码段QuadrotorGeometry执行此操作：

...
  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()}};
}

在我的例子中，我有一个基于浮动的多体系统（想想一个附有钟摆的四旋翼），我已经创建了一个自定义植物 ( LeafSystem)。这样一个系统的最小坐标是 4（四元数）+ 3（x,y,z）+1（关节角）= 7。如果我按照 QuadrotorGeometry 示例，我相信我需要指定完整RigidTransformd的四旋翼和全RigidTransformd摆。

问题

是否可以设置可视化/指定姿势，这样我只需要指定 7（四旋翼姿势 + 关节角度）状态最小坐标，并让内部MultibodyPlant处理每个个体（四旋翼和钟摆）的RigidTransform计算然后可以传递给SceneGraph可视化吗？

我相信这是可能的“阁楼版”（我认为这意味着“被弃用”）RigidBodyTree，这是在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));

get_floating_base_state_output_port()仅输出CompassGait7 个状态（3 rpy + 3 xyz + 1 髋角）的状态在哪里。

MultibodyPlant相当于什么SceneGraph？

更新（使用Russ 已删除答案中的MultibodyPositionToGeometryPose

我创建了以下函数，它尝试MultibodyPlant从给定创建一个model_file并pose_output_port通过MultibodyPositionToGeometryPose.

pose_output_port我使用的是 4(quaternion) + 3(xyz) + 1(joint angle) 最小状态。

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);
}

以上失败，但有以下异常

abort: Failure at multibody/plant/multibody_plant.cc:2015 in get_geometry_poses_output_port(): condition 'geometry_source_is_registered()' failed.
4

2 回答 2

1

所以，这里有很多。我怀疑有一个简单的答案，但我们可能必须集中精力。

首先，我的假设：

  1. 你有一个“内部”MultibodyPlant (MBP)。大概，您也有它的上下文，允许您执行有意义的状态相关计算。
  2. 此外，我认为 MBP 负责注册几何图形（可能在您解析它时发生）。
  3. 您LeafSystem将直接连接到SceneGraph提供姿势。
  4. 给定您的状态，您通常会在 MBP 的上下文中设置状态以进行评估。

选项 1（已编辑）：

在您的自定义LeafSystem中，创建输出端口，为其创建 calc 回调，FramePoseVector并在该回调中，只需调用您自己Eval()的内部的姿势输出端口的指向您的回调提供的指针）。MBPLeafSystemContextMBPFramePoseVectorLeafSystem

本质上（以非常粗略的方式）：

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);
}

选项 2：

您可以拥有一个包含 MBP 并直接通过图表导出 MBP 的输出以进行连接的，而不是实现LeafSystem具有内部工厂的 a 。DiagramFramePoseVector

于 2019-06-14T18:31:55.800 回答
1

该答案特别解决了您尝试使用该MultibodyPositionToGeometryPose方法的编辑。它没有解决更大的设计问题。

您的问题是MultibodyPositiontToGeometryPose系统引用 anMBP并保持对它的引用MBP。这意味着MBP至少与现在一样，它必须活着并且健康MPTGP。但是，在您的代码片段中，您MBP是函数的本地add_plant_visuals()函数，因此函数一结束它就会被销毁。

您需要创建由其他人保留和拥有的东西。

（这与我的选项 2 密切相关 - 现在进行了编辑以提高清晰度。）

于 2019-06-19T18:42:21.017 回答