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 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 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))
# ---------------------------------------------------------------------------
# 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))
elbow = osim.PinJoint(
"elbow",
humerus, # PhysicalFrame
osim.Vec3(0, 0, 0),
osim.Vec3(0, 0, 0),
radius, # PhysicalFrame
osim.Vec3(0, 1, 0),
osim.Vec3(0, 0, 0))
# ---------------------------------------------------------------------------
# Add a muscle that flexes the elbow (actuator for robotics people).
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);
block = opensim.ContactSphere(0.4, opensim.Vec3(0, 0, 0), blockos)
model.addContactGeometry(block)
# Reinitialize the system with the new controller
ground = pendulumDF.updGround()
# body name, mass ,massCenter, inertia
rodBdy = osim.Body("rod", rodMass, rodCoM, rodInertia)
rodGeom = osim.Cylinder(rodRd, rodHeight)
rodTrsfrm = osim.Transform(osim.Vec3(0))
rodCenter = osim.PhysicalOffsetFrame("rodCenter", rodBdy, rodTrsfrm)
rodBdy.addComponent(rodCenter)
rodGeom.setColor(rodColor)
#//set the color of the first block so they are different
rodCenter.attachGeometry(rodGeom.clone())
pendulumDF.addBody(rodBdy)
# Connect the bodies with pin joints. Assume each body is 1 m long.
groundTrodJnt = osim.PinJoint("groundTrodJnt", pendulumDF.getGround(),
osim.Vec3(0), osim.Vec3(0), rodBdy, rodPos,
rodOrient)
pendulumDF.addJoint(groundTrodJnt)
sphBdy = osim.Body("sph", sphMass, sphCoM, sphInertia)
sphGeom = osim.Sphere(sphRd)
sphTrsfrm = osim.Transform(osim.Vec3(0))
sphCenter = osim.PhysicalOffsetFrame("sphCenter", sphBdy, sphTrsfrm)
sphBdy.addComponent(sphCenter)
sphGeom.setColor(sphColor)
#//set the color of the first block so they are different
sphCenter.attachGeometry(sphGeom.clone())
pendulumDF.addBody(sphBdy)
#rodTsphJnt = osim.PinJoint ("rodTsphJnt",rodBdy, osim.Vec3(sphX-rodHeight,sphY,0), rodOrient, sphBdy, sphPos, sphOrient);
rodTsphJnt = osim.PinJoint("rodTsphJnt", rodBdy, rodPos, rodOrient, sphBdy,
# Find bodies
ground = myModel.getGround()
calcn_r = find_body(myModel,"calcn_r")
calcn_l = find_body(myModel,"calcn_l")
## MODELING GEOMETRIES AND BODIES
# === Crank
crank = osim.Body("crank",crank_mass,crank_orParent,crank_inertia)
crank_geometry = osim.Mesh("gear.stl")
crank_geometry.setColor(osim.Black)
crank.attachGeometry(crank_geometry.clone())
myModel.addBody(crank)
crankToGround = osim.PinJoint("crankToGround",crank,crank_locParent,crank_orParent,ground,crank_locChild,crank_orChild)
myModel.addJoint(crankToGround)
# === Cleats
cleat_r = osim.Body("cleat_r",cleat_mass,cleat_orParent,cleat_inertia)
cleat_l = osim.Body("cleat_l",cleat_mass,cleat_orParent,cleat_inertia)
cleat_geometry = osim.Mesh("cleat_simple.stl")
cleat_geometry.setColor(osim.Red)
cleat_r.attachGeometry(cleat_geometry.clone())
cleat_l.attachGeometry(cleat_geometry.clone())
myModel.addBody(cleat_r)
myModel.addBody(cleat_l)
cyl4Frame = osim.PhysicalOffsetFrame(
linkage4, osim.Transform(osim.Vec3(0.0, linkageLength4 / 2.0, 0.0)))
cyl4Frame.setName("Cyl4_frame")
cyl4Frame.attachGeometry(cyl4.clone())
osimModel.addComponent(cyl4Frame)
orientationInGround = Vec3(0.)
locationInGround = Vec3(0.)
locationInParent1 = Vec3(0.0, linkageLength1, 0.0)
locationInParent2 = Vec3(0.0, linkageLength2, 0.0)
locationInParent3 = Vec3(0.0, linkageLength3, 0.0)
locationInParent4 = Vec3(0.0, linkageLength4, 0.0)
orientationInChild = Vec3(0.)
locationInChild = Vec3(0.)
tip = osim.PinJoint("tip", ground, locationInGround, orientationInGround,
linkage1, locationInChild, orientationInChild)
ankle = osim.PinJoint("ankle", linkage1, locationInParent1, orientationInChild,
linkage2, locationInChild, orientationInChild)
knee = osim.PinJoint("knee", linkage2, locationInParent2, orientationInChild,
linkage3, locationInChild, orientationInChild)
hip = osim.PinJoint("hip", linkage3, locationInParent3, orientationInChild,
linkage4, locationInChild, orientationInChild)
toeRange = [0 * pi / 180, pi / 2]
ankleRange = [10 * pi / 180 - pi, 20 * pi / 180 - pi]
kneeRange = [0, pi - 30 * pi / 180]
hipRange = [50 * pi / 180 - pi, 60 * pi / 180 - pi]
model.setName('Robot')
model.setGravity(opensim.Vec3(0, 0, 0))
for index in range(2):
indexString = str(1)
body = opensim.Body("body_" + indexString, 1.0, opensim.Vec3(0, 0, 0),
opensim.Inertia(1, 1, 1))
model.addBody(body)
refFrame = model.getGround() if index == 0 else model.getBodySet().get(
0)
pinJoint = opensim.PinJoint("joint_" + indexString, refFrame,
opensim.Vec3(0, BODY_DISTANCE, 0),
opensim.Vec3(0, 0, 0), body,
opensim.Vec3(0, 0, 0),
opensim.Vec3(0, 0, 0))
model.addJoint(pinJoint)
bodyMesh = opensim.Cylinder(BODY_SIZE, BODY_SIZE)
bodyMesh.setColor(BLOCK_COLORS[index])
offsetFrame = opensim.PhysicalOffsetFrame()
offsetFrame.setParentFrame(body)
offsetFrame.set_orientation(
opensim.Vec3(opensim.SimTK_PI / 2, 0.0, 0.0))
offsetFrame.attachGeometry(bodyMesh)
body.addComponent(offsetFrame)
offsetFrame = opensim.PhysicalOffsetFrame()
offsetFrame.setParentFrame(body)
offsetFrame.set_translation(opensim.Vec3(0.0, BODY_DISTANCE / 2, 0.0))
# This example shows how to use the StatesTrajectory to analyze a simulation
# (computing joint reaction forces).
import opensim as osim
import math
model = osim.Model()
model.setName('model')
# Create a pendulum model.
# ------------------------
# In the default pose, the system is a mass hanging 1 meter down from a hinge.
body = osim.Body('body', 1.0, osim.Vec3(0), osim.Inertia(1))
model.addComponent(body)
joint = osim.PinJoint('joint', model.getGround(), osim.Vec3(0), osim.Vec3(0),
body, osim.Vec3(0, 1.0, 0), osim.Vec3(0))
joint.updCoordinate().setName('q')
model.addComponent(joint)
# This reporter will collect states throughout the simulation at intervals of
# 0.05 seconds.
reporter = osim.StatesTrajectoryReporter()
reporter.setName('reporter')
reporter.set_report_time_interval(0.05)
model.addComponent(reporter)
# Simulate.
# ---------
state = model.initSystem()
# The initial position is that the pendulum is rotated 45 degrees
