% DesignMPC.m % This code requires the Multi-Parametric Toolbox % (http://control.ee.ethz.ch/~mpt/). clear all; %% arm_parameters ----------------------------------------------- % Segment length l1_ = 0.15; l2_ = 0.21; % Segment center of mass s1_ = l1_/2; s2_ = 0.11; % Moment arms J = [2.6 -1.3 0 0 0.7 -2.5; 0 0 1.2 -1.7 1.6 -1.1]/100; % Hand position x0 = 0; y0 = 0.24; %% Inverse kinematics-------------------------------------------- l0_ = sqrt(x0^2 + y0^2); cos_theta2 = -((x0^2+y0^2)-(l1_^2+l2_^2))/(2*l1_*l2_); sin_theta2 = sqrt((2*l1_*l2_)^2-(l0_^2 - (l1_^2+l2_^2))^2)/(2*l1_*l2_); theta2 = atan2(real(sin_theta2),real((-cos_theta2))); kc = l1_ + l2_*cos(theta2); ks = l2_*sin(theta2); cos_theta1 = (kc*x0+ks*y0)/(kc^2+ks^2); sin_theta1 = (-ks*x0+kc*y0)/(kc^2+ks^2); theta1 = atan2(sin_theta1,cos_theta1); theta = [theta1 theta2]; % Jacobian matrix Jacob =[-l1_*sin(theta1)-l2_*sin(theta1+theta2) -l2_*sin(theta1+theta2); l1_*cos(theta1)+l2_*cos(theta1+theta2) l2_*cos(theta1+theta2)]; %% Dynamics model ----------------------------------------------- dt = 0.01; delta = 0.04; A = zeros(8,8); A(1,1) = 1 - dt/delta; A(2,2) = 1 - dt/delta; A(1:2,3:8) = dt/delta*J; A(3:8,3:8) = (1 - dt/delta)*eye(6); B = zeros(8,6); B(3:8,1:6) = dt/delta*eye(6); C = zeros(4,8); C(1:2,1:2) = eye(2); C(3:4,1:2) = inv(Jacob'); D = zeros(4,6); szX = size(A,1); szU = size(B,2); szY = size(C,1); % Cost weight cost.wt = 1e+3; % Torque cost.r = 1; % Efort q = zeros(2,szY); q(1:2,1:2) = cost.wt*eye(2); Q = q'*q; R = cost.r * eye(szU); model = LTISystem('A', A, 'B', B, 'C', C, 'D', D); model.u.min = zeros(6,1); model.u.max = [Inf; Inf; Inf; Inf; Inf; Inf]; model.y.penalty = QuadFunction(Q); model.u.penalty = QuadFunction(R); model.y.with('reference'); model.y.reference = 'free'; %% Model predictive controller ---------------------------------- N = 3; mpc = MPCController(model, N); ctrl = mpc.toExplicit(); %% Optimizer ---------------------------------------------------- rNum = ctrl.optimizer.Num; for i=1:rNum F{i} = ctrl.optimizer.Set(i).Functions('primal').F; g{i} = ctrl.optimizer.Set(i).Functions('primal').g; AA{i} = ctrl.optimizer.Set(i).A; bb{i} = ctrl.optimizer.Set(i).b; end save data;