I am playing with PiecewisePose. I want to create a linear interpolation trajectory from an inital pose to the goal pose (that is, my interpolation trajectory only contains one segment). After creating the trajectory, I add it to the TrajectorySource and use LogVectorOutput to save the log. Here is my code:
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)
To see the result, I print the log:
log_R = logger_R.FindLog(context)
print(log_R.data())
However, the result is quiet surprising:
[[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.]]
The quaternion is always (1, 0, 0, 0)! I then check the translation log:
log_p = logger_p.FindLog(context)
print(log_p.data())
which is correct:
[[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. ]]
After I change the goal pose to X_1 = RigidTransform(RollPitchYaw(np.pi/3, np.pi/3, 0), [1, 1, 1]), I seems to be able to get a variable trajectory for orientation:
[[ 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 ]]
However, the returned quaternion is not right. To see it
print(X_1.rotation().ToQuaternion())
returns
Quaternion_[float](w=0.8660254037844388, x=0.0, y=0.0, z=0.5)
While the last column in above returned result is (0.5, 0, -0.8660254, 0.75) which is not even a unit quaternion.
In order to find what happend when
X_1 = RigidTransform(RotationMatrix.MakeXRotation(np.pi/2), [1, 1, 1])
I checked the quaternion slerp traj_R, and it seems that the traj_R is correct:
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)
So it seems that PiecewisePose returns a correct trajectory, but something wrong happens with TrajectorySource. Is it a bug? Thank you!
Another question I have is that in order to control the number of data logged, I have to add a MultibodyPlant, which serves no other purpose but to define the timestep size. Is there another way to control the communication frequency between the TrajectorySource and the vector logger? Thank you.
Seems like a bug in PiecewiseQuaternion to me. TrajectorySource (with 0-order derivative) just evaluates the the trajectory at the the sample times by invoking Trajectory::value(). However, PiecewiseQuaternion spits out rotation matrices as values instead of quarternions. Therefore the first column of the rotation matrix and the first entry in the second column is being copied to the Trajectory source, which is garbage.
The documentation of Trajectory says that the size of the output returned by value() should have rows() and cols(). For PiecewiseQuaternion, rows and cols are 4 and 1 respectively, but value() spits out a 3x3 rotation matrix.
Please consider posting an issue.
To your second question: LogVectorOutput has an optional argument that lets you set the publish period. See here.