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 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 addCoordinateActuator(model, coordName, optForce):
coordSet = model.updCoordinateSet()
actu = osim.CoordinateActuator()
actu.setName('tau_' + coordName)
actu.setCoordinate(coordSet.get(coordName))
actu.setOptimalForce(optForce)
actu.setMinControl(-1)
actu.setMaxControl(1)
model.addComponent(actu)
def add_reserve(model, coord, max_control):
actu = osim.CoordinateActuator(coord)
if coord.startswith('lumbar'):
prefix = 'torque_'
elif coord.startswith('pelvis'):
prefix = 'residual_'
else:
prefix = 'reserve_'
actu.setName(prefix + coord)
actu.setMinControl(-1)
actu.setMaxControl(1)
actu.setOptimalForce(max_control)
model.addForce(actu)
def addCoordinateActuator(model, coordinateName, optForce):
coordSet = model.getCoordinateSet();
actu = osim.CoordinateActuator();
actu.setName('torque_' + coordinateName);
actu.setCoordinate(coordSet.get(coordinateName));
actu.setOptimalForce(optForce);
actu.setMinControl(-1);
actu.setMaxControl(1);
# Add to ForceSet
model.addForce(actu);
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))
def addCoordinateActuator(modelObject = None, coordinate = None, optForce = 1.0,
controlLevel = [float('inf'),float('-inf')], appendStr = '_torque'):
# Convenience function for adding a coordinate actuator to model
#
# Input: modelObject - Opensim model object to add actuator to
# coordinate - string of coordinate name for actuator
# optForce - value for actuators optimal force
# controlLevel - [x,y] values for max and min control
# appendStr - string to append to coordinate name in setting actuator name
#Check for model object
if modelObject is None:
raise ValueError('A model object must be included')
#Check for coordinate
if coordinate is None:
raise ValueError('A coordinate string must be specified')
#Create actuator
actu = osim.CoordinateActuator()
#Set name
actu.setName(coordinate+appendStr)
#Set coordinate for actuator
actu.setCoordinate(modelObject.getCoordinateSet().get(coordinate))
#Set optimal force
actu.setOptimalForce(optForce)
#Set control levels
actu.setMaxControl(controlLevel[0])
actu.setMinControl(controlLevel[1])
#Add actuator to models forceset
modelObject.updForceSet().append(actu)
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))
model.finalizeConnections()
# Create MucoTool.
# ================
muco = osim.MucoTool()
muco.setName('sliding_mass')
# Define the optimal control problem.
masterOffsetFrame = opensim.PhysicalOffsetFrame()
masterOffsetFrame.setParentFrame(master)
masterOffsetFrame.setOffsetTransform(
opensim.Transform(opensim.Vec3(0, 0, 0.5)))
master.addComponent(masterOffsetFrame)
masterOffsetFrame.attachGeometry(blockMesh.clone())
blockMesh.setColor(opensim.Blue)
slaveOffsetFrame = opensim.PhysicalOffsetFrame()
slaveOffsetFrame.setParentFrame(slave)
slaveOffsetFrame.setOffsetTransform(
opensim.Transform(opensim.Vec3(0, 0, -0.5)))
slave.addComponent(slaveOffsetFrame)
slaveOffsetFrame.attachGeometry(blockMesh.clone())
masterCoordinate = masterToGround.updCoordinate()
masterInputActuator = opensim.CoordinateActuator('masterInput')
masterInputActuator.setCoordinate(masterCoordinate)
model.addForce(masterInputActuator)
masterFeedbackActuator = opensim.CoordinateActuator('masterFeedback')
masterFeedbackActuator.setCoordinate(masterCoordinate)
model.addForce(masterFeedbackActuator)
slaveCoordinate = slaveToGround.updCoordinate()
slaveInputActuator = opensim.CoordinateActuator('slaveInput')
slaveInputActuator.setCoordinate(slaveCoordinate)
model.addForce(slaveInputActuator)
slaveFeedbackActuator = opensim.CoordinateActuator('slaveFeedback')
slaveFeedbackActuator.setCoordinate(slaveCoordinate)
model.addForce(slaveFeedbackActuator)
#model.finalizeFromProperties()
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()
opensim.Vec3(opensim.SimTK_PI / 2, 0.0, 0.0))
bodyMesh = opensim.Cylinder(BODY_SIZE / 2, BODY_SIZE)
bodyMesh.setColor(BLOCK_COLORS[index])
offsetFrame.attachGeometry(bodyMesh)
body.addComponent(offsetFrame)
if index == 1:
markerX = opensim.Marker('effector_x', body,
opensim.Vec3(0, BODY_DISTANCE, 0))
model.addMarker(markerX)
markerY = opensim.Marker('effector_y', body,
opensim.Vec3(0, BODY_DISTANCE, 0))
model.addMarker(markerY)
coordinate = pinJoint.getCoordinate()
userActuator = opensim.CoordinateActuator(coordinate.getName())
model.addForce(userActuator)
controlActuator = opensim.CoordinateActuator(coordinate.getName())
model.addForce(controlActuator)
systemState = model.initSystem()
file = open('robot_model.osim', 'w')
file.write(model.dump())
file.close()
manager = opensim.Manager(model)
systemState.setTime(0)
manager.initialize(systemState)
systemState = manager.integrate(0.001)
