Homec4science
Diffusion MSM (master) Browse

msm/ShoulderModel_3DOF/StateSpace_CL.m
35b4c71bda0dmaster

Last Change
Raw File

StateSpace_CL.m
View Options

function [du] = StateSpace_CL(t, u, TimeID, IDDATA, DYDATA)
%{
Function for generating the state space of the closed-loop system.
--------------------------------------------------------------------------
Syntax :
[du] = StateSpace(t, u, TimeID, IDDATA)
--------------------------------------------------------------------------
Function Description :
This function consturcts the state space form of the equations of motion in
order to let the numerical solver of matlab handle the problem.
--------------------------------------------------------------------------
%}
q1 = u(1); q2 = u(2); q3 = u(3);
dq1 = u(4); dq2 = u(5); dq3 = u(6);
% Quantify the constants
M = DYDATA.Parameters(1,1);g = DYDATA.Parameters(1,2);CG = DYDATA.Parameters(1,3);
Ixx = DYDATA.Parameters(2,1);Iyy = DYDATA.Parameters(2,2);Izz = DYDATA.Parameters(2,3);
% Re-write the equations of motion introducing the values above
EquationsofMotion
% Construct the error
lambda = 5;
q_d = interp1(IDDATA.t(TimeID:TimeID+1),IDDATA.ImposedMotion([1 4 7]',TimeID:TimeID+1)',t)';
Dq_d = interp1(IDDATA.t(TimeID:TimeID+1),IDDATA.ImposedMotion([2 5 8]',TimeID:TimeID+1)',t)';
DDq_d = interp1(IDDATA.t(TimeID:TimeID+1),IDDATA.ImposedMotion([3 6 9]',TimeID:TimeID+1)',t)';
De = [u(4) u(5) u(6)]' - Dq_d; % the velocity error
e= [u(1) u(2) u(3)]' - q_d; % the position error
% Define the control input of the closed-loop system
ControlInput = MDYN(:,:)*(DDq_d - 2*lambda*De - lambda^2*e) - RHS;
% define the muscle force direction vector, magnitude and lever arms at
% each time step of the integrator (solver)
% for muscleID = 1:6
% Dir_inT(:, muscleID) = interp1(IDDATA.t(TimeID:TimeID+1),[IDDATA.ForceDirection{TimeID}(:,muscleID)';IDDATA.ForceDirection{TimeID+1}(:,muscleID)'],t)';
% Rho_inT(:, muscleID) = interp1(IDDATA.t(TimeID:TimeID+1),[IDDATA.LeverArm{TimeID}(:,muscleID)';IDDATA.LeverArm{TimeID+1}(:,muscleID)'],t)'; ;
% F_final(muscleID) = interp1(IDDATA.t(TimeID:TimeID+1),IDDATA.MuscleForces(muscleID,TimeID:TimeID+1),t);
% end
%
%
%
% GeneralizedForces
% in this feedback leanirization controller we can say a part is
% proportional derivative controller and a part is a feedforward controller
% as it explained in page 193 of Professor Slotin's book.
% q1 = q_d(1); q2 = q_d(2); q3 = q_d(3);
% dq1 = Dq_d(1); dq2 = Dq_d(2); dq3 = Dq_d(3);
%
% EquationsofMotion
% Q1 = Q(1,1); Q2 = Q(2,1); Q3 = Q(3,1);
Q1 = ControlInput(1) - RHS(1,1);
Q2 = ControlInput(2) - RHS(2,1);
Q3 = ControlInput(3) - RHS(3,1);
GeneralizedAccelerations;
du(1) = u(4);
du(4) = DDq(1);
du(2) = u(5);
du(5) = DDq(2);
du(3) = u(6);
du(6) = DDq(3);
du=du';
return;
c4science · Help