def test_connecting(self):
# We'll create a model from scratch and set up its joints with
# the socket interface.
model = osim.Model()
b1 = osim.Body("b1", 1, osim.Vec3(1), osim.Inertia(1))
b2 = osim.Body("b2", 2, osim.Vec3(1), osim.Inertia(1))
j1 = osim.PinJoint()
j1.setName("j1")
j1.updSocket("parent_frame").connect(model.getGround())
j1.connectSocket_child_frame(b1)
j2 = osim.PinJoint()
j2.setName("j2")
j2.connectSocket_parent_frame(b1)
j2.updSocket("child_frame").connect(b2)
model.addBody(b1)
model.addBody(b2)
model.addJoint(j1)
model.addJoint(j2)
state = model.initSystem()
# Check that the connectees point to the correct object.
assert j1.getConnectee("parent_frame").this == model.getGround().this
assert j1.getConnectee("child_frame").this == b1.this
assert j2.getConnectee("parent_frame").this == b1.this
assert j2.getConnectee("child_frame").this == b2.this
# Make sure we can call methods of the concrete connectee type
# (that the downcast succeeded).
assert j1.getConnectee("child_frame").getMass() == 1
assert j2.getConnectee("child_frame").getMass() == 2
def createDoublePendulumModel():
model = osim.Model()
model.setName("double_pendulum")
# Create two links, each with a mass of 1 kg, center of mass at the body's
# origin, and moments and products of inertia of zero.
b0 = osim.Body("b0", 1, osim.Vec3(0), osim.Inertia(1))
model.addBody(b0)
b1 = osim.Body("b1", 1, osim.Vec3(0), osim.Inertia(1))
model.addBody(b1)
# Add markers to body origin locations.
m0 = osim.Marker("m0", b0, osim.Vec3(0))
m1 = osim.Marker("m1", b1, osim.Vec3(0))
model.addMarker(m0)
model.addMarker(m1)
# Connect the bodies with pin joints. Assume each body is 1 m long.
j0 = osim.PinJoint("j0", model.getGround(), osim.Vec3(0), osim.Vec3(0), b0,
osim.Vec3(-1, 0, 0), osim.Vec3(0))
q0 = j0.updCoordinate()
q0.setName("q0")
j1 = osim.PinJoint("j1", b0, osim.Vec3(0), osim.Vec3(0), b1,
osim.Vec3(-1, 0, 0), osim.Vec3(0))
q1 = j1.updCoordinate()
q1.setName("q1")
model.addJoint(j0)
model.addJoint(j1)
tau0 = osim.CoordinateActuator()
tau0.setCoordinate(j0.updCoordinate())
tau0.setName("tau0")
tau0.setOptimalForce(1)
model.addComponent(tau0)
tau1 = osim.CoordinateActuator()
tau1.setCoordinate(j1.updCoordinate())
tau1.setName("tau1")
tau1.setOptimalForce(1)
model.addComponent(tau1)
# Add display geometry.
bodyGeometry = osim.Ellipsoid(0.5, 0.1, 0.1)
transform = osim.Transform(osim.Vec3(-0.5, 0, 0))
b0Center = osim.PhysicalOffsetFrame("b0_center", b0, transform)
b0.addComponent(b0Center)
b0Center.attachGeometry(bodyGeometry.clone())
b1Center = osim.PhysicalOffsetFrame("b1_center", b1, transform)
b1.addComponent(b1Center)
b1Center.attachGeometry(bodyGeometry.clone())
model.finalizeConnections()
model.printToXML("double_pendulum.osim")
return model
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 __init__(self,
*args,
one_target=False,
max_speed=5.,
kin_coef=0.,
vel_prof_coef=0.,
acc_coef=0.,
**kwargs):
super(Arm2DEnv, self).__init__(*args, **kwargs)
blockos = opensim.Body('target', 0.0001, opensim.Vec3(0),
opensim.Inertia(1, 1, .0001, 0, 0, 0))
self.target_joint = opensim.PlanarJoint(
'target-joint',
self.osim_model.model.getGround(), # PhysicalFrame
opensim.Vec3(0, 0, 0),
opensim.Vec3(0, 0, 0),
blockos, # PhysicalFrame
opensim.Vec3(0, 0, -0.25),
opensim.Vec3(0, 0, 0))
geometry = opensim.Ellipsoid(0.02, 0.02, 0.02)
geometry.setColor(opensim.Green)
blockos.attachGeometry(geometry)
self.one_target = one_target
self.target_generated = False
self.osim_model.model.addJoint(self.target_joint)
self.osim_model.model.addBody(blockos)
self.osim_model.model.initSystem()
self.kin_coef = kin_coef
self.vel_prof_coef = vel_prof_coef
self.max_speed = max_speed
def test_markAdopted2(self):
a = osim.Model()
# We just need the following not to cause a segfault.
# Model add*
pa = osim.PathActuator()
pa.setName('pa')
a.addForce(pa)
probe = osim.Umberger2010MuscleMetabolicsProbe()
probe.setName('probe')
a.addProbe(probe)
ma = osim.MuscleAnalysis()
ma.setName('ma')
a.addAnalysis(ma)
pc = osim.PrescribedController()
pc.setName('pc')
a.addController(pc)
body = osim.Body('body1', 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)
print "creating Weld Joint.."
joint = osim.WeldJoint("weld_joint", a.getGround(), loc_in_parent,
orient_in_parent, body, loc_in_body,
orient_in_parent)
print "adding a body .."
a.addBody(body)
print "adding a joint .."
a.addJoint(joint)
print "Creating a ConstantDistanceConstraint.."
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)
f = osim.BushingForce("bushing", "ground", "body", osim.Vec3(2, 2, 2),
osim.Vec3(1, 1, 1), osim.Vec3(0, 0, 0),
osim.Vec3(0, 0, 0))
a.addForce(f)
f2 = osim.BushingForce()
a.addForce(f2)
f3 = osim.SpringGeneralizedForce()
a.addForce(f3)
model = osim.Model(os.path.join(test_dir, "arm26.osim"))
g = osim.CoordinateActuator('r_shoulder_elev')
model.addForce(g)
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 print_model():
model = osim.Model()
model.setName('model')
# Create a body with name 'body', mass of 1 kg, center of mass at
# the origin of the body, and unit inertia
# (Ixx = Iyy = Izz = 1 kg-m^2).
body = osim.Body('body', 1.0, osim.Vec3(0), osim.Inertia(1))
# Create a free joint (all 6 degrees of freedom) with Ground as
# the parent body and 'body' as the child body.
joint = osim.FreeJoint('joint', model.getGround(), body)
# Add the body and joint to the model.
model.addComponent(body)
model.addComponent(joint)
# Create a TableReporter to save quantities to a file after
# simulating.
reporter = osim.TableReporterVec3()
reporter.setName('reporter')
reporter.set_report_time_interval(0.1)
reporter.addToReport(model.getOutput('com_position'))
reporter.getInput('inputs').setAlias(0, 'com_pos')
# Display what input-output connections look like in XML
# (in .osim files).
print("Reporter input-output connections in XML:\n" + \
reporter.dump())
model.addComponent(reporter)
model.printToXML(model_filename)
def print_model():
model = osim.Model()
model.setName('model')
# Create a body with name 'body', mass of 1 kg, center of mass at the
# origin of the body, and unit inertia (Ixx = Iyy = Izz = 1 kg-m^2).
body = osim.Body('body', 1.0, osim.Vec3(0), osim.Inertia(1))
# Create a free joint (all 6 degrees of freedom) with Ground as the parent
# body and 'body' as the child body.
joint = osim.FreeJoint('joint', model.getGround(), body)
# Add the body and joint to the model.
model.addComponent(body)
model.addComponent(joint)
# Create a TableReporter to save quantities to a file after simulating.
reporter = osim.TableReporterVec3()
reporter.setName('reporter')
reporter.set_report_time_interval(0.1)
# Report the position of the origin of the body.
reporter.addToReport(body.getOutput('position'))
# For comparison, we will also get the center of mass position from the
# Model, and we can check that the two outputs are the same for our
# one-body system. The (optional) second argument is an alias for the name
# of the output; it is used as the column label in the table.
reporter.addToReport(model.getOutput('com_position'), 'com_pos')
# Display what input-output connections look like in XML (in .osim files).
print("Reporter input-output connections in XML:\n" + reporter.dump())
model.addComponent(reporter)
model.printToXML(model_filename)
def test_connecting_and_iterate_inputs(self):
m = osim.Model()
b = osim.Body('b1', 2.0, osim.Vec3(1, 0, 0), osim.Inertia(1))
j = osim.PinJoint('pin', m.getGround(), b)
# Source.
source = osim.TableSource()
source.setName("source")
table = osim.TimeSeriesTable()
table.setColumnLabels(('col1', 'col2', 'col3', 'col4'))
row = osim.RowVector([1, 2, 3, 4])
table.appendRow(0.0, row)
row = osim.RowVector([2, 3, 4, 5])
table.appendRow(1.0, row)
source.setTable(table)
# Reporter.
rep = osim.ConsoleReporter()
rep.setName("rep")
m.addBody(b)
m.addJoint(j)
m.addComponent(source)
m.addComponent(rep)
# Connect.
# There are multiple ways to perform the connection, especially
# for reporters.
coord = j.get_coordinates(0)
rep.updInput('inputs').connect(coord.getOutput('value'))
rep.connectInput_inputs(coord.getOutput('speed'), 'spd')
rep.connectInput_inputs(source.getOutput('column').getChannel('col1'))
rep.addToReport(
source.getOutput('column').getChannel('col2'), 'second_col')
s = m.initSystem()
# Access and iterate through AbstractInputs, using names.
expectedLabels = [
'/model/jointset/pin/pin_coord_0|value', 'spd',
'/model/source|column:col1', 'second_col'
]
i = 0
for name in rep.getInputNames():
# Actually, there is only one input, which we connected to 4
# channels.
assert rep.getInput(name).getNumConnectees() == 4
for j in range(4):
assert (rep.getInput(name).getLabel(j) == expectedLabels[j])
i += 1
# Access concrete Input.
# Input value is column 2 at time 0.
assert (osim.InputDouble.safeDownCast(rep.getInput('inputs')).getValue(
s, 3) == 2.0)
def test_Joint(self):
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.FreeJoint("joint", a.getGround(), loc_in_parent,
orient_in_parent, body, loc_in_body,
orient_in_parent)
del joint
spatialTransform = osim.SpatialTransform()
joint = osim.CustomJoint("joint", a.getGround(), loc_in_parent,
orient_in_parent, body, loc_in_body,
orient_in_parent, spatialTransform)
del joint
joint = osim.EllipsoidJoint("joint", a.getGround(), loc_in_parent,
orient_in_parent, body, loc_in_body,
orient_in_parent, osim.Vec3(1, 1, 1))
del joint
joint = osim.BallJoint("joint", a.getGround(), loc_in_parent,
orient_in_parent, body, loc_in_body,
orient_in_parent)
del joint
joint = osim.PinJoint("joint", a.getGround(), loc_in_parent,
orient_in_parent, body, loc_in_body,
orient_in_parent)
del joint
joint = osim.SliderJoint("joint", a.getGround(), loc_in_parent,
orient_in_parent, body, loc_in_body,
orient_in_parent)
del joint
joint = osim.WeldJoint("joint", a.getGround(), loc_in_parent,
orient_in_parent, body, loc_in_body,
orient_in_parent)
del joint
joint = osim.GimbalJoint("joint", a.getGround(), loc_in_parent,
orient_in_parent, body, loc_in_body,
orient_in_parent)
del joint
joint = osim.UniversalJoint("joint", a.getGround(), loc_in_parent,
orient_in_parent, body, loc_in_body,
orient_in_parent)
del joint
joint = osim.PlanarJoint("joint", a.getGround(), loc_in_parent,
orient_in_parent, body, loc_in_body,
orient_in_parent)
del joint
def __init__(self, *args, **kwargs):
super(Arm2DEnv, self).__init__(*args, **kwargs)
blockos = opensim.Body('target', 0.0001 , opensim.Vec3(0), opensim.Inertia(1,1,.0001,0,0,0) );
self.target_joint = opensim.PlanarJoint('target-joint',
self.osim_model.model.getGround(), # PhysicalFrame
opensim.Vec3(0, 0, 0),
opensim.Vec3(0, 0, 0),
blockos, # PhysicalFrame
opensim.Vec3(0, 0, -0.25),
opensim.Vec3(0, 0, 0))
geometry = opensim.Ellipsoid(0.02, 0.02, 0.02);
geometry.setColor(opensim.Green);
blockos.attachGeometry(geometry)
self.osim_model.model.addJoint(self.target_joint)
self.osim_model.model.addBody(blockos)
self.osim_model.model.initSystem()
def test_createRep(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)
model.finalizeConnections()
study = osim.MocoStudy()
study.setName('sliding_mass')
mp = study.updProblem()
mp.setModel(model)
pr = mp.createRep()
assert (len(pr.createStateInfoNames()) == 2)
def create_obstacles(self):
x = 0.
y = 0.
r = 0.1
for i in range(self.max_obstacles):
name = i.__str__()
blockos = opensim.Body(name + '-block', 0.0001, opensim.Vec3(0),
opensim.Inertia(1., 1., .0001, 0., 0., 0.))
pj = opensim.PlanarJoint(
name + '-joint',
self.osim_model.model.getGround(), # PhysicalFrame
opensim.Vec3(0., 0., 0.),
opensim.Vec3(0., 0., 0.),
blockos, # PhysicalFrame
opensim.Vec3(0., 0., 0.),
opensim.Vec3(0., 0., 0.))
self.osim_model.model.addJoint(pj)
self.osim_model.model.addBody(blockos)
block = opensim.ContactSphere(r, opensim.Vec3(0., 0., 0.), blockos)
block.setName(name + '-contact')
self.osim_model.model.addContactGeometry(block)
force = opensim.HuntCrossleyForce()
force.setName(name + '-force')
force.addGeometry(name + '-contact')
force.addGeometry("r_heel")
force.addGeometry("l_heel")
force.addGeometry("r_toe")
force.addGeometry("l_toe")
force.setStiffness(1.0e6 / r)
force.setDissipation(1e-5)
force.setStaticFriction(0.0)
force.setDynamicFriction(0.0)
force.setViscousFriction(0.0)
self.osim_model.model.addForce(force)
def test_markAdopted():
a = osim.Model()
# We just need the following not to not cause a segfault.
# Model add*
a.addComponent(osim.PathActuator())
a.addProbe(osim.Umberger2010MuscleMetabolicsProbe())
a.addAnalysis(osim.MuscleAnalysis())
a.addController(osim.PrescribedController())
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)
a.addConstraint(constr)
# Force requires body names. If not provided, you get a segfault.
f = osim.BushingForce()
f.setBody1ByName("ground")
f.setBody2ByName("body")
a.addForce(f)
model = osim.Model(os.environ['OPENSIM_HOME'] + "/Models/Arm26/arm26.osim")
g = osim.CoordinateActuator('r_shoulder_elev')
model.addForce(g)
def createSlidingMassModel():
model = osim.Model()
model.setName("sliding_mass")
model.set_gravity(osim.Vec3(0, 0, 0))
body = osim.Body("body", 10.0, osim.Vec3(0), osim.Inertia(0))
model.addComponent(body)
# Allows translation along x.
joint = osim.SliderJoint("slider", model.getGround(), body)
coord = joint.updCoordinate(osim.SliderJoint.Coord_TranslationX)
coord.setName("position")
model.addComponent(joint)
actu = osim.CoordinateActuator()
actu.setCoordinate(coord)
actu.setName("actuator")
actu.setOptimalForce(1)
actu.setMinControl(-10)
actu.setMaxControl(10)
model.addComponent(actu)
return model
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))
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),
opensim.Vec3(0, 0, 0))
bodyGeometry = opensim.Ellipsoid(0.1, 0.1, 0.1)
bodyGeometry.setColor(opensim.Gray)
blockos.attachGeometry(bodyGeometry)
model.addComponent(pj)
model.addComponent(blockos)
#block = opensim.ContactMesh('block.obj',opensim.Vec3(0,0,0), opensim.Vec3(0,0,0), blockos);
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)
# This script goes through OpenSim funcionalties
# required for OpenSim-RL
import opensim
# Settings
stepsize = 0.01
# Load existing model
model_path = "../osim/models/gait9dof18musc.osim"
model = opensim.Model(model_path)
model.setUseVisualizer(True)
# Create the ball
r = 0.000001
ballBody = opensim.Body('ball', 0.0001 , opensim.Vec3(0), opensim.Inertia(1,1,.0001,0,0,0) );
ballGeometry = opensim.Ellipsoid(r, r, r)
ballGeometry.setColor(opensim.Gray)
ballBody.attachGeometry(ballGeometry)
# Attach ball to the model
ballJoint = opensim.FreeJoint("weldball",
model.getGround(), # PhysicalFrame
opensim.Vec3(0, 0, 0),
opensim.Vec3(0, 0, 0),
ballBody, # PhysicalFrame
opensim.Vec3(0, 0, 0),
opensim.Vec3(0, 0, 0))
model.addComponent(ballJoint)
model.addComponent(ballBody)
# Add contact
import opensim as osim
import sys
# Are we running this script as a test? Users can ignore this line!
running_as_test = 'unittest' in str().join(sys.argv)
# Define global model where the arm lives.
arm = osim.Model()
if not running_as_test: arm.setUseVisualizer(True)
# ---------------------------------------------------------------------------
# Create two links, each with a mass of 1 kg, centre of mass at the body's
# origin, and moments and products of inertia of zero.
# ---------------------------------------------------------------------------
humerus = osim.Body("humerus", 1.0, osim.Vec3(0, 0, 0), osim.Inertia(0, 0, 0))
radius = osim.Body("radius", 1.0, osim.Vec3(0, 0, 0), osim.Inertia(0, 0, 0))
# ---------------------------------------------------------------------------
# Connect the bodies with pin joints. Assume each body is 1m long.
# ---------------------------------------------------------------------------
shoulder = osim.PinJoint(
"shoulder",
arm.getGround(), # PhysicalFrame
osim.Vec3(0, 0, 0),
osim.Vec3(0, 0, 0),
humerus, # PhysicalFrame
osim.Vec3(0, 1, 0),
osim.Vec3(0, 0, 0))
random.seed(0)
setpoints = ravel(2 * (random.rand(1, SIM_TIME_STEPS_NUMBER) - 0.5)).tolist()
b, a = butter(2, 0.01, btype='low', analog=False)
setpoints = lfilter(b, a, setpoints)
try:
model = opensim.Model()
if useInput: model.setUseVisualizer(True)
model.setName('TelerehabSimulator')
model.setGravity(opensim.Vec3(0, 0, 0))
master = opensim.Body('master', OPERATOR_INERTIA, opensim.Vec3(0, 0, 0),
opensim.Inertia(0, 0, 0))
model.addBody(master)
slave = opensim.Body('slave', OPERATOR_INERTIA, opensim.Vec3(0, 0, 0),
opensim.Inertia(0, 0, 0))
model.addBody(slave)
ground = model.getGround()
masterToGround = opensim.SliderJoint('master2ground', ground, master)
model.addJoint(masterToGround)
slaveToGround = opensim.SliderJoint('slave2ground', ground, slave)
model.addJoint(slaveToGround)
blockMesh = opensim.Brick(opensim.Vec3(0.5, 0.5, 0.5))
blockMesh.setColor(opensim.Red)
masterOffsetFrame = opensim.PhysicalOffsetFrame()
masterOffsetFrame.setParentFrame(master)
for body in model.getBodyList():
if body.getName() == name_body:
this_body = body
return this_body
# Function that finds joints
def find_joint(model,name_joint):
for joint in model.getJointList():
if joint.getName() == name_joint:
this_joint = joint
return this_joint
## Parameters
# === Pedal holder
cleat_mass = 0.25
cleat_inertia = osim.Inertia(1,1,1,0,0,0)
cleat_locParent = osim.Vec3(0.125,-0.015,0) # (0.125,-0.018,0)
cleat_orParent = osim.Vec3(0,0,0)
cleat_locChild = osim.Vec3(0,0,0)
cleat_orChild = osim.Vec3(0,0,0)
# === Pedal
pedal_mass = 0.5
pedal_inertia = osim.Inertia(1,1,1,0,0,0)
pedal_orParent = osim.Vec3(0,0,0)
pedal_locChild = osim.Vec3(0,0,0)
pedal_orChild = osim.Vec3(0,0,0)
pedal_locParent_r = osim.Vec3(0.02, 0.133, 0.091)
pedal_locParent_l = osim.Vec3(0.02, -0.146, -0.091)
# each degree of freedom:
#
# G = [2*tx, -1]
#
# This example is related to an explanation of kinematic constraints in the
# appendix of the Moco paper.
import opensim as osim
import numpy as np
model = osim.Model()
model.setName('planar_point_mass')
g = abs(model.get_gravity()[1])
# We use an intermediate massless body for the X translation degree of freedom.
massless = osim.Body('massless', 0, osim.Vec3(0), osim.Inertia(0))
model.addBody(massless)
mass = 1.0
body = osim.Body('body', mass, osim.Vec3(0), osim.Inertia(0))
model.addBody(body)
body.attachGeometry(osim.Sphere(0.05))
jointX = osim.SliderJoint('tx', model.getGround(), massless)
coordX = jointX.updCoordinate(osim.SliderJoint.Coord_TranslationX)
coordX.setName('tx')
model.addJoint(jointX)
# The joint's x axis must point in the global "+y" direction.
jointY = osim.SliderJoint('ty', massless, osim.Vec3(0),
osim.Vec3(0, 0, 0.5 * np.pi), body, osim.Vec3(0),
# 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. #
# -------------------------------------------------------------------------- #
import os
import opensim as osim
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)
# Allows translation along x.
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)
body.attachGeometry(osim.Sphere(0.05))
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)
# correct trajectory specified by the state bounds and the MocoMarkerFinalGoal.
import os
import opensim as osim
import numpy as np
# Setting variables
stiffness = 100.
mass = 5.
finalTime = np.pi * np.sqrt(mass / stiffness)
# Defining the model
model = osim.Model()
model.setName('oscillator')
model.set_gravity(osim.Vec3(0, 0, 0))
body = osim.Body('body', np.multiply(0.5, mass), osim.Vec3(0), osim.Inertia(0))
model.addComponent(body)
# Adding a marker to body in the model
marker = osim.Marker('marker', body, osim.Vec3(0))
model.addMarker(marker)
# Allows translation along x.
joint = osim.SliderJoint('slider', model.getGround(), body)
coord = joint.updCoordinate()
coord.setName('position')
model.addComponent(joint)
# Adds the spring component
spring = osim.SpringGeneralizedForce()
spring.set_coordinate('position')
# 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),
opensim.Vec3(0, 0, 0))
bodyGeometry = opensim.Ellipsoid(0.1, 0.1, 0.1)
bodyGeometry.setColor(opensim.Gray)
blockos.attachGeometry(bodyGeometry)
model.addComponent(pj)
model.addComponent(blockos)
adjustedModel.getMarkerSet().adoptAndAppend(scaledModel.getMarkerSet().get('R.Tibia.Distal.Medial'))
#Finalise connections
adjustedModel.finalizeConnections()
#Re-print model
adjustedModel.printToXML(subList[ii]+'_scaledModelAdjusted.osim')
#Create a model with 'sensor' bodies added
##### TODO: loop for multiple sensors
#Create the distal medial tibia sensor body
distalMedialSensorBody = osim.Body()
distalMedialSensorBody.setName('distalMedialTibia_sensor_r')
distalMedialSensorBody.setMass(sensorMass)
distalMedialSensorBody.setMassCenter(osim.Vec3(0,0,0))
distalMedialSensorBody.setInertia(osim.Inertia(Ixx,Iyy,Izz,0,0,0))
#TODO: check order these should be in
distalMedialSensorBody.attachGeometry(osim.Brick(osim.Vec3(sensorWidth/2, sensorLength/2, sensorDepth/2)))
distalMedialSensorBody.get_attached_geometry(0).setColor(osim.Vec3(0/255,102/255,255/255)) #blue
adjustedModel.addBody(distalMedialSensorBody)
#Join the distal medial tibia sensor to the model
#Get the marker location for the parent location
locationInParent = adjustedModel.getMarkerSet().get('R.Tibia.Distal.Medial').get_location()
orientationInParent = osim.Vec3(0,0,0) #Align to same axis as tibia
#Set child details
locationInChild = osim.Vec3(0,0,0)
orientationInChild = osim.Vec3(0,0,0)
#Create the joint
distalMedialSensorJoint = osim.WeldJoint('distalMedialTibia_joint_r', #joint name
adjustedModel.getBodySet().get('tibia_r'), #parent body
gravity = -9.81 stepSize = 0.001 initialTime = 0.0 finalTime = 10 th1Deg = 90 #degrees th1Rad = th1Deg * ((22.0 / 7.0) / 180.0) #// radians rodRd = 0.1 rodHeight = 1 rodMass = 1 rodCoM = osim.Vec3(0) # center of mass rodInertia = osim.Inertia(1) #Inertia(1); rodColor = osim.Vec3(1.0, 1.0, 0.0) rodPos = osim.Vec3(0, rodHeight, 0) #//it will be under the ground rodOrient = osim.Vec3(0, 0, th1Rad) sphRd = 0.3 sphMass = 1 sphCoM = osim.Vec3(0) sphInertia = osim.Inertia(1) sphColor = osim.Vec3(1.0, 0.0, 0.0) # sphOrient = osim.Vec3(0.0,0.0,0.0) # sphX = rodHeight * math.sin( th1Rad); # sphY = rodHeight * math.cos( th1Rad); # sphPos = osim.Vec3(sphX, sphY , 0) ;
def createHangingMuscleModel(ignore_activation_dynamics,
ignore_tendon_compliance):
model = osim.Model()
model.setName("hanging_muscle")
model.set_gravity(osim.Vec3(9.81, 0, 0))
body = osim.Body("body", 0.5, osim.Vec3(0), osim.Inertia(1))
model.addComponent(body)
# Allows translation along x.
joint = osim.SliderJoint("joint", model.getGround(), body)
coord = joint.updCoordinate()
coord.setName("height")
model.addComponent(joint)
# The point mass is supported by a muscle.
# The DeGrooteFregly2016Muscle is the only muscle model in OpenSim that
# has been tested with Moco.
actu = osim.DeGrooteFregly2016Muscle()
actu.setName("muscle")
actu.set_max_isometric_force(30.0)
actu.set_optimal_fiber_length(0.10)
actu.set_tendon_slack_length(0.05)
actu.set_tendon_strain_at_one_norm_force(0.10)
actu.set_ignore_activation_dynamics(ignore_activation_dynamics)
actu.set_ignore_tendon_compliance(ignore_tendon_compliance)
actu.set_fiber_damping(0.01)
# The DeGrooteFregly2016Muscle is the only muscle model in OpenSim that
# can express its tendon compliance dynamics using an implicit
# differential equation.
actu.set_tendon_compliance_dynamics_mode("implicit")
actu.set_max_contraction_velocity(10)
actu.set_pennation_angle_at_optimal(0.10)
actu.addNewPathPoint("origin", model.updGround(), osim.Vec3(0))
actu.addNewPathPoint("insertion", body, osim.Vec3(0))
model.addForce(actu)
# Add metabolics probes: one for the total metabolic rate,
# and one for each term in the metabolics model.
probe = osim.Umberger2010MuscleMetabolicsProbe()
probe.setName("metabolics")
probe.addMuscle("muscle", 0.5)
model.addProbe(probe)
probe = osim.Umberger2010MuscleMetabolicsProbe()
probe.setName("activation_maintenance_rate")
probe.set_activation_maintenance_rate_on(True)
probe.set_shortening_rate_on(False)
probe.set_basal_rate_on(False)
probe.set_mechanical_work_rate_on(False)
probe.addMuscle("muscle", 0.5)
model.addProbe(probe)
probe = osim.Umberger2010MuscleMetabolicsProbe()
probe.setName("shortening_rate")
probe.set_activation_maintenance_rate_on(False)
probe.set_shortening_rate_on(True)
probe.set_basal_rate_on(False)
probe.set_mechanical_work_rate_on(False)
probe.addMuscle("muscle", 0.5)
model.addProbe(probe)
probe = osim.Umberger2010MuscleMetabolicsProbe()
probe.setName("basal_rate")
probe.set_activation_maintenance_rate_on(False)
probe.set_shortening_rate_on(False)
probe.set_basal_rate_on(True)
probe.set_mechanical_work_rate_on(False)
probe.addMuscle("muscle", 0.5)
model.addProbe(probe)
probe = osim.Umberger2010MuscleMetabolicsProbe()
probe.setName("mechanical_work_rate")
probe.set_activation_maintenance_rate_on(False)
probe.set_shortening_rate_on(False)
probe.set_basal_rate_on(False)
probe.set_mechanical_work_rate_on(True)
probe.addMuscle("muscle", 0.5)
model.addProbe(probe)
body.attachGeometry(osim.Sphere(0.05))
model.finalizeConnections()
return model
