Main Content

lookupPose

Obtain pose information for certain time

Since R2023a

collapse all in page

    Syntax

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

    Description

    example

    [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.

    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         2.4897e-13    -2.7471e-12    -2.6352e-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

    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)")

    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)")

    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