Trajectory planning

Based on chapter 3 and chapter 7 of Robotics, Vision and Control by Peter Corke, and on chapter 4 of Robotics: Modelling, Planning and Control by Siciliano, Sciavicco, Villani and Oriolo.

It uses the The robotics toolbox for MATLAB.

Link to the theoretical session on Kinematics

Trajectories: Interpolation motions

The Robotics Toolbox incorporates both polynomial interpolation and trapezoidal interpolation.

Polynomial Interpolation

The trajectory between two points can be obtained by using quintic polynomials (t\in[0,T]):

S(t) &= A t^5 + B t^5 + C t^3 + D t^2 + E t + F \dot{S}(t) &= 5 A t^4 + 4B t^3 + 3C t^2 + 2D t + E \ddot{S}(t) &= 20 A t^3 + 12B t^2 + 6C t + 2D

with the following specifications:

\textrm{at } &t=0 \; \Rightarrow \; S_0,\;\dot{S}_0, \ddot{S}_0 \textrm{at } &t=T \; \Rightarrow \; S_T,\;\dot{S}_T, \ddot{S}_T

The trajectory is found by solving the following system of equations (six equations and six unknowns):

S_0 &= F S_T &= A T^5 + B T^4 + C T^3 + D T^2 + E T + F \dot{S}_0 &= E \dot{S}_T &= 5 A T^4 + 4B T^3 + 3C T^2 + 2D T + E \ddot{S}_0 &= 2D \ddot{S}_T &= 20 A T^3 + 12B T^2 + 6C T + 2D

The Robotics Toolbox has the function tpoly that allows to interpolate between (S_0,\dot{S}_0) and (S_T,\dot{S}_T) considering \ddot{S}_0=\ddot{S}_T=0, for a given time interval t, e.g. t=[0:0.02:2]. If not specified, the velocities are defaulted to zero.

>> t=[0:0.02:2];
>> [q,qd,qdd] = tpoly(0,1,t);
>> plot(t,[q,qd,qdd])

Trapezoidal Interpolation

The function lspb does this interpolation considering, by default, acceleration/cruise/decelarion phases with equal duration:

>> t=[0:0.02:2];
>> [q,qd,qdd] = lspb(0,1,t);
>> figure
>> plot(t,[q,qd,qdd])

Joint-space trajectories

The function jtraj uses the polynomial interpolation implemented in tpoly to interpolate the motion between two robot configurations (however the function mtraj can be configured to work with either tpoly or lspb).

>> mdl_puma560
>> Ti = transl(0.4,0.2,0)*trotx(pi)
>> Tf = transl(0.4,-0.2,0)*trotx(pi/2)
>> qi = p560.ikine6s(Ti)
>> qf = p560.ikine6s(Tf)
>> t=[0:0.02:2];
>> [q,qd,qdd] = jtraj(qi,qf,t)
>> qplot(t,q)

Alternatively, this can be done by using the method jtraj from the SerialLink class, and animated using the method plot:

>> q = p560.jtraj(Ti,Tf,t)
>> figure
>> p560.plot(q)

Let’s visualize how the position and orientation of the tool frame varies along the path:

>> T = p560.fkine(q)
>> p = transl(T)
>> plot(t,p)
>> xlabel('time')
>> ylabel('xyz')
>> figure
>> plot(t,tr2rpy(T))
>> xlabel('time');
>> ylabel('rpy')

In the xy-plane we can see that the position does not vary in a rectilinear way:

>> figure
>> plot(p(:,1), p(:,2))
>> xlabel('x')
>> ylabel('y')

The Robotics Toolbox also offers a Simulink file to visualize this example:

>> sl_jspace

>>> EXERCISE
 Use sl_jspace to reproduce the previous example. Repeat for different initial and  goal configurations.

Other Simulink blocks provided by the Toolbox can be found by calling:

>> roblocks

Cartesian-space trajectories

When straight-line motions in Cartesian space are required, we can use the ctraj function:

>> Ts = ctraj(Ti,Tf,length(t))
>> ps = transl(Ts)
>> plot(t,ps)
>> xlabel('time')
>> ylabel('xyz')
>> figure
>> plot(t,tr2rpy(Ts))
>> xlabel('time');
>> ylabel('rpy')

In the xy-plane we can see that now the position varies in a rectilinear way:

>> figure
>> plot(ps(:,1), ps(:,2))
>> xlabel('x')
>> ylabel('y')

The corresponding trajectory in the joint space is:

>> qs = p560.ikine6s(Ts)
>> figure
>> qplot(qs)
>> figure
>> p560.plot(qs)

There are only slight differences in the joint trajectory with respect to the previous one, but these make the motion in Cartesian space be rectilinear.

>>> EXERCISE
 Repeat for:
  - Ti = transl(0.5, 0.3,0.44)*troty(pi/2)
  - Tf = transl(0.5, -0.3,0.44)*troty(pi/2)
 What happens at the rate of change of joints *q4* and *q6*?
../_images/traj_planning.png