def simulate(self):
self.lqpc = LQPcontroller(self.gforcemax,
self.dgforcemax,
qlim=self.qlim,
vlim=self.vlim,
tasks=self.tasks,
options=self.options,
solver_options={"show_progress": False})
self.w.register(self.lqpc)
simulate(self.w, arange(0, .04, .01), [])
def testJoinLimits(self):
world = simplearm()
a = WeightController()
world.register(a)
joints = world.getjoints()
c = JointLimits(joints['Shoulder'], -3.14/2, 3.14/2)
world.register(c)
joints['Shoulder'].gpos[0] = 3.14/2 - 0.1
time = arange(0., 0.1, 1e-3)
simulate(world, time)
self.assertTrue(3.14/2 >= joints['Shoulder'].gpos[0])
joints['Shoulder'].gpos[0] = -3.14/2 + 0.1
time = arange(0., 0.1, 1e-3)
simulate(world, time)
self.assertTrue(-3.14/2 <= joints['Shoulder'].gpos[0])
############################
# #
# SET OBSERVERS & SIMULATE #
# #
############################
obs = get_usual_observers(w)
from common import RecordCoMPosition, RecordZMPPosition
obs.append(RecordCoMPosition(icub_bodies))
obs.append(RecordZMPPosition(icub_bodies))
## SIMULATE
from numpy import arange
from arboris.core import simulate
simulate(w, arange(0,6.,dt), obs)
###########
# #
# RESULTS #
# #
###########
print("end of the simulation")
from numpy import array
import pylab as pl
pl.figure()
com = array(obs[-2].get_record())
pl.plot(com[:,[0,1]])
############################
# #
# SET OBSERVERS & SIMULATE #
# #
############################
obs = get_usual_observers(w)
from common import RecordFramePosition
obs.append(RecordFramePosition(frames["EndEffector"]))
## SIMULATE
from numpy import arange
from arboris.core import simulate
simulate(w, arange(0,8,dt), obs)
###########
# #
# RESULTS #
# #
###########
print("end of the simulation")
import pylab as pl
res = obs[-1].get_record()
pl.plot(res)
xlim = pl.xlim()
pl.plot(x[:len(res)], 'b:', lw=2)
pl.plot(y[:len(res)], 'g:', lw=2)
else:
H_bc = eye(4)
half_extents = (.5, .5, .5)
mass = 1.
body = Body(name='box_body',
mass=massmatrix.transport(massmatrix.box(half_extents, mass),
H_bc))
subframe = SubFrame(body, H_bc, name="box_com")
if True:
twist_c = array([0., 0., 0., 0., 0., 0.])
else:
twist_c = array([1, 1, 1, 0, 0, 0.])
twist_b = dot(homogeneousmatrix.adjoint(H_bc), twist_c)
freejoint = FreeJoint(gpos=homogeneousmatrix.inv(H_bc), gvel=twist_b)
w.add_link(w.ground, freejoint, body)
w.register(Box(subframe, half_extents))
weightc = WeightController()
w.register(weightc)
obs = TrajLog(w.getframes()['box_com'], w)
from arboris.visu_osg import Drawer
timeline = arange(0, 1, 5e-3)
simulate(w, timeline, [obs, Drawer(w)])
time = timeline[:-1]
obs.plot_error()
show()
from arboris.core import World, simulate, SubFrame from arboris.homogeneousmatrix import transl from numpy import array, arange, eye from arboris.controllers import WeightController from arboris.observers import Hdf5Logger time = arange(0, .05, 0.01) file_name = "the_temporary_file_where_we_save_simulation_data.h5" dest_in_file = "test/xp" world = World() world.register( WeightController() ) h5obs = Hdf5Logger(world, len(time)-1, file_name, dest_in_file, 'w') from arboris.robots import simplearm simplearm.add_simplearm(world) world.getjoints()[0].gpos = array([0.1]) simulate(world, time,[h5obs] ) import os os.remove(file_name)
def simulate(self, options):
self.lqpc = LQPcontroller(self.gforcemax, tasks=self.tasks, options=options, solver_options={"show_progress":False})
self.w.register(self.lqpc)
simulate(self.w, arange(0, 4., .01), [])
self.check_results()
]
add_shapes_to_dae("./scene.dae", shapes_info) # add argument output_file="out.dae" if you want to keep original dae file
obs = []
pobs = PerfMonitor(True)
dobs = pydaenimCom("./scene.dae", flat=flat)
h5obs = Hdf5Logger("sim.h5", mode="w", flat=flat)
obs.append(pobs)
obs.append(dobs)
obs.append(h5obs)
##### Simulate
from arboris.core import simulate
from numpy import arange
timeline = arange(0, 5., 0.005)
simulate(w, timeline, obs)
##### Results
print pobs.get_summary()
write_collada_animation("anim.dae", "scene.dae", "sim.h5")
############################
# #
# SET OBSERVERS & SIMULATE #
# #
############################
obs = get_usual_observers(w)
from common import RecordCoMPosition, RecordZMPPosition
obs.append(RecordCoMPosition(icub_bodies))
obs.append(RecordZMPPosition(icub_bodies))
## SIMULATE
from numpy import arange
from arboris.core import simulate
simulate(w, arange(0, 6., dt), obs)
###########
# #
# RESULTS #
# #
###########
print("end of the simulation")
from numpy import array
import pylab as pl
pl.figure()
com = array(obs[-2].get_record())
pl.plot(com[:, [0, 1]])
pl.ylabel("Position (m)")
pl.legend(['x', 'y'])
def test_min_joint_limit(self):
self.shoulder.gpos[0] = -3.14/2 + 0.1
time = arange(0., 0.1, 1e-3)
simulate(self.world, time)
self.assertTrue(-3.14/2 <= self.shoulder.gpos[0])
def test_max_joint_limit(self):
self.shoulder.gpos[0] = 3.14/2 - 0.1
time = arange(0., 0.1, 1e-3)
simulate(self.world, time)
self.assertTrue(3.14/2 >= self.shoulder.gpos[0])
def simulate_py(world, timeline, name):
observers = [Hdf5Logger(name+'_py.h5')]
simulate(world, timeline, observers)
def simulate_py(world, timeline, name):
world.observers.append(Hdf5Logger(world))
simulate(world, timeline)
world.observers[-1].write(name + "_py.h5")
def simulate_py(world, timeline, name):
observers = [Hdf5Logger(name + '_py.h5')]
simulate(world, timeline, observers)
njoints = 9
lengths = [1., .9, .8 , .7 , .6, .5, .4 , .3, .2]
masses = [1., .9, .8 , .7 , .6, .5, .4 , .3, .2]
gvel = gvel=[2.]*njoints
gpos = [0, 3.14159/4., 0, 0, 0, 0, 0, 0, 0]
#gpos = [0.]*njoints
if use_snake:
add_snake(w, njoints, lengths=lengths, masses=masses, gpos=gpos, gvel=gvel, is_fixed=False)
else:
assert njoints == 9
add_robot(w, gpos=gpos, gvel=gvel, is_fixed=is_fixed)
if with_weight:
w.register(arboris.controllers.WeightController())
t_end = 2.08
else:
t_end = 1.430
nrj = EnergyMonitor(w)
mM = MassMonitor(w)
w.observers.append(mM)
#w.observers.append(Drawer(w))
timeline = arange(0, t_end, 0.005)
simulate(w, timeline, (nrj, mM))
nrj.plot()
#mM.plot()
w.register(lqpc)
############################
# #
# SET OBSERVERS & SIMULATE #
# #
############################
obs = get_usual_observers(w)
from common import RecordFramePosition
obs.append(RecordFramePosition(frames["EndEffector"]))
## SIMULATE
from numpy import arange
from arboris.core import simulate
simulate(w, arange(0, 10, 0.01), obs)
###########
# #
# RESULTS #
# #
###########
print("end of the simulation")
import pylab as pl
pl.plot(obs[-1].get_record())
xlim = pl.xlim()
pl.plot(xlim, [goal[0, 3], goal[0, 3]], 'r:')
pl.plot(xlim, [goal[1, 3], goal[1, 3]], 'r:')
pl.ylim([-1., 1.])
pl.ylabel("Position (m)")
############################
# #
# SET OBSERVERS & SIMULATE #
# #
############################
obs = get_usual_observers(w)
from common import RecordWrench
obs.append(RecordWrench(const["const"]))
## SIMULATE
from numpy import arange
from arboris.core import simulate
simulate(w, arange(0,3,0.01), obs)
###########
# #
# RESULTS #
# #
###########
print("end of the simulation")
import pylab as pl
pl.plot(obs[-1].get_record())
pl.ylim([0,.015])
pl.ylabel("Force (N)")
pl.legend(['tau_z', 'x','y','z'])
pl.xlabel("step")
def simulate(self):
self.lqpc = LQPcontroller(self.gforcemax, self.dgforcemax, qlim=self.qlim, vlim=self.vlim, tasks=self.tasks, options=self.options, solver_options={"show_progress":False})
self.w.register(self.lqpc)
simulate(self.w, arange(0, .04, .01), [])
def test_max_joint_limit(self):
self.shoulder.gpos[0] = 3.14 / 2 - 0.1
time = arange(0., 0.1, 1e-3)
simulate(self.world, time)
self.assertTrue(3.14 / 2 >= self.shoulder.gpos[0])
def test_min_joint_limit(self):
self.shoulder.gpos[0] = -3.14 / 2 + 0.1
time = arange(0., 0.1, 1e-3)
simulate(self.world, time)
self.assertTrue(-3.14 / 2 <= self.shoulder.gpos[0])
body = Body(
name='box_body',
mass=massmatrix.transport(massmatrix.box(lengths, mass), H_bc))
subframe = SubFrame(body, H_bc, name="box_com")
if True:
twist_c = array([0.,0.,0.,0.,0.,0.])
else:
twist_c = array([1,1,1,0,0,0.])
twist_b = dot(homogeneousmatrix.adjoint(H_bc), twist_c)
freejoint = FreeJoint(gpos=homogeneousmatrix.inv(H_bc), gvel=twist_b)
w.add_link(w.ground, freejoint, body)
w.register(Box(subframe, lengths))
weightc = WeightController(w)
w.register(weightc)
obs = TrajLog(w.getframes()['box_com'], w)
w.observers.append(obs)
from arboris.visu_osg import Drawer
w.observers.append(Drawer(w))
timeline = arange(0,1,5e-3)
simulate(w, timeline)
time = timeline[:-1]
obs.plot_error()
show()
half_extents = (.5,.5,.5)
mass = 1.
body = Body(
name='box_body',
mass=massmatrix.transport(massmatrix.box(half_extents, mass), H_bc))
subframe = SubFrame(body, H_bc, name="box_com")
if True:
twist_c = array([0.,0.,0.,0.,0.,0.])
else:
twist_c = array([1,1,1,0,0,0.])
twist_b = dot(homogeneousmatrix.adjoint(H_bc), twist_c)
freejoint = FreeJoint(gpos=homogeneousmatrix.inv(H_bc), gvel=twist_b)
w.add_link(w.ground, freejoint, body)
w.register(Box(subframe, half_extents))
weightc = WeightController()
w.register(weightc)
obs = TrajLog(w.getframes()['box_com'], w)
from arboris.visu_osg import Drawer
timeline = arange(0,1,5e-3)
simulate(w, timeline, [obs, Drawer(w)])
time = timeline[:-1]
obs.plot_error()
show()
(shapes['r_hand_palm'], shapes['wall'])]
})
opt = {'avoidance horizon': .1}
sopt = {"show_progress": False}
lqpc = LQPcontroller(gforcemax,
tasks=tasks,
data=data,
options=opt,
solver_options=sopt)
w.register(lqpc)
w.register(KpTable(bodies['wall'], 0.05, 2., 100.))
############################
# #
# SET OBSERVERS & SIMULATE #
# #
############################
obs = get_usual_observers(w)
## SIMULATE
from numpy import arange
from arboris.core import simulate
simulate(w, arange(0, 5., 0.01), obs)
###########
# #
# RESULTS #
# #
###########
print("end of the simulation")
############################
# #
# SET OBSERVERS & SIMULATE #
# #
############################
obs = get_usual_observers(w)
from common import RecordCoMPosition, RecordGforce
obs.append(RecordGforce(lqpc))
## SIMULATE
from numpy import arange
from arboris.core import simulate
simulate(w, arange(0,1.5,0.01), obs)
###########
# #
# RESULTS #
# #
###########
print("end of the simulation")
import pylab as pl
pl.plot(obs[-1].get_record())
xlim = pl.xlim()
pl.ylabel("Tau (N.m)")
pl.xlabel("step")