我可以在不显式计算每个 body 的完整变换的情况下可视化 Multibody 姿势吗？
Can I visualize a Multibody pose without explicitly calculating every body's full transform?
在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()}};
}
在我的例子中，我有一个基于浮动的多 body 系统（想象一个带有钟摆的四旋翼），我已经在其中创建了一个自定义工厂 (LeafSystem)。这种系统的最小坐标为 4（四元数）+ 3（x，y，z）+ 1（关节角）= 7。如果我要遵循 QuadrotorGeometry 示例，我相信我需要指定完整的 RigidTransformd 用于四旋翼飞行器和完整的 RigidTransformd 钟摆。
问题
是否可以设置可视化/指定姿势，以便我只需要指定 7（四旋翼姿势 + 关节角度）状态最小坐标并让内部 MultibodyPlant 处理计算每个人 body 的（四旋翼和钟摆）完整 RigidTransform 然后可以传递给 SceneGraph 进行可视化？
我相信 "attic-ed"（我的意思是 "to be deprecated"）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() 输出的 CompassGait 状态只有 7 个状态（3 rpy + 3 xyz + 1 臀部角度）。
MultibodyPlant、SceneGraph 等价于什么？
更新（使用 MultibodyPositionToGeometryPose 来自 Russ 已删除的答案
我创建了以下函数，它尝试从给定的 model_file 创建一个 MultibodyPlant 并通过 MultibodyPositionToGeometryPose 连接给定的工厂 pose_output_port。
我用的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.
所以，这里有很多东西。我怀疑有一个简单的答案，但我们可能不得不收敛它。
首先，我的假设：
- 你有一个 "internal" MultibodyPlant (MBP)。据推测，您也有它的上下文，允许您执行有意义的 state-dependent 计算。
- 此外，我认为 MBP 负责注册几何图形（可能是在您解析它时发生的）。
- 您的
LeafSystem 将直接连接到 SceneGraph 以提供姿势。
- 鉴于您的状态，您通常会在 MBP 的上下文中设置状态以进行该评估。
选项 1（已编辑）：
在您的自定义 LeafSystem 中，创建 FramePoseVector 输出端口，为其创建 calc 回调，并在该回调中，简单地调用姿势输出端口的 Eval()您的 LeafSystem 拥有的内部 MBP（之前已在您本地拥有的 Context 中为 MBP 设置状态，并将指针传递给 FramePoseVector您的 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);
}
选项 2：
与其实施具有内部工厂的 LeafSystem，不如实施包含 MBP 的 Diagram 并通过图表直接导出 MBP 的 FramePoseVector 输出以进行连接。
此答案特别针对您尝试使用 MultibodyPositionToGeometryPose 方法的编辑。它没有解决更大的设计问题。
您的问题是 MultibodyPositiontToGeometryPose 系统引用了 MBP 并保留了对同一个 MBP 的引用。这意味着 MBP 必须至少与 MPTGP 一样长寿。但是，在您的代码片段中，您的 MBP 是 add_plant_visuals() 函数的本地函数，因此函数一结束它就会被销毁。
您需要创建一些东西，让其他人保留和拥有。
（这与我的选项 2 密切相关 - 现在已编辑以提高清晰度。）
在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()}};
}
在我的例子中，我有一个基于浮动的多 body 系统（想象一个带有钟摆的四旋翼），我已经在其中创建了一个自定义工厂 (LeafSystem)。这种系统的最小坐标为 4（四元数）+ 3（x，y，z）+ 1（关节角）= 7。如果我要遵循 QuadrotorGeometry 示例，我相信我需要指定完整的 RigidTransformd 用于四旋翼飞行器和完整的 RigidTransformd 钟摆。
问题
是否可以设置可视化/指定姿势，以便我只需要指定 7（四旋翼姿势 + 关节角度）状态最小坐标并让内部 MultibodyPlant 处理计算每个人 body 的（四旋翼和钟摆）完整 RigidTransform 然后可以传递给 SceneGraph 进行可视化？
我相信 "attic-ed"（我的意思是 "to be deprecated"）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() 输出的 CompassGait 状态只有 7 个状态（3 rpy + 3 xyz + 1 臀部角度）。
MultibodyPlant、SceneGraph 等价于什么？
更新（使用 MultibodyPositionToGeometryPose 来自 Russ 已删除的答案
我创建了以下函数，它尝试从给定的 model_file 创建一个 MultibodyPlant 并通过 MultibodyPositionToGeometryPose 连接给定的工厂 pose_output_port。
我用的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.
所以，这里有很多东西。我怀疑有一个简单的答案，但我们可能不得不收敛它。
首先，我的假设：
- 你有一个 "internal" MultibodyPlant (MBP)。据推测，您也有它的上下文，允许您执行有意义的 state-dependent 计算。
- 此外，我认为 MBP 负责注册几何图形（可能是在您解析它时发生的）。
- 您的
LeafSystem将直接连接到SceneGraph以提供姿势。 - 鉴于您的状态，您通常会在 MBP 的上下文中设置状态以进行该评估。
选项 1（已编辑）：
在您的自定义 LeafSystem 中，创建 FramePoseVector 输出端口，为其创建 calc 回调，并在该回调中，简单地调用姿势输出端口的 Eval()您的 LeafSystem 拥有的内部 MBP（之前已在您本地拥有的 Context 中为 MBP 设置状态，并将指针传递给 FramePoseVector您的 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);
}
选项 2：
与其实施具有内部工厂的 LeafSystem，不如实施包含 MBP 的 Diagram 并通过图表直接导出 MBP 的 FramePoseVector 输出以进行连接。
此答案特别针对您尝试使用 MultibodyPositionToGeometryPose 方法的编辑。它没有解决更大的设计问题。
您的问题是 MultibodyPositiontToGeometryPose 系统引用了 MBP 并保留了对同一个 MBP 的引用。这意味着 MBP 必须至少与 MPTGP 一样长寿。但是，在您的代码片段中，您的 MBP 是 add_plant_visuals() 函数的本地函数，因此函数一结束它就会被销毁。
您需要创建一些东西，让其他人保留和拥有。
（这与我的选项 2 密切相关 - 现在已编辑以提高清晰度。）