simtk.org

Code: Select all

# -------------------------------------------------------------------------- #
# OpenSim Moco: exampleMocoTrack.py                                          #
# -------------------------------------------------------------------------- #
# Copyright (c) 2019 Stanford University and the Authors                     #
#                                                                            #
# Author(s): Nicholas Bianco                                                 #
#                                                                            #
# Licensed under the Apache License, Version 2.0 (the "License") you may     #
# not use this file except in compliance with the License. You may obtain a  #
# copy of the License at http://www.apache.org/licenses/LICENSE-2.0          #
#                                                                            #
# Unless required by applicable law or agreed to in writing, software        #
# distributed under the License is distributed on an "AS IS" BASIS,          #
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   #
# See the License for the specific language governing permissions and        #
# limitations under the License.                                             #
# -------------------------------------------------------------------------- #

# This example features two different tracking problems solved using the
# MocoTrack tool. 
#  - The first problem demonstrates the basic usage of the tool interface
#    to solve a torque-driven marker tracking problem. 
#  - The second problem shows how to customize a muscle-driven state tracking 
#    problem using more advanced features of the tool interface.
# 
# See the README.txt next to this file for more information.

import os
import math
import opensim as osim

def torqueDrivenMarkerTracking():

    # Create and name an instance of the MocoTrack tool.
    track = osim.MocoTrack()
    track.setName("torque_driven_marker_tracking1")

    # Construct a ModelProcessor and add it to the tool. ModelProcessors
    # accept a base model and allow you to easily modify the model by appending
    # ModelOperators. Operations are performed in the order that they are
    # appended to the model.
    # Create the base Model by passing in the model file.
    modelProcessor = osim.ModelProcessor("standingperturbationScaled_Prescribed.osim")
    # Add ground reaction external loads in lieu of a ground-contact model.
    modelProcessor.append(osim.ModOpAddExternalLoads("Trial26_GRFFile_wrtOrigin.xml"))
    # Remove all the muscles in the model's ForceSet.
    modelProcessor.append(osim.ModOpRemoveMuscles())
    # Add CoordinateActuators to the model degrees-of-freedom. This ignores the 
    # pelvis coordinates which already have residual CoordinateActuators.
    modelProcessor.append(osim.ModOpAddReserves(250))
    track.setModel(modelProcessor)

    # Use this convenience function to set the MocoTrack markers reference
    # directly from a TRC file. By default, the markers data is filtered at
    # 6 Hz and if in millimeters, converted to meters.
    track.setMarkersReferenceFromTRC("Trial26_markers_rotated.trc")

    # There is marker data in the 'marker_trajectories.trc' associated with
    # model markers that no longer exists (i.e. markers on the arms). Set this
    # flag to avoid an exception from being thrown.
    track.set_allow_unused_references(True)

    # Increase the global marker tracking weight, which is the weight
    # associated with the internal MocoMarkerTrackingCost term.
    track.set_markers_global_tracking_weight(20)

    # Increase the tracking weights for individual markers in the data set 
    # placed on bony landmarks compared to markers located on soft tissue.
    markerWeights = osim.MocoWeightSet()
    markerWeights.cloneAndAppend(osim.MocoWeight("RASI", 20))    
    markerWeights.cloneAndAppend(osim.MocoWeight("LASI", 20))
    markerWeights.cloneAndAppend(osim.MocoWeight("SACR", 10))
    markerWeights.cloneAndAppend(osim.MocoWeight("RKNE", 10))
    markerWeights.cloneAndAppend(osim.MocoWeight("RANK", 10))
    markerWeights.cloneAndAppend(osim.MocoWeight("RHEE", 5))
    markerWeights.cloneAndAppend(osim.MocoWeight("RTOE", 2))
    track.set_markers_weight_set(markerWeights)
    ##
    

    # Initial time, final time, and mesh interval. The number of mesh points
    # used to discretize the problem is computed internally using these values.
    track.set_initial_time(0.5)
    track.set_final_time(2.5)
    track.set_mesh_interval(0.005)
    
    ######## adding control effort as a goal
    #effort = osim.MocoControlGoal()
    #effort.setName("effort")
    #effort.setWeight(5)
    #problem.addGoal(effort)
    #################################################
    
    #adding torso orientation goal:
    #torsoGoal= osim.MocoOrientationTrackingGoal('torsoGoal',10)
    #torsoTable = osim.TableProcessor('refQ_torso_orientation.sto')
    #torsoGoal.setStatesReference(torsoTable)
    
    #torsoGoal.setRotationReferenceFile('torso_orientation.sto');
    #paths = []
    #paths.append('/bodyset/torso')
    #torsoGoal.setFramePaths(paths)
    #problem.addGoal(torsoGoal);
    ###################################################
    
    #####################################################

    # Solve! Use track.solve() to skip visualizing.
    study=track.initialize()
    problem=study.updProblem()
    solver = study.initCasADiSolver()
    solver.set_num_mesh_intervals(80);
    solver.set_verbosity(2);
    solver.set_optim_solver("ipopt");
    solver.set_optim_convergence_tolerance(1e-4);
    solver.set_optim_constraint_tolerance(1e-4);
    solver.set_optim_max_iterations(2000)
    ##solver.set_optim_convergence_tolerance(1e-4)
    a=solver.get_optim_ipopt_print_level()
    print("solver.get_optim_ipopt_print_level()",a)
    c=solver.get_optim_max_iterations()
    print("solver.get_optim_max_iterations",c)
    d=solver.get_optim_convergence_tolerance()
    print("solver.get_optim_convergence_tolerance()",d)
    
    solution = study.solve()
    
    b=solver.get_optim_ipopt_print_level()
    print("solver.get_optim_ipopt_print_level",b)
    

    
    #solution = track.solve()
    solution.unseal()
    #track.visualize(solution)
    solution.write('track_balance_perturbed_control_No_pelvisPosition_moco_0_4.sto')
    b=solution.getFinalTime()
    c=solution.getNumIterations()
    d=solution.getSolverDuration()
    e=solution.getStatus()
    a=solution.getParametersMat()
    print("finaltime", b)
    print("iteration", c)
    print("duration", d)
    print("statuse=", e)
    print("getParametersMat",a)
    #solution.write('track_balance_perturbed_control.sto')
    
    '''
    osim.STOFileAdapter.write(solution.exportToStatesTable(),
                      "moco_track_reactionGoal_Muscle_states.sto")
    osim.STOFileAdapter.write(solution.exportToControlsTable(),
                      "moco_track_reactionGoal_controls.sto")
    '''
    
    #JR_paths = []
    #JR_paths.append('.*reaction_on_child')
    #statesTable = solution.exportToStatesTable();
    #controlsTable = solution.exportToControlsTable();
    #model=osim.Model("subject_walk_armless.osim")
   # JR_outputs_table = osim.analyzeSpatialVec(model, statesTable, controlsTable, JR_paths).flatten()
    #JR_outputs_table.write('reaction.sto')
    
################# running the solution  ##################################################  

torqueDrivenMarkerTracking()