Minimization of CoP - Custom Goal

[Burak Kula]

Hello Everyone,

Recently I am working on minimization of oscillations of CoP. As far as I realized, in Moco there no built-in codes for this task so I am planing to use a specific goal. But I faced with a problem that even if I am able to find CoP, I don't know how can I implement them with moco problem. I am going to give my codes below. I am waiting for tips or hints to complete the task.

I don't know how to establish a connection between, points and moco problem.

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);

[Nicholas Bianco]

Hi Burak,

The Matlab code is available just as a prototyping tool. The code will need to be converted to C++ to be incorporated into Moco.

Have you confirmed that this code correctly computes COP (e.g., given an existing motion)? Also, is the purpose of this goal to minimize COP or track an existing COP trajectory? I'm not sure the usefulness of minimizing the COP on its own, as the COP will need to vary over a gait cycle.

-Nick

[Simon Jeng]

Hi Nick,

I have tried to prototype a custom goal of minimizing CoP displacement using examplePrototypeCustomGoal.m. I successfully write my goal in the "...GoalIntegrand" and "...GoalValue" functions and invoke evaluateCustomGoal() to calculate the integral of squared CoP displacement without error. My goal is only used for human movements that the foot does not move relative to the ground(e.g. squat-to-stand) because the location of CoP is obtained by calculating the joint reaction force/moment(the joint is a weld joint connecting the foot and ground). Next, is it possible for me to please Moco developers convert my goal into a plugin for me so that I can use it in Matlab? If it's possible, I would show the code.

Best,
Simon

[Simon Jeng]

Thanks Nick,

I have finished plugin-making.

Best,
Simon

[Nicholas Bianco]

Hi Simon,

I see -- you're leaving in the weld joint between the feet and the ground, but you want the COP to remain beneath the feet to make the motion realistic, good idea!

I think we were to create a new goal, I'd want to make sure it's a generally useful as possible. I'd want a COP goal to be able to track any COP trajectory, not just minimize the COP (which is the equivalent to tracking a "zero" trajectory). But if your plugin is working for you now, maybe we can wait on this.

-Nick