def generate_results(self, root_dir, args): model = osim.Model() body = osim.Body("b", 1, osim.Vec3(0), osim.Inertia(0)) model.addBody(body) joint = osim.SliderJoint("j", model.getGround(), body) joint.updCoordinate().setName("coord") model.addJoint(joint) damper = osim.SpringGeneralizedForce("coord") damper.setViscosity(-1.0) model.addForce(damper) actu = osim.CoordinateActuator("coord") model.addForce(actu) model.finalizeConnections() moco = osim.MocoStudy() problem = moco.updProblem() problem.setModel(model) problem.setTimeBounds(0, 2) problem.setStateInfo("/jointset/j/coord/value", [-10, 10], 0, 5) problem.setStateInfo("/jointset/j/coord/speed", [-10, 10], 0, 2) problem.setControlInfo("/forceset/coordinateactuator", [-50, 50]) problem.addGoal(osim.MocoControlGoal("effort", 0.5)) solver = moco.initCasADiSolver() solver.set_num_mesh_intervals(50) solution = moco.solve() solution.write(self.solution_file % root_dir)
def predict(self, root_dir): model = self.muscle_driven_model(root_dir) moco = self.create_study(model) problem = moco.updProblem() problem.addGoal(osim.MocoControlGoal('effort')) problem.addGoal(osim.MocoInitialActivationGoal('init_activation')) problem.addGoal(osim.MocoFinalTimeGoal('time')) solver = osim.MocoCasADiSolver.safeDownCast(moco.updSolver()) solver.resetProblem(problem) guess = solver.createGuess() N = guess.getNumTimes() for muscle in model.getMuscles(): dgf = osim.DeGrooteFregly2016Muscle.safeDownCast(muscle) if not dgf.get_ignore_tendon_compliance(): guess.setState( '%s/normalized_tendon_force' % muscle.getAbsolutePathString(), osim.createVectorLinspace(N, 0.1, 0.1)) guess.setState( '%s/activation' % muscle.getAbsolutePathString(), osim.createVectorLinspace(N, 0.05, 0.05)) guess.setControl(muscle.getAbsolutePathString(), osim.createVectorLinspace(N, 0.05, 0.05)) solver.setGuess(guess) solution = moco.solve() solution.write(self.predict_solution_file % root_dir) # moco.visualize(solution) return solution
def solveMarkerTracking(markersRef, guess): # Predict the optimal trajectory for a minimum time swing-up. study = osim.MocoStudy() study.setName("double_pendulum_track") problem = study.updProblem() # Model (dynamics). problem.setModel(createDoublePendulumModel()) # Bounds. # Arguments are name, [lower bound, upper bound], # initial [lower bound, upper bound], # final [lower bound, upper bound]. finalTime = markersRef.getMarkerTable().getIndependentColumn()[-1] problem.setTimeBounds(0, finalTime) problem.setStateInfo("/jointset/j0/q0/value", [-10, 10], 0) problem.setStateInfo("/jointset/j0/q0/speed", [-50, 50], 0) problem.setStateInfo("/jointset/j1/q1/value", [-10, 10], 0) problem.setStateInfo("/jointset/j1/q1/speed", [-50, 50], 0) problem.setControlInfo("/tau0", [-100, 100]) problem.setControlInfo("/tau1", [-100, 100]) # Cost: track provided marker data. markerTracking = osim.MocoMarkerTrackingGoal() markerTracking.setMarkersReference(markersRef) problem.addGoal(markerTracking) effort = osim.MocoControlGoal() effort.setName("effort") effort.setWeight(0.0001) # problem.addGoal(effort) # Configure the solver. solver = study.initTropterSolver() solver.set_num_mesh_intervals(50) solver.set_verbosity(2) solver.set_optim_solver("ipopt") solver.set_optim_jacobian_approximation("exact") solver.set_optim_hessian_approximation("exact") solver.set_exact_hessian_block_sparsity_mode("dense") solver.setGuess(guess) # Save the problem to a setup file for reference. study.printToXML("examplePredictAndTrack_track_markers.omoco") # Solve the problem. solution = study.solve() solution.write("examplePredictAndTrack_track_markers_solution.sto") if visualize: study.visualize(solution) return solution
def test_changing_costs(self): # Changes to the costs are obeyed. study = osim.MocoStudy() problem = study.updProblem() problem.setModel(createSlidingMassModel()) problem.setTimeBounds(0, [0, 10]) problem.setStateInfo("/slider/position/value", [0, 1], 0, 1) problem.setStateInfo("/slider/position/speed", [-100, 100], 0, 0) problem.updPhase().addGoal(osim.MocoFinalTimeGoal()) effort = osim.MocoControlGoal("effort") problem.updPhase().addGoal(effort) solver = study.initTropterSolver() solver.set_transcription_scheme("trapezoidal") finalTime0 = study.solve().getFinalTime() # Change the weights of the costs. effort.setWeight(0.1) assert (study.solve().getFinalTime() < 0.8 * finalTime0)
def predict(self, run_time_stepping=False, exponent=2): study = self.create_study() problem = study.updProblem() problem.setTimeBounds(0, [0.4, 0.8]) effort = osim.MocoControlGoal("effort") effort.setExponent(exponent) problem.addGoal(effort) problem.addGoal(osim.MocoFinalTimeGoal("time", 0.01)) solution = study.solve() # study.visualize(solution) time_stepping = None if run_time_stepping: time_stepping = osim.simulateIterateWithTimeStepping( solution, self.build_model()) return solution, time_stepping
def predict_assisted(self, root_dir): model = self.assisted_model(root_dir) moco = self.create_study(model) problem = moco.updProblem() problem.addGoal(osim.MocoControlGoal('effort')) problem.addGoal(osim.MocoInitialActivationGoal('init_activation')) problem.addGoal(osim.MocoFinalTimeGoal('time')) problem.addParameter( osim.MocoParameter('stiffness', '/forceset/spring', 'stiffness', osim.MocoBounds(0, 100))) solver = osim.MocoCasADiSolver.safeDownCast(moco.updSolver()) solver.resetProblem(problem) guess = solver.createGuess() N = guess.getNumTimes() for muscle in model.getMuscles(): dgf = osim.DeGrooteFregly2016Muscle.safeDownCast(muscle) if not dgf.get_ignore_tendon_compliance(): guess.setState( '%s/normalized_tendon_force' % muscle.getAbsolutePathString(), osim.createVectorLinspace(N, 0.1, 0.1)) guess.setState( '%s/activation' % muscle.getAbsolutePathString(), osim.createVectorLinspace(N, 0.05, 0.05)) guess.setControl(muscle.getAbsolutePathString(), osim.createVectorLinspace(N, 0.05, 0.05)) solver.setGuess(guess) solver.set_parameters_require_initsystem(False) solution = moco.solve() # moco.visualize(solution) solution.write(self.predict_assisted_solution_file % root_dir)
# Set to a percentage of the final guess time to end timeLB = 0.9 * osim.Storage(taskName + '_StartingGuess.sto').getLastTime() timeUB = 1.1 * osim.Storage(taskName + '_StartingGuess.sto').getLastTime() problem.setTimeBounds(osim.MocoInitialBounds(0.0), osim.MocoFinalBounds(timeLB, timeUB)) # problem.setTimeBounds(0,[]) #Define marker end point goals osimHelper.addMarkerEndPoints(taskName, problem, simModel) #Define kinematic task bounds osimHelper.addTaskBounds(taskName, problem, simModel, taskBoundsElv, taskBoundsRot, taskBoundsAng) #Add a control cost to the problem problem.addGoal(osim.MocoControlGoal('effort', 1)) #Add a final time goal to the problem problem.addGoal(osim.MocoFinalTimeGoal('time', 1)) #Configure the solver solver = study.initCasADiSolver() solver.set_num_mesh_intervals(meshInterval) solver.set_verbosity(2) solver.set_optim_solver('ipopt') solver.set_optim_convergence_tolerance(1e-4) solver.set_optim_constraint_tolerance(1e-4) #Set the guess to an existing solution for this task from existing work #Note that this will be slightly off given the previous study included forces #that approximated ligaments and joint capsule passive resistance, but should
# Time bounds problem.setTimeBounds(0, 1) # Position bounds: the model should start in a squat and finish # standing up. problem.setStateInfo('/jointset/hip_r/hip_flexion_r/value', [-2, 0.5], -2, 0) problem.setStateInfo('/jointset/knee_r/knee_angle_r/value', [-2, 0], -2, 0) problem.setStateInfo('/jointset/ankle_r/ankle_angle_r/value', [-0.5, 0.7], -0.5, 0) # Velocity bounds: all model coordinates should start and end at rest. problem.setStateInfoPattern('/jointset/.*/speed', [], 0, 0) # Part 1d: Add a MocoControlCost to the problem. problem.addGoal(osim.MocoControlGoal('myeffort')) # Part 1e: Configure the solver. solver = study.initCasADiSolver() solver.set_num_mesh_intervals(25) solver.set_optim_convergence_tolerance(1e-4) solver.set_optim_constraint_tolerance(1e-4) if not os.path.isfile('predictSolution.sto'): # Part 1f: Solve! Write the solution to file, and visualize. predictSolution = study.solve() predictSolution.write('predictSolution.sto') study.visualize(predictSolution) ## Part 2: Torque-driven Tracking Problem # Part 2a: Construct a tracking reference TimeSeriesTable using filtered
def run_tracking_problem(self, root_dir, config): modelProcessor = self.create_model_processor(root_dir, for_inverse=False, config=config) model = modelProcessor.process() model.initSystem() # Count the number of Force objects in the model. We'll use this to # normalize the control effort cost. numForces = 0 for actu in model.getComponentsList(): if (actu.getConcreteClassName().endswith('Muscle') or actu.getConcreteClassName().endswith('Actuator')): numForces += 1 # Construct the base tracking problem # ----------------------------------- track = osim.MocoTrack() track.setName('tracking_walking') track.setModel(modelProcessor) if self.coordinate_tracking: tableProcessor = osim.TableProcessor(os.path.join(root_dir, 'resources/Rajagopal2016/coordinates.mot')) tableProcessor.append(osim.TabOpLowPassFilter(6)) tableProcessor.append(osim.TabOpUseAbsoluteStateNames()) track.setStatesReference(tableProcessor) track.set_states_global_tracking_weight( config.tracking_weight / (2 * model.getNumCoordinates())) # Don't track some pelvis coordinates to avoid poor walking motion # solutions. stateWeights = osim.MocoWeightSet() weightList = list() weightList.append(('/jointset/ground_pelvis/pelvis_ty/value', 0)) weightList.append(('/jointset/ground_pelvis/pelvis_tz/value', 0)) weightList.append(('/jointset/ground_pelvis/pelvis_list/value', 0)) weightList.append(('/jointset/ground_pelvis/pelvis_tilt/value', 0)) weightList.append(('/jointset/ground_pelvis/pelvis_rotation/value', 0)) weightList.append(('/jointset/hip_r/hip_rotation_r/value', 0)) # weightList.append(('/jointset/hip_r/hip_adduction_r/value', 0)) weightList.append(('/jointset/hip_l/hip_rotation_l/value', 0)) # weightList.append(('/jointset/hip_l/hip_adduction_l/value', 0)) # weightList.append(('/jointset/ankle_r/ankle_angle_r/value', 10)) # weightList.append(('/jointset/ankle_l/ankle_angle_l/value', 10)) for weight in weightList: stateWeights.cloneAndAppend(osim.MocoWeight(weight[0], weight[1])) track.set_states_weight_set(stateWeights) track.set_apply_tracked_states_to_guess(True) # track.set_scale_state_weights_with_range(True); else: track.setMarkersReferenceFromTRC(os.path.join(root_dir, 'resources/Rajagopal2016/markers.trc')) track.set_markers_global_tracking_weight( config.tracking_weight / (2 * model.getNumMarkers())) iktool = osim.InverseKinematicsTool(os.path.join(root_dir, 'resources/Rajagopal2016/ik_setup_walk.xml')) iktasks = iktool.getIKTaskSet() markerWeights = osim.MocoWeightSet() for marker in model.getComponentsList(): if not type(marker) is osim.Marker: continue for i in np.arange(iktasks.getSize()): iktask = iktasks.get(int(i)) if iktask.getName() == marker.getName(): weight = osim.MocoWeight(iktask.getName(), iktask.getWeight()) markerWeights.cloneAndAppend(weight) track.set_markers_weight_set(markerWeights) track.set_allow_unused_references(True) track.set_track_reference_position_derivatives(True) track.set_control_effort_weight(config.effort_weight / numForces) track.set_initial_time(self.initial_time) track.set_final_time(self.half_time) track.set_mesh_interval(self.mesh_interval) # Customize the base tracking problem # ----------------------------------- study = track.initialize() problem = study.updProblem() problem.setTimeBounds(self.initial_time, [self.half_time-0.2, self.half_time+0.2]) # Set the initial values for the lumbar and pelvis coordinates that # produce "normal" walking motions. problem.setStateInfo('/jointset/back/lumbar_extension/value', [], -0.12) problem.setStateInfo('/jointset/back/lumbar_bending/value', [], 0) problem.setStateInfo('/jointset/back/lumbar_rotation/value', [], 0.04) problem.setStateInfo('/jointset/ground_pelvis/pelvis_tx/value', [], 0.446) problem.setStateInfo('/jointset/ground_pelvis/pelvis_tilt/value', [], 0) problem.setStateInfo('/jointset/ground_pelvis/pelvis_list/value', [], 0) problem.setStateInfo('/jointset/ground_pelvis/pelvis_rotation/value', [], 0) # Update the control effort goal to a cost of transport type cost. effort = osim.MocoControlGoal().safeDownCast( problem.updGoal('control_effort')) effort.setDivideByDisplacement(True) # Weight residual and reserve actuators low in the effort cost since # they are already weak. if config.effort_weight: for actu in model.getComponentsList(): actuName = actu.getName() if actu.getConcreteClassName().endswith('Actuator'): effort.setWeightForControl(actu.getAbsolutePathString(), 0.001) for muscle in ['psoas', 'iliacus']: for side in ['l', 'r']: effort.setWeightForControl( '/forceset/%s_%s' % (muscle, side), 0.25) speedGoal = osim.MocoAverageSpeedGoal('speed') speedGoal.set_desired_average_speed(1.235) problem.addGoal(speedGoal) # MocoFrameDistanceConstraint # --------------------------- if self.coordinate_tracking: distanceConstraint = osim.MocoFrameDistanceConstraint() distanceConstraint.setName('distance_constraint') # Step width is 0.13 * leg_length # distance = 0.10 # TODO Should be closer to 0.11. # Donelan JM, Kram R, Kuo AD. Mechanical and metabolic determinants # of the preferred step width in human walking. # Proc Biol Sci. 2001;268(1480):1985–1992. # doi:10.1098/rspb.2001.1761 # https://www.ncbi.nlm.nih.gov/pmc/articles/PMC1088839/ distanceConstraint.addFramePair( osim.MocoFrameDistanceConstraintPair( '/bodyset/calcn_l', '/bodyset/calcn_r', 0.09, np.inf)) distanceConstraint.addFramePair( osim.MocoFrameDistanceConstraintPair( '/bodyset/toes_l', '/bodyset/toes_r', 0.06, np.inf)) # distanceConstraint.addFramePair( # osim.MocoFrameDistanceConstraintPair( # '/bodyset/calcn_l', '/bodyset/toes_r', distance, np.inf)) # distanceConstraint.addFramePair( # osim.MocoFrameDistanceConstraintPair( # '/bodyset/toes_l', '/bodyset/calcn_r', distance, np.inf)) distanceConstraint.setProjection("vector") distanceConstraint.setProjectionVector(osim.Vec3(0, 0, 1)) problem.addPathConstraint(distanceConstraint) # Symmetry constraints # -------------------- statesRef = osim.TimeSeriesTable('tracking_walking_tracked_states.sto') initIndex = statesRef.getNearestRowIndexForTime(self.initial_time) symmetry = osim.MocoPeriodicityGoal('symmetry') # Symmetric coordinate values (except for pelvis_tx) and speeds. for coord in model.getComponentsList(): if not type(coord) is osim.Coordinate: continue coordName = coord.getName() coordValue = coord.getStateVariableNames().get(0) coordSpeed = coord.getStateVariableNames().get(1) if coordName.endswith('_r'): symmetry.addStatePair(osim.MocoPeriodicityGoalPair( coordValue, coordValue.replace('_r/', '_l/'))) symmetry.addStatePair(osim.MocoPeriodicityGoalPair( coordSpeed, coordSpeed.replace('_r/', '_l/'))) elif coordName.endswith('_l'): symmetry.addStatePair(osim.MocoPeriodicityGoalPair( coordValue, coordValue.replace('_l/', '_r/'))) symmetry.addStatePair(osim.MocoPeriodicityGoalPair( coordSpeed, coordSpeed.replace('_l/', '_r/'))) elif (coordName.endswith('_bending') or coordName.endswith('_rotation') or coordName.endswith('_tz') or coordName.endswith('_list')): # This does not include hip rotation, # because that ends with _l or _r. symmetry.addStatePair(osim.MocoPeriodicityGoalPair( coordValue)) symmetry.addNegatedStatePair(osim.MocoPeriodicityGoalPair( coordSpeed)) elif not coordName.endswith('_tx'): symmetry.addStatePair(osim.MocoPeriodicityGoalPair( coordValue)) symmetry.addStatePair(osim.MocoPeriodicityGoalPair( coordSpeed)) symmetry.addStatePair(osim.MocoPeriodicityGoalPair( '/jointset/ground_pelvis/pelvis_tx/speed')) # Symmetric activations. for actu in model.getComponentsList(): if (not actu.getConcreteClassName().endswith('Muscle') and not actu.getConcreteClassName().endswith('Actuator')): continue if actu.getName().endswith('_r'): symmetry.addStatePair(osim.MocoPeriodicityGoalPair( actu.getStateVariableNames().get(0), actu.getStateVariableNames().get(0).replace('_r/', '_l/'))) elif actu.getName().endswith('_l'): symmetry.addStatePair(osim.MocoPeriodicityGoalPair( actu.getStateVariableNames().get(0), actu.getStateVariableNames().get(0).replace('_l/', '_r/'))) elif (actu.getName().endswith('_bending') or actu.getName().endswith('_rotation') or actu.getName().endswith('_tz') or actu.getName().endswith('_list')): symmetry.addNegatedStatePair(osim.MocoPeriodicityGoalPair( actu.getStateVariableNames().get(0), actu.getStateVariableNames().get(0))) else: symmetry.addStatePair(osim.MocoPeriodicityGoalPair( actu.getStateVariableNames().get(0), actu.getStateVariableNames().get(0))) problem.addGoal(symmetry) # Contact tracking # ---------------- forceNamesRightFoot = ['forceset/contactSphereHeel_r', 'forceset/contactLateralRearfoot_r', 'forceset/contactLateralMidfoot_r', 'forceset/contactLateralToe_r', 'forceset/contactMedialToe_r', 'forceset/contactMedialMidfoot_r'] forceNamesLeftFoot = ['forceset/contactSphereHeel_l', 'forceset/contactLateralRearfoot_l', 'forceset/contactLateralMidfoot_l', 'forceset/contactLateralToe_l', 'forceset/contactMedialToe_l', 'forceset/contactMedialMidfoot_l'] if self.contact_tracking: contactTracking = osim.MocoContactTrackingGoal('contact', 0.001) contactTracking.setExternalLoadsFile( 'resources/Rajagopal2016/grf_walk.xml') contactTracking.addContactGroup(forceNamesRightFoot, 'Right_GRF') contactTracking.addContactGroup(forceNamesLeftFoot, 'Left_GRF') contactTracking.setProjection("plane") contactTracking.setProjectionVector(osim.Vec3(0, 0, 1)) problem.addGoal(contactTracking) # Configure the solver # -------------------- solver = osim.MocoCasADiSolver.safeDownCast(study.updSolver()) solver.resetProblem(problem) solver.set_optim_constraint_tolerance(1e-3) solver.set_optim_convergence_tolerance(1e-3) solver.set_multibody_dynamics_mode('implicit') solver.set_minimize_implicit_multibody_accelerations(True) solver.set_implicit_multibody_accelerations_weight( 1e-6 / model.getNumCoordinates()) solver.set_implicit_multibody_acceleration_bounds( osim.MocoBounds(-200, 200)) # Set the guess # ------------- # Create a guess compatible with this problem. guess = solver.createGuess() # Load the inverse problem solution and set its states and controls # trajectories to the guess. inverseSolution = osim.MocoTrajectory( os.path.join(root_dir, self.inverse_solution_relpath)) inverseStatesTable = inverseSolution.exportToStatesTable() guess.insertStatesTrajectory(inverseStatesTable, True) # Changing this initial guess has a large negative effect on the # solution! Lots of knee flexion. # guess.setState('/jointset/ground_pelvis/pelvis_ty/value', # osim.Vector(guess.getNumTimes(), 1.01)) inverseControlsTable = inverseSolution.exportToControlsTable() guess.insertControlsTrajectory(inverseControlsTable, True) solver.setGuess(guess) # Solve and print solution. # ------------------------- solution = study.solve() solution.write(self.get_solution_path(root_dir, config)) # Create a full gait cycle trajectory from the periodic solution. fullTraj = osim.createPeriodicTrajectory(solution) fullTraj.write(self.get_solution_path_fullcycle(root_dir, config)) # Compute ground reaction forces generated by contact sphere from the # full gait cycle trajectory. externalLoads = osim.createExternalLoadsTableForGait( model, fullTraj, forceNamesRightFoot, forceNamesLeftFoot) osim.writeTableToFile(externalLoads, self.get_solution_path_grfs(root_dir, config))