def __init__(self,
visualize=True,
min_obstacles=0,
max_obstacles=3,
obstacles_bias=1.1,
psoas=0.1,
obstacle_radius=0.05,
random_push=0.025):
self.max_obstacles = np.random.randint(min_obstacles,
max_obstacles + 1)
self.obstacles_bias = np.random.uniform(0.9, obstacles_bias)
if self.max_obstacles == 2: self.max_obstacles = 0
print("Max obstacles = ", self.max_obstacles)
print("Obstacles bias = ", self.obstacles_bias)
self.psoas = psoas
print("psoas value = ", self.psoas)
self.obstacle_radius = obstacle_radius
print("Obstacle base radius = ", self.obstacle_radius)
self.random_push = random_push
print("Random push max value = ", self.random_push)
super(RunEnv, self).__init__(visualize=False, noutput=self.noutput)
self.osim_model.model.setUseVisualizer(visualize)
self.create_obstacles()
state = self.osim_model.model.initSystem()
if visualize:
manager = opensim.Manager(self.osim_model.model)
manager.setInitialTime(-0.00001)
manager.setFinalTime(0.0)
manager.integrate(state)
def reset_manager(self):
self.manager = opensim.Manager(self.model)
self.manager.setIntegratorMethod(self.integrator_method)
self.manager.setIntegratorAccuracy(self.integrator_accuracy)
#self.manager.setIntegratorInternalStepLimit(self.integrator_numsteps)
#self.manager.setIntegratorMinimumStepSize(self.integrator_minstepsize)
#self.manager.setIntegratorMaximumStepSize(self.integrator_maxstepsize)
self.manager.initialize(self.state)
def test_ManagerConstructorCreatesIntegrator(self):
# Make sure that the Manager is able to create a default integrator.
# This tests a bug fix: previously, it was impossible to use the
# Manager to integrate from MATLAB/Python, since it was not possible
# to provide an Integrator to the Manager.
model = osim.Model(os.path.join(test_dir, "arm26.osim"))
state = model.initSystem()
manager = osim.Manager(model)
state.setTime(0)
manager.initialize(state)
state = manager.integrate(0.00001)
def __init__(self, visualize=True, max_obstacles=3):
self.max_obstacles = max_obstacles
super(RunEnv, self).__init__(visualize=False, noutput=self.noutput)
self.osim_model.model.setUseVisualizer(visualize)
self.create_obstacles()
state = self.osim_model.model.initSystem()
if visualize:
manager = opensim.Manager(self.osim_model.model)
manager.setInitialTime(-0.00001)
manager.setFinalTime(0.0)
manager.integrate(state)
def reset_manager(self):
self.manager = opensim.Manager(self.model)
self.manager.setIntegratorAccuracy(self.integrator_accuracy)
#self.manager.setIntegratorMaximumStepSize(self.integrator_MaxStepSize) #Shakiba
'''
self.manager.setUseSpecifiedDT(True)
dt_step = self.stepsize/80
DT_Array = opensim.ArrayDouble(dt_step, int(50/dt_step)).getAsVector()
self.manager.setDTArray(DT_Array)
'''
#self.manager.setIntegratorMethod(opensim.Manager.IntegratorMethod_SemiExplicitEuler2)
self.manager.initialize(self.state)
print('old integrator')
def __init__(self, visualize = False):
self.iepisode = 0
self.shoulder = 0.0
self.elbow = 0.0
super(ArmEnv, self).__init__(visualize = visualize)
self.osim_model.model.setUseVisualizer(visualize)
state = self.osim_model.model.initSystem()
self.timestep_limit = 200
self.spec.timestep_limit = 200
if visualize:
manager = opensim.Manager(self.osim_model.model)
manager.setInitialTime(-0.00001)
manager.setFinalTime(0.0)
manager.integrate(state)
def _step(self, action):
self.activate_muscles(action)
# Integrate one step
manager = opensim.Manager(self.osim_model.model)
manager.setInitialTime(self.stepsize * self.istep)
manager.setFinalTime(self.stepsize * (self.istep + 1))
try:
manager.integrate(self.osim_model.state)
except Exception as e:
print (e)
return self.get_observation(), -500, True, {}
self.istep = self.istep + 1
res = [ self.get_observation(), self.compute_reward(), self.is_done(), {} ]
return res
def walker_simulation_objective_function(candsol):
global model, initial_state, all_distances, all_candsols
# Set the initial hip and knee angles.
initial_state.updQ()[3] = candsol[0] # left hip
initial_state.updQ()[4] = candsol[1] # right hip
initial_state.updQ()[5] = candsol[2] # left knee
initial_state.updQ()[6] = candsol[3] # right knee
# Set the initial forward velocity of the pelvis.
vx0 = candsol[4] # needed to compute the objective function, below
initial_state.updU()[1] = vx0
# Set the initial hip and knee angular velocities.
initial_state.updU()[3] = candsol[5] # left hip
initial_state.updU()[4] = candsol[6] # right hip
initial_state.updU()[5] = candsol[7] # left knee
initial_state.updU()[6] = candsol[8] # right knee
# Simulate.
manager = osim.Manager(model)
manager.initialize(initial_state)
manager.integrate(10.0)
# Get the final location of the pelvis in the X direction.
st = manager.getStatesTable()
dc = st.getDependentColumn('/jointset/PelvisToPlatform/Pelvis_tx/value')
x = dc[ dc.nrow()-1 ]
# Store the candidate solution and the distance traveled.
all_candsols.append(candsol)
all_distances.append(x)
print('Distance traveled: %f meters' % (x))
# To maximize distance, minimize its negative. Also penalize candidate
# solutions that increase the initial pelvis velocity beyond 2 m/s (to
# avoid simply launching the model forward). This could also be done by
# adding a hard constraint.
k = 10.0
penalty1 = k*(vx0**2.0) if vx0 < 0 else 0.0 # lower bound: 0 m/s
penalty2 = k*((vx0-2.0)**2.0) if vx0 > 2 else 0.0 # upper bound: 2 m/s
J = -x + penalty1 + penalty2
return (J)
def step(self, action):
self.activate_muscles(action)
# Integrate one step
if self.istep == 0:
print("Initializing the model!")
self.manager = opensim.Manager(self.osim_model.model)
self.osim_model.state.setTime(self.stepsize * self.istep)
self.manager.initialize(self.osim_model.state)
try:
self.osim_model.state = self.manager.integrate(self.stepsize *
(self.istep + 1))
except Exception as e:
print(e)
return self.get_observation(), -500, True, {}
self.istep = self.istep + 1
res = [
self.get_observation(),
self.compute_reward(),
self.is_done(), {}
]
return res
model.addComponent(ballBody)
# Add contact
ballContact = opensim.ContactSphere(r, opensim.Vec3(0,0,0), ballBody);
model.addContactGeometry(ballContact)
# Reinitialize the system with the new controller
state = model.initSystem()
for i in range(6):
ballJoint.getCoordinate(i).setLocked(state, True)
# Simulate
for i in range(100):
t = state.getTime()
manager = opensim.Manager(model)
manager.integrate(state, t + stepsize)
# Restart the model every 10 frames, with the new position of the ball
if (i + 1) % 10 == 0:
newloc = opensim.Vec3(float(i) / 5, 0, 0)
opensim.PhysicalOffsetFrame.safeDownCast(ballJoint.getChildFrame()).set_translation(newloc)
r = i * 0.005
#ballGeometry.set_radii(opensim.Vec3(r,r,r))
#ballBody.scale(opensim.Vec3(r, r, r))
ballContact.setRadius(r)
state = model.initializeState()
ballJoint.getCoordinate(3).setValue(state, i / 100.0)
#radiusCenter.setParentFrame(radius)
#radiusCenter.setOffsetTransform(osim.Transform(osim.Vec3(0, 0.5, 0)))
#radius.addComponent(radiusCenter)
#radiusCenter.attachGeometry(bodyGeometry.clone())
radius.attachGeometry(bodyGeometry.clone())
# ---------------------------------------------------------------------------
# Configure the model.
# ---------------------------------------------------------------------------
state = arm.initSystem()
# Fix the shoulder at its default angle and begin with the elbow flexed.
shoulder.getCoordinate().setLocked(state, True)
elbow.getCoordinate().setValue(state, 0.5 * osim.SimTK_PI)
arm.equilibrateMuscles(state)
# ---------------------------------------------------------------------------
# Simulate.
# ---------------------------------------------------------------------------
manager = osim.Manager(arm)
state.setTime(0)
manager.initialize(state)
state = manager.integrate(10.0)
# ---------------------------------------------------------------------------
# Print/save model file
# ---------------------------------------------------------------------------
#arm.printToXML("SimpleArm.osim")
# Create the force reporter for obtaining the forces applied to the model
# during a forward simulation
fReporter = osim.ForceReporter(pendulumDF)
sReporter = osim.StatesReporter(pendulumDF)
bKinematics = osim.BodyKinematics(pendulumDF, True)
bodiesToRecord = osim.ArrayStr()
bKinematics.setModel(pendulumDF)
bKinematics.setBodiesToRecord(bodiesToRecord)
pendulumDF.addAnalysis(fReporter)
pendulumDF.addAnalysis(sReporter)
pendulumDF.addAnalysis(bKinematics)
manager = osim.Manager(pendulumDF)
manager.setIntegratorMethod(
osim.Manager.IntegratorMethod_RungeKuttaFeldberg)
manager.setIntegratorAccuracy(1.0e-12)
manager.setIntegratorMaximumStepSize(stepSize)
# print simulation information
# pendulumDF.printDetailedInfo(si , sys.stdout);
pendulumDF.printDetailedInfo(si)
# Integrate from initial time to final time
si.setTime(initialTime)
print("\nIntegrating from ", initialTime, " to ", finalTime)
manager.initialize(si)
manager.integrate(finalTime)
# Some meta parameters
stepsize = 0.02
nsteps = 100
# Get the model
model = osim.Model("../models/gait9dof18musc_Thelen_BigSpheres_20161017.osim")
# Enable the visualizer
model.setUseVisualizer(True)
# Get the muscles
muscleSet = model.getMuscles()
# Initialize simulation
state = model.initSystem()
model.equilibrateMuscles(state)
manager = osim.Manager(model)
# Simulatie
for i in range(0, nsteps):
# activate the muscles corresponding to the current step
# yes, this makes no sense, it's just a test
for j in range(0, 18):
muscle = muscleSet.get(j)
muscle.setActivation(state, 1.0 * (-1)**j)
# Integrate one step
manager.setInitialTime(stepsize * i)
manager.setFinalTime(stepsize * (i + 1))
manager.integrate(state)
def reset(self):
if not self.state0:
self.state0 = self.model.initSystem()
self.manager = opensim.Manager(self.model)
self.state = opensim.State(self.state0)
# 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
# counter-clockwise from its default pose.
model.setStateVariableValue(state, 'joint/q/value', 0.25 * math.pi)
manager = osim.Manager(model, state)
manager.integrate(1.0)
# Analyze the simulation.
# -----------------------
# Retrieve the StatesTrajectory from the reporter.
statesTraj = reporter.getStates()
for itime in range(statesTraj.getSize()):
state = statesTraj[itime]
time = state.getTime()
q = model.getStateVariableValue(state, 'joint/q/value')
# Calculating joint reactions requires realizing to Acceleration.
model.realizeAcceleration(state)
# This returns a SpatialVec, which is two Vec3's ([0]: moment, [1]: force).
def reset_manager(self):
self.manager = opensim.Manager(self.model)
self.manager.setIntegratorAccuracy(self.integrator_accuracy)
self.manager.initialize(self.state)
# Create and print the model file.
print_model()
# Load the model file.
deserialized_model = osim.Model(model_filename)
state = deserialized_model.initSystem()
# We can fetch the TableReporter from within the deserialized model.
reporter = osim.TableReporterVec3.safeDownCast(
deserialized_model.getComponent('reporter'))
# We can access the names of the outputs that the reporter is connected to.
print('Outputs connected to the reporter:')
for i in range(reporter.getInput('inputs').getNumConnectees()):
print(reporter.getInput('inputs').getConnecteeName(i))
# Simulate the model.
manager = osim.Manager(deserialized_model)
manager.setInitialTime(0)
manager.setFinalTime(1.0)
manager.integrate(state)
# Now that the simulation is done, get the table from the TableReporter and
# write it to a file.
# This returns the TimeSeriesTableVec3 that holds the history of positions.
table = reporter.getTable()
# Create a FileAdapter, which handles writing to (and reading from) .sto files.
sto = osim.STOFileAdapterVec3()
sto.write(table, 'wiring_inputs_and_outputs_with_TableReporter.sto')
# You can open the .sto file in a text editor and see that both outputs
# (position of body's origin, and position of system mass center) are the same.
def reset_manager(self):
self.manager = opensim.Manager()
type_ = plugins.my_manager_factory.integrator.SemiExplicitEuler2
plugins.my_manager_factory.my_manager_factory.create(self.model, self.manager,type_,self.integrator_accuracy)
self.manager.initialize(self.state)
