Question

在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示例，我相信我需要指定完整的{ {1}}用于四旋翼和整个摆锤RigidTransformd。

问题

是否可以设置可视化效果/指定姿势，这样我只需指定7（四旋翼的姿势+关节角）状态的最小坐标，并让内部RigidTransformd处理每个人的计算身体（四极杆和摆锤）完整的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()只有7个状态（3 rpy + 3 xyz +1髋部角度）。

CompassGait，MultibodyPlant等价于什么？

更新（使用拉斯删除的答案中的MultibodyPositionToGeometryPose

我创建了以下函数，该函数尝试从给定的SceneGraph创建一个MultibodyPlant并通过model_file连接给定的工厂pose_output_port。

我正在使用的MultibodyPositionToGeometryPose是4（四元数）+ 3（xyz）+1（关节角）最小状态。

pose_output_port

以上操作失败，但出现以下异常

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

Answer 1

所以，这里有很多东西。我怀疑有一个简单的答案，但我们可能不得不对此予以收敛。

首先，我的假设：

您拥有一个“内部” MultibodyPlant（MBP）。大概，您还有一个上下文，可以执行有意义的与状态相关的计算。
此外，我认为MBP负责注册几何图形（可能是在解析几何图形时发生的）。
您的LeafSystem将直接连接到SceneGraph以提供姿势。
鉴于您的状态，您通常会在MBP的上下文中设置状态以进行评估。

选项1（已编辑）：

在您的自定义LeafSystem中，创建FramePoseVector输出端口，为其创建calc回调，然后在该回调内部，仅调用内部设备的位姿输出端口的Eval() MBP是您的LeafSystem所拥有的（先前已在本地Context中为MBP设置了状态，并传递了指向{ {1}}的回调已提供）。

本质上（以一种非常粗糙的方式）：

FramePoseVector

选项2：

您可以拥有一个包含MBP并直接通过该图导出MBP的LeafSystem输出以进行连接的方法，而不是实现一个具有内部工厂的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); }。

Answer 2

此答案专门针对您尝试使用MultibodyPositionToGeometryPose方法的编辑。它没有解决更大的设计问题。

您的问题是MultibodyPositiontToGeometryPose系统引用了MBP，并保留了对同一MBP的引用。这意味着MBP必须至少在MPTGP生命周期内还活着。但是，在您的代码段中，MBP是add_plant_visuals()函数的本地代码，因此一旦函数结束，它就会被销毁。

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

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

是否可以在不显式计算每个人体的完整变换的情况下可视化多体姿态？

问题

更新（使用拉斯删除的答案中的MultibodyPositionToGeometryPose

2 个答案:

选项1（已编辑）：

选项2：