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 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 __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()
# 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
# Add a console reporter to print the muscle fibre force and elbow angle.
# ---------------------------------------------------------------------------
# We want to write our simulation results to the console.
reporter = osim.ConsoleReporter()
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())
humerus.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())
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
state0 = model.initSystem()
state = opensim.State(state0)
# Change max force
muscleSet.get(0).setMaxIsometricForce(100000.0)
