PROPT MK2 5-Link robot: Difference between revisions
From TomWiki
Jump to navigationJump to search
No edit summary |
|||
Line 113: | Line 113: | ||
[[File:fiveLinkMK2Robot_01.png]] | [[File:fiveLinkMK2Robot_01.png]] | ||
[[Category:PROPT Examples]] |
Latest revision as of 05:01, 14 February 2012
This page is part of the PROPT Manual. See PROPT Manual. |
Singular time-optimal of the MK2 5-Link robot. Implementation without mass matrix inversion.
Problem description
The dynamic model of the MK2 robot was generated automatically by AUTOLEV that produces Fortran 77 code: http://www.autolev.com/
The transfer to matlab code was performed partly automatically using
1) to_f90: http://users.bigpond.net.au/amiller/ 2) f2matlab.m: http://www.mathworks.com/matlabcentral/fileexchange/5260
Programmer: Gerard Van Willigenburg (Wageningen University)
% Copyright (c) 2009-2009 by Tomlab Optimization Inc.
Problem setup
toms t t_f % Free final time
p = tomPhase('p', t, 0, t_f, 20);
setPhase(p);
% Dimension state and control vector
np = 5; nx = 2*np; nu = np;
% Define the state and control vector
tomStates a1 a2 a3 a4 a5 w1 w2 w3 w4 w5
phi = [a1; a2; a3; a4; a5];
omega = [w1; w2; w3; w4; w5];
tomControls u1 u2 u3 u4 u5
u = [u1; u2; u3; u4; u5];
% Initial and terminal states
znp = zeros(np,1);
phif = [0.975; 0.975; 0; 0; 0.975];
% Maximum values controls
umax = [15; 10; 5; 5; 5];
% Initial guess
x0 = {t_f==0.8;
icollocate({phi == phif*t/t_f; omega == phif*t*(t_f-t)/t_f})
collocate({u == 0})};
% Box constraints
cbox = {0.7 <= t_f <= 0.9;
collocate({-umax <= u <= umax})};
% Boundary constraints
cbnd = {initial({phi == znp; omega == znp})
final({phi == phif; omega == znp})};
% Compute mass matrix
[mass, rhs] = fiveLinkMK2Robotdyn([phi; omega], u);
% Equality differential equation constraints
ceq = collocate({dot(phi) == omega; mass*dot(omega) == rhs});
% Objective
objective = t_f;
Solve the problem
options = struct;
options.use_d2c = 0;
options.use_H = 0;
options.type = 'lpcon';
options.name = 'Five Link MK2 Robot';
%options.derivatives = 'automatic';
solution = ezsolve(objective, {cbox, cbnd, ceq}, x0, options);
% Plot intermediate result
subplot(2,1,1);
ezplot([phi; omega]);
title('Robot states');
subplot(2,1,2);
ezplot(u);
title('Robot controls');
Time for symbolic processing: 52.1277 seconds Starting numeric solver ===== * * * =================================================================== * * * TOMLAB - TOMLAB Development license 999007. Valid to 2011-12-31 ===================================================================================== Problem: --- 1: Five Link MK2 Robot f_k 0.781121278753005410 sum(|constr|) 0.000036222179585579 f(x_k) + sum(|constr|) 0.781157500932590950 f(x_0) 0.800000000000000040 Solver: snopt. EXIT=0. INFORM=1. SNOPT 7.2-5 NLP code Optimality conditions satisfied FuncEv 1 ConstrEv 747 ConJacEv 747 Iter 172 MinorIter 2172 CPU time: 23.103748 sec. Elapsed time: 23.171000 sec.