Model with crm damper

.gitignore

@@ -0,0 +1,5 @@
+model/rowerdynamics.m
+*.asv
+model/tmp.m
+model/rowerdynamics.m
+model/rowerdynamics.m

Setup.txt

@@ -0,0 +1,9 @@
+In order to perform the optimization code the "main.m" should be ran
+
+-"problem.nconpath" represent the number of path constraints (currently = 1).  
+-"optimize.m" is coded in a way that it can be increased to 2 also
+-To start it is better to set problem.Wtrack =1 while problem.Weffort =0.
+ The reason is we are sure that the constaraints are satisfied in tracking
+ problem. In addition, when using only tracking, the optimization should 
+ be solved within few iterations
+ -The rest can be the way they are.

main/main.m

@@ -0,0 +1,47 @@
+function main
+    % performs all the optimizations and produces the plots and tables
+    close all
+    clc
+    clear global
+    global result problem
+
+    % Add that folder plus all subfolders to the path.
+    %       mkdir('optimizer')
+    addpath(genpath('optimizer'))
+    addpath(genpath('model'))
+    addpath(genpath('tools'))
+    addpath(genpath('data'))
+
+    problem.initialguess = 'sim';%pu th efilename here
+    problem.cgain =1; % scaling factor for path constraint
+    problem.dyngain = 1; % scaling factor for non path constraint
+    problem.np =1;                 % on/off switch for constraints
+    problem.N = 70;		        % number of collocation points
+%     problem.Reqpower = 0.0976; % we can define the woring power here 
+    %( we can either use power or path constraint-so if using problem.Reqpower make sure that problem.nconpath  = 0)
+    if problem.np == 1
+        problem.task.Lmin = 0.2077;       % min and max of cable lenght from the data
+        problem.task.Lmax =  1.1288;
+        % task constraints---> when increased even by 1 either for cable for wrist position, the IPOPT cannot solve the
+        % problem anymore
+        problem.cablecnst = 2; % number of constraints for task when doing predictive simulation (cable length) Lmin & Lmax
+        problem.nconpath  = 1; % Zwrist constraint (depends on the number of nodes can be varies between 0 and 2 for now)
+        % when increasing the number of path constraint to 2 IPOPT connot
+        % solve it anymore!
+    else
+        problem.ntask = 0;
+    end
+
+
+    problem.component = 'spring';   % choose if we want to use damper or spring
+    problem.discretization = 'BE';  % Backward Euler(BE) or Midpoint Euler(ME) discretization
+    problem.tracking = 3;           % if 1 tracking the angles only else track all states (but flywheel velocity)
+    % objective function gains (Wtrack=1 for the tracking  and Weffort=1 for predictive simulation) 
+    problem.Wtrack =0;
+    problem.Weffort =1; 
+    % tracking problem:when problem.Wtrack =1 and problem.Weffort =0 works perfectly;
+    % peredictive simulation problem: when problem.Wtrack =0 and problem.Weffort =1 works perfectly;
+    % for now we only use predictive simulation
+    result = optimize(problem);
+    save 'rowingtest' result
+end

model/.gitignore

@@ -0,0 +1,2 @@
+/rowerdynamics.m
+/tmp.avi

model/animate.m

@@ -19,7 +19,7 @@ function animate(q, par)
     for i = 1:nframes
         % use the rowerdynamics function to get the stick figure
         % coordinates at frame i
-        [~,~,~,~,~,s] = rowerdynamics(q(i,:)',qd,qdd,xfw,par);
+        [~,~,~,~,~,~,~,~,s] = rowerdynamics(q(i,:)',qd,qdd,xfw,par);
         clf
         plot(s(:,1),s(:,2),'o-');
         hold on
@@ -29,7 +29,11 @@ function animate(q, par)
         y = x*par.a + par.b;
         plot(x,y,'LineWidth',2);
         axis('equal');
-
+%         
+%         if i==37
+%             pause
+%         end
+%         
  % Write each frame to the file
         if i==1
             currFrame = getframe;

model/dynfun.m

@@ -1,79 +1,79 @@
-    function [f, dfdx, dfdxdot, dfdu,L , dLdq] = dynfun(x, xdot, u)%dLdx (x2 to 6 which are the angles) dLdq 1 by 5 matrix
-    % the 13 dynamics equations of the system, in the form
+    function [f, dfdx, dfdxdot, dfdu, L, dL_dq] = dynfun(x, xdot, u)%dLdx (x2 to 6 which are the angles) dLdq 1 by 5 matrix
+    % The 13 dynamics equations of the system, in the form
     %   f(x, dx/dt, u) = 0
-    % where x contains the 13 state variables and u are the 5 torques
-    % p contains the 5 model parameters
+    % where x contains the 13 state variables and u are the 5 joint torques
+	% This function also outputs the wrist-sprocket distance L and related Jacobian
 
     global model 
 
-	% copy parameter values into variables with meaningful
+	% copy parameter values into variables with shorter names
     c =  model.parameters.C;
 	m =  model.parameters.m ;
     K =  model.parameters.K;      % shock cord stiffness
     L0 = model.parameters.L0;     % the wrist-sprocket distance when shock cord has zero force
+
+	% Kcrm is probably no longer needed (remove it when confirmed)
     Kcrm = model.parameters.Kcrm; % chain rachet mechanism stiffness
+    Bcrm = model.parameters.Bcrm; % chain rachet mechanism damping
 
-    f = zeros(13,1);
-
-    dfdx = spalloc(13,13,75);
+    % initialize the matrices
+    f = zeros(13,1);    
+    dfdx = spalloc(13,13,74); % number of non-zero elements can be determint by using a breakpoint at the end of dynfun.m and then use nnz(dfdx)
     dfdxdot = spalloc(13,13,32);
     dfdu = spalloc(13,5,5);
 
     % x = [x_fw, q1 ...q5, v_fw, qd1...qd5, F]
-    % velocities are derivative of positions (2:01:00 recording 4)
-    f(1:6) = xdot(1:6) - x(7:12); %velocities
+    % velocities are derivative of positions
+    f(1:6) = xdot(1:6) - x(7:12); 
     dfdx(1:6, 7:12)   = -speye(6);
     dfdxdot(1:6, 1:6) = speye(6);
 
     % human dynamics
     q = x(2:6);     % the five coordinates of the human model
     qd = x(8:12);
     qdd = xdot(8:12);   
-    % rowerdynamics function generated by Autolev computes 5 torques
-    % using inverse dynamics and also 1 cable force using the 
-    % cable ratchet mechanism model (z is a 6x1 output)
-    % 13 equations to model the dynamics. The
-    % first 6 (multibody dynamics and cable force)
-    [z, dz_dq, dz_dqd, dz_dqdd, dz_dFc] = rowerdynamics(q,qd,qdd,x(13),model.parameters);% inverse dynamics so gives us the z(torques) 
-    % z has 6 elements, the first 5s are the torques, the 6th one is the
-    % wrist to sprocket distance, which depends on the q which is x(2:6) and
-
-    %that we need to produce this motion with that force being applied to the
-    %wrist and then we compare those torques (motion trajectory) to the torques that we have in
-    %our trajectory (torque trajectory) and they have to be equal.
 
+    % rowerdynamics function generated by Autolev computes 5 torques
+    % using inverse dynamics and also the wrist-sprocket length L
+    [z, dz_dq, dz_dqd, dz_dqdd, dz_dFc, L, dL_dq, dLdot_dq, stick] = rowerdynamics(q,qd,qdd,x(13),model.parameters);% inverse dynamics so gives us the z(torques)
+    Ldot = dL_dq * qd;
 
-    f(8:12) = z(1:5) - u;  % f is in Nm 
-    dfdx(8:12, 2:6)  = dz_dq(1:5,1:5);
-    dfdx(8:12, 8:12) = dz_dqd(1:5,1:5);
-    dfdx(8:12, 13)    = dz_dFc(1:5);
-    dfdxdot(8:12, 8:12) = dz_dqdd(1:5,1:5);
+    % multibody dynamics
+    f(8:12) = z - u;
+    dfdx(8:12, 2:6)  = dz_dq;
+    dfdx(8:12, 8:12) = dz_dqd;
+    dfdx(8:12, 13)   = dz_dFc;
+    dfdxdot(8:12, 8:12) = dz_dqdd;
     dfdu(8:12, :) = -speye(5);
 
-    % flywheel dynamics, flywheel sees cable force, minus the shock cord
-    % force
-    L = z(6);
-    dLdq = dz_dq(6,:);
-
     % dynamics of the flywheel
     % m*a - Fdamping - Fcrm = 0
     % or m*a - Fdamping - (Ftotal - Fshockcord) = 0
     f(7) = m * xdot(7) + c*x(7)*abs(x(7)) - (x(13) - K*(L-L0)); % x(7):velocity of the flyhweel ==> xdot(7): acceleration of the flywheel, m*xdot(7)--> intertia, c*x(7)^2--> damping, x(13): amount of flywheel motion
     dfdx(7,7) = 2*c*abs(x(7));
     dfdx(7,13) = -1;
     dfdxdot(7,7) = m;
-    dfdx(7, 2:6) = K*dLdq;
+    dfdx(7, 2:6) = K*dL_dq;
+
+    % ratchet mechanism force is a function of the wrist-sprocket distance
+    % L and flywheel position x(1)
+
+    % start with a linear function
+    F = Kcrm * (L - x(1)) + Bcrm * (Ldot - x(7));
+
+    % a nonlinear transformation to change negative forces to zero
+    epsilon = 0.01;
+%     epsilon = 0.0001;
+    Fcrm = (F + sqrt(F^2 + epsilon^2) )/2;
+    dFcrm_dF = (1 + F/sqrt(F^2 + epsilon^2))/2; 
 
-    %cable force must be equal to cable ratchet force + shock cord force
-    %use epsilon for non-linear spring to remove the non-linearity in the
-    %derivatives
-    epsilon = 0.000001;
-    Fsc= Kcrm * (L - x(1) );
-    Fcrm = (Fsc + sqrt(Fsc^2+epsilon^2))/2  ;
-    dFcrmdFsc = (1+Fsc/sqrt(Fsc^2+epsilon^2))/2; 
+    % this says that the force on the wrist is the sum of flywheel force
+    % and shock cord
     f(13) = x(13) - Fcrm - K*(L - L0);
     dfdx(13,13) = 1;
-    dfdx(13,1) = Kcrm*dFcrmdFsc;
-    dfdx(13,2:6) = -(dFcrmdFsc*Kcrm + K)*dLdq;
+    dfdx(13,1) = Kcrm * dFcrm_dF;
+    dfdx(13,7) = Bcrm * dFcrm_dF;
+    dfdx(13,2:6) = -(dFcrm_dF * Kcrm + K) * dL_dq - dFcrm_dF * Bcrm * dLdot_dq;
+	dfdx(13,8:12) = -dFcrm_dF * Bcrm * dL_dq;		% because dLdot/dqdot = dL/dq
 
 end

model/makerowerdynamics.m

@@ -33,13 +33,13 @@
 
 	% write function header
 	fprintf(fid2,'%% This file was generated by makerowerdynamics.m on %s\n', datetime);
-	fprintf(fid2,'function [f, df_dq, df_dqd, df_dqdd, df_dxfw, stick] = rowerdynamics(q,qd,qdd,xfw,par);\n');
+	fprintf(fid2,'function [f, df_dq, df_dqd, df_dqdd, df_dFc, L, dL_dq, dLdot_dq, stick] = rowerdynamics(q,qd,qdd,Fc,par);\n');
     fprintf(fid2,'%%\n');
     fprintf(fid2,'%% Input:\n');
     fprintf(fid2,'%%      q...................(5x1) generalized coordinates (rad)\n');
     fprintf(fid2,'%%      qd..................(5x1) generalized velocities (rad/s)\n');
     fprintf(fid2,'%%      qdd.................(5x1) generalized accelerations (rad/s^2)\n');
-    fprintf(fid2,'%%      xfw.................(scalar) flywheel position (m)\n');
+    fprintf(fid2,'%%      Fc..................(scalar) cable force (N)\n');
     fprintf(fid2,'%%      par.................(structure) model parameters:\n');
     fprintf(fid2,'%%            par.TrunkMass,   par.TrunkInertia,   par.TrunkCM,   par.TrunkLen\n');
     fprintf(fid2,'%%            par.ThighMass,   par.ThighInertia,   par.ThighCM,   par.ThighLen\n');
@@ -49,15 +49,17 @@
     fprintf(fid2,'%%            par.Xankle, par.Yankle\n');
     fprintf(fid2,'%%            par.Xsprocket, par.Ysprocket		%% point where the cable comes out of the machine\n');
     fprintf(fid2,'%%            par.a, par.b						%% seat path is Y = a*X + b\n');
-    fprintf(fid2,'%%            par.K, par.C						%% seat stiffness and damping\n');
-    fprintf(fid2,'%%            par.Kcrm						    %% cable stiffness parameter	\n');
+    fprintf(fid2,'%%            par__Kseat, par__Cseat				%% seat stiffness and damping\n');
     fprintf(fid2,'%%\n');
     fprintf(fid2,'%% Output:\n');
-    fprintf(fid2,'%%      f...................(6x1) 5 joint torques (Nm) and cable force (N)\n');
-    fprintf(fid2,'%%      df_dq...............(6x5) Jacobian matrix df/dq\n');
-    fprintf(fid2,'%%      df_dqd..............(6x5) Jacobian matrix df/dqdot\n');
-    fprintf(fid2,'%%      df_dqdd.............(6x5) Jacobian matrix df/dqdotdot\n');
-    fprintf(fid2,'%%      df_dxfw.............(6x1) Jacobian matrix df/dxfw\n');
+    fprintf(fid2,'%%      f...................(5x1) 5 joint torques\n');
+    fprintf(fid2,'%%      df_dq...............(5x5) Jacobian matrix df/dq\n');
+    fprintf(fid2,'%%      df_dqd..............(5x5) Jacobian matrix df/dqdot\n');
+    fprintf(fid2,'%%      df_dqdd.............(5x5) Jacobian matrix df/dqdotdot\n');
+    fprintf(fid2,'%%      df_dFc..............(5x1) Jacobian matrix df/dFc\n');
+    fprintf(fid2,'%%      L...................(scalar) wrist-sprocket distance (m)\n');
+    fprintf(fid2,'%%      dL_dq...............(1x5) Jacobian matrix dL/dq\n');
+    fprintf(fid2,'%%      dLdot_dq............(1x5) Jacobian matrix dLdot/dq\n');
     fprintf(fid2,'%%      stick...............(7x2) x,y of ankle,knee,hip,shoulder,elbow,wrist,sprocket\n');
     fprintf(fid2,'%%\n');
 
@@ -68,8 +70,8 @@
 		fprintf(fid2,'    q%1dpp = qdd(%1d);\n', i,i);	
     end
 
-    % make sure that f comes out as a 6x1 column vector
-    fprintf(fid2,'    f = zeros(6,1); \n');
+    % make sure that f comes out as a 5x1 column vector
+    fprintf(fid2,'    f = zeros(5,1); \n');
 
 	% copy the necessary parts of C code from fid1 to fid2
 	copying = 0;
@@ -97,7 +99,7 @@
 	fprintf(fid2,'    df_dq = sparse(df_dq);\n');
 	fprintf(fid2,'    df_dqd = sparse(df_dqd);\n');
 	fprintf(fid2,'    df_dqdd = sparse(df_dqdd);\n');
-	fprintf(fid2,'    df_dxfw = sparse(df_dxfw);\n');
+	fprintf(fid2,'    df_dFc = transpose(sparse(df_dFc));\n');
 	fprintf(fid2,'\nend \n');
 	fclose(fid2);