def addIMUFrame(model, bodyName, translation, orientation):
body = model.updBodySet().get(bodyName)
name = str(body.getName()) + '_imu_offset'
bodyOffset = osim.PhysicalOffsetFrame(name, body, osim.Transform())
bodyOffset.set_translation(translation)
bodyOffset.set_orientation(orientation)
body.addComponent(bodyOffset)
model.finalizeConnections()
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
# sphY = rodHeight * math.cos( th1Rad);
# sphPos = osim.Vec3(sphX, sphY , 0) ;
sphPos = osim.Vec3(0, 2 * rodHeight, 0)
sphOrient = osim.Vec3(0, 0, th1Rad)
# Define global model.
pendulumDF = osim.Model()
pendulumDF.setName("pendulumDF")
pendulumDF.setGravity(osim.Vec3(0, gravity, 0))
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)
reporter.set_report_time_interval(1.0)
reporter.addToReport(biceps.getOutput("fiber_force"))
elbow_coord = elbow.getCoordinate().getOutput("value")
reporter.addToReport(elbow_coord, "elbow_angle")
arm.addComponent(reporter)
# ---------------------------------------------------------------------------
# Add display geometry.
# ---------------------------------------------------------------------------
bodyGeometry = osim.Ellipsoid(0.1, 0.5, 0.1)
bodyGeometry.setColor(osim.Gray)
humerusCenter = osim.PhysicalOffsetFrame()
humerusCenter.setName("humerusCenter")
humerusCenter.setParentFrame(humerus)
humerusCenter.setOffsetTransform(osim.Transform(osim.Vec3(0, 0.5, 0)))
humerus.addComponent(humerusCenter)
humerusCenter.attachGeometry(bodyGeometry.clone())
radiusCenter = osim.PhysicalOffsetFrame()
radiusCenter.setName("radiusCenter")
radiusCenter.setParentFrame(radius)
radiusCenter.setOffsetTransform(osim.Transform(osim.Vec3(0, 0.5, 0)))
radius.addComponent(radiusCenter)
radiusCenter.attachGeometry(bodyGeometry.clone())
# ---------------------------------------------------------------------------
# Configure the model.
# ---------------------------------------------------------------------------
state = arm.initSystem()
def run(self):
# Allow for the passing of a model OR a model file
if not self.model:
try:
self.model = opensim.Model(self.model_file)
except RuntimeError:
raise RuntimeError("No model or valid model file was specified.")
self.model.finalizeFromProperties()
_calibrated = False
# Load in data
quat_table = self.orientations_file
rotations = self.sensor_to_opensim_rotations
# Generate an rotation matrix
sensor_to_opensim = opensim.Rotation(opensim.SpaceRotationSequence,
rotations.get(0), opensim.CoordinateAxis(0),
rotations.get(1), opensim.CoordinateAxis(1),
rotations.get(2), opensim.CoordinateAxis(2))
# Rotate data so that the Y Axis is up
opensim.OpenSenseUtilities_rotateOrientationTable(quat_table, sensor_to_opensim)
# Check there is consistent heading correction specification, both base_heading_axis
# and base_imu_label should be specified. Finder error checking done downstream
if (self.base_heading_axis and self.base_imu_label):
perform_heading_requested = True
else:
perform_heading_requested=False
if perform_heading_requested:
print("do something")
imu_axis = self.base_heading_axis.lower()
direction_on_imu = opensim.CoordinateDirection(opensim.CoordinateAxis(2))
direction = 1
if (imu_axis[0] == '-'):
direction = -1
back = imu_axis[-1]
if (back == 'x'):
direction_on_imu = opensim.CoordinateDirection(opensim.CoordinateAxis(0),direction)
elif (back == 'y'):
direction_on_imu = opensim.CoordinateDirection(opensim.CoordinateAxis(1),direction)
elif (back == 'z'):
direction_on_imu = opensim.CoordinateDirection(opensim.CoordinateAxis(2),direction)
else:
raise Exception("Invalid specification of heading axis '{}' found.".format(imu_axis))
# Compute rotation matrix so that (e.g. "pelvis_imu" + SimTK::ZAxis) lines up with model forward (+X)
heading_rotation_vec3 = opensim.OpenSenseUtilities_computeHeadingCorrection(self.model, quat_table, self.base_imu_label, direction_on_imu)
heading_rotation = opensim.Rotation(opensim.SpaceRotationSequence,
heading_rotation_vec3[0], opensim.CoordinateAxis(0),
heading_rotation_vec3[1], opensim.CoordinateAxis(1),
heading_rotation_vec3[2], opensim.CoordinateAxis(2))
opensim.OpenSenseUtilities_rotateOrientationTable(quat_table, heading_rotation)
orientations_data = opensim.OpenSenseUtilities_convertQuaternionsToRotations(quat_table)
imu_labels = orientations_data.getColumnLabels()
times = orientations_data.getIndependentColumn()
# The rotations of the IMUs at the start time in order
# The labels in the TimeSeriesTable of orientations
rotations = orientations_data.updRowAtIndex(0)
s = self.model.initSystem()
s.setTime(times[0])
# default pose of the model defined by marker-based IK
self.model.realizePosition(s)
imu_ix = 0
# bodies = imu_labels.size() # not sure what here
bodies={}
imuBodiesInGround={}
for imu_name in imu_labels:
ix = (imu_name[-4:]=="_imu")
if (ix == True):
body = self.model.get_BodySet().get(imu_name[:-4])
if body:
bodies[imu_ix] = body.safeDownCast(body)
imuBodiesInGround[imu_name] = body.getTransformInGround(s).R()
imu_ix += 1
# Now cycle through each imu with a body and compute the relative offset of the
# imu measurement relative to the body and update the modelOffset OR add an offset
# if none exists
imu_ix = 0
for imu_name in imu_labels:
if imu_name in imuBodiesInGround:
# operator * doesn't work with the opensim.Rotation() class
_11 = imuBodiesInGround[imu_name].get(0,0)
_12 = imuBodiesInGround[imu_name].get(0,1)
_13 = imuBodiesInGround[imu_name].get(0,2)
_21 = imuBodiesInGround[imu_name].get(1,0)
_22 = imuBodiesInGround[imu_name].get(1,1)
_23 = imuBodiesInGround[imu_name].get(1,2)
_31 = imuBodiesInGround[imu_name].get(2,0)
_32 = imuBodiesInGround[imu_name].get(2,1)
_33 = imuBodiesInGround[imu_name].get(2,2)
mat_inGround = np.mat(([_11,_12,_13],[_21,_22,_23],[_31,_32,_33]))
_11 = rotations(imu_ix).get(0,0)
_12 = rotations(imu_ix).get(0,1)
_13 = rotations(imu_ix).get(0,2)
_21 = rotations(imu_ix).get(1,0)
_22 = rotations(imu_ix).get(1,1)
_23 = rotations(imu_ix).get(1,2)
_31 = rotations(imu_ix).get(2,0)
_32 = rotations(imu_ix).get(2,1)
_33 = rotations(imu_ix).get(2,2)
mat_fromRot = np.mat(([_11,_12,_13],[_21,_22,_23],[_31,_32,_33]))
r_fb = np.dot(mat_inGround,mat_fromRot)
r_fb_asMat33 = opensim.Mat33(r_fb[0,0],r_fb[0,1],r_fb[0,2],r_fb[1,0],r_fb[1,1],r_fb[1,2],r_fb[2,0],r_fb[2,1],r_fb[2,2])
r_fb = opensim.Rotation(r_fb_asMat33)
mo = self.model.getBodySet().get(imu_name[:-4]).findComponent(imu_name)
if (mo):
imuOffset = self.model.getBodySet().get(imu_name[:-4])
def_T = imuOffset.getComponent(imu_name).getOffsetTransform().T()
new_transform = opensim.Transform(r_fb,def_T)
imuOffset.updComponent(imu_name).setOffsetTransform(new_transform)
else:
# Create an offset frame
body = bodies[imu_ix]
p_fb = opensim.Vec3(0)
if body:
p_fb = body.getMassCenter()
imuOffset = opensim.PhysicalOffsetFrame(imu_name, bodies[imu_ix], opensim.Transform(r_fb, p_fb))
brick = opensim.Brick(opensim.Vec3(0.02,0.01,0.005))
brick.upd_Appearance().set_color(opensim.Orange)
imuOffset.attachGeometry(brick)
bodies[imu_ix].addComponent(imuOffset)
imu_ix+=1
self.model.finalizeConnections()
if self.output_file_name:
self.model.printToXML(self.output_file_name)
# Skipped results visualisation
return self.model
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)
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')
linkage1 = osim.Body("foot", linkageMass1, linkageMassCenter1,
osim.Inertia(0.1, 0.1, 0.00662, 0., 0., 0.))
linkage2 = osim.Body("shank", linkageMass2, linkageMassCenter2,
osim.Inertia(0.1, 0.1, 0.1057, 0., 0., 0.))
linkage3 = osim.Body("thigh", linkageMass3, linkageMassCenter3,
osim.Inertia(0.2, 0.1, 0.217584, 0., 0., 0.))
linkage4 = osim.Body("HAT", linkageMass4, linkageMassCenter4,
osim.Inertia(1.1, 1.1, 1.48, 0., 0., 0.))
ZAxis = osim.CoordinateAxis(2)
R = osim.Rotation(-pi / 2, ZAxis)
sphere = osim.Sphere(0.04)
exo = osim.Cylinder(.05, exothick)
linkage1.attachGeometry(sphere.clone())
cyl1 = osim.Cylinder(linkageDiameter / 2, linkageLength1 / 2)
cyl1Frame = osim.PhysicalOffsetFrame(
linkage1, osim.Transform(osim.Vec3(0.0, linkageLength1 / 2, 0.0)))
cyl1Frame.setName("Cyl1_frame")
cyl1Frame.attachGeometry(cyl1.clone())
osimModel.addComponent(cyl1Frame)
linkage2.attachGeometry(sphere.clone())
cyl2 = osim.Cylinder(linkageDiameter / 2, linkageLength2 / 2)
cyl2Frame = osim.PhysicalOffsetFrame(
linkage2, osim.Transform(osim.Vec3(0.0, linkageLength2 / 2, 0.0)))
cyl2Frame.setName("Cyl2_frame")
cyl2Frame.attachGeometry(cyl2.clone())
cyl2Frame.attachGeometry(exo.clone())
osimModel.addComponent(cyl2Frame)
linkage3.attachGeometry(sphere.clone())
cyl3 = osim.Cylinder(linkageDiameter / 2, linkageLength3 / 2)