def testSoftFingerContact(self):
world = World()
add_sphere(world, name='ball0')
add_sphere(world, name='ball1')
bodies = world.getbodies()
bodies['ball0'].parentjoint.gpos[1,3] = 2.5
bodies['ball0'].parentjoint.gvel[4] = -1.
c = SoftFingerContact(world._shapes, 0.1)
world.register(c)
world.init()
world.update_dynamic()
dt = 0.001
world.update_controllers(dt)
world.update_constraints(dt)
world.integrate(dt)
world.update_dynamic()
self.assertListsAlmostEqual(c.jacobian,
[ [ 0., 1., 0., 0., 0., 0., 0. , -1. , 0. , 0., 0., 0.],
[-1., 0., 0., 0., 0., 1., -1.499, 0. , 0. , 0., 0., -1.],
[ 0., 0., -1., -1., 0., 0., 0. , 0. , -1.499, 1., 0., 0.],
[ 0., 0., 0., 0., 1., 0., 0. , 0. , 0. , 0., -1., 0.]])
self.assertListsAlmostEqual(bodies['ball0'].pose,
[ [ 1. , 0. , 0. , 0. ],
[ 0. , 1. , 0. , 2.499],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ] ])
class Test_simple_3R(unittest.TestCase):
def setUp(self):
self.w = World()
add_simplearm(self.w)
self.joints = self.w.getjoints()
self.frames = self.w.getframes()
self.w.update_dynamic()
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, .04, .01), [])
def runTest(self):
b0 = Body(mass=eye(6))
w = World()
w.add_link(w.ground, FreeJoint(), b0)
w.init()
ctrl = WeightController()
w.register(ctrl)
c0 = BallAndSocketConstraint(frames=(w.ground, b0))
w.register(c0)
w.init()
w.update_dynamic()
dt = 0.001
w.update_controllers(dt)
w.update_constraints(dt)
self.assertListsAlmostEqual(c0._force, [0., 9.81, 0.])
w.integrate(dt)
w.update_dynamic()
self.assertListsAlmostEqual(
b0.pose, [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
def create_3r_and_init(gpos=(0,0,0), gvel=(0,0,0), gravity=False):
## CREATE THE WORLD
w = World()
simplearm.add_simplearm(w, with_shapes=True)
## INIT
joints = w.getjoints()
joints["Shoulder"].gpos[:] = gpos[0]
joints["Elbow"].gpos[:] = gpos[1]
joints["Wrist"].gpos[:] = gpos[2]
joints["Shoulder"].gvel[:] = gvel[0]
joints["Elbow"].gvel[:] = gvel[1]
joints["Wrist"].gvel[:] = gvel[2]
w.update_dynamic()
## CTRL
if gravity:
w.register(WeightController())
return w
def runTest(self):
b0 = Body(mass=eye(6))
w = World()
w.add_link(w.ground, FreeJoint(), b0)
w.init()
ctrl = WeightController()
w.register(ctrl)
c0 = BallAndSocketConstraint(frames=(w.ground, b0))
w.register(c0)
w.init()
w.update_dynamic()
dt = 0.001
w.update_controllers(dt)
w.update_constraints(dt)
self.assertListsAlmostEqual(c0._force, [ 0. , 9.81, 0. ])
w.integrate(dt)
w.update_dynamic()
self.assertListsAlmostEqual(b0.pose,
[[1.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
simplearm.add_simplearm(w, with_shapes=True)
##### Init
joints = w.getjoints()
for i in range(3):
joints[i].gpos[:] = 0.5
w.update_dynamic()
##### Add ctrl
from arboris.controllers import WeightController
w.register(WeightController())
##### Add observers
from arboris.observers import PerfMonitor, Hdf5Logger
from arboris.visu.dae_writer import write_collada_scene, write_collada_animation, add_shapes_to_dae
from arboris.visu import pydaenimCom
flat = False
write_collada_scene(w, "./scene.dae", flat=flat)
shapes_info = [
["Arm" , "./dae/icub_simple.dae#head"], # parent frame, child shape node id
["Forearm" , "./dae/icub_simple.dae#head", transl(0,0.3,0)], # parent frame, child shape node id, H_frame_shape
["EndEffector", "./dae/icub_simple.dae" , transl(0,0.3,0), (0.3,0.3,0.3)], # parent frame, child shape file , H_frame_shape, shape_scale
]
This example shows how to use BalanceController.
"""
__author__ = ("Joseph Salini <*****@*****.**>")
from arboris.core import World, simulate, SubFrame
from arboris.homogeneousmatrix import transl
from arboris.robots.simplearm import add_simplearm
from arboris.visu_osg import Drawer
from arboris.qpcontroller import BalanceController, Task
#from arboris.controllers import WeightController
from numpy import arange
world = World()
add_simplearm(world, name='Left')
world.register(SubFrame(world.ground, transl(0.5, 0.5, 0), 'LeftTarget'))
joints = world.getjoints()
frames = world.getframes()
# set initial position:
joints[0].gpos = [0.1]
task = Task(controlled_frame=frames['LeftEndEffector'],
target_frame=frames['LeftTarget'],
world=world)
#world.register(WeightController())
world.register(BalanceController(world, [task]))
from arboris.core import JointsList
add_simplearm(world, name='Right')
joints=world.getjoints()
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()
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()
def create_icub_and_init(chair=False, gravity=False):
## CREATE THE WORLD
w = World()
w._up[:] = [0, 0, 1]
icub.add(w, create_shapes=False)
w.register(Plane(w.ground, (0, 0, 1, 0), "floor"))
if chair is True:
w.register(Sphere(w.getbodies()['l_hip_2'], .0325, name='l_gluteal'))
w.register(Sphere(w.getbodies()['r_hip_2'], .0325, name='r_gluteal'))
w.register(
Box(SubFrame(w.ground, transl(.2, 0, 0.26)), (.075, .1, .02),
name='chair'))
w.register(
Box(SubFrame(w.ground, transl(.255, 0, 0.36)), (.02, .1, .1),
name='chair_back'))
## INIT
joints = w.getjoints()
joints['root'].gpos[0:3, 3] = [0, 0, .598]
joints['l_shoulder_roll'].gpos[:] = pi / 8
joints['r_shoulder_roll'].gpos[:] = pi / 8
joints['l_elbow_pitch'].gpos[:] = pi / 8
joints['r_elbow_pitch'].gpos[:] = pi / 8
joints['l_knee'].gpos[:] = -0.1
joints['r_knee'].gpos[:] = -0.1
joints['l_ankle_pitch'].gpos[:] = -0.1
joints['r_ankle_pitch'].gpos[:] = -0.1
shapes = w.getshapes()
floor_const = [
SoftFingerContact((shapes[s], shapes['floor']), 1.5, name=s)
for s in ['lf1', 'lf2', 'lf3', 'lf4', 'rf1', 'rf2', 'rf3', 'rf4']
]
for c in floor_const:
w.register(c)
if chair is True:
chair_const = [
SoftFingerContact((shapes[s], shapes['chair']), 1.5, name=s)
for s in ['l_gluteal', 'r_gluteal']
]
for c in chair_const:
w.register(c)
w.update_dynamic()
## CTRL
if gravity:
w.register(WeightController())
return w
w = World()
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()
def create_icub_and_init(chair=False, gravity=False):
## CREATE THE WORLD
w = World()
w._up[:] = [0,0,1]
icub.add(w, create_shapes=False)
w.register(Plane(w.ground, (0,0,1,0), "floor"))
if chair is True:
w.register(Sphere(w.getbodies()['l_hip_2'], .0325, name='l_gluteal'))
w.register(Sphere(w.getbodies()['r_hip_2'], .0325, name='r_gluteal'))
w.register(Box(SubFrame(w.ground, transl(.2, 0, 0.26 )), (.075, .1, .02), name='chair'))
w.register(Box(SubFrame(w.ground, transl(.255, 0, 0.36 )), (.02, .1, .1), name='chair_back'))
## INIT
joints = w.getjoints()
joints['root'].gpos[0:3,3] = [0,0,.598]
joints['l_shoulder_roll'].gpos[:] = pi/8
joints['r_shoulder_roll'].gpos[:] = pi/8
joints['l_elbow_pitch'].gpos[:] = pi/8
joints['r_elbow_pitch'].gpos[:] = pi/8
joints['l_knee'].gpos[:] = -0.1
joints['r_knee'].gpos[:] = -0.1
joints['l_ankle_pitch'].gpos[:] = -0.1
joints['r_ankle_pitch'].gpos[:] = -0.1
shapes = w.getshapes()
floor_const = [SoftFingerContact((shapes[s], shapes['floor']), 1.5, name=s)for s in ['lf1', 'lf2', 'lf3', 'lf4', 'rf1', 'rf2', 'rf3', 'rf4']]
for c in floor_const:
w.register(c)
if chair is True:
chair_const = [SoftFingerContact((shapes[s], shapes['chair']), 1.5, name=s)for s in ['l_gluteal', 'r_gluteal']]
for c in chair_const:
w.register(c)
w.update_dynamic()
## CTRL
if gravity:
w.register(WeightController())
return w
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)
