def __init__(self, model_path, visualize, integrator_accuracy = 5e-5):
self.integrator_accuracy = integrator_accuracy
self.model = opensim.Model(model_path)
self.model.initSystem()
self.brain = opensim.PrescribedController()
# Enable the visualizer
self.model.setUseVisualizer(visualize)
self.muscleSet = self.model.getMuscles()
self.forceSet = self.model.getForceSet()
self.bodySet = self.model.getBodySet()
self.jointSet = self.model.getJointSet()
self.markerSet = self.model.getMarkerSet()
self.contactGeometrySet = self.model.getContactGeometrySet()
if self.verbose:
self.list_elements()
# Add actuators as constant functions. Then, during simulations
# we will change levels of constants.
# One actuartor per each muscle
for j in range(self.muscleSet.getSize()):
func = opensim.Constant(1.0)
self.brain.addActuator(self.muscleSet.get(j))
self.brain.prescribeControlForActuator(j, func)
self.maxforces.append(self.muscleSet.get(j).getMaxIsometricForce())
self.curforces.append(1.0)
self.noutput = self.muscleSet.getSize()
self.model.addController(self.brain)
self.model.initSystem()
def test(model_path, visualize):
model = opensim.Model(model_path)
brain = opensim.PrescribedController()
model.addController(brain)
state = model.initSystem()
muscleSet = model.getMuscles()
for j in range(muscleSet.getSize()):
brain.addActuator(muscleSet.get(j))
func = opensim.Constant(1.0)
brain.prescribeControlForActuator(j, func)
block = opensim.Body('block', 0.0001, opensim.Vec3(0),
opensim.Inertia(1, 1, .0001, 0, 0, 0))
model.addComponent(block)
pj = opensim.PlanarJoint(
'pin',
model.getGround(), # PhysicalFrame
opensim.Vec3(0, 0, 0),
opensim.Vec3(0, 0, 0),
block, # PhysicalFrame
opensim.Vec3(0, 0, 0),
opensim.Vec3(0, 0, 0))
model.addComponent(pj)
model.initSystem()
pj.getCoordinate(1)
def test_PrescribedController_prescribeControlForActuator(self):
# Test memory management for
# PrescribedController::prescribeControlForActuator().
model = osim.Model()
# Body.
body = osim.Body('b1', 1.0, osim.Vec3(0, 0, 0), osim.Inertia(0, 0, 0))
model.addBody(body)
# Joint.
joint = osim.PinJoint('j1', model.getGround(), body)
model.addJoint(joint)
# Actuator.
actu = osim.CoordinateActuator()
actu.setName('actu')
actu.setCoordinate(joint.get_coordinates(0))
model.addForce(actu)
# Controller.
contr = osim.PrescribedController()
contr.addActuator(actu)
self.assertRaises(RuntimeError, contr.prescribeControlForActuator, 1,
osim.Constant(3))
# The following calls should not cause a memory leak:
contr.prescribeControlForActuator(0, osim.Constant(2))
contr.prescribeControlForActuator('actu', osim.Constant(4))
def test_set_iterator(self):
fs = osim.FunctionSet()
f1 = osim.Constant()
f1.setName("myfunc1")
fs.adoptAndAppend(f1)
f2 = osim.Constant()
f2.setName("myfunc2")
fs.adoptAndAppend(f2)
f3 = osim.Constant()
f3.setName("myfunc3")
fs.adoptAndAppend(f3)
names = ['myfunc1', 'myfunc2', 'myfunc3']
i = 0
for func in fs:
assert func.getName() == names[i]
i += 1
# Test key-value iterator.
j = 0
for k, v in fs.items():
assert k == names[j]
assert k == v.getName()
j += 1
def __init__(self, model_path, k_paths_dict, visualize, integrator_accuracy = 1e-3): # Reduced accuracy for greater speed
self.integrator_accuracy = integrator_accuracy
self.model = opensim.Model(model_path)
self.model.initSystem()
self.brain = opensim.PrescribedController()
self.k_path = k_paths_dict[list(k_paths_dict.keys())[0]]
self.states = pd.read_csv(self.k_path, index_col=False).drop('Unnamed: 0', axis=1)
for speed in list(k_paths_dict.keys()):
self.states_trajectories_dict[speed] = pd.read_csv(k_paths_dict[speed], index_col=False).drop('Unnamed: 0', axis=1)
# Enable the visualizer
self.model.setUseVisualizer(visualize)
self.muscleSet = self.model.getMuscles()
self.forceSet = self.model.getForceSet()
self.bodySet = self.model.getBodySet()
self.jointSet = self.model.getJointSet()
self.markerSet = self.model.getMarkerSet()
self.contactGeometrySet = self.model.getContactGeometrySet()
self.actuatorSet = self.model.getActuators()
if self.verbose:
self.list_elements()
for j in range(self.actuatorSet.getSize()):
func = opensim.Constant(1.0)
self.brain.addActuator(self.actuatorSet.get(j))
self.brain.prescribeControlForActuator(j, func)
self.curforces.append(1.0)
try:
self.maxforces.append(self.muscleSet.get(j).getMaxIsometricForce())
except:
self.maxforces.append(np.inf)
self.noutput = self.actuatorSet.getSize()
self.model.addController(self.brain)
self.model.initSystem()
def __init__(self, model_path, visualize):
self.model = opensim.Model(model_path)
self.model.initSystem()
self.brain = opensim.PrescribedController()
# Enable the visualizer
self.model.setUseVisualizer(visualize)
self.muscleSet = self.model.getMuscles()
self.forceSet = self.model.getForceSet()
self.bodySet = self.model.getBodySet()
self.jointSet = self.model.getJointSet()
self.contactGeometrySet = self.model.getContactGeometrySet()
for j in range(self.muscleSet.getSize()):
func = opensim.Constant(1.0)
self.brain.addActuator(self.muscleSet.get(j))
self.brain.prescribeControlForActuator(j, func)
self.maxforces.append(self.muscleSet.get(j).getMaxIsometricForce())
self.curforces.append(1.0)
self.model.addController(self.brain)
def test_markAdoptedSets():
# Set's.
fus = osim.FunctionSet()
fu1 = osim.Constant()
fus.adoptAndAppend(fu1)
del fus
del fu1
s = osim.ScaleSet()
o = osim.Scale()
s.adoptAndAppend(o)
del s
del o
s = osim.ControlSet()
o = osim.ControlLinear()
s.adoptAndAppend(o)
del s
del o
s = osim.BodyScaleSet()
o = osim.BodyScale()
s.adoptAndAppend(o)
del s
del o
s = osim.PathPointSet()
o = osim.PathPoint()
s.adoptAndAppend(o)
del s
del o
s = osim.IKTaskSet()
o = osim.IKMarkerTask()
s.adoptAndAppend(o)
del s
del o
s = osim.MarkerPairSet()
o = osim.MarkerPair()
s.adoptAndAppend(o)
del s
del o
s = osim.MeasurementSet()
o = osim.Measurement()
s.adoptAndAppend(o)
del s
del o
s = osim.FrameSet()
o = osim.Body()
s.adoptAndAppend(o)
del s
del o
s = osim.ForceSet()
o = osim.CoordinateLimitForce()
s.adoptAndAppend(o)
del s
del o
s = osim.ForceSet()
o = osim.SpringGeneralizedForce()
s.append(o)
s = osim.ProbeSet()
o = osim.Umberger2010MuscleMetabolicsProbe()
s.adoptAndAppend(o)
del s
del o
a = osim.Model()
body = osim.Body('body',
1.0,
osim.Vec3(0, 0, 0),
osim.Inertia(0, 0, 0)
)
loc_in_parent = osim.Vec3(0, -0, 0)
orient_in_parent = osim.Vec3(0, 0, 0)
loc_in_body = osim.Vec3(0, 0, 0)
orient_in_body = osim.Vec3(0, 0, 0)
joint = osim.WeldJoint("weld_joint",
a.getGround(),
loc_in_parent, orient_in_parent,
body,
loc_in_body, orient_in_parent)
a.addBody(body)
constr = osim.ConstantDistanceConstraint()
constr.setBody1ByName("ground")
constr.setBody1PointLocation(osim.Vec3(0, 0, 0))
constr.setBody2ByName("body")
constr.setBody2PointLocation(osim.Vec3(1, 0, 0))
constr.setConstantDistance(1)
a.addConstraint(constr)
def test_markAdoptedSets():
# Set's.
fus = osim.FunctionSet()
fu1 = osim.Constant()
fus.adoptAndAppend(fu1)
del fus
del fu1
gs = osim.GeometrySet()
dg = osim.DisplayGeometry()
gs.adoptAndAppend(dg)
del gs
del dg
s = osim.ScaleSet()
o = osim.Scale()
s.adoptAndAppend(o)
del s
del o
s = osim.ForceSet()
o = osim.BushingForce()
s.adoptAndAppend(o)
del s
del o
cs = osim.ControllerSet()
csc = osim.PrescribedController()
cs.adoptAndAppend(csc)
del cs
del csc
s = osim.ContactGeometrySet()
o = osim.ContactHalfSpace()
s.adoptAndAppend(o)
del s
del o
s = osim.AnalysisSet()
o = osim.MuscleAnalysis()
s.adoptAndAppend(o)
del s
del o
s = osim.ControlSet()
o = osim.ControlLinear()
s.adoptAndAppend(o)
del s
del o
s = osim.MarkerSet()
o = osim.Marker()
s.adoptAndAppend(o)
del s
del o
s = osim.BodySet()
o = osim.Body()
s.adoptAndAppend(o)
del s
del o
s = osim.BodyScaleSet()
o = osim.BodyScale()
s.adoptAndAppend(o)
del s
del o
s = osim.CoordinateSet()
o = osim.Coordinate()
s.adoptAndAppend(o)
del s
del o
s = osim.JointSet()
o = osim.BallJoint()
s.adoptAndAppend(o)
del s
del o
s = osim.PathPointSet()
o = osim.PathPoint()
s.adoptAndAppend(o)
del s
del o
s = osim.IKTaskSet()
o = osim.IKMarkerTask()
s.adoptAndAppend(o)
del s
del o
s = osim.MarkerPairSet()
o = osim.MarkerPair()
s.adoptAndAppend(o)
del s
del o
s = osim.MeasurementSet()
o = osim.Measurement()
s.adoptAndAppend(o)
del s
del o
# TODO
# s = osim.ProbeSet()
# o = osim.Umberger2010MuscleMetabolicsProbe()
# s.adoptAndAppend(o)
# del s
# del o
s = osim.ConstraintSet()
a = osim.Model()
body = osim.Body('body', 1.0, osim.Vec3(0, 0, 0), osim.Inertia(0, 0, 0))
loc_in_parent = osim.Vec3(0, -0, 0)
orient_in_parent = osim.Vec3(0, 0, 0)
loc_in_body = osim.Vec3(0, 0, 0)
orient_in_body = osim.Vec3(0, 0, 0)
joint = osim.WeldJoint("weld_joint", a.getGroundBody(), loc_in_parent,
orient_in_parent, body, loc_in_body,
orient_in_parent)
a.addBody(body)
constr = osim.ConstantDistanceConstraint()
constr.setBody1ByName("ground")
constr.setBody1PointLocation(osim.Vec3(0, 0, 0))
constr.setBody2ByName("body")
constr.setBody2PointLocation(osim.Vec3(1, 0, 0))
constr.setConstantDistance(1)
s.adoptAndAppend(constr)
model_path = "../osim/models/gait9dof18musc.osim"
model = opensim.Model(model_path)
model.setUseVisualizer(True)
# Build the controller (we will iteratively
# update it at every step of the simulation)
brain = opensim.PrescribedController()
controllers = []
# Add actuators to the controller
state = model.initSystem() # we need to initialize the system (?)
muscleSet = model.getMuscles()
forceSet = model.getForceSet()
for j in range(muscleSet.getSize()):
func = opensim.Constant(1.0)
controllers.append(func)
brain.addActuator(muscleSet.get(j))
brain.prescribeControlForActuator(j, func)
model.addController(brain)
blockos = opensim.Body('blockos', 0.0001, opensim.Vec3(0),
opensim.Inertia(1, 1, .0001, 0, 0, 0))
pj = opensim.PinJoint(
"pinblock",
model.getGround(), # PhysicalFrame
opensim.Vec3(-0.5, 0, 0),
opensim.Vec3(0, 0, 0),
blockos, # PhysicalFrame
opensim.Vec3(0, 0, 0),
def __init__(self, model_path, visualize, integrator_accuracy = 5e-8, integrator_MaxStepSize = 1e-1, Perturbtime = 50): #Shakiba
self.integrator_accuracy = integrator_accuracy
self.integrator_MaxStepSize = integrator_MaxStepSize #Shakiba
self.model = opensim.Model(model_path)
self.model_state = self.model.initSystem()
self.brain = opensim.PrescribedController()
# Enable the visualizer
self.model.setUseVisualizer(visualize)
self.muscleSet = self.model.getMuscles()
self.forceSet = self.model.getForceSet()
self.bodySet = self.model.getBodySet()
self.jointSet = self.model.getJointSet()
self.markerSet = self.model.getMarkerSet()
self.contactGeometrySet = self.model.getContactGeometrySet()
if self.verbose:
self.list_elements()
# Add actuators as constant functions. Then, during simulations
# we will change levels of constants.
# One actuartor per each muscle
for j in range(self.muscleSet.getSize()):
func = opensim.Constant(1.0)
self.brain.addActuator(self.muscleSet.get(j))
self.brain.prescribeControlForActuator(j, func)
self.maxforces.append(self.muscleSet.get(j).getMaxIsometricForce())
self.curforces.append(1.0)
# Shakiba
# Add external forces as constant functions. Then, during simulations
# we will change levels of constants.
# One actuartor per each ankle
PerturbSites = ['talus_r', 'talus_l'] # perturbation site
external_force = {} # external forces dictionary
for j in PerturbSites:
external_force[j] = opensim.PrescribedForce()
sim_t = 50
sim_dt = self.stepsize
t_pert = np.arange(0, sim_t+sim_dt, sim_dt)
# construct a spline for fx
perturb = opensim.SimmSpline()
sigma = 0.005
miu = Perturbtime
#miu = 8.1
#fx = 0.1*(((1/(sigma*np.sqrt(2*np.pi)))*np.exp(-((t_pert-miu)**2)/(2*sigma**2)))/2) # weighted Gaussian function divided by 2 to account for each leg
fx = 0.5*(((1/(sigma*np.sqrt(2*np.pi)))*np.exp(-((t_pert-miu)**2)/(2*sigma**2)))/2) # weighted Gaussian function divided by 2 to account for each leg
for i in range(t_pert.shape[0]):
perturb.addPoint(t_pert[i], fx[i])
for j in PerturbSites:
#make sure to add the absolute path of the body frame
external_force[j].setBodyName('/bodyset/{}'.format(j))
#define the point of application in body fixed frame
external_force[j].setPointFunctions(opensim.Constant(0),
opensim.Constant(0),
opensim.Constant(0))
#apply external forces
external_force[j].set_forceIsGlobal(True)
external_force[j].setForceFunctions(perturb, opensim.Constant(0), opensim.Constant(0))
self.model.addForce(external_force[j])
#
self.noutput = self.muscleSet.getSize()
self.model.addController(self.brain)
self.model_state = self.model.initSystem()
def _populate_references(self):
# Initialize objects needed to populate the references.
coord_functions = opensim.FunctionSet()
marker_weights = opensim.SetMarkerWeights()
# Load coordinate data, if available.
if self.coordinate_file and (self.coordinate_file != ""
and self.coordinate_file != "Unassigned"):
coordinate_values = opensim.Storage(self.coordinate_file)
# Convert to radians, if in degrees.
if not coordinate_values.isInDegrees():
self.model.getSimbodyEngine().convertDegreesToRadians(
coordinate_values)
coord_functions = opensim.GCVSplineSet(5, coordinate_values)
index = 0
for i in range(0, self.ik_task_set.getSize()):
if not self.ik_task_set.get(i).getApply():
continue
if opensim.IKCoordinateTask.safeDownCast(self.ik_task_set.get(i)):
# NOTE: Opposed to C++, a variable cannot be declared in the above if statement, so do it now.
coord_task = opensim.IKCoordinateTask.safeDownCast(
self.ik_task_set.get(i))
coord_ref = opensim.CoordinateReference()
if coord_task.getValueType(
) == opensim.IKCoordinateTask.FromFile:
if not coord_functions:
raise Exception(
"InverseKinematicsTool: value for coordinate " +
coord_task.getName() + " not found.")
index = coord_functions.getIndex(coord_task.getName(),
index)
if index >= 0:
coord_ref = opensim.CoordinateReference(
coord_task.getName(), coord_functions.get(index))
elif coord_task.getValueType(
) == opensim.IKCoordinateTask.ManualValue:
reference = opensim.Constant(
opensim.Constant(coord_task.getValue()))
coord_ref = opensim.CoordinateReference(
coord_task.getName(), reference)
else: # Assume it should be held at its default value
value = self.model.getCoordinateSet().get(
coord_task.getName()).getDefaultValue()
reference = opensim.Constant(value)
coord_ref = opensim.CoordinateReference(
coord_task.getName(), reference)
if not coord_ref:
raise Exception(
"InverseKinematicsTool: value for coordinate " +
coord_task.getName() + " not found.")
else:
coord_ref.setWeight(coord_task.getWeight())
self.coordinate_references.push_back(coord_ref)
elif opensim.IKMarkerTask.safeDownCast(self.ik_task_set.get(i)):
# NOTE: Opposed to C++, a variable cannot be declared in the above if statement, so do it now.
marker_task = opensim.IKMarkerTask.safeDownCast(
self.ik_task_set.get(i))
if marker_task.getApply():
# Only track markers that have a task and it is "applied"
marker_weights.cloneAndAppend(
opensim.MarkerWeight(marker_task.getName(),
marker_task.getWeight()))
self.markers_reference.initializeFromMarkersFile(
self.marker_file, marker_weights)
# This script demonstrates how one can apply a prescribed body force on a body. # # author Dimitar Stanev [email protected] import numpy as np import opensim model = opensim.Model('tug_of_war.osim') # create the prescribed force object prescribed_force = opensim.PrescribedForce() # make sure to add the absolute path of the body frame prescribed_force.setBodyName('/bodyset/block') # define the point of application in body fixed frame prescribed_force.setPointFunctions(opensim.Constant(0), opensim.Constant(0), opensim.Constant(0)) # construct a spline for the z-component of the force fz = opensim.SimmSpline() t = np.linspace(0, 1, 10, endpoint=True) y = 100 * np.sin(t) for i in range(t.shape[0]): fz.addPoint(t[i], y[i]) prescribed_force.setForceFunctions(opensim.Constant(0), opensim.Constant(0), fz) model.addForce(prescribed_force) # simulate state = model.initSystem()
