Ground Reaction Forces after MocoContactTracking

[Ross Miller]

I second Nick's recommendation to start simple and add complexity from there.

When I first started using Moco, I started with the "example2Dwalk" example, which runs fast and converges on a good result. Then I gradually made one change at a time to that model and setup, for example first I changed it so it tracked my own experimental data, then I added GRF tracking, then added a two-segment foot, then added a few more muscles, then added 2-D arms, then gradually added more DOF, more muscles, etc., each time using the result from the simpler model as the initial guess for the more complex model. Now I have lots of good solutions with complex models and don't have to repeat this process every time, but I have not had good results starting from a complex model and using an arbitrary initial guess. Usually the optimizer doesn't converge and if it does it usually doesn't converge on a good result.

Ross

[Ross Miller]

For example, this morning I started a simulation of walking with a prosthetic limb that adds a degree of freedom (pistoning) between the residual limb and prosthesis. Previously I've been modeling that interface as rigid. As the initial guess for this simulation, I used a previous simulation with the same model that lacked this single degree of freedom. It will probably converge. I wouldn't expect it to converge if, for example, I set the initial guess for the kinematics equal to the tracking targets, and the muscle variables all to zero, or something similar. In my experience, that approach will only converge consistently when using simpler models.

Ross

[Christian Greve]

Dear Nick and Ross,

Thanks a lot for your feedback!!

I tried to process your feedback and did the following:
1. Added coordinate actuators to the model and removed all muscles
2. Created an initial guess where the coordinates are good.
3. Re-run the analyses. Tried different weightings for GRF and Coord tracking.

Results:
The Coordinates and GRF tracking is better but the simulation does still not converge. And requires > 10.000 iterations "Restoration Failed". Attached is a screenshot of the visualization and GRF data and the code I used.

Do you have any suggestions to improve the simulation outcomes?

Code:

Code: Select all

import opensim as osim
import os
start_time = 1.5#1.5
end_time = 2.2#2.5
GRFTrackingWeight = 1
CoordTrackingWeight = 1
EffortWeight = 0.1
# MarkerTrackingWeight = 1
convergence_tol = 1e-3
constraint_tol = 1e-3
num_mesh_intervals = 100
ReserveActuatorStrength = 2500



def addCoordinateActuator(model, optForce):
    CoordSet = model.updCoordinateSet()
    for cntrCoord in range(CoordSet.getSize()):
        coord = CoordSet.get(cntrCoord)
        coordName = coord.getName()
        #coord.setWeightForControl(coordName,2)
        
        # if 'pelvis' in coordName:
        print(coordName)
        actu = osim.CoordinateActuator()
        actu.setName('tau_' + coordName)
        actu.setCoordinate(CoordSet.get(coordName))
        actu.setOptimalForce(optForce)
        actu.setMinControl(-1)
        actu.setMaxControl(1)
        model.addComponent(actu)
                     
    model.finalizeConnections()
    model.printToXML(Scaled_modelFileName.replace('.osim','CoordActu.osim'))
    
    return model

def RunMocoInverse(model, CoordFilename, start_time, end_time, num_mesh_intervals, Path):
    # used to create initial guess where coordinates are good. 
    #% Construct MocoInverse
    inverse = osim.MocoInverse()
    inverse.setModel(modelProcessor)
    inverse.setKinematics(osim.TableProcessor(CoordFilename))
    # inverse.set_minimize_sum_squared_activations(True)
    inverse.set_kinematics_allow_extra_columns(True)


    inverse.set_initial_time(start_time)
    inverse.set_final_time(end_time)
    inverse.set_mesh_interval(1/num_mesh_intervals)
    inverse.set_constraint_tolerance(1e-4)#1e-4
    inverse.set_convergence_tolerance(1e-4)
    [ModelPath, ModelFilename] = os.path.split(Scaled_modelFileName)

    #Solve
    inverseSolution = inverse.solve()
    solutionWritable = inverseSolution.getMocoSolution()
    # inverseSolution.unseal()
    MocoInverseFilename = os.path.join(Path,'MocoInverse.sto')
    solutionWritable.write(MocoInverseFilename)
    
    return MocoInverseFilename



#%% Build Coordinate Tracking Task
# Create and name an instance of the MocoTrack tool. 
GRF_tracking = 1
CoordTrack = osim.MocoTrack()
CoordTrack.setName("PiG_CoordTracking")

#%% modify model
# remove muscles
modelProcessor = osim.ModelProcessor(ScaledCoordActu_modelFileName)
# append GRF
modelProcessor.append(osim.ModOpAddExternalLoads(GRF_SourceFilename))
# add reserves
modelProcessor.append(osim.ModOpAddReserves(ReserveActuatorStrength))
#%% build MocoCoordTrack and attach model
CoordTrack.setModel(modelProcessor)


#%% generate IK filename which can be used by the tracking tool
IK_Filename = RunMocoInverse(modelProcessor.process(), CoordFilename, start_time, end_time, num_mesh_intervals, Path)

#%% Use MocoInverseSolution as guess file
tableProcessor = osim.TableProcessor(IK_Filename)
CoordTrack.setStatesReference(tableProcessor)
CoordTrack.set_states_global_tracking_weight(CoordTrackingWeight);
CoordTrack.set_allow_unused_references(True);
CoordTrack.set_track_reference_position_derivatives(True);
CoordTrack.set_initial_time(start_time)
CoordTrack.set_final_time(end_time)

