waypointTrajectory orientation fluctuating when it should be zero.

sampleRate = 100;
waypoints = 3;

Trajectory.waypoints = [0,0,0;
                        0,0,0;
                        3,3,3];

Trajectory.velocities = [0,0,0;
                         0,0,0;
                         0,0,0];

Trajectory.orientations = quaternion([0,0,0; ...
                                      0,0,0; ...
                                      0,10,0], 'eulerd','XYZ','point');

Trajectory.timeEnd = 4;

Trajectory.arrivalTimes = linspace(0, Trajectory.timeEnd, waypoints);

path = waypointTrajectory("Waypoints", Trajectory.waypoints,...
                          "TimeOfArrival", Trajectory.arrivalTimes,...
                          "Orientation", Trajectory.orientations,...
                          "Velocities", Trajectory.velocities,...
                          "SampleRate", sampleRate,...
                          "SamplesPerFrame", 1,...
                          "ReferenceFrame", 'NED');

while ~isDone(path)
   [position, orientation, velocity, acceleration, angularVelocity] = path();

   eul = rad2deg(quat2eul(orientation));
end

Could someone please explain why I am seeing the orientation fluctuate a couple of degrees during the first 2 seconds of the path? I would assume that the orientation would behave identically to the position and not start changing until 2 seconds in.

Answer 1

[Kartik]

Hi,

The issue you are seeing with the orientation fluctuating a couple of degrees during the first 2 seconds of the path is related to the way quaternions interpolate between rotations.

The waypointTrajectory object creates a sequence of quaternions for the orientations based on the specified euler angles at each waypoint. The quaternion representation is more robust than euler angles because it avoids issues such as gimbal lock. However, quaternions involve interpolation between rotations that can lead to small errors when transitioning between rotations.

In your code, the orientations specified for the first two waypoints have a difference of 10 degrees in the yaw angle (around Z-axis). Since the time to move from the first waypoint to the second is short, the orientation quaternion has to interpolate between these two orientations. As a result, you may see some fluctuations in the orientation at the beginning of the trajectory.

To minimize the effect of quaternion interpolation, you can try to increase the number of waypoints along the trajectory. This will allow the orientation to change more gradually, reducing the amount of interpolation required. Another option is to use a different type of interpolation function for the orientations, such as "SLERP", which may provide smoother transitions between rotations.

Keep in mind that small variations in the orientation may not be easily noticeable in practice, especially when the vehicle is moving at high speeds or is subjected to external disturbances. Overall, the orientation variation you are seeing is due to the nature of quaternion interpolation and is expected behavior.

[Dylan White]

Thanks for the reply.

The R2022b release notes have this:

• To let the trajectory wait at a specific waypoint, simply repeat the waypoint coordinate in two consecutive rows when specifying the Waypoints property.

I don't understand why the orientations cannot be treated the same way in a special case. There should be no interpolation between my first two orientations (both 0,0,0).