simtk.org

Code: Select all

function value = CoPMinimizationGoal
% This example allows Matlab users of Moco to prototype custom costs/goals
% to send to Moco developers for implementing in C++.
% If you can write your goal in the "...GoalIntegrand" and "...GoalValue"
% functions below and you can invoke evaluateCustomGoal() (shown below)
% without error, then Moco developers can probably easily convert your goal
% into a proper C++ goal for you. Once your prototype works, create an issue
% on GitHub.
%
% In C++, MocoGoals can be in cost mode or endpoint constraint mode. In this
% example, we expect that your goal is in cost mode.
%
% In this example, the custom goal is minimizing the sum of squared controls
% divided by the time duration of the phase.

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, [1 1.1]);
% 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();
trajectory=solver.createGuess('bounds'); % Use bounds as initial guess
trajectory.resampleWithNumTimes(2);
trajectory.setTime([0 1]); 
trajectory.setState('/jointset/groundPelvis/pelvis_tilt/value', ...
    [-20*pi/180, -10*pi/180]);
trajectory.setState('/jointset/groundPelvis/pelvis_tx/value', ...
    [0, 1]);
trajectory.setState('/jointset/groundPelvis/pelvis_ty/value', ...
    [0.75, 1.25]);
trajectory.setState("/jointset/lumbar/lumbar/value", ...
    [0, 20*pi/180]);
trajectory.setState('/jointset/exo_hip_r/exo_hip_flexion_r/value', ...
    [-10*pi/180, 60*pi/180]);
trajectory.setState('/jointset/exo_hip_l/exo_hip_flexion_l/value', ...
    [-10*pi/180, 60*pi/180]);
trajectory.setState('/jointset/exo_knee_r/exo_knee_angle_r/value', ...
    [-50*pi/180, 0]);
trajectory.setState('/jointset/exo_knee_l/exo_knee_angle_l/value', ...
    [-50*pi/180, 0]);
trajectory.setState('/jointset/exo_ankle_r/exo_ankle_angle_r/value', ...
    [-15*pi/180, 25*pi/180]);
trajectory.setState('/jointset/exo_ankle_l/exo_ankle_angle_l/value', ...
    [-15*pi/180, 25*pi/180]);
solver.setGuess(trajectory);

%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);
data = osimMocoTableToStruct(externalForcesTableFlat);

Right_GRF_X=data.ground_force_r_vx;
Right_GRF_Y=data.ground_force_r_vy;
Right_GRF_Z=data.ground_force_r_vz;
Right_Torque_X=data.ground_torque_r_x;
Right_Torque_Y=data.ground_torque_r_y;
Right_Torque_Z=data.ground_torque_r_z;

Left_GRF_X=data.ground_force_l_vx;
Left_GRF_Y=data.ground_force_l_vy;
Left_GRF_Z=data.ground_force_l_vz;
Left_Torque_X=data.ground_torque_l_x;
Left_Torque_Y=data.ground_torque_l_y;
Left_Torque_Z=data.ground_torque_l_z;

point_Right_X=Right_Torque_X./Right_GRF_X;
point_Right_Y=Right_Torque_Y./Right_GRF_Y;
point_Right_Z=Right_Torque_Z./Right_GRF_Z;
point_Left_X=Left_Torque_X./Left_GRF_X;
point_Left_Y=Left_Torque_Y./Left_GRF_Y;
point_Left_Z=Left_Torque_Z./Left_GRF_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);