interpolate
Interpolate poses along UAV Dubins path segment
Description
interpolates poses along the path segment at the specified path lengths. Transitions between
motion types are always included.poses
= interpolate(pathSegObj
,lengths
)
Examples
Interpolate Poses for UAV Dubins Path
This example shows how to connect poses using the uavDubinsConnection
object and interpolate the poses along the path segment at the specified path lengths.
Connect Poses Using UAV Dubins Connection Path
Create a uavDubinsConnection
object.
connectionObj = uavDubinsConnection;
Define start and goal poses as [x
, y
, z
, headingAngle
] vectors.
startPose = [0 0 0 0]; % [meters, meters, meters, radians]
goalPose = [0 0 20 pi];
Calculate a valid path segment and connect the poses.
[pathSegObj,pathCost] = connect(connectionObj,startPose,goalPose);
Show the generated path.
show(pathSegObj{1})
Interpolate the Poses
Specify the interval to interpolate along the path.
stepSize = pathSegObj{1}.Length/10; lengths = 0:stepSize:pathSegObj{1}.Length;
Interpolate the poses along the path segment at the specified path lengths.
poses = interpolate(pathSegObj{1},lengths); % [x, y, z, headingAngle, flightPathAngle, rollAngle]
Visualize the Transition Poses
Compute the translation and rotation matrix of the transition poses, excluding the start and goal poses. The posesTranslation
matrix consists of the first three columns of the poses
matrix specifying the position x
, y
, and z
.
posesTranslation = poses(2:end-1,1:3); % [x, y, z]
Increment the elements of the fourth column of the poses
matrix representing the headingAngle
by pi
and assign it as the first column of the rotation matrix posesEulRot
in ZYX Euler angle representation. A column of pi
and a column of zeros forms the second and the third columns of the posesEulRot
matrix, respectively. Convert the posesEulRot
matrix from Euler angles to quaternion and assign to posesRotation
.
N = size(poses,1)-2; posesEulRot = [poses(2:end-1,4)+pi,ones(N,1)*pi,zeros(N,1)]; % [headingAngle + pi, pi, 0] posesRotation = quaternion(eul2quat(posesEulRot,'ZYX'));
Plot transform frame of the transition poses by specifying their translations and rotations using plotTransforms
.
hold on plotTransforms(posesTranslation,posesRotation,'MeshFilePath','fixedwing.stl','MeshColor','cyan')
Input Arguments
pathSegObj
— Path segment
uavDubinsPathSegment
object
Path segment, specified as a uavDubinsPathSegment
object.
lengths
— Lengths along path to interpolate poses
positive numeric vector
Lengths along path to interpolate poses, specified as a positive numeric vector in meters.
For example, specify
0:stepSize:pathSegObj{1}.Length
to interpolate
at the interval specified by stepSize along the path. Transitions
between motion types are always included.
Data Types: double
Output Arguments
poses
— Interpolated poses
six-element numeric matrix
Interpolated poses along the path segment, returned as a six-element numeric matrix [x, y, z, headingAngle, flightPathAngle, rollAngle]. Each row of the matrix corresponds to a different interpolated pose along the path.
x, y, and z specify the position in meters. headingAngle, flightPathAngle, and rollAngle specify the orientation in radians.
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Version History
Introduced in R2019b
See Also
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)