Design a Trajectory Planner for a Robotic Manipulator

Review the model

Open the test harness containing the planner model and navigate to the planner subsystem to inspect its contents.

open_system('TrajectoryPlanning_Harness')
open_system('TrajectoryPlanning_Harness/Planner Subsystem')

There are several parts to the planner module:

These sections detail these steps:

Compute Inverse Kinematics

In order to compute a collision-free trajectory, the planner must know four things: the start and goal joint configurations, the robot model, and the environment model. While the start configuration is known (it is the robot's current configuration), the goal configuration is often unknown. Instead, the motion planner command may just pass a target pose: a Cartesian-space target for the end effector. When this is the case, the planner first needs to convert the pose to a joint configuration that reaches that pose, which is done using an Inverse Kinematics block.

Plan Collision-Free Trajectories and Optimize Given Kinematic Constraints

The bulk of the trajectory planning happens in two blocks within this area. Both blocks are system objects, which is a type of file similar to a MATLAB Function block that can be used to run MATLAB code in Simulink. Unlike a MATLAB function block, however, the system object can be executed in code generation and interpreted modes, allowing more flexible execution.

This routine uses two system objects:

edit('MotionPlannerCHOMP.m'); % System object
edit('exampleHelperCHOMPMotionPlanner.m'); % Planner creation & execution within the system object

edit('Contopptraj.m')

These two blocks in sequence return a sampled collision-free trajectory that is time-optimal given kinematic constraints. However, while contopptraj returns a complete trajectory, it is not uniformly sampled.

As mentioned, the planner code relies on a mexed version for performance, which is attached. You can generate this code yourself with the following command:

generateMEXforPlanner

Resample Trajectory with Uniform Step Size

A system object containing an interpolator is used to take the non-uniformly-sampled trajectory and re-sample it using a fine-grained, uniform sample. This complete trajectory can then be output to a target controller for execution.

Lastly, the outputs are reassembled into an output bus format. This is detailed in the interfaces section below.

Bus Overview

The model accepts a bus of robot commands, indicating the nature of the trajectory planning task, and motion planning response bus, which contains the generated trajectory and some associated solution data. The buses have the structure described below.

You could choose to build a completely different interface, but it would be necessary to use the same buses to communicate. Note that the buses have many fields, but only a few are absolutely necessary.

Robot Command Bus

The input type is a bus of robot commands. The structure can be seen by looking at the initialization struct:

busCmdToTest = motionPlannerCmdInitValue;

There are a few key parameters:

Variable-Size Commands

Note that Tasks and ObstacleObjects are initialized to 20-by-1 structures, but the number of tasks and provided obstacles can vary. To accommodate deployment requirements, these arrays are provided as a fixed size corresponding to a set maximum number. For each array, the associated parameter NumTasks or NumObstacleObjects defines the number of items for the planner to actually query.

Tasks Array

Each task is stored in a task array, with the following format:

busCmdToTest.Tasks(1)

ans = struct with fields:
                    Name: [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ... ] (1x256 uint8)
                    Type: [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ... ] (1x256 uint8)
               FrameName: [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ... ] (1x256 uint8)
                       p: [3x1 double]
                  eulZYX: [3x1 double]
                       q: [6x1 double]
               MaxLinVel: 0
               MaxAngVel: 0
                  MaxVel: 0
                  Weight: 0
             MaxLinError: 0
             MaxAngError: 0
           MaxJointError: 0
    ConvergenceSpeedGain: 0
            SecurityDist: 0
           InfluenceDist: 0

This array has several important fields:

char(busCmdToTest.Tasks(1).Name)

ans = 
'                                                                                                                                                                                                                                                                '

The remaining parameters are optional and may be used to provide constraints to the planner.

ObstacleObjects Array

The obstacle objects array provides another way for the user to provide primitive obstacles of type collisionBox. This is unused in this test harness. To understand the usage more closely, open the following helper, which builds the environment from the structured input:

edit('exampleHelperGenerateCollisionEnvironmentInPlanner.m')

Unused Properties in Input Buses

The buses provided as input have been designed for use with several different subsystems. That means that many of the properties may be left unused. For example, this planner module makes several simplifications:

Other planner modules could make use of these inputs. Using this harness, you could similarly verify.

Motion Planner Response Bus

After the trajectory has been planned, they must be output. First the trajectories are assembled into a structure that provides the complete, sampled trajectory:

motionPlannerResponseInitValue.JointTrajectory

ans = struct with fields:
       NumPoints: 0
    MaxNumPoints: 30000
            Time: [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ... ] (1x30000 double)
        JointPos: [6x30000 double]
        JointVel: [6x30000 double]
        JointAcc: [6x30000 double]
        JointTau: [6x30000 double]

Here the MaxNumPoints again indicates the upper bound on trajectory size. All properties are important:

Finally, the JointTrajectory bus is inserted into the motion planner response bus. This bus is mostly a set of flags:

motionPlannerResponseInitValue

motionPlannerResponseInitValue = struct with fields:
          RequestID: -1
             Status: -1
    JointTrajectory: [1x1 struct]
         IsPosValid: -1
         IsVelValid: -1
         IsAccValid: -1
         IsTauValid: -1

The bus has the following properties:

Validate Controller with Simple Planning Tasks

We can validate the motion planning module using a simple test harness. The harness is designed to test the two main functionalities that this block must achieve:

Initialize Inputs to the Test Harness

The provided harness does this using two separate subsystems that alternate execution. Start by loading the system.

open_system('TrajectoryPlanning_Harness');

Next, initialize the parameters and assign meaningful values to the input bus:

busCmdToTest = motionPlannerCmdInitValue;
busCmdToTest.RequestID = int32(1);
busCmdToTest.NumTasks = uint32(4);
busCmdToTest.InitialPositions = homePosition(:);
busCmdToTest.MaxIterations = uint32(300);
busCmdToTest.SampleTime = 0.2;
busCmdToTest.NumObstacleObjects = uint32(2); % This value should be nonzero to ensure the planner runs

The specific functionalities are defined in the task structure. Two tasks are created. First, a pick task is defined:

pickTask = busCmdToTest.Tasks(1);
pickTask.Name(1:length('TooltipTaskSE3')) = uint8('TooltipTaskSE3');
pickTask.Type(1:length('Pick')) = uint8('Pick');
pickTask.FrameName(1:length('Bellow')) = uint8('Bellow');
pickTask.p = [0.485; -0.0032; -0.0368];
pickTask.eulZYX = [1.2041; pi; 0];
pickTask.MaxLinVel = 0.1;
pickTask.MaxAngVel = 0.4;
pickTask.Weight = 1;
pickTask.MaxLinError = 0.01;
pickTask.MaxAngError = 0.0349;

Next, a place task is created by copying the picking task and making some minor edits to the goal configuration:

placeTask = pickTask;
placeTask.Type(1:length('Place')) = uint8('Place');
placeTask.p = [0.1 0.6 0.07]';
placeTask.eulZYX = [1.2041; pi; 0];

Plan Paths for Each Test Task Using the Test Harness

Run the test harness for each task, storing the outputs:

busCmdToTest.Tasks(1) = pickTask;
pickout = sim('TrajectoryPlanning_Harness.slx');

busCmdToTest.Tasks(1) = placeTask;
busCmdToTest.InitialPositions = [-0.2850 -1.5035 2.0352 -2.1025 -1.5708 0.0817]';
placeout = sim('TrajectoryPlanning_Harness.slx');

Analyze and Validate Outcomes

To validate the simulation, we will visualize the results in MATLAB. This makes use of two helper functions defined at the bottom of this script. The helpers simply convert the stored model outputs into meaningful data that can be visualized.

% Create the environment in which the robot is to be visualized
task_obst = exampleHelperGenerateCollisionEnvironmentInPlanner(uint8(targetEnv),binLength, binWidth, binHeight, binCenterPosition, binRotation, ...
    1, zeros(1,4));

Visualize Picking Task Outcomes

[pick_status, pick_task_traj, picktargetRegion] = helperReadModelData(pickout);

figure;
title('Animate the Picking Task');
show(ur5e, pick_task_traj(:,1)');
hold on
show(picktargetRegion)
showCollisionArray(task_obst);
axis equal

Animate the complete trajectory

rc = rateControl(25);
for i = 1:size(pick_task_traj,2)
    drawnow;
    show(ur5e, pick_task_traj(:,i)', FastUpdate=true, PreservePlot=false);
    waitfor(rc);
end

Visualize Placing Task Outcomes

[task_status, place_task_traj, placetargetRegion] = helperReadModelData(placeout);

figure;
title('Animate the Placing Task');
show(ur5e, place_task_traj(:,1)',Collisions="on");
hold on
show(placetargetRegion)
showCollisionArray(task_obst);
axis equal
rc = rateControl(25);
for i = 1:size(place_task_traj,2)
    drawnow;
    show(ur5e, place_task_traj(:,i)', FastUpdate=true, PreservePlot=false,Collisions="on");
    waitfor(rc);
end

Helper Functions

function [task_status, task_traj, targetRegion] = helperReadModelData(out)

% Read input data
tasks = out.yout{4}.Values.Tasks(1);
tgtPoses = [tasks.p.Data; tasks.eulZYX.Data];

% Read output data
reqs = out.yout{1}.Values.Data;
status = out.yout{2}.Values.Data;
trajs = out.yout{3}.Values;

% Determine the intervals where the request changes and sort into indices
reqchanges = find(diff(reqs)~=0); %Better than unique because duplicates are not filtered out
reqidx = [[1; reqchanges+1] [reqchanges; numel(reqs)]];

% Get the data corresponding to the first req
taskIDX = 1;
taskind = reqidx(taskIDX+1,:);

% Get the task status and trajectory corresponding to the task
task_status = status(taskind(1):taskind(2));
task_trajsize = trajs.NumPoints.Data(taskind(1));
task_traj = trajs.JointPos.Data(:,1:task_trajsize,taskind(1));

% Create a target region to indicate where the robot trajectory is supposed
% to end. Since this planner treats the target orientation as Z only, the
% valid region is set as a rotation about Z.
targetPoseVec = tgtPoses(:,:,taskind(1));
targetPoseTF = [axang2rotm([0 0 1 targetPoseVec(6)]) targetPoseVec(1:3); 0 0 0 1];
targetRegion = workspaceGoalRegion('tcp',ReferencePose=targetPoseTF);
targetRegion.Bounds(4,:) = [-pi pi]; % Not strictly required but shows that with suction, this is really a region and not a specific target

end