trajectoryExample.m

This example shows how to use the Trajectory class to create a trajectory with waypoints and visualize it.

This example shows how to use the Trajectory class to create a trajectory with waypoints and visualize it.

% =========================================================================== %
%> @example trajectoryExample.m
%>
%> This example shows how to use the Trajectory class to create a trajectory
%> with waypoints and visualize it.
% =========================================================================== %
 
% Create a set start times and stay times for each waypoint
t_start = seconds(0:10:120);
t_stay  = repmat(seconds(0), size(t_start));
 
% Create a set of waypoint positions
pos     = [...
    0 0 0; ...
    1 0 0; ...
    1 1 0; ...
    0 1 0; ...
    0 0 1; ...
    1 0 1; ...
    1 1 1; ...
    0 1 1; ...
    0 0 2; ...
    1 0 2; ...
    1 1 2; ...
    0 1 2; ...
    0 0 0; ...
    ];
 
% Create a set of waypoint orientations
% this is only a yaw-rotation of 90 ° per row and after 360° the pitch also
% changes by 90°.
ori = quaternion([...
     0     1     0     0;    ...
     0    -0.71 -0.71  0;    ...
     0     0     1     0;    ...
     0     0.71 -0.71  0;    ...
     0    -0.71  0     0.71; ...
    -0.5  -0.5  -0.5   0.5;  ...
    -0.71  0    -0.71  0;    ...
    -0.5   0.5  -0.5  -0.5;  ...
     0     0     0     1;    ...
    -0.71  0     0     0.71; ...
     1     0     0     0;    ...
    -0.71  0     0    -0.71; ...
     0     1     0     0;    ...
    ]);
 
% Create a trajectory object
traj = Trajectory();
 
% Add the waypoints to the trajectory
traj.addWaypoints(t_start, t_stay, pos, ori);
 
% display the waypoints
traj.plotWaypoints;
 
% generate the Trajectory
traj.generateTrajectory(pos="minSnap", ori="makima");
 
% visualize the trajectory
trajectoryVisualizationApp(traj);