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