waypointTrajectory generator : TOA vs SPEED

Hi

Using Waypoint trajectory generator in Navigation toolbox, a "travel" can be defined using WAYPOINTs and TOAs.

Its also possible to define a SPEED parameter for each of the WAYPOINTs.

For me it seems that defining TOA and SPEED for a waypoint can be "contradictory" or "unconsistant" because TOA is the result of a distance/speed.

I guess, I missed something in the documentation.

is there an example with speed parameter used or an explaination ?

BR

Juliette

Answer 1

[Ryan Salvo]

Hi Juliette,

There is an example in the help for waypointTrajectory, but it is not on the documentation page. I have created a request to add this example to the documentation page. For now, you can access this example by executing the following command on the Command Window:

help waypointTrajectory

The example is:

% EXAMPLE 2: Generate a racetrack trajectory by specifying the velocity
% and orientation at each waypoint.
Fs = 100;
wps = [0 0 0;
    20 0 0;
    20 5 0;
    0 5 0;
    0 0 0];
t = cumsum([0 10 1.25*pi 10 1.25*pi]).';
vels = [2 0 0;
    2 0 0;
    -2 0 0;
    -2 0 0;
    2 0 0];
eulerAngs = [0 0 0;
    0 0 0;
    180 0 0;
    180 0 0;
    0 0 0];
q = quaternion(eulerAngs, 'eulerd', 'ZYX', 'frame');
traj = waypointTrajectory(wps, 'SampleRate', Fs, ...
    'TimeOfArrival', t, 'Velocities', vels, 'Orientation', q);

% fetch pose information one buffer frame at a time
[pos, orient, vel, acc, angvel] = traj();
i = 1;
spf = traj.SamplesPerFrame;
while ~isDone(traj)
    idx = (i+1):(i+spf);
    [pos(idx,:), orient(idx,:), ...
        vel(idx,:), acc(idx,:), angvel(idx,:)] = traj();
    i = i+spf;
end
% Plot generated positions and specified waypoints.
plot(pos(:,1),pos(:,2), wps(:,1),wps(:,2), '--o')
title('Position')
xlabel('X (m)')
ylabel('Y (m)')
zlabel('Z (m)')
legend({'Position', 'Waypoints'})
axis equal

The Velocities property is the velocity of the object at the time is passes the corresponding waypoint, not the velocity as the object travels from waypoint to waypoint. These Velocities values, along with the Waypoints and TimeOfArrival values, determine the object's speed from waypoint to waypoint.

The Algorithms section of the documentation page also has some more details on how the trajectory is generated.

Thanks,

Ryan

[juliette soula]

Hi Ryan,

Thanks for reminding me to use the command window "help waypointTrajectory" because I was just looking within the navigator like help.

However this help and example is related to :

Velocities — Velocity in navigation coordinate system at each waypoint (m/s)

while I'm looking for information related to:

GroundSpeed — Groundspeed at each waypoint (m/s)

thanks

Juliette

[Ryan Salvo]

Hi Juliette,

Thank you for the clarification and apologies for my confusion. I have modified the example to use the GroundSpeed input argument instead. I have also added a plot to show the generated groundspeed and the input groundspeed at each waypoint/TimeOfArrival value.

Thanks,

Ryan

% Generate a circular trajectory by specifying the groundspeed
% and orientation at each waypoint.
Fs = 100;
wps = [0 0 0;
    20 0 0;
    20 5 0;
    0 5 0;
    0 0 0];
t = cumsum([0 10 1.25*pi 10 1.25*pi]).';
vels = [2 0 0;
    2 0 0;
    -2 0 0;
    -2 0 0;
    2 0 0];
groundspeed = sqrt(sum(vels(:,1:2).^2, 2));
eulerAngs = [0 0 0;
    0 0 0;
    180 0 0;
    180 0 0;
    0 0 0];
q = quaternion(eulerAngs, 'eulerd', 'ZYX', 'frame');
traj = waypointTrajectory(wps, 'SampleRate', Fs, ...
    'TimeOfArrival', t, 'GroundSpeed', groundspeed, 'Orientation', q);
% fetch pose information one buffer frame at a time
[pos, orient, vel, acc, angvel] = traj();
i = 1;
spf = traj.SamplesPerFrame;
while ~isDone(traj)
    idx = (i+1):(i+spf);
    [pos(idx,:), orient(idx,:), ...
        vel(idx,:), acc(idx,:), angvel(idx,:)] = traj();
    i = i+spf;
end
% Plot generated positions and specified waypoints.
figure
plot(pos(:,1),pos(:,2), wps(:,1),wps(:,2), '--o')
title('Position')
xlabel('X (m)')
ylabel('Y (m)')
zlabel('Z (m)')
legend({'Position', 'Waypoints'})
axis equal
% Plot generated groundspeeds.
figure
gs = sqrt(sum(vel(:,1:2).^2, 2));
times = ((0:numel(gs)-1)./traj.SampleRate).';
plot(times, gs, t, groundspeed, '--o')
title('Groundspeed')
xlabel('Time (s)')
ylabel('Groundspeed (m/s)')
legend({'Groundspeed', 'Input groundspeed at TOA'})

[juliette soula]

Hi Ryan,

thanls for the code above which is very helpful.

In order to improve my understanding I've experimented settings combinations.

BR

Juliette