从 state_output 转换为 RollPitchYaw 时没有变量 u0_0 的条目

Question

背景

为了控制我的 MultibodyPlant，我需要知道特定 link 的当前滚动、俯仰、偏航和 x、y、z。我相信我是从 MultibodyPlant 的 get_state_output_port() 那里获得这些信息的。植物是浮动的，即它使用 .

问题

解码浮动基状态向量 () 的含义后，我将 get_state_output_port() 连接到自定义 LeafSystem 的输入端口并尝试将四元数矩阵转换为roll pitch yaw 用下面的线

Eigen::Quaternion<T> q(state[0], state[1], state[2], state[3]);
drake::math::RollPitchYaw<T> rpy(q);

这给了我以下 std::runtime_error

what():  The following environment does not have an entry for the variable u0_0

如何正确地将当前状态转换为横滚俯仰偏航？

Answer 1

感谢 Sherm 的评论，我需要 disable symbolic scalar conversion 我的自定义 LeafSystem，因为 symbolic::Expression

不支持从四元数转换为滚动偏航

我必须将以下行添加到我的 LeafSystem 的定义中

namespace drake {
namespace systems {
namespace scalar_conversion {
template <> struct Traits<MyLeafSystem> : public NonSymbolicTraits {};
}  // namespace scalar_conversion
}  // namespace systems
}  // namespace drake

从 state_output 转换为 RollPitchYaw 时没有变量 u0_0 的条目

No entry for variable u0_0 when converting from state_output to RollPitchYaw

drake

背景

问题