Plan Path of Robotic Arm Mounted on Quadrotor

Since R2024a

This example shows how to plan collision-free geometric paths for floating base systems using the "floating" type rigidBodyJoint and manipulatorRRT. A floating-base system has a base with a "floating" joint which can translate and rotate freely in space, thus, has six degrees of freedom. This example uses a quadrotor UAV with a robotic arm mounted to it as an example floating-base system.

To model floating base systems using the fixed base rigidBodyTree object, you must define the floating body as a rigidBody object attached to the fixed base via a "floating" joint. Note that you cannot perform the inverse kinematics for floating base systems modeled in this way. To model a floating base system for inverse kinematics calculations, see Inverse Kinematics for Robots with Floating Base.

Create UAV as Floating Base

First, create an empty rigid body tree and a rigid body with a floating joint.

uavWithArm = rigidBodyTree(DataFormat="row");
uav = rigidBody("uav");
uav.Joint = rigidBodyJoint("uav_base_floating_joint","floating");

Get the geometry of a quadrotor UAV as a triangulation object by reading from the multirotor STL file.

tr = stlread("multirotor.stl");

Scale the geometry so that the dimensions are more accurate to the size of a real quadrotor UAV.

scaledtr = triangulation(tr.ConnectivityList,tr.Points/200);
trimesh(scaledtr);
title("UAV Triangulation")
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")
axis equal

Figure contains an axes object. The axes object with title UAV Triangulation, xlabel X (m), ylabel Y (m) contains an object of type patch.

[az,el] = view;

Notice that the collision geometry is nonconvex. To accurately model this geometry for collision checking, use the collisionVHACD function to decompose this into a composite of approximate convex meshes.

options = vhacdOptions("IndividualMesh",MaxNumConvexHulls=8,MaxNumVerticesPerHull=50);
compositeMeshes = collisionVHACD(scaledtr,options);

Add each of the decomposed meshes to the rigid body using addCollision.

for i = 1:length(compositeMeshes)
    addCollision(uav,compositeMeshes{i});
end

Add the UAV rigid body to the floating-base rigid body tree and visualize the UAV.

addBody(uavWithArm,uav,uavWithArm.BaseName);
show(uavWithArm,Collisions="on");
title("UAV Collision Mesh")
axis equal
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")
view([az,el])

Figure contains an axes object. The axes object with title UAV Collision Mesh, xlabel X (m), ylabel Y (m) contains 11 objects of type patch, line. These objects represent base, uav, uav_coll_mesh.

Mount Robotic Arm on UAV

Load the rigid body tree for the Open Manipulator robotic arm from Robotis.

openmanip = loadrobot("robotisOpenManipulator",DataFormat="row");

To mount to the robotic arm on the underside of the UAV, set the fixed transform of the first body of the arm that rotates the arm pi radians in the y-axis and applies a small z-axis translation offset. The z-axis offset is to avoid self-collisions. Alternatively, one can specify the SkippedSelfCollisions property of the planner with the link pair to be ignored while self-collision checking. See example Skip Self-Collision Checking Between Specific Body Pairs for more details.

fixedTF = eul2tform([0 pi 0])*trvec2tform([0 0 1e-4]);
setFixedTransform(openmanip.Bodies{1}.Joint,fixedTF);

Mount the robotic arm rigid body tree model to the UAV floating base and visualize the UAV with the mounted arm.

addSubtree(uavWithArm,"uav",openmanip);
show(uavWithArm,Collisions="on",Frames="off");
title("UAV With Mounted Robotic Arm")
axis equal
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")
view(225,15)

Motion Planning Problem

The motion planning problem is to find a collision free path from a starting joint configuration to a desired goal configuration. To do this, you must define these aspects of the problem:

Starting joint configuration
Goal joint configuration
UAV collision model
Obstacles in the environment
Robot's Kinematic Constraints

Starting and Goal Joint Configurations

The joint configuration of the UAV represents the pose of the UAV in the world. Thus, define the configuration vector of the UAV to describe its pose in the world is given as $[q_{uav} t_{uav} q_{robotic arm}]$ where,

$q_{uav}$ is the orientation of the floating joint on the rigid body "uav" with respect to the fixed base represented as a 1-by-4 unit quaternion.
$t_{uav}$ is the position of the floating joint on the rigid body "uav" with respect to the fixed base represented as a 1-by-3 vector.
$q_{robotic arm}$ is the joint configuration vector of the robotic arm openmanip represented as a 1-by-6 vector.

Set the starting configuration with the floating body "uav" at the xyz-coordinate [0 0 -1] and -pi/3 rad for the third joint on the arm.

qstart = homeConfiguration(uavWithArm);
qstart(5:7) = [0 0 -1];
qstart(end-3) = -pi/3;

Use the start configuration and modify it to get a goal configuration. Set the goal configuration of the body "uav" to the xyz-coordinate [2 0 2] at an orientation of [pi/2 0 0] Euler ZYX.

qgoal = qstart;
qgoal(5:7) = [2 0 2];
qgoal(1:4) = eul2quat([pi/2 0 0]);

Add Obstacles in Environment

Create an environment using collision boxes and set the positions of the boxes such that they create a wall between the start and goal configurations.

env={collisionBox(0.5,4,3,Pose=trvec2tform([1.0 0.0 -1.3])),...
     collisionBox(0.5,4,1.5,Pose=trvec2tform([1.0 0.0 1.8]))};

Visualize the planning problem. Show the collision geometries and hide the frames on the UAV.

figure
show(uavWithArm,qstart,Collisions="on",Frames="off");
hold on
showCollisionArray(env);
show(uavWithArm,qgoal,Collisions="on",Frames="off");
title("Planning Problem")
view(0,0)
hold off

Robot's Kinematic Constraints

In this example, set the translational PositionLimits of the floating joint associated with the "uav" rigidBody such that it is constrained to fly within cartesian bounds (say a physical safety cage) of [-1,3] in X, [-2,2] in Y, and [-2.5,2.5] in Z, of the workspace's frame. Note that the rotational limits of a floating rigidBodyJoint cannot be set and are NaNs.

uavBody=getBody(uavWithArm,"uav");
uavBody.Joint.PositionLimits(5:end,:)=[-1,3;-2,2;-2.5,2.5];

Find Collision Free Path

Find a collision free path between the start and goal joint configuration. To find such a path, this example uses manipulatorRRT to plan a geometric collision-free path.

rrt = manipulatorRRT(uavWithArm,env, ...
    MaxConnectionDistance=0.5, ...
    ValidationDistance=0.1, ...
    SkippedSelfCollisions="parent");

Seed the random number generator and plan a path using RRT, shorten the output plan and interpolate the shortened path to make it smoother.

rng(0,"twister");
plannedpath = plan(rrt,qstart,qgoal);
shortenedPath = shorten(rrt,plannedpath,40);
rrtpath = interpolate(rrt,shortenedPath,10);

Visualize Planned Path

Visualize the planned geometric path of UAV with a mounted robotic arm as an animation.

show(uavWithArm,qstart,Collisions="on");
title("UAV Flight Animation")
view(0,0)
hold on
plot3(rrtpath(:,5),rrtpath(:,6),rrtpath(:,7),"k-",LineWidth=1)
showCollisionArray(env);
rc = rateControl(10);
for i = 1:1:size(rrtpath,1)
    show(uavWithArm,rrtpath(i,:),...
        PreservePlot=false,...
        FastUpdate=true, ...
        Frames="off", ...
        Collisions="on");
    waitfor(rc);
    view(0,0)
end
hold off

Figure contains an axes object. The axes object with title UAV Flight Animation, xlabel X, ylabel Y contains 26 objects of type patch, line. These objects represent uav_coll_mesh, link1_coll_mesh, link2_coll_mesh, link3_coll_mesh, link4_coll_mesh, link5_coll_mesh, gripper_link_coll_mesh, gripper_link_sub_coll_mesh.

Note that the arm also moves along the path to avoid colliding with the obstacles in the environment. Plot the joint angles of the robotic arm in the planned path.

plot(rrtpath(:,8:end),"-")
title("Robotic Arm Joint Angles")
xlabel("Time (s)")
ylabel("Joint Angle (rad)")
legend("Joint "+string(1:6))

Figure contains an axes object. The axes object with title Robotic Arm Joint Angles, xlabel Time (s), ylabel Joint Angle (rad) contains 6 objects of type line. These objects represent Joint 1, Joint 2, Joint 3, Joint 4, Joint 5, Joint 6.

Next Steps

Adding Constraints to Motion Plan

Notice how the planned geometric path doesn't impose any orientation restrictions on the UAV nor do the interpolated states impose kinematic constraints to achieve a feasible trajectory. In order to add such kinematic constraints to the geometric path, use a custom nav.StateSpace derived from a manipulatorStateSpace. Refer to Plan Paths With End-Effector Constraints Using State Spaces For Manipulators Documentation example as a reference.

Copyrights The MathWorks, Inc. 2023