带有 PiecewisePose 的 TrajectorySource 没有给出预期的旋转结果

Question

我正在玩 PiecewisePose。我想创建一个从初始姿势到目标姿势的线性插值轨迹（也就是说，我的插值轨迹只包含一段）。创建轨迹后，我将其添加到 TrajectorySource 并使用 LogVectorOutput 保存日志。这是我的代码：

sample_times = [0., 3.0]
X_0 = RigidTransform()
X_1 = RigidTransform(RotationMatrix.MakeXRotation(np.pi/2), [1, 1, 1])
Xs = [X_0, X_1]
traj = PiecewisePose.MakeLinear(sample_times, Xs)

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.1)
plant.Finalize()
traj_R = traj.get_orientation_trajectory()
traj_p = traj.get_position_trajectory()

R_source = builder.AddSystem(TrajectorySource(traj_R))
p_source = builder.AddSystem(TrajectorySource(traj_p))
logger_R = LogVectorOutput(R_source.get_output_port(), builder)
logger_p = LogVectorOutput(p_source.get_output_port(), builder)

diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_context()

simulator.AdvanceTo(3.0)

为了查看结果，我打印日志：

log_R = logger_R.FindLog(context)
print(log_R.data())

然而，结果却出乎意料：

[[1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1.
  1. 1. 1. 1. 1. 1. 1.]
 [0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.
  0. 0. 0. 0. 0. 0. 0.]
 [0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.
  0. 0. 0. 0. 0. 0. 0.]
 [0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.
  0. 0. 0. 0. 0. 0. 0.]]

四元数总是(1, 0, 0, 0)！然后我查看翻译日志：

log_p = logger_p.FindLog(context)
print(log_p.data())

哪个是正确的：

[[0.         0.03333333 0.06666667 0.1        0.13333333 0.16666667
  0.2        0.23333333 0.26666667 0.3        0.33333333 0.36666667
  0.4        0.43333333 0.46666667 0.5        0.53333333 0.56666667
  0.6        0.63333333 0.66666667 0.7        0.73333333 0.76666667
  0.8        0.83333333 0.86666667 0.9        0.93333333 0.96666667
  1.        ]
 [0.         0.03333333 0.06666667 0.1        0.13333333 0.16666667
  0.2        0.23333333 0.26666667 0.3        0.33333333 0.36666667
  0.4        0.43333333 0.46666667 0.5        0.53333333 0.56666667
  0.6        0.63333333 0.66666667 0.7        0.73333333 0.76666667
  0.8        0.83333333 0.86666667 0.9        0.93333333 0.96666667
  1.        ]
 [0.         0.03333333 0.06666667 0.1        0.13333333 0.16666667
  0.2        0.23333333 0.26666667 0.3        0.33333333 0.36666667
  0.4        0.43333333 0.46666667 0.5        0.53333333 0.56666667
  0.6        0.63333333 0.66666667 0.7        0.73333333 0.76666667
  0.8        0.83333333 0.86666667 0.9        0.93333333 0.96666667
  1.        ]]

我把目标位姿改成X_1 = RigidTransform(RollPitchYaw(np.pi/3, np.pi/3, 0), [1, 1, 1])后，我好像可以得到一个方向的可变轨迹：

[[ 1.          0.99933683  0.99734887  0.99404073  0.98942009  0.98349767
   0.97628722  0.96780548  0.95807213  0.94710977  0.93494383  0.92160256
   0.90711693  0.89152055  0.87484963  0.85714286  0.83844134  0.81878847
   0.79822988  0.77681329  0.75458839  0.73160678  0.7079218   0.68358843
   0.65866314  0.63320379  0.60726947  0.58092037  0.55421767  0.52722332
   0.5       ]
 [ 0.         -0.01770677 -0.03437769 -0.04997407 -0.06445971 -0.07780097
  -0.08996691 -0.10092927 -0.11066262 -0.11914437 -0.12635481 -0.13227723
  -0.13689787 -0.14020602 -0.14219398 -0.14285714 -0.14219398 -0.14020602
  -0.13689787 -0.13227723 -0.12635481 -0.11914437 -0.11066262 -0.10092927
  -0.08996691 -0.07780097 -0.06445971 -0.04997407 -0.03437769 -0.01770677
   0.        ]
 [ 0.         -0.03181766 -0.0641358  -0.09687939 -0.12997243 -0.16333811
  -0.19689899 -0.23057717 -0.26429449 -0.29797267 -0.33153355 -0.36489923
  -0.39799227 -0.43073586 -0.46305399 -0.49487166 -0.52611501 -0.55671151
  -0.58659017 -0.61568162 -0.64391833 -0.67123478 -0.69756756 -0.72285555
  -0.74704004 -0.77006491 -0.79187672 -0.81242483 -0.83166156 -0.84954225
  -0.8660254 ]
 [ 0.          0.01870152  0.03835438  0.05891297  0.08032957  0.10255447
   0.12553607  0.14922105  0.17355443  0.19847972  0.22393907  0.24987339
   0.27622248  0.30292519  0.32991953  0.35714286  0.38453197  0.41202331
   0.43955305  0.4670573   0.49447223  0.52173419  0.54877992  0.57554663
   0.6019722   0.62799529  0.6535555   0.67859351  0.70305119  0.72687179
   0.75      ]]

但是，返回的四元数不对。去看看

print(X_1.rotation().ToQuaternion())

returns

Quaternion_[float](w=0.8660254037844388, x=0.0, y=0.0, z=0.5)

而上面返回结果的最后一列是(0.5, 0, -0.8660254, 0.75) 这甚至不是单位四元数

为了找出

时发生了什么

X_1 = RigidTransform(RotationMatrix.MakeXRotation(np.pi/2), [1, 1, 1])

我检查了四元数 slerp traj_R，看来 traj_R 是正确的：

print(traj_R.orientation(0.0))
print(traj_R.orientation(1.0))
print(traj_R.orientation(2.0))
print(traj_R.orientation(3.0))

Quaternion_[float](w=1.0, x=0.0, y=0.0, z=0.0)
Quaternion_[float](w=0.9659258262890684, x=0.2588190451025208, y=0.0, z=0.0)
Quaternion_[float](w=0.8660254037844387, x=0.5, y=0.0, z=0.0)
Quaternion_[float](w=0.7071067811865476, x=0.7071067811865476, y=0.0, z=0.0)

所以看起来 PiecewisePose returns 是一个正确的轨迹，但是 TrajectorySource 出了问题。这是一个错误吗？谢谢！

我的另一个问题是，为了控制记录的数据数量，我必须添加一个 MultibodyPlant，它除了定义时间步长外没有其他用途。还有其他方法可以控制 TrajectorySource 和矢量记录器之间的通信频率吗？谢谢。

Answer 1

对我来说似乎是 PiecewiseQuaternion 中的错误。 TrajectorySource（具有 0 阶导数）仅通过调用 Trajectory::value() 计算采样时间的轨迹。但是，PiecewiseQuaternion 吐出旋转矩阵作为值而不是四元数。因此旋转矩阵的第一列和第二列的第一个条目被复制到轨迹源，这是垃圾。

Trajectory的文档说value()返回的输出大小应该有rows()和cols()。对于PiecewiseQuaternion，rows和cols分别是4和1，但是value()吐出一个3x3的旋转矩阵。

请考虑发布问题。

关于您的第二个问题：LogVectorOutput 有一个可选参数，可让您设置发布周期。参见 here。

带有 PiecewisePose 的 TrajectorySource 没有给出预期的旋转结果

TrajectorySource with PiecewisePose does not give expected rotation result

drake