Plan Path for Manipulator in Simulink with Robotics System Toolbox

Open Live Script

This example shows how to execute and simulate a basic path planning task in Simulink®. This example uses code generation to illustrate a pattern for executing autonomy functions from MATLAB® in Simulink.

This example is similar to the Generate Code for Manipulator Motion Planning in Perceived Environment example, which shows how to solve a planning problem using only MATLAB and code generation. For demonstration purposes, this example uses a fixed set of collision objects as the environment to focus more on implementing path planning and trajectory generation in Simulink.

Modeling Robot Algorithms in Simulink

The Robotics System Toolbox offer an array of algorithms for controlling robotic systems. In Simulink, these algorithms take two forms:

Simulink blocks — When possible, the block library supplies efficient implementations of robotics algorithms for use in Simulink.
MATLAB classes and functions — Features that do not have dedicated blocks can still be implemented in Simulink by leveraging code generation capabilities. By wrapping code generation-compatible functions and classes in MATLAB Function (Simulink) blocks or as System objects, these algorithms can be incorporated in Simulink.

Algorithms Implemented as Blocks

In Robotics System Toolbox™, algorithm blocks typically have two modes:

Interpreted Execution — This is the default mode. In this case, the block is executed directly by MATLAB. This has very little compilation time, but typically runs slower than the alternative (code generation) by 1-2 orders of magnitude. This setting is ideal for prototyping.
Code generation — Select this option in the dropdown to generate C/C++ code from the block and run that instead. This approach requires extra compilation time, but runtime is generally much faster.

This mechanism is used in this example to model the plant using the Joint-Space Motion Model block.

Algorithms Without Dedicated Blocks

Not all algorithms are presently supported in block format. However, as long as they support code generation, they can still be easily integrated into Simulink. The best practice is to use MATLAB Function blocks containing functions that supports code generation. By implementing the function in a MATLAB Function block, you can implement those algorithms in Simulink. This example uses this approach twice to create a sampling-based motion planner and the generate a trajectory that has velocity and acceleration constraints.

Follow these principles for efficient code generation:

Use persistence when necessary — Creating MATLAB objects is computationally expensive and typically only needs to be done once. For most algorithms in Robotics System Toolbox, you should only need to initialize the robot model once. If possible, the environment, planners, and even some trajectories should be initialized only as many times as needed. You can use persistent to declare variables as persistent. These objects may still be updated at later steps.
Compute the algorithms only when necessary — Planned paths and trajectories do not need to be continuously updated. Instead, they may be computed at discrete instants. Use model-based design patterns like the triggered subsystem to ensure that the algorithms are executed on an as-needed basis.

Model Overview

In this example, the Simulink model simulates in three main steps:

Plan Path Using RRT — When the clock surpasses 0.5 seconds, the model triggers a subsystem that creates a robot model, path planner, and environment, and then plans a path using the manipulatorRRT object. The subsystem then shortens and interpolates the path before passing the path to the trajectory generator. Note that in a more advanced model where the environment, or start and goal positions change, the model could trigger the subsystem multiple times using a Stateflow® chart or similar mechanism.
Generate and Sample Trajectory — The model generates a trajectory from the path when the path is first planned. Then the model samples the trajectory at each time step using interpolation. Then the model sends the waypoint configuration, velocity, and acceleration from the trajectory to the plant model at each time step.
Simulate Closed-Loop Plant Dynamics — The Joint Space Motion Model block plant model uses the configuration, velocity, and acceleration of the current time step to simulate the behavior of the manipulator under closed-loop position control.

Open the model.

open_system("manipulatorPathPlannerSystem.slx");

Plan Collision-Free Path Using Triggered Subsystem

The Triggered Planner subsystem is responsible for instantiating planners and generating a collision-free path. This subsystem only needs to run once, so it is a triggered subsystem that is enabled at the start of the simulation when the simulation time surpasses 0.5 seconds. The Triggered Planner subsystem only contains a MATLAB Function block that instantiates the robot model, environment, and planner, and then calls the planner to return a collision-free path.

Open the Triggered Planner subsystem.

open_system("manipulatorPathPlannerSystem/Triggered Planner")

The Plan Generator block is a MATLAB Function block that contains the rrtHelperPlanner function, which instantiates the robot model, path planner, environment, and plans a path between a start and goal. During the code execution, the function plans a path, then shortens the path to eliminate waypoints that may appear jerky. Then rrtHelperPlanner interpolates the path to add more waypoints to the path for more continuity. As a result, the total number of waypoints in the path is variable. So the function saves the path in a vector of fixed size. If there are not enough waypoints in the output path, rrtHelperPlanner holds last waypoint in the path for each of the remaining waypoints. The second output of the triggered subsystem is the total number of waypoints, which the model uses to identify where the path really ends within the output path vector. You can also model this behavior with a variable-sized output by changing the MATLAB Function block parameters.

This is the rrtHelperPlanner function code contained in the Plan Generator block.

function [fixedDimInterpPlan, numComputedSamples] = rrtHelperPlanner(start,goal)
%This function is for internal use only and may be removed in the future.

%rrtHelperPlanner Plan in a known environment
%   Plan a path between a start and goal configuration. Because the planner
%   can return variably-sized outputs, this function returns a
%   fixed-dimension array as well as the number of computed samples, which
%   corresponds to the index at which the goal state is reached. All
%   subsequent indices will hold steady at the goal state.

%Copyright 2023 The MathWorks, Inc.

    % Hard code known constants
    NUMDOF = 7; % Number of DoF in the robot (change if the robot changes)
    NUMPATHSAMPLES = 200;

    % Load the rigid body tree for which the planner will be defined and
    % make it persistent
    persistent rbt planner env
    if isempty(rbt)
        % Create the robot by loading it from the robot library. Other
        % codegen-supported options include calling importrobot to load
        % from a URDF, SDF, or Simscape model, building a custom
        % rigidbodytree object, or calling a function that creates and
        % outputs a rigidbodytree object
        rbt = loadrobot("kinovaGen3",DataFormat="column");
    end
    if isempty(env)
        % Create the environment. Use a helper so that a later
        % visualization can refer to the same environment
        env = helperCreateCollisionEnv;
    end
    if isempty(planner)
        % Create the planner from the rigid body tree and environment. Note
        % that the planner will need to be re-created if the environment or
        % robot change.
        planner = manipulatorRRT(rbt,env);
    end

    % Configure the planner. This can happen without re-instantiating it
    planner.SkippedSelfCollisions = "parent";
    planner.MaxConnectionDistance = 0.4;
    planner.ValidationDistance = 0.05;

    % For repeatable results, seed the random number generator and store
    % the current seed value.
    prevseed = rng(0);

    % Plan and interpolate.
    planOut = planner.plan(start(:)',goal(:)');
    shortenedPlan = planner.shorten(planOut,5);
    interpolatedPlan = planner.interpolate(shortenedPlan);
    numComputedSamples = size(interpolatedPlan,1);

    % Move the samples into one of fixed dimension
    fixedDimInterpPlan = zeros(NUMDOF, NUMPATHSAMPLES);
    fixedDimInterpPlan(:,1:numComputedSamples) = interpolatedPlan(1:numComputedSamples,:)';

    % Pad the end of the output matrix with the goal state if the
    % interpolated plan doesn't have enough samples
    if numComputedSamples < NUMPATHSAMPLES
        fixedDimInterpPlan(:,(numComputedSamples+1):NUMPATHSAMPLES) = repmat(goal(:),1,NUMPATHSAMPLES-numComputedSamples);
    end

    % Restore the random number generator to the previously stored seed.
    rng(prevseed);
end

As noted previously, this model assumes that the robot model and environment do not need to change during the simulation. So Plan Generator instantiates the robot model, environment, and planner as persistent variables so the model only needs to initialize them once. If the robot model or environment must change at a later simulation time, then it would be necessary to modify the model such that the subsystem can rerun as necessary.

Generate Trajectory Subject to Kinematic Constraints

The path that the model generates is time-independent. To easily control the motion, you must associate the simulation time with the path such that you can provide a reference trajectory to the position-controlled robot. While there are several polynomial trajectory blocks that you can use, the contopptraj function enables you to generate a trajectory by specifying velocity and acceleration limits for the trajectory in addition to the waypoints. Since this function does not have an equivalent Simulink block, you can implement it in Simulink using a MATLAB Function block.

In the model, the Trajectory Generator MATLAB Function block implements the contropptraj function in the contopptrajMLFcn helper function.

function [q,qd,qdd,maxTime] = contopptrajMLFcn(path,numPathSamples,pathComputationTimeStamp,t)
% This function is for internal use only and may be removed in a future release

%contopptrajMLFcn Function to compute contopptraj solution and output the corresponding values at an instant in time t
%   This MATLAB Function block accepts the target path computed by the
%   planner, specified as an NUMDOFxM array of waypoints, the number of
%   samples in the computed path, the time stamp at which the path is
%   computed, and the current time stamp t. The function computes the
%   trajectory given defined velocity and acceleration constraints. The
%   target path, supplied by the planner, is a fixed length M, but only the
%   first NUMPATHSAMPLES correspond to values computed by the planner. If
%   NUMPATHSAMPLES is not greater than zero, then the planner has not been
%   run, and a trajectory cannot be computed.

% Hard-code known constants
NUMDOF = 7;                             % Number of DoF in the robot
VELLIM = repmat([-10 10],NUMDOF,1);   % Velocity limits for trajectory
ACCELLIM = repmat([-50 50],NUMDOF,1); % Acceleration limits for trajectory

% Define the trajectory output by contopptraj (i.e. the corresponding joint
% configurations, velocities, and accelerations and their time stamps) as
% persistent variables and initialize them based on known dimensions
persistent qSolve qdSolve qddSolve tSolve
if isempty(qSolve)
    qSolve = zeros(NUMDOF,500);
    qdSolve = zeros(NUMDOF,500);
    qddSolve = zeros(NUMDOF,500);
    tSolve = linspace(0,1,500);
end

% Store the last time at which the path was computed as a persistent
% variable. This allows the function to detect changes
persistent prevPathComputationTime
if isempty(prevPathComputationTime)
    prevPathComputationTime = -1;
end

% Initialize outputs
q = zeros(NUMDOF,1);
qd = zeros(NUMDOF,1);
qdd = zeros(NUMDOF,1);

% When the planner subsystem is called, a new computation time is provided.
% If the planner is triggered and it produces nontrivial results (the
% number of samples is nonzero), recompute the trajectory.
if (pathComputationTimeStamp ~= prevPathComputationTime) && (numPathSamples > 0)
    [qSolve,qdSolve,qddSolve,tSolve] = contopptraj(path(:,1:numPathSamples),VELLIM,ACCELLIM,NumSamples=100);
end

% Compute the output at an instant in time t. Because contopptraj defines
% the time stamp relative to a clock that begins at zero, subtract the time
% stamp at computation time from the current time.
maxTime = max(tSolve);
queryTime = t - pathComputationTimeStamp;
if queryTime == 0
    q(:) = qSolve(:,1);
    qd(:) = qdSolve(:,1);
    qdd(:) = qddSolve(:,1);
elseif queryTime <= maxTime
    % Use linear interpolation to approximate the reference trajectory
    % inside the time range for which it is defined
    q(:) = interp1(tSolve,qSolve',queryTime);
    qd(:) = interp1(tSolve,qdSolve',queryTime);
    qdd(:) = interp1(tSolve,qddSolve',queryTime);
else
    % After the trajectory end is reached, if no new trajectory is
    % computed, hold the last value
    q(:) = qSolve(:,end);
    qd(:) = qdSolve(:,end);
    qdd(:) = qddSolve(:,end);
end

% Update the stored path computation time stamp
prevPathComputationTime = pathComputationTimeStamp;

end

The Trajectory Generator block regenerates the entire trajectory only if the plan is new or if the model updates the plan. Instead it only generates a reference trajectory and derivatives of the trajectory. The model samples the reference trajectory at the current time step using interpolation and sends these reference trajectory waypoints to the controller.

Alternatively, you can move this trajectory generating functionality into the triggered subsystem. Or, if the trajectory does not need kinematic constraints such as velocity or acceleration, you can replace both the Triggered Planner subsystem and the Trajectory Generator block with a dedicated block such as the Minimum Jerk Polynomial Trajectory block.

Simulate the Closed-Loop Plant Dynamics

Once the reference trajectory is known, the model can send the configurations to a position-controlled robot. When controlling prebuilt systems, the model often supplies these values directly to robot hardware. For custom systems, the model may instead supply the trajectory to a controller designed for a known plant model. For more information about how you can replace the plant dynamics with systems with greater modeling detail, see Model and Control a Manipulator Arm with Robotics and Simscape.

In this example, the model sets the Motion type parameter of the Joint Space Motion Model block to Independent Joint Motion, which simulates the idealized closed-loop dynamics by modeling the joints as independent second-order systems. The model also sets Simulate using parameter of the Joint Space Motion Model block to Code generation to enable faster runtime.

Run the Model

Open and simulate the model. Note that because the blocks are using the Code generation simulation mode, the compilation time takes longer to complete than if it were running in Interpreted execution mode. But the code generation mode enables the robot plant model to run faster during simulation.

out = sim("manipulatorPathPlannerSystem.slx");

Visualize the resulting motion using the helperVisualizeConfigs helper function. The helper function iterates over the output trajectory using the show object function with the FastUpdate name-value argument set to true.

robot = loadrobot("kinovaGen3");
env = helperCreateCollisionEnv;
trajConfigs = out.yout{1}.Values.Data;
helperVisualizeConfigs(robot,env,trajConfigs);

Other Modeling Variations

This example showed how you can implement MATLAB robotics algorithms in Simulink even when block equivalents are not available. The example achieved this using the MATLAB Function block. You can also use System objects to do this as well. An advantage of the System object is that it offers both interpreted execution and code generation modes. Also, because it is an object, you can store persistent variables as object properties. However, several additional methods of a System object™ need to be user-defined to achieve the same set of functionality shown in this example when using the Code generation mode.

Additionally, this example used RRT, a sampling-based planner, together with a trajectory generation tool. Alternatively, an optimization-based planner like CHOMP can directly produce trajectories, simplifying the planning and reducing problem complexity. For an example of how you can use CHOMP, see Pick-And-Place Workflow Using CHOMP for Manipulators.