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.
# -----
endpointCost = osim.MocoMarkerFinalGoal()
endpointCost.setPointName('/markerset/marker')
endpointCost.setReferenceLocation(osim.Vec3(0.5, 0, 0))
problem.addGoal(endpointCost)
# Configure the solver.
# =====================
solver = moco.initTropterSolver()
# Now that we've finished setting up the study, print it to a file.
moco.printToXML('optimize_mass.omoco')
# Solve the problem.
# ==================
solution = moco.solve()
solution.write('optimize_mass_solution.sto')
def solvePrediction():
# Predict the optimal trajectory for a minimum time swing-up.
#
# o
# |
# o
# |
# +---o---o +
#
# iniital pose final pose
#
study = osim.MocoStudy()
study.setName("double_pendulum_predict")
problem = study.updProblem()
# Model (dynamics).
problem.setModel(createDoublePendulumModel())
# Bounds.
problem.setTimeBounds(0, [0, 5])
# Arguments are name, [lower bound, upper bound],
# initial [lower bound, upper bound],
# final [lower bound, upper bound].
problem.setStateInfo("/jointset/j0/q0/value", [-10, 10], 0)
problem.setStateInfo("/jointset/j0/q0/speed", [-50, 50], 0, 0)
problem.setStateInfo("/jointset/j1/q1/value", [-10, 10], 0)
problem.setStateInfo("/jointset/j1/q1/speed", [-50, 50], 0, 0)
problem.setControlInfo("/tau0", [-100, 100])
problem.setControlInfo("/tau1", [-100, 100])
# Cost: minimize final time and error from desired
# end effector position.
ftCost = osim.MocoFinalTimeGoal()
ftCost.setWeight(0.001)
problem.addGoal(ftCost)
finalCost = osim.MocoMarkerFinalGoal()
finalCost.setName("final")
finalCost.setWeight(1000.0)
finalCost.setPointName("/markerset/m1")
finalCost.setReferenceLocation(osim.Vec3(0, 2, 0))
problem.addGoal(finalCost)
# Configure the solver.
solver = study.initTropterSolver()
solver.set_num_mesh_intervals(100)
solver.set_verbosity(2)
solver.set_optim_solver("ipopt")
guess = solver.createGuess()
guess.setNumTimes(2)
guess.setTime([0, 1])
guess.setState("/jointset/j0/q0/value", [0, -math.pi])
guess.setState("/jointset/j1/q1/value", [0, 2 * math.pi])
guess.setState("/jointset/j0/q0/speed", [0, 0])
guess.setState("/jointset/j1/q1/speed", [0, 0])
guess.setControl("/tau0", [0, 0])
guess.setControl("/tau1", [0, 0])
guess.resampleWithNumTimes(10)
solver.setGuess(guess)
# Save the problem to a setup file for reference.
study.printToXML("examplePredictAndTrack_predict.omoco")
# Solve the problem.
solution = study.solve()
solution.write("examplePredictAndTrack_predict_solution.sto")
if visualize:
study.visualize(solution)
return solution
def addMarkerEndPoints(taskName = None, mocoProblem = None, modelObject = None):
# Convenience function for adding a coordinate actuator to model
#
# Input: taskName - string of relevant task name options
# mocoProblem - MocoProblem object to add goals to
# modelObject - Opensim model object to add actuator to
#Check for appropriate inputs
if taskName is None or mocoProblem is None or modelObject is None:
raise ValueError('All three input arguments are needed for addMarkerEndPoints function')
if taskName == 'ConcentricUpwardReach105':
#Get the desired end point of the movement. This will be at a point 15
#degrees above the shoulder at a distance of 200% of forearm length.
#(note there is no prescribed distance in the Vidt paper)
#Get the position of the shoulder joint centre. Note that the 1 corresponds
#to the humphant_offset frame. This command also transforms it to the
#ground frame.
modelObject_state = modelObject.initSystem()
SJC_ground = modelObject.getJointSet().get('shoulder0').get_frames(1).getPositionInGround(modelObject_state)
#Calculate the distance of the forearm (i.e. between the elbow and wrist
#joint centre).
#Get the position of the joint centres. Joint 1 corresponds to ulna offset
#frame for elbow and joint 0 the radius offset for the radius hand joint
EJC_ground = modelObject.getJointSet().get('elbow').get_frames(1).getPositionInGround(modelObject_state)
WJC_ground = modelObject.getJointSet().get('radius_hand_r').get_frames(0).getPositionInGround(modelObject_state)
#Calculate the distance between the joint centres
elbow = [EJC_ground.get(0),EJC_ground.get(1),EJC_ground.get(2)]
wrist = [WJC_ground.get(0),WJC_ground.get(1),WJC_ground.get(2)]
FA_length = (((wrist[0]-elbow[0])**2) + ((wrist[1]-elbow[1])**2) + ((wrist[2]-elbow[2])**2))**0.5
#Calculate the position 200% forearm length in front of the shoulder. In
#front is represented by positive X
inFrontPoint = [SJC_ground.get(0)+(FA_length*2),SJC_ground.get(1),SJC_ground.get(2)]
#Calculate how far above this point is needed to generate a 15 degree angle
#above the level of the shoulder joint. Calculate this using a 2D triangle
#encompassing the X and Y axis
#Calculate horizontal distance from shoulder to in front point
Xdist = (FA_length*2) - SJC_ground.get(0)
#Set angle to calculate height with
theta = math.radians(15)
#Calculate height of triangle
Ydist = math.tan(theta) * Xdist
#Prescribe upward reach point
upwardReachPoint = [inFrontPoint[0],inFrontPoint[1]+Ydist,inFrontPoint[2]]
#Create a marker end point cost for the reach position. Need to use the
#markers on both sides of the wrist and the top of the hand to ensure that
#the hand is placed level and palmar side down at the end - as such, need
#to create markers end points for each of these.
#Identify the distance between the two wrist markers
RS = modelObject.getMarkerSet().get('RS').getLocationInGround(modelObject_state)
US = modelObject.getMarkerSet().get('US').getLocationInGround(modelObject_state)
RS = [RS.get(0),RS.get(1),RS.get(2)]
US = [US.get(0),US.get(1),US.get(2)]
wristWidth = (((RS[0]-US[0])**2) + ((RS[1]-US[1])**2) + ((RS[2]-US[2])**2))**0.5
#Add and subtract half of the wrist distance from the original marker end
#point along the Z-axis to get the proposed end points for the markers. It
#is positive Z in the ground frame for the ulna marker and negative Z for
#the radius marker
US_endLoc = osim.Vec3(upwardReachPoint[0],upwardReachPoint[1],upwardReachPoint[2]+(wristWidth/2))
RS_endLoc = osim.Vec3(upwardReachPoint[0],upwardReachPoint[1],upwardReachPoint[2]-(wristWidth/2))
#Measure the distance from the wrist joint centre to the wri_out marker for
#prescribing where the hand needs to go.
wri_out = modelObject.getMarkerSet().get('wri_out').getLocationInGround(modelObject_state)
wri_out = [wri_out.get(0),wri_out.get(1),wri_out.get(2)]
wrist = [WJC_ground.get(0),WJC_ground.get(1),WJC_ground.get(2)]
wristHeight = (((wri_out[0]-wrist[0])**2) + ((wri_out[1]-wrist[1])**2) + ((wri_out[2]-wrist[2])**2))**0.5
#Add the wirst height amount along the y-axis from the proposed reach point
#to get the point where the wri_out marker needs to go
W_endLoc = osim.Vec3(upwardReachPoint[0],upwardReachPoint[1]+wristHeight,upwardReachPoint[2])
#Add the end point costs equally weighted to contribute 50# to the problem
endPointCost1 = osim.MocoMarkerFinalGoal('RS_endPoint',5)
endPointCost1.setPointName('/markerset/RS')
endPointCost1.setReferenceLocation(RS_endLoc)
endPointCost2 = osim.MocoMarkerFinalGoal('US_endPoint',5)
endPointCost2.setPointName('/markerset/US')
endPointCost2.setReferenceLocation(US_endLoc)
endPointCost3 = osim.MocoMarkerFinalGoal('W_endPoint',5)
endPointCost3.setPointName('/markerset/wri_out')
endPointCost3.setReferenceLocation(W_endLoc)
#Add the end point cost along with an effort cost.
mocoProblem.addGoal(endPointCost1)
mocoProblem.addGoal(endPointCost2)
mocoProblem.addGoal(endPointCost3)