0

背景

为了控制 my MultibodyPlant，我需要知道特定链接的当前滚动、俯仰、偏航和 x、y、z。我相信我是从我MultibodyPlant的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

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

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

4

1 回答 1

1

感谢 Sherm 的评论，我需要为我的自定义 LeafSystem禁用符号标量转换，因为不支持从四元数转换为滚动俯仰偏航symbolic::Expression

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

namespace drake {
namespace systems {
namespace scalar_conversion {
template <> struct Traits<MyLeafSystem> : public NonSymbolicTraits {};
}  // namespace scalar_conversion
}  // namespace systems
}  // namespace drake
于 2019-05-24T00:09:15.750 回答