lookupPose

Obtain pose information for certain time

Since R2023a

collapse all in page

    Syntax

    [position,orientation,velocity,acceleration,angularVelocity] = lookupPose(trajectory,sampleTimes)

    Description

    [position,orientation,velocity,acceleration,angularVelocity] = lookupPose(trajectory,sampleTimes) returns the pose information of the polynomial trajectory at the specified sample times. If any sample time is beyond the duration of the trajectory, the function returns the corresponding pose information as NaN.

    example

    Examples

    collapse all

    This example uses:

    Open Live Script

    Use the minsnappolytraj function to generate the piecewise polynomial and the time samples for the specified waypoints of a trajectory.

    waypoints = [0 20 20 0 0; 0 0 5 5 0; 0 5 10 5 0];
timePoints = linspace(0,30,5);
numSamples = 100;
[~,~,~,~,~,pp,~,~] = minsnappolytraj(waypoints,timePoints,numSamples);

    Use the polynomialTrajectory System object™ to generate a trajectory from the piecewise polynomial. Specify the sample rate of the trajectory.

    traj = polynomialTrajectory(pp,SampleRate=100);

    Inspect the waypoints and times of arrival by using waypointInfo.

    waypointInfo(traj)
    ans=5×2 table
    TimeOfArrival                   Waypoints                
    _____________    ________________________________________

           0                  0              0              0
         7.5                 20              0              5
          15                 20              5             10
        22.5                  0              5              5
          30         6.5364e-14    -2.6699e-12    -2.8679e-12

    Obtain the time of arrival between the second and fourth waypoint. Create timestamps to sample the trajectory.

    t0 = traj.TimeOfArrival(2);
tf = traj.TimeOfArrival(4);
sampleTimes = linspace(t0,tf,1000);

    Obtain the position, orientation, velocity, and acceleration information at the sampled timestamps using the lookupPose object function.

    [pos,orient,vel,accel,~] = lookupPose(traj,sampleTimes);

    Get the yaw angle from the orientation.

    eulOrientation = quat2eul(orient);
yawAngle = eulOrientation(:,1);

    Plot the generated positions and orientations, as well as the specified waypoints.

    plot3(pos(:,1),pos(:,2),pos(:,3), ...
      waypoints(1,:),waypoints(2,:),waypoints(3,:),"--o")
hold on
% Plot the yaw using quiver.
quiverIdx = 1:100:length(pos);
quiver3(pos(quiverIdx,1),pos(quiverIdx,2),pos(quiverIdx,3), ...
        cos(yawAngle(quiverIdx)),sin(yawAngle(quiverIdx)), ...
        zeros(numel(quiverIdx),1))
title("Position and Orientation")
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")
legend({"Position","Waypoints","Orientation"})
axis equal
hold off

    Figure contains an axes object. The axes object with title Position and Orientation, xlabel X (m), ylabel Y (m) contains 3 objects of type line, quiver. These objects represent Position, Waypoints, Orientation.

    Plot the velocity profiles.

    figure
subplot(3,1,1)
plot(sampleTimes,vel(:,1))
title("Velocity Profile")
ylabel("v_x (m/s)")
subplot(3,1,2)
plot(sampleTimes,vel(:,2))
ylabel("v_y (m/s)")
subplot(3,1,3)
plot(sampleTimes,vel(:,3))
ylabel("v_z (m/s)")
xlabel("Time (sec)")

    Figure contains 3 axes objects. Axes object 1 with title Velocity Profile, ylabel v_x (m/s) contains an object of type line. Axes object 2 with ylabel v_y (m/s) contains an object of type line. Axes object 3 with xlabel Time (sec), ylabel v_z (m/s) contains an object of type line.

    Plot the acceleration profiles.

    figure
subplot(3,1,1)
plot(sampleTimes,accel(:,1))
title("Acceleration Profile")
ylabel("a_x (m/s^2)")
subplot(3,1,2)
plot(sampleTimes,accel(:,2))
ylabel("a_y (m/s^2)")
subplot(3,1,3)
plot(sampleTimes,accel(:,3))
ylabel("a_z (m/s^2)")
xlabel("Time (sec)")

    Figure contains 3 axes objects. Axes object 1 with title Acceleration Profile, ylabel a_x (m/s^2) contains an object of type line. Axes object 2 with ylabel a_y (m/s^2) contains an object of type line. Axes object 3 with xlabel Time (sec), ylabel a_z (m/s^2) contains an object of type line.

    Input Arguments

    collapse all

    Polynomial trajectory, specified as a polynomialTrajectory object.

    Sample times, in seconds, specified as an M-element vector of nonnegative numbers.

    Output Arguments

    collapse all

    Position in the local navigation coordinate system, in meters, returned as an M-by-3 matrix.

    M is specified by the sampleTimes input.

    Data Types: double

    Orientation in the local navigation coordinate system, returned as an M-element quaternion column vector or a 3-by-3-by-M real array.

    Each quaternion or 3-by-3 rotation matrix is a frame rotation from the local navigation coordinate system to the current body coordinate system at the corresponding sample time.

    M is specified by the sampleTimes input.

    Data Types: double

    Velocity in the local navigation coordinate system, in meters per second, returned as an M-by-3 matrix.

    M is specified by the sampleTimes input.

    Data Types: double

    Acceleration in the local navigation coordinate system, in meters per second squared, returned as an M-by-3 matrix.

    M is specified by the sampleTimes input.

    Data Types: double

    Angular velocity in the local navigation coordinate system, in radians per second, returned as an M-by-3 matrix.

    M is specified by the sampleTimes input.

    Data Types: double

    Version History

    Introduced in R2023a

    See Also

    Objects

    Functions