def track(self, prediction_solution, exponent=2): track = osim.MocoTrack() track.setName('suspended_mass_tracking') track.setModel(osim.ModelProcessor(self.build_model())) track.setStatesReference( osim.TableProcessor(prediction_solution.exportToStatesTable())) track.set_states_global_tracking_weight(10.0) track.set_allow_unused_references(True) # track.set_control_effort_weight(1.0) track.set_mesh_interval(0.003) study = track.initialize() problem = study.updProblem() problem.setStateInfo("/jointset/tx/tx/value", [-self.width, self.width], self.xinit) problem.setStateInfo("/jointset/ty/ty/value", [-2 * self.width, 0], self.yinit) problem.setStateInfo("/forceset/left/activation", [0, 1], 0) problem.setStateInfo("/forceset/middle/activation", [0, 1], 0) problem.setStateInfo("/forceset/right/activation", [0, 1], 0) problem.updPhase().setDefaultSpeedBounds(osim.MocoBounds(-15, 15)) tracking = osim.MocoStateTrackingGoal.safeDownCast( problem.updGoal('state_tracking')) tracking.setPattern('.*(value|speed)$') effort = osim.MocoControlGoal.safeDownCast( problem.updGoal('control_effort')) effort.setExponent(exponent) solver = osim.MocoCasADiSolver.safeDownCast(study.updSolver()) solver.set_optim_convergence_tolerance(-1) solver.set_optim_constraint_tolerance(-1) solution = study.solve() return solution
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)
independentCoords.append('tx') constraint.setIndependentCoordinateNames(independentCoords) constraint.setDependentCoordinateName('ty') coefficients = osim.Vector(3, 0) # 3 elements initialized to 0. # The polynomial is c0*tx^2 + c1*tx + c2; set c0 = 1, c1 = c2 = 0. coefficients.set(0, 1) constraint.setFunction(osim.PolynomialFunction(coefficients)) model.addConstraint(constraint) study = osim.MocoStudy() problem = study.updProblem() problem.setModel(model) phase0 = problem.getPhase(0) # Setting stricter bounds for the speeds improves convergence. phase0.setDefaultSpeedBounds(osim.MocoBounds(-5, 5)) problem.setTimeBounds(0, 0.8) # Start the motion at (-1, 1). problem.setStateInfo('/jointset/tx/tx/value', [-2, 2], -1.0) problem.setStateInfo('/jointset/ty/ty/value', [-2, 2], 1.0) solver = study.initCasADiSolver() solution = study.solve() # If matplotlib is installed, plot the trajectory of the mass and the # constraint force applied to the mass throughout the motion. has_pylab = False try: import pylab as pl from scipy.interpolate import InterpolatedUnivariateSpline
# =================================== problem = moco.updProblem() # Model (dynamics). # ----------------- problem.setModel(model) # Bounds. # ------- # Initial time must be 0, final time is finalTime. problem.setTimeBounds(osim.MocoInitialBounds(0.), osim.MocoFinalBounds(finalTime)) # Position must be within [-5, 5] throughout the motion. # Initial position must be -0.5, final position must be within [0.25, 0.75]. problem.setStateInfo('/slider/position/value', osim.MocoBounds(-5., 5.), osim.MocoInitialBounds(-0.5), osim.MocoFinalBounds(0.25, 0.75)) # Speed must be within [-20, 20] throughout the motion. # Initial and final speed must be 0. Use compact syntax. problem.setStateInfo('/slider/position/speed', [-20, 20], [0], [0]) # Add Parameter. The default initial guess for a parameter is the midpoint of # its bounds, *not* the value of a property in the model. problem.addParameter( osim.MocoParameter('oscillator_mass', 'body', 'mass', osim.MocoBounds(0, 10))) # Cost. # -----
problem = moco.updProblem() #Set the time bounds (from kinematics file) problem.setTimeBounds(osim.MocoInitialBounds(3.572), osim.MocoFinalBounds(6.044)) #Set kinematic bounds (mins and maxes) #Shoulder elevation charValue = '/jointset/' + coordSet.get('shoulder_elv').getJoint().getName( ) + '/' + coordSet.get('shoulder_elv').getName() + '/value' charSpeed = '/jointset/' + coordSet.get('shoulder_elv').getJoint().getName( ) + '/' + coordSet.get('shoulder_elv').getName() + '/speed' mn = coordSet.get('shoulder_elv').getRangeMin() mx = coordSet.get('shoulder_elv').getRangeMax() problem.setStateInfo(charValue, osim.MocoBounds(mn, mx)) problem.setStateInfo(charSpeed, osim.MocoBounds(-50, 50)) del (mn, mx, charValue, charSpeed) #Shoulder rotation charValue = '/jointset/' + coordSet.get('shoulder_rot').getJoint().getName( ) + '/' + coordSet.get('shoulder_rot').getName() + '/value' charSpeed = '/jointset/' + coordSet.get('shoulder_rot').getJoint().getName( ) + '/' + coordSet.get('shoulder_rot').getName() + '/speed' mn = coordSet.get('shoulder_rot').getRangeMin() mx = coordSet.get('shoulder_rot').getRangeMax() problem.setStateInfo(charValue, osim.MocoBounds(mn, mx)) problem.setStateInfo(charSpeed, osim.MocoBounds(-50, 50)) del (mn, mx, charValue, charSpeed) #Elevation plane
# Define the optimal control problem. # =================================== problem = study.updProblem() # Model (dynamics). # ----------------- problem.setModel(model) # Bounds. # ------- # Initial time must be 0, final time can be within [0, 5]. problem.setTimeBounds(osim.MocoInitialBounds(0.), osim.MocoFinalBounds(0., 5.)) # Position must be within [-5, 5] throughout the motion. # Initial position must be 0, final position must be 1. problem.setStateInfo('/slider/position/value', osim.MocoBounds(-5, 5), osim.MocoInitialBounds(0), osim.MocoFinalBounds(1)) # Speed must be within [-50, 50] throughout the motion. # Initial and final speed must be 0. Use compact syntax. problem.setStateInfo('/slider/position/speed', [-50, 50], [0], [0]) # Applied force must be between -50 and 50. problem.setControlInfo('/actuator', osim.MocoBounds(-50, 50)) # Cost. # ----- problem.addGoal(osim.MocoFinalTimeGoal()) # Configure the solver. # ===================== solver = study.initCasADiSolver()
def test_bounds(self): model = osim.Model() model.setName('sliding_mass') model.set_gravity(osim.Vec3(0, 0, 0)) body = osim.Body('body', 2.0, osim.Vec3(0), osim.Inertia(0)) model.addComponent(body) joint = osim.SliderJoint('slider', model.getGround(), body) coord = joint.updCoordinate() coord.setName('position') model.addComponent(joint) actu = osim.CoordinateActuator() actu.setCoordinate(coord) actu.setName('actuator') actu.setOptimalForce(1) model.addComponent(actu) study = osim.MocoStudy() study.setName('sliding_mass') mp = study.updProblem() mp.setModel(model) ph0 = mp.getPhase() mp.setTimeBounds(osim.MocoInitialBounds(0.), osim.MocoFinalBounds(0.1, 5.)) assert ph0.getTimeInitialBounds().getLower() == 0 assert ph0.getTimeInitialBounds().getUpper() == 0 self.assertAlmostEqual(ph0.getTimeFinalBounds().getLower(), 0.1) self.assertAlmostEqual(ph0.getTimeFinalBounds().getUpper(), 5.0) mp.setTimeBounds([0.2, 0.3], [3.5]) self.assertAlmostEqual(ph0.getTimeInitialBounds().getLower(), 0.2) self.assertAlmostEqual(ph0.getTimeInitialBounds().getUpper(), 0.3) self.assertAlmostEqual(ph0.getTimeFinalBounds().getLower(), 3.5) self.assertAlmostEqual(ph0.getTimeFinalBounds().getUpper(), 3.5) # Use setter on MocoPhase. ph0.setTimeBounds([2.2, 2.3], [4.5]) self.assertAlmostEqual(ph0.getTimeInitialBounds().getLower(), 2.2) self.assertAlmostEqual(ph0.getTimeInitialBounds().getUpper(), 2.3) self.assertAlmostEqual(ph0.getTimeFinalBounds().getLower(), 4.5) self.assertAlmostEqual(ph0.getTimeFinalBounds().getUpper(), 4.5) mp.setStateInfo('slider/position/value', osim.MocoBounds(-5, 5), osim.MocoInitialBounds(0)) assert -5 == ph0.getStateInfo( 'slider/position/value').getBounds().getLower() assert 5 == ph0.getStateInfo( 'slider/position/value').getBounds().getUpper() assert isnan( ph0.getStateInfo( 'slider/position/value').getFinalBounds().getLower()) assert isnan( ph0.getStateInfo( 'slider/position/value').getFinalBounds().getUpper()) mp.setStateInfo('slider/position/speed', [-50, 50], [-3], 1.5) assert -50 == ph0.getStateInfo( 'slider/position/speed').getBounds().getLower() assert 50 == ph0.getStateInfo( 'slider/position/speed').getBounds().getUpper() assert -3 == ph0.getStateInfo( 'slider/position/speed').getInitialBounds().getLower() assert -3 == ph0.getStateInfo( 'slider/position/speed').getInitialBounds().getUpper() self.assertAlmostEqual( 1.5, ph0.getStateInfo( 'slider/position/speed').getFinalBounds().getLower()) self.assertAlmostEqual( 1.5, ph0.getStateInfo( 'slider/position/speed').getFinalBounds().getUpper()) # Use setter on MocoPhase. ph0.setStateInfo('slider/position/speed', [-6, 10], [-4, 3], [0]) assert -6 == ph0.getStateInfo( 'slider/position/speed').getBounds().getLower() assert 10 == ph0.getStateInfo( 'slider/position/speed').getBounds().getUpper() assert -4 == ph0.getStateInfo( 'slider/position/speed').getInitialBounds().getLower() assert 3 == ph0.getStateInfo( 'slider/position/speed').getInitialBounds().getUpper() assert 0 == ph0.getStateInfo( 'slider/position/speed').getFinalBounds().getLower() assert 0 == ph0.getStateInfo( 'slider/position/speed').getFinalBounds().getUpper() # Controls. mp.setControlInfo('actuator', osim.MocoBounds(-50, 50)) assert -50 == ph0.getControlInfo('actuator').getBounds().getLower() assert 50 == ph0.getControlInfo('actuator').getBounds().getUpper() mp.setControlInfo('actuator', [18]) assert 18 == ph0.getControlInfo('actuator').getBounds().getLower() assert 18 == ph0.getControlInfo('actuator').getBounds().getUpper()
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))