#%% Initiate MocoStudy to add tracking goal
study = CoordTrack.initialize()
problem = study.updProblem()

#%%GRF tracking
if GRF_tracking == 1:
    #% GRF Tracking
    # % Track the right and left vertical and fore-aft ground reaction forces.
    contactTracking = osim.MocoContactTrackingGoal('contact', GRFTrackingWeight)
    contactTracking.setExternalLoadsFile(GRF_SourceFilename)
    forceNamesRightFoot = osim.StdVectorString()
    forceNamesRightFoot.append('R_PostHeel_Force')
    forceNamesRightFoot.append('R_MedHeel_Force')
    forceNamesRightFoot.append('R_LatHeel_Force')
    forceNamesRightFoot.append('R_ProxMet1_Force')
    forceNamesRightFoot.append('R_ProxMet3_Force')
    forceNamesRightFoot.append('R_ProxMet5_Force')
    forceNamesRightFoot.append('R_DistMet1_Force')
    forceNamesRightFoot.append('R_DistMet3_Force')
    forceNamesRightFoot.append('R_DistMet5_Force')
    forceNamesRightFoot.append('R_Hallux_Force')
    forceNamesRightFoot.append('R_Toes_Force')
    #Left
    forceNamesLeftFoot = osim.StdVectorString()
    forceNamesLeftFoot.append('L_PostHeel_Force')
    forceNamesLeftFoot.append('L_MedHeel_Force')
    forceNamesLeftFoot.append('L_LatHeel_Force')
    forceNamesLeftFoot.append('L_ProxMet1_Force')
    forceNamesLeftFoot.append('L_ProxMet3_Force')
    forceNamesLeftFoot.append('L_ProxMet5_Force')
    forceNamesLeftFoot.append('L_DistMet1_Force')
    forceNamesLeftFoot.append('L_DistMet3_Force')
    forceNamesLeftFoot.append('L_DistMet5_Force')
    forceNamesLeftFoot.append('L_Hallux_Force')
    forceNamesLeftFoot.append('L_Toes_Force')
    
    contactTracking.addContactGroup(forceNamesRightFoot, 'externalforce')
    contactTracking.addContactGroup(forceNamesLeftFoot, 'externalforce_0')
    contactTracking.setProjection('plane')
    contactTracking.setProjectionVector(osim.Vec3(0, 0, 1))
    problem.addGoal(contactTracking)

#%% Modify effort cost term
effort = osim.MocoControlGoal("control")
effort.setWeight(EffortWeight)
problem.addGoal(effort)

#%% Define solver and solve.
solver = osim.MocoCasADiSolver.safeDownCast(study.updSolver())
solver.resetProblem(problem)
solver.set_multibody_dynamics_mode('explicit')
solver.set_num_mesh_intervals(100)
solver.set_optim_convergence_tolerance(convergence_tol)#.....10^-1
solver.set_optim_constraint_tolerance(constraint_tol)#1e-4 is default
solver.set_optim_max_iterations(50000)
solver.set_parameters_require_initsystem(False)

#%% create guess file
guess = solver.createGuess()
guess.randomizeAdd()
prevSolution = osim.MocoTrajectory(IK_Filename)
StatesTableGuess = guess.exportToStatesTable()
columnLabels_to_keep = StatesTableGuess.getColumnLabels()
StatesTablePrevSol = prevSolution.exportToStatesTable()
columnLabels_all = StatesTablePrevSol.getColumnLabels()
for column in columnLabels_all:
    if not column in columnLabels_to_keep:
        StatesTablePrevSol.removeColumn(column)
guess.insertStatesTrajectory(StatesTablePrevSol, True);
ControlsTable = prevSolution.exportToControlsTable();
guess.insertControlsTrajectory(ControlsTable, True);
#set guess file
solver.setGuess(guess)

#%% solve and write
CoordSolution = study.solve()
CoordSolution.unseal()
CoordSolution.write(Output_FilenameMocoCoordTrack)
#//////////////////////////////////////////


#%% extract GRF data
contact_r = osim.StdVectorString()
contact_l = osim.StdVectorString()
contact_r.append('R_PostHeel_Force')
# contact_r.add('contactFront_r')
contact_l.append('L_PostHeel_Force')
# contact_l.add('contactFront_l')

externalForcesTableFlat = osim.createExternalLoadsTableForGait(modelProcessor.process(), CoordSolution,contact_r,contact_l)
osim.STOFileAdapter.write(externalForcesTableFlat, 'gaitTracking_solutionGRF.sto');
#//////////////////////////////////////////

[Nicholas Bianco]

Hi Christian,

If you're continuing to have trouble with convergence, then it's possible that the coordinate and ground reaction force data are not dynamically consistent with each other. You could try only tracking the ground reaction forces to see if you can get something to convergence (even if the kinematics look weird).

You should also try not tracking some of the pelvis coordinates (especially pelvis_ty) since that can make convergence difficult if the data isn't perfectly consistent. Increase the control effort cost term a little bit could help too, but not sure without looking at a full breakdown if your objective.

Best,
Nick