Currently I am trying to minimize oscillations of center of pressure during a gait in MATLAB. To do so, I used "examplePrototypeCustomGoal.m".
I can run the code without any error or warning but I am not sure it works properly. Also I am not sure that minimization works.
I am going to add my code and output screen below.
Please help me to verify it is okay or not.
By the way, When assigned integrand to 0, I obtained 0 at the end, When I assigned integrand to 1, I obtained 400 at the and and also I tried to assign 5 to integrand and I got 2000.
Code: Select all
function value = CoPMinimizationGoal
import org.opensim.modeling.*;
study = MocoStudy();
problem = study.updProblem();
mp=ModelProcessor('2D_Exo_Gait_V1_exo_integrated.osim');
problem.setModelProcessor(mp);
model = mp.process();
model.initSystem();
% State Bounds
problem.setTimeBounds(0, [0.4 0.6]);
% States of Upper Body Musculoskeletal System.
problem.setStateInfo('/jointset/groundPelvis/pelvis_tilt/value', ...
[-10*pi/180, 10*pi/180]);
problem.setStateInfo('/jointset/groundPelvis/pelvis_tx/value', ...
[-0.2 2]);
problem.setStateInfo('/jointset/groundPelvis/pelvis_ty/value', ...
[0.75, 1.25]);
problem.setStateInfo("/jointset/lumbar/lumbar/value", ...
[0, 20*pi/180]);
% States of Exoskeleton.
problem.setStateInfo('/jointset/exo_hip_r/exo_hip_flexion_r/value', ...
[-25*pi/180, 45*pi/180]);
problem.setStateInfo('/jointset/exo_hip_l/exo_hip_flexion_l/value', ...
[-25*pi/180, 45*pi/180]);
problem.setStateInfo('/jointset/exo_knee_r/exo_knee_angle_r/value', ...
[-75*pi/180, 0]);
problem.setStateInfo('/jointset/exo_knee_l/exo_knee_angle_l/value', ...
[-75*pi/180, 0]);
problem.setStateInfo('/jointset/exo_ankle_r/exo_ankle_angle_r/value', ...
[-30*pi/180, 20*pi/180]);
problem.setStateInfo('/jointset/exo_ankle_l/exo_ankle_angle_l/value', ...
[-30*pi/180, 20*pi/180]);
solver = study.initCasADiSolver();
trajectory = solver.createGuess();
%Contact Determination
contact_r = StdVectorString();
contact_l = StdVectorString();
contact_r.add('/contactHeel_r');
contact_r.add('/contactFront_r');
contact_l.add('/contactHeel_l');
contact_l.add('/contactFront_l');
externalForcesTableFlat = opensimMoco.createExternalLoadsTableForGait(model,trajectory,contact_r,contact_l);
opensimMoco.writeTableToFile(externalForcesTableFlat,'solution.sto');
solution1='solution.sto';
Right_GFR_X=solution1(:,1);
Right_GFR_Y=solution1(:,2);
Right_GFR_Z=solution1(:,3);
Left_GFR_X=solution1(:,4);
Left_GFR_Y=solution1(:,5);
Left_GFR_Z=solution1(:,6);
Right_Torque_X=solution1(:,7);
Right_Torque_Y=solution1(:,8);
Right_Torque_Z=solution1(:,9);
Left_Torque_X=solution1(:,10);
Left_Torque_Y=solution1(:,11);
Left_Torque_Z=solution1(:,12);
point_Right_X=Right_Torque_X./Right_GFR_X;
point_Right_Y=Right_Torque_Y./Right_GFR_Y;
point_Right_Z=Right_Torque_Z./Right_GFR_Z;
point_Left_X=Left_Torque_X./Left_GFR_X;
point_Left_Y=Left_Torque_Y./Left_GFR_Y;
point_Left_Z=Left_Torque_Z./Left_GFR_Z;
points_Right=[point_Right_X point_Right_Y point_Right_Z];
points_Left=[point_Left_X point_Left_Y point_Left_Z];
points=[points_Right points_Left];
value = evaluateCustomGoal(problem, trajectory , ...
@calcMyCoPGoalIntegrand, @calcMyCoPValue);
end
function integrand = calcMyCoPGoalIntegrand(model, point_Right_X)
% firstly only right x will be minimized, if it works properly all points will be added.
% Compute the integrand for the integral portion of the cost goal.
controls1 = point_Right_X;
integrand = 5;
for i = 1:length(controls1) - 1
integrand = integrand + controls1(i)^2;
end
end
function value = calcMyCoPValue(...
model, initial_state, final_state, integral)
% Compute the goal value from the phase's initial and final states, and from
% the integral of the integrand function over the phase. The integration is
% performed for you by evaluateCustomGoal().
value = integral / (final_state.getTime() - initial_state.getTime());
end
function goalValue = evaluateCustomGoal(...
problem, mocoTraj, integrandFunc, goalFunc)
% Test a custom goal, defined by integrand and goal functions, on the
% provided problem and MocoTrajectory.
model = problem.getPhase(0).getModelProcessor().process();
org.opensim.modeling.opensimMoco.prescribeControlsToModel(mocoTraj, model);
statesTraj = mocoTraj.exportToStatesTrajectory(problem);
model.initSystem();
N = statesTraj.getSize();
integrand = zeros(N, 1);
for i = 0:(N - 1)
integrand(i + 1) = integrandFunc(model, statesTraj.get(i));
end
integral = trapz(integrand);
goalValue = goalFunc(model, statesTraj.front(), statesTraj.back(), integral);
end
Burak KULA.