def transform_trajectory(trajectory, R, p):
"""Create new trajectory relative the given transformation
If R1(t) and p1(t) is the rotation and translation given by inout trajectory, then
this function returns a new trajectory that fulfills the following.
Let X1 = R1(t).T[X - p1(t)] be the coordinate of point X in input trajectory coordinates.
Then X2 = R X1 + p is the same point in the coordinate frame of the new trajectory
Since X2 = R2(t).T [X - p2(t)] then we have
R2(t) = R1(t)R and p2(t) = p1(t) + R1(t)p
"""
ts_q1 = trajectory.sampled.rotationKeyFrames
q1 = ts_q1.values
ts_p1 = trajectory.sampled.positionKeyFrames
p1 = ts_p1.values
# Rotation
Q = Quaternion.fromMatrix(R)
q2 = q1 * Q
ts_q2 = TimeSeries(ts_q1.timestamps, q2)
# Translation
p2 = p1 + q1.rotateVector(p)
ts_p2 = TimeSeries(ts_p1.timestamps, p2)
sampled = SampledTrajectory(positionKeyFrames=ts_p2,
rotationKeyFrames=ts_q2)
smoothen = False # MUST be the same as used in Dataset class
splined = SplinedTrajectory(sampled, smoothRotations=smoothen)
return splined
