Trajectory Generation and Related Setup

First, assign the start time and duration for the trajectory.

tStart $=0\mathrm{.}5$;

tDuration $=3$;

Next, assign the initial and target configuration. q$0$ is the initial configuration and q$1$ is the target configuration.

q$0=[3\mathrm{.}3161]$;

q$1=$ 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, a Polynomial Trajectory block is used to compute 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$(\text{'}$robotSafeTrajectoryTracking$.$slx$\text{'})$;

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$).$

QUESTION $1.[25]$

For a manipulator with n non$-$fixed joints, the system dynamics can be expressed as:

M$($q$)$q $+$ C$($q$,$q$)$q $+$ G$($q$)=$ u

where q$,$q$,$q$,$ in Rn are the position, velocity and acceleration of the generalized coordinate, u in Rn is the control input $($torque$),$ M$($q$)$ is the joint space mass matrix, C$($q$,$q$)$q is the velocity product torque, G$($q$)$ is the gravity torque. To track along a desired joint trajectory with desired position qd$,$ velocity $$qd and acceleration $$qd$,$ the computed torque controller calculates the torque needed to obtain a given configuration and velocity, provided the robot dynamics variables M$($q$),$ C$($q$,$q$)$q$,$ and G$($q$).$ In Simulink, these variables can be easily derived using robotics manipulator blocks from Robotics System Toolbox to design the following computed torque controller:

u $=$ M$($q$)($qd$$Kd$$qe$$Kpqe$)+$ C$($q$,$q$)$q $+$ G$($q$)$

where qe$=$q$$qd is the position error and $$qe$=$q$$qd is the velocity error. With this controller input, the system dynamics becomes a second$-$order ODE:

$$qe $+$ Kd$$qe $+$ Kpqe $=0$

By choosing Kd and Kp properly, the tracking error qe will converge to zero when time approaches infinity.

$$ Gain Kp: The proportional gain when correcting the robot configuration

$$ Gain Kd: The derivative gain when correcting the robot configuration

A standard way to determine the Kp and Kd is to calculate them as:

Kp$=2\backslash$omega n

Kd $=2\backslash$omega n$\backslash$xi

In this example, the default value of Kp and Kd are derived by setting the natural angular frequency of $2\mathrm{.}0$Hz and a damped frequency of $1\mathrm{.}8$ Hz$.$

$1.$ Calculate the damping ratio from the general equation for the system is $,$ to make the second$-$order ODE damping system. $(5)$

$2.$ Find Kp and Kd$,$ where $\backslash$omega n and $\backslash$xi are the natural frequency and damping ratio of the second$-$order ODE. $(10)$

$3.$ Determine these variables by substituting the calculated values in $1$ and $2$ using design computed torque controller $(10)$