PROPT Time-optimal Trajectories for Robot Manipulators

This page is part of the PROPT Manual. See PROPT Manual.

Users Guide for dyn.Opt, Example 2

Dissanayake, M., Goh, C. J., & Phan-Thien, N., Time-optimal Trajectories for Robot Manipulators, Robotica, Vol. 9, pp. 131-138, 1991.

Problem Formulation

Find u over t in [0; t_F ] to minimize

$J=t_{F}$

subject to:

$x(0)=[0\ -2\ 0\ 0]$

$x(t_{F})=[1\ -1\ 0\ 0]$

$L_{1}=0.4;$

$L_{2}=0.4;$

$m_{1}=0.5;$

$m_{2}=0.5;$

$Eye_{1}=0.1;$

$Eye_{2}=0.1;$

$el_{1}=0.2;$

$el_{2}=0.2;$

$cos(x2)=cos(x_{2});$

$H_{11}=Eye_{1}+Eye_{2}+m_{1}*el_{1}^{2}+m_{2}*(L_{1}^{2}+el_{2}^{2}+2.0*L_{1}*el_{2}*cos(x2));$

$H_{12}=Eye_{2}+m_{2}*el_{2}^{2}+m_{2}*L_{1}*el_{2}*cos(x2);$

$H_{22}=Eye_{2}+m_{2}*el_{2}^{2};$

$h=m_{2}*L_{1}*el_{2}*sin(x_{2});$

$delta={\frac {1.0}{H_{11}*H_{22}-H_{12}*H_{12}}};$

${\frac {dx_{1}}{dt}}=x_{3}$

${\frac {dx_{2}}{dt}}=x_{4}$

${\frac {dx_{3}}{dt}}=delta*(2.0*h*H_{22}*x_{3}*x_{4}+h*H_{22}*x_{4}^{2}+h*H_{12}*x_{3}^{2}+H_{22}*u_{1}-H_{12}*u_{2});$

${\frac {dx_{4}}{dt}}=delta*(-2.0*h*H_{12}*x_{3}*x_{4}-h*H_{11}*x_{3}^{2}-h*H_{12}*x_{4}^{2}+H_{11}*u_{2}-H_{12}*u_{1});$

$-10<=u<=10$

% Copyright (c) 2007-2008 by Tomlab Optimization Inc.

Problem setup

toms t
toms t_f

tfopt = 7;
x1opt = 1*t/t_f;
x2opt = -2+1*t/t_f;
x3opt = 2;
x4opt = 4;
u1opt = 10-20*t/t_f;
u2opt = -10+20*t/t_f;

Solve the problem, using a successively larger number collocation points

for n=[30 60]

    p = tomPhase('p', t, 0, t_f, n);
    setPhase(p);

    tomStates x1 x2 x3 x4
    tomControls u1 u2

    % Initial guess
    x0 = {t_f == tfopt
        icollocate({
        x1 == x1opt
        x2 == x2opt
        x3 == x3opt
        x4 == x4opt})
        collocate({
        u1 == u1opt
        u2 == u2opt})};

    % Box constraints
    cbox = {
        0.1 <= t_f <= 50
        -10 <= collocate(u1) <= 10
        -10 <= collocate(u2) <= 10};

    % Boundary constraints
    cbnd = {initial({x1 == 0; x2 == -2
        x3 == 0; x4 == 0})
        final({x1 == 1; x2 == -1
        x3 == 0; x4 == 0})};

    % ODEs and path constraints
    L_1 = 0.4;   L_2 = 0.4;
    m_1 = 0.5;   m_2 = 0.5;
    Eye_1 = 0.1; Eye_2 = 0.1;
    el_1 = 0.2;  el_2 = 0.2;

    H_11  = Eye_1 + Eye_2 + m_1*el_1^2+ ...
        m_2*(L_1^2+el_2^2+2.0*L_1*el_2*cos(x2));
    H_12  = Eye_2 + m_2*el_2^2 + m_2*L_1*el_2*cos(x2);
    H_22  = Eye_2 + m_2*el_2^2;
    h     = m_2*L_1*el_2*sin(x2);
    delta = 1.0./(H_11.*H_22-H_12.^2);

    ceq = collocate({
        dot(x1) == x3
        dot(x2) == x4
        dot(x3) == delta.*(2.0*h.*H_22.*x3.*x4 ...
        +h.*H_22.*x4.^2+h.*H_12.*x3.^2+H_22.*u1-H_12.*u2)
        dot(x4) == delta.*(-2.0*h.*H_12.*x3.*x4 ...
        -h.*H_11.*x3.^2-h.*H_12.*x4.^2+H_11.*u2-H_12.*u1)});

    % Objective
    objective = t_f;

Solve the problem

    options = struct;
    options.name = 'Robot Manipulators';
    solution = ezsolve(objective, {cbox, cbnd, ceq}, x0, options);

    % Optimal x, y, and speed, to use as starting guess
    % in the next iteration
    tfopt = subs(final(t), solution);
    x1opt = subs(x1, solution);
    x2opt = subs(x2, solution);
    x3opt = subs(x3, solution);
    x4opt = subs(x4, solution);
    u1opt = subs(u1, solution);
    u2opt = subs(u2, solution);

Problem type appears to be: lpcon
Time for symbolic processing: 0.59011 seconds
Starting numeric solver
===== * * * =================================================================== * * *
TOMLAB - TOMLAB Development license  999007. Valid to 2011-12-31
=====================================================================================
Problem: ---  1: Robot Manipulators             f_k       0.391698237386178090
                                       sum(|constr|)      0.000063099633743277
                              f(x_k) + sum(|constr|)      0.391761337019921390
                                              f(x_0)      7.000000000000000000

Solver: snopt.  EXIT=0.  INFORM=1.
SNOPT 7.2-5 NLP code
Optimality conditions satisfied

FuncEv    1 ConstrEv   68 ConJacEv   68 Iter   26 MinorIter  437
CPU time: 0.124801 sec. Elapsed time: 0.134000 sec. 

Problem type appears to be: lpcon
Time for symbolic processing: 0.58064 seconds
Starting numeric solver
===== * * * =================================================================== * * *
TOMLAB - TOMLAB Development license  999007. Valid to 2011-12-31
=====================================================================================
Problem: ---  1: Robot Manipulators             f_k       0.391820155673057110
                                       sum(|constr|)      0.000000000018899679
                              f(x_k) + sum(|constr|)      0.391820155691956770
                                              f(x_0)      0.391698237386178090

Solver: snopt.  EXIT=0.  INFORM=1.
SNOPT 7.2-5 NLP code
Optimality conditions satisfied

FuncEv    1 ConstrEv   16 ConJacEv   16 Iter   12 MinorIter  440
CPU time: 0.187201 sec. Elapsed time: 0.191000 sec. 

end

t  = subs(collocate(t),solution);
x1 = subs(collocate(x1opt),solution);
x2 = subs(collocate(x2opt),solution);
x3 = subs(collocate(x3opt),solution);
x4 = subs(collocate(x4opt),solution);
u1 = subs(collocate(u1opt),solution);
u2 = subs(collocate(u2opt),solution);

Plot result

subplot(2,1,1)
plot(t,x1,'*-',t,x2,'*-',t,x3,'*-',t,x4,'*-');
legend('x1','x2','x3','x4');
title('Robot Manipulators state variables');

subplot(2,1,2)
plot(t,u1,'+-',t,u2,'+-');
legend('u1','u2');
title('Robot Manipulators control');