Question

背景

为了控制我的MultibodyPlant，我需要知道特定链接的当前滚动，俯仰，偏航和x，y，z。我相信我是从MultibodyPlant的{{1}}获得此信息的。植物是浮动的，即它使用default quaternion-based floating base。

问题

在解码了浮动基本状态向量（not sure if I did it right）的含义之后，我将get_state_output_port()连接到自定义get_state_output_port()的输入端口，然后尝试将四元数矩阵转换为滚动间距用以下行偏航

LeafSystem

这给了我以下Eigen::Quaternion<T> q(state[0], state[1], state[2], state[3]); drake::math::RollPitchYaw<T> rpy(q);

std::runtime_error

如何正确地将当前状态转换为侧倾偏航？

Answer 1

由于Sherm的评论，symbolic::Expression不支持disable symbolic scalar conversion的自定义LeafSystem，因为从四元数转换为滚动变桨偏航

我必须在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没有条目

背景

问题

1 个答案: