Perform Safe Trajectory Tracking Control Using Robotics Manipulator Blocks
This example shows you how to use Simulink® with manipulator algorithm blocks to achieve safe trajectory tracking control of a simulated robot.
Both Robotics System Toolbox™ and Simscape Multibody™ are required to run this example.
Introduction
This example provides a model that implements a computed-torque controller with joint position and velocity feedback using manipulator algorithm blocks. The controller receives joint position and velocity information from a simulated robot (implemented using Simscape Multibody) and sends torque commands to drive the robot along a given joint trajectory. A planar object is placed in front of the robot so the end effector of the robot arm will collide with it when the trajectory is executed. Without any additional setup, the torque felt from colliding with the object can cause damage on the robot or the object. To achieve safe trajectory tracking, a trajectory scaling block is built to adjust the time stamp when assigning the desired motion to the controller. You may adjust some parameters and interact with the robot while the model is running and observe the effect on the simulated robot.
Set Up Robot Model
This example uses a model of the Rethink Sawyer, a seven degree-of-freedom robot manipulator. Call importrobot
to generate a rigidBodyTree
model from a description stored in a Unified Robot Description Format (URDF) file. Set the DataFormat
and Gravity
properties to be consistent with Simscape. The Simulink model accesses this robot model from the workspace in the simulation.
sawyer = importrobot('sawyer.urdf'); sawyer.removeBody('head'); sawyer.DataFormat = 'column'; sawyer.Gravity = [0, 0, -9.80665];
Trajectory Generation
First, assign the start time and duration for the trajectory.
tStart = 0.5; tDuration = 3;
Next, assign the initial and target configuration. q0
is the initial configuration and q1
is the target configuration.
q0 = [0; -1.18; 0; 2.18; 0; -1.0008; 3.3161]; q1 = zeros(7,1);
The following figures show the robot visualization of the initial configuration and the target configuration related to the location of the planar object. The planar object is placed so that the robot will collide to it during trajectory tracking.
In the Simulink model, the Polynomial Trajectory block computes the robot's position, velocity, and acceleration at any instant in the trajectory using a fifth-order polynomial.
Simulink Model Overview
Next, open the Simulink model. The variables generated above are already stored in Simulink model workspace:
open_system('robotSafeTrajectoryTracking.slx');
The robotSafeTrajectoryTracking
model implements a computed torque controller with trajectory scaling for safe trajectory tracking. There are three main subsystems in this model:
Computed Torque Controller
Trajectory Scaling and Desired Motion
Simscape Multibody Model with Simple Contact Mechanics
On each time step, if the trajectory scaling switch is on, the modified time stamp is used for evaluating the desired joint position, velocity and acceleration. Then, the computed torque controller uses the manipulator blocks associated with the rigidBodyTree
model to track the desired motion. The derived control input is fed into the Sawyer model in Simscape Multibody, where the planar object for interacting with the robot is included.
Build Computed Torque Controller
For a manipulator with non-fixed joints, the system dynamics can be expressed as:
where , , , are the position, velocity and acceleration of the generalized coordinate, is the control input (torque), is the joint space mass matrix, is the velocity product torque, is the gravity torque. To track along a desired joint trajectory with desired position , velocity and acceleration , the computed torque controller calculates the torque needed to obtain a given configuration and velocity, provided the robot dynamics variables , , and . In Simulink, these variables can be easily derived using robotics manipulator blocks from Robotics System Toolbox to design the following computed torque controller:
where is the position error and is the velocity error. With this controller input, the system dynamics becomes a second-order ODE:
By choosing and properly, the tracking error will converge to zero when time approaches infinity.
The Computed Torque Controller subsystem is built using three robotics manipulator blocks: Joint Space Mass Matrix,
Velocity Product Torque, and
Gravity Torque. Note that the associated
rigidBodyTree
model, sawyer
, is assigned in all those blocks, and the configuration and velocity need to be specified as column vectors.
open_system('robotSafeTrajectoryTracking/Computed Torque Controller');
Inside the Computed Torque Controller, there are two tunable parameters (indicated by colored blocks):
Gain
Kp
: The proportional gain when correcting the robot configurationGain
Kd
: The derivative gain when correcting the robot configuration
A standard way to determine the Kp
and Kd
is to calculate them as:
where and are the natural frequency and damping ratio of the second-order ODE. In this example, the default value of Kp
and Kd
are derived by setting the natural frequency and damping ratio as and to make the second-order ODE a critical damped system.
Trajectory Scaling
There are two main blocks in this subsystem:
Trajectory Scaling
Desired Motion
Trajectory Scaling is the main block deployed for safe trajectory tracking in this example. At each time step , the original time stamp is calculated as . However, when the robot collides with an unexpected object, the increasing torque and deviance from the planned trajectory can be destructive for both the robot and the object. The main idea of trajectory scaling is to calculate the time stamp as by introducing , which is a function of the desired motion and measured torque . The function controls the speed of the robot motion and is determined based on the interference felt by the robot. If the measured torques are greater than expected, is decreased to make the robot slow down or even move backward until the desired torques are achieved. These values of have the following effects on the robot's motion:
, the robot moves forward ( is the normal speed).
, the robot stops.
, the robot moves backward.
In the Trajectory Scaling block, it is required to estimate the external torque to calculate , where is the measured torque from the Simscape model, and is the expected torque of the desired motion on the previous time stamp. In the External Torque Observer section of the model, the Inverse Dynamics block calculates the expected torque which is subtracted from the measure torque. Expected torque is: .
In the Desired Motion area, the output of the trajectory scaling algorithm is fed as an input to a Polynomial Trajectory block. This block compute a quintic polynomial trajectory given the known values of q0
and q1
and their boundary conditions of zero velocity and acceleration. It outputs the position, velocity, and acceleration: , , and , which are fed to the Computed Torque Controller subsystem.
Simscape Multibody Robot Model and Simple Contact Mechanics
The Simscape Multibody robot model is imported from the same .urdf
file using smimport
, where a set of torque actuators and sensors for joint torque, joint position and velocity are added. A contact mechanism block simulates the reaction force between the end effector and the obstacle as a sphere and a plane, where a simple linear spring-damper model is used.
Note: The contact mechanism has only been implemented between the end effector and the planar object. Therefore, other parts of the robot arm may still pass through the obstacle.
Run the Model
Run the model and observe the behavior of Sawyer in the robot simulator and interact with it.
First, open the viewer by clicking on the scope icon shown below on the left of the Simscape model block. The scope displays signals including the joint torques, reaction contact force between the end effector and the planar object, and the time stamp for calculating desired motion for trajectory tracking.
Toggle the trajectory scaling switch to "Off".
Use the following command or click Run to start the simulation.
sim('robotSafeTrajectoryTracking.slx','StopTime','5');
You should see the arm collide with the object yielding high torque inputs and a large reaction force. Note in this case the original time stamp is used. Stop the simulation afterwards.
Now, toggle trajectory scaling switch to On and rerun the model.
Notice the differences in the computed torques and the reduced reaction force after the collision.
While the simulation is running, adjust the slider to move the object towards or away from the robot. The robot should react to its position while still trying to execute the trajectory safely.
Summary
This example showed how to use robotics manipulator blocks in Simulink to design a computed torque controller, and integrate it with trajectory scaling and dynamic simulation in Simscape Multibody to achieve safe trajectory tracking. The resultant torque, reaction force, and time stamp demonstrated the capability of trajectory scaling for performing safe trajectory tracking.