StateSpace_new.m
8406fed2a624master

StateSpace_new.m
View Options

			function [du] = StateSpace_new(t, u, IDDATA, DYDATA)
			%{
			Function for generating the state space.
			--------------------------------------------------------------------------
			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.
			--------------------------------------------------------------------------
			%}
			TimeID = floor((t/0.01)+1);

			q1 = u(1); q2 = u(2); q3 = u(3);
			dq1 = u(4); dq2 = u(5); dq3 = u(6);

			% 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

			Q1 = Q(1,1); Q2 = Q(2,1); Q3 = Q(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;