def dynamicalModelTest(self): w = World() add_human36(w) w.update_dynamic() self.assertEqual(w.mass[5,5], 73.000000000000014) self.assertEqual(w.mass[41, 41], 0.10208399155688053) # neck self.assertEqual(w.mass[40,40], 0.020356790291165189) # neck self.assertEqual(w.mass[39, 39], 0.10430013572386694) # neck self.assertEqual(w.mass[16, 16], 0.0093741757009949949) # foot self.assertEqual(w.mass[17, 17], 0.001397215796713388) # foot self.assertEqual(w.mass[10, 10], 0.0093741757009949949) # foot self.assertEqual(w.mass[11, 11], 0.001397215796713388) # foot
def testGeometricUpdate(self):
w = World()
add_simplearm(w)
bodies = w.getbodies()
arm = bodies['Arm']
forearm = bodies['ForeArm']
hand = bodies['Hand']
w.update_geometric()
self.assertListsAlmostEqual(arm.pose, eye(4))
self.assertListsAlmostEqual(forearm.pose, [ [ 1. , 0. , 0. , 0. ],
[ 0. , 1. , 0. , 0.5],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ] ])
self.assertListsAlmostEqual(hand.pose, [ [ 1. , 0. , 0. , 0. ],
[ 0. , 1. , 0. , 0.9],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ] ])
def test(self):
w = World()
(bodies, tags) = _human36(w)
w.update_geometric()
def tag_positions(tag_frames):
pos_dict= {}
for (key, val) in tag_frames.iteritems():
pos_dict[key] = dot(val.body.pose, val._bpose)[0:3,3]
pos_array = zeros((len(tag_frames),3))
for t in _humans_tags(1.741):
pos_array[t['HumansId']-1,:] = pos_dict[t['HumansName']]
return pos_array
p = tag_positions(tags)
self.assertListsAlmostEqual(p,
[[ 2.15013500e-01, 2.08166817e-17, 8.72241000e-02],
[ -4.31768000e-02, 2.08166817e-17, 8.72241000e-02],
[ 1.15254200e-01, 2.08166817e-17, 1.40324600e-01],
[ 1.15254200e-01, 2.08166817e-17, 3.41236000e-02],
[ 0.00000000e+00, 3.86502000e-02, 1.30575000e-01],
[ 0.00000000e+00, 4.72681500e-01, 1.37713100e-01],
[ 0.00000000e+00, 8.94874000e-01, 1.63828100e-01],
[ 4.71811000e-02, 9.58594600e-01, 1.21347700e-01],
[ 2.15013500e-01, 2.08166817e-17, -8.72241000e-02],
[ -4.31768000e-02, 2.08166817e-17, -8.72241000e-02],
[ 1.15254200e-01, 2.08166817e-17, -1.40324600e-01],
[ 1.15254200e-01, 2.08166817e-17, -3.41236000e-02],
[ 0.00000000e+00, 3.86502000e-02, -1.30575000e-01],
[ 0.00000000e+00, 4.72681500e-01, -1.37713100e-01],
[ 0.00000000e+00, 8.94874000e-01, -1.63828100e-01],
[ 4.71811000e-02, 9.58594600e-01, -1.21347700e-01],
[ 1.20651300e-01, 1.25613150e+00, 0.00000000e+00],
[ 9.15766000e-02, 1.42674950e+00, 0.00000000e+00],
[ 0.00000000e+00, 1.47932770e+00, 2.25459500e-01],
[ 0.00000000e+00, 1.16316210e+00, 2.62194600e-01],
[ 0.00000000e+00, 8.96266800e-01, 2.83086600e-01],
[ 0.00000000e+00, 7.04408600e-01, 2.25459500e-01],
[ 0.00000000e+00, 1.47932770e+00, -2.25459500e-01],
[ 0.00000000e+00, 1.16316210e+00, -2.62194600e-01],
[ 0.00000000e+00, 8.96266800e-01, -2.83086600e-01],
[ 0.00000000e+00, 7.04408600e-01, -2.25459500e-01],
[ 0.00000000e+00, 1.49813050e+00, 0.00000000e+00],
[ 0.00000000e+00, 1.74100000e+00, 0.00000000e+00]])
def testLinksReplacement(self):
w = World()
bulb = RyRxJoint(name='bulb')
stem = Body(name='stem', mass=eye(6)*0.1)
w.add_link(w.ground, bulb, stem)
new_bulb = RzRxJoint(name='bulb')
w.replace_joint(bulb, new_bulb) # By another joint
joints = w.getjoints()
self.assertEqual(len(joints), 1)
self.assertEqual(joints[0], new_bulb)
new_new_bulb = RzRyRxJoint(name='bulb')
w.replace_joint(new_bulb, w.ground, new_new_bulb, stem) # By a kinematic chain
joints = w.getjoints()
self.assertEqual(len(joints), 1)
self.assertEqual(joints[0], new_new_bulb)
def testLinksCreation(self):
w = World()
bulb = RyRxJoint(name='bulb')
stem = Body(name='stem', mass=eye(6)*0.1)
w.add_link(w.ground, bulb, stem)
stem_top = SubFrame(stem, bpose=transl(0, 0, 1), name='stem_top')
flowerR = RyJoint(gpos=.5, name='flowerR')
leafR = Body('leafR', mass=eye(6)*0.01)
w.add_link(stem_top, flowerR, leafR)
flowerL = RyJoint(gpos=(-.5), name='flowerL')
leafL = Body('leafL', mass=eye(6)*.01)
w.add_link(stem_top, flowerL, leafL)
frames = [ w.ground, stem, leafR, leafL, stem_top ]
for frame1, frame2 in zip(w.iterframes(), frames):
self.assertEqual(frame1, frame2)
joints = [ bulb, flowerR, flowerL ]
for joint1, joint2 in zip(w.iterjoints(), joints):
self.assertEqual(joint1, joint2)
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 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 testGlobal(self):
world = World()
Ba = Body()
Rzyx = RzRyRxJoint()
world.add_link(world.ground, Rzyx, Ba)
Bzy = Body(name='zy')
Byx = Body(name='yx')
Bb = Body()
Rz = RzJoint()
world.add_link(world.ground, Rz, Bzy)
Ry = RyJoint()
world.add_link(Bzy, Ry, Byx)
Rx = RxJoint()
world.add_link(Byx, Rx, Bb)
world.init()
(az, ay, ax) = (3.14/6, 3.14/4, 3.14/3)
Rzyx.gpos[:] = (az, ay, ax)
(Rz.gpos[0], Ry.gpos[0], Rx.gpos[0]) = (az, ay, ax)
world.update_dynamic()
Ja = Ba.jacobian[:,0:3]
Jb = Bb.jacobian[:,3:6]
self.assertTrue(norm(Ja-Jb) < 1e-10)
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]])
#!/urs/bin/python #coding=utf-8 #author=joseph salini #date=24 jan 2010 from arboris.core import World, simulate, NamedObjectsList from numpy import array, arange import os ######################################################## ## WORLD BUILDING ######################################################## w = World() from arboris.robots import human36 human36.add_human36(w) w.update_dynamic() ######################################################## ## INIT ######################################################## joints = w.getjoints() bodies = w.getbodies() frames = w.getframes() shapes = w.getshapes() joints[0].gpos[0:3,3] = array([.1, .2, .3])
#!/usr/bin/env python
"""
This example show how to add mesh from a collada file to another,
in order to display complex meshes during visualization.
"""
from arboris.core import World
from arboris.robots import icub
##### Create world
w = World()
icub.add(w, create_shapes=False)
w._up[:] = [0,0,1]
##### 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)
add_shapes_to_dae("./scene.dae", "./dae/icub_simple.dae")
#!/usr/bin/env python """ This thest_arboris_coonection is a test to create an Arboris observer to communicate with a html page. This page will be update to show the robot moving in a three.js instance... """ from arboris.core import World from arboris.robots import simplearm, icub ##### Create world w = World() #simplearm.add_simplearm(w, with_shapes=True) icub.add(w, create_shapes=False) w._up[:] = [0,0,1] ##### Init World joints = w.getjoints() # for n in ["Shoulder", "Elbow", "Wrist"]: # joints[n].gpos[0] = .5 ##### Add ctrl from arboris.controllers import WeightController w.register(WeightController())
def get_error(self):
error = []
for (sh, th) in zip(self.height, self.get_theoric()):
error.append(sh - th)
return error
def plot_error(self):
plot(self.timeline, self.get_error())
def plot_height(self):
plot(self.timeline, self.height, self.timeline, self.get_theoric())
legend(('simulated', 'theoric'))
w = World()
if True:
from arboris.homogeneousmatrix import transl
H_bc = transl(1, 1, 1)
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.])
#!/urs/bin/python #coding=utf-8 #author=joseph salini #date=24 jan 2010 from arboris.core import World, simulate, NamedObjectsList from numpy import array, arange import os ######################################################## ## WORLD BUILDING ######################################################## w = World() from arboris.robots import human36 human36.add_human36(w) w.update_dynamic() ######################################################## ## INIT ######################################################## joints = w.getjoints() bodies = w.getbodies() frames = w.getframes() shapes = w.getshapes() joints[0].gpos[0:3, 3] = array([.1, .2, .3]) w.update_dynamic()
ylabel('impedance condition')
figure()
plot(self.c_rank)
title('jacobians rank')
xlabel('step')
ylabel('rank')
show()
#with_weight = True
with_weight = False
is_fixed = False
use_snake = True
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())
def setUp(self):
"""
Ensure multi-dof Rzyx behaves like the combination of Rz, Ry, Rx.
"""
world = World()
self.Ba = Body()
Rzyx = RzRyRxJoint()
world.add_link(world.ground, Rzyx, self.Ba)
Bzy = Body(name='zy')
Byx = Body(name='yx')
self.Bb = Body()
Rz = RzJoint()
world.add_link(world.ground, Rz, Bzy)
Ry = RyJoint()
world.add_link(Bzy, Ry, Byx)
Rx = RxJoint()
world.add_link(Byx, Rx, self.Bb)
world.init()
(az, ay, ax) = (3.14/6, 3.14/4, 3.14/3)
Rzyx.gpos[:] = (az, ay, ax)
(Rz.gpos[0], Ry.gpos[0], Rx.gpos[0]) = (az, ay, ax)
world.update_dynamic()
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)
def testDynamicUpdate(self):
w = World()
add_simplearm(w)
joints = w.getjoints()
joints[0].gpos[0] = 0.5
joints[0].gvel[0] = 2.5
joints[1].gpos[0] = 1.0
joints[1].gvel[0] = -1.0
joints[2].gpos[0] = 2.0/3.0
joints[2].gvel[0] = -0.5
w.update_dynamic()
bodies = w.getbodies()
self.assertListsAlmostEqual(bodies['Arm'].pose,
[[ 0.87758256, -0.47942554, 0. , 0. ],
[ 0.47942554, 0.87758256, 0. , 0. ],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ]])
self.assertListsAlmostEqual(bodies['ForeArm'].pose,
[[ 0.0707372 , -0.99749499, 0. , -0.23971277],
[ 0.99749499, 0.0707372 , 0. , 0.43879128],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ]])
self.assertListsAlmostEqual(bodies['Hand'].pose,
[[-0.56122931, -0.82766035, 0. , -0.63871076],
[ 0.82766035, -0.56122931, 0. , 0.46708616],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ]])
self.assertListsAlmostEqual(bodies['ground'].jacobian,
[[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.]])
self.assertListsAlmostEqual(bodies['Arm'].jacobian,
[[ 0., 0., 0.],
[ 0., 0., 0.],
[ 1., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.]])
self.assertListsAlmostEqual(bodies['ForeArm'].jacobian,
[[ 0. , 0. , 0. ],
[ 0. , 0. , 0. ],
[ 1. , 1. , 0. ],
[-0.27015115, 0. , 0. ],
[ 0.42073549, 0. , 0. ],
[ 0. , 0. , 0. ]])
self.assertListsAlmostEqual(bodies['Hand'].jacobian,
[[ 0. , 0. , 0. ],
[ 0. , 0. , 0. ],
[ 1. , 1. , 1. ],
[-0.26649313, -0.3143549 , 0. ],
[ 0.7450519 , 0.24734792, 0. ],
[ 0. , 0. , 0. ]])
self.assertListsAlmostEqual(bodies['ground'].twist,
[ 0., 0., 0., 0., 0., 0.])
self.assertListsAlmostEqual(bodies['Arm'].twist,
[ 0. , 0. , 2.5, 0. , 0. , 0. ])
self.assertListsAlmostEqual(bodies['ForeArm'].twist,
[ 0., 0., 1.5, -0.67537788, 1.05183873, 0. ])
self.assertListsAlmostEqual(bodies['Hand'].twist,
[ 0., 0., 1., -0.35187792, 1.61528183, 0. ])
self.assertListsAlmostEqual(w.viscosity,
[[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.]])
self.assertListsAlmostEqual(bodies['Arm'].mass,
[[ 8.35416667e-02, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 2.50000000e-01],
[ 0.00000000e+00, 4.16666667e-04, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 8.35416667e-02,
-2.50000000e-01, 0.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, -2.50000000e-01,
1.00000000e+00, 0.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 1.00000000e+00, 0.00000000e+00],
[ 2.50000000e-01, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
self.assertListsAlmostEqual(bodies['ForeArm'].mass,
[[ 4.27733333e-02, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 1.60000000e-01],
[ 0.00000000e+00, 2.13333333e-04, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 4.27733333e-02,
-1.60000000e-01, 0.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, -1.60000000e-01,
8.00000000e-01, 0.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 8.00000000e-01, 0.00000000e+00],
[ 1.60000000e-01, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 8.00000000e-01]])
self.assertListsAlmostEqual(bodies['Hand'].mass,
[[ 2.67333333e-03, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 2.00000000e-02],
[ 0.00000000e+00, 1.33333333e-05, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 2.67333333e-03,
-2.00000000e-02, 0.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, -2.00000000e-02,
2.00000000e-01, 0.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 2.00000000e-01, 0.00000000e+00],
[ 2.00000000e-02, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 2.00000000e-01]])
self.assertListsAlmostEqual(w.mass,
[[ 0.55132061, 0.1538999 , 0.0080032 ],
[ 0.1538999 , 0.09002086, 0.00896043],
[ 0.0080032 , 0.00896043, 0.00267333]])
self.assertListsAlmostEqual(bodies['ground'].djacobian,
[[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.]])
self.assertListsAlmostEqual(bodies['Arm'].djacobian,
[[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.],
[ 0., 0., 0.]])
self.assertListsAlmostEqual(bodies['ForeArm'].djacobian,
[[ 0. , 0. , 0. ],
[ 0. , 0. , 0. ],
[ 0. , 0. , 0. ],
[-0.42073549, 0. , 0. ],
[-0.27015115, 0. , 0. ],
[ 0. , 0. , 0. ]])
self.assertListsAlmostEqual(bodies['Hand'].djacobian,
[[ 0. , 0. , 0. ],
[ 0. , 0. , 0. ],
[ 0. , 0. , 0. ],
[-0.87022993, -0.12367396, 0. ],
[-0.08538479, -0.15717745, 0. ],
[ 0. , 0. , 0. ]])
self.assertListsAlmostEqual(bodies['ground'].nleffects,
[[ 0., 0., 0., 0., 0., 0.],
[ 0., 0., 0., 0., 0., 0.],
[ 0., 0., 0., 0., 0., 0.],
[ 0., 0., 0., 0., 0., 0.],
[ 0., 0., 0., 0., 0., 0.],
[ 0., 0., 0., 0., 0., 0.]])
self.assertListsAlmostEqual(bodies['Arm'].nleffects,
[[ 0.00000000e+00, -1.04166667e-03, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00],
[ 5.26041667e-02, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 6.25000000e-01, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, -2.50000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, -6.25000000e-01,
2.50000000e+00, 0.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00]])
self.assertListsAlmostEqual(bodies['ForeArm'].nleffects,
[[ 0.00000000e+00, -3.20000000e-04, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00],
[ 1.61600000e-02, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, -2.22261445e-18],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 2.40000000e-01, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, -1.20000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, -2.40000000e-01,
1.20000000e+00, 0.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00]])
self.assertListsAlmostEqual(bodies['Hand'].nleffects,
[[ 0.00000000e+00, -1.33333333e-05, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00],
[ 6.73333333e-04, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, -1.10961316e-18],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 2.00000000e-02, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, -2.00000000e-01, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, -2.00000000e-02,
2.00000000e-01, 0.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00]])
self.assertListsAlmostEqual(w.nleffects,
[[ 0.11838112, -0.15894538, -0.01490104],
[ 0.27979997, 0.00247348, -0.00494696],
[ 0.03230564, 0.00742044, 0. ]])
def get_error(self):
error = []
for (sh, th) in zip(self.height, self.get_theoric()):
error.append(sh-th)
return error
def plot_error(self):
plot(self.timeline, self.get_error())
def plot_height(self):
plot(self.timeline, self.height,
self.timeline, self.get_theoric())
legend(('simulated', 'theoric'))
w = World()
if True:
from arboris.homogeneousmatrix import transl
H_bc = transl(1,1,1)
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.])
#!/usr/bin/python
#coding=utf-8
#author=joseph salini
"""This tutorial presents the structure of Arboris-python.
It presents the World, Body, Joint, SubFrame and Constraint classes.
"""
##### About the World ##########################################################
from arboris.core import World
from arboris.robots import simplearm
w = World() # create an instance of world.
simplearm.add_simplearm(w) # add a simple arm robot
# The world is composed of ...
bodies = w.getbodies() # ... named list of bodies
joints = w.getjoints() # ... named list of joints
frames = w.getframes() # ... named list of frames (including bodies)
shapes = w.getshapes() # ... named list of shapes
consts = w.getconstraints() # ... named list of constraints
ctrls = w.getcontrollers() # ... named list of controllers
# And that's all!
print("bodies", bodies)
print("joints", joints)
# These lists are named, so instances can be retrieved as follows:
body_Arm = bodies["Arm"]
joint_Shoulder = joints["Shoulder"]
print("Arm", body_Arm)
def testConstructor(self):
w1 = World()
wc = WeightController()
j = w1.getjoints()
pdc = ProportionalDerivativeController(j)
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. ] ])
#!/usr/bin/python
#coding=utf-8
#author=joseph salini
"""This tutorial presents the structure of Arboris-python.
It presents the World, Body, Joint, SubFrame and Constraint classes.
"""
##### About the World ##########################################################
from arboris.core import World
from arboris.robots import simplearm
w = World() # create an instance of world.
simplearm.add_simplearm(w) # add a simple arm robot
# The world is composed of ...
bodies = w.getbodies() # ... named list of bodies
joints = w.getjoints() # ... named list of joints
frames = w.getframes() # ... named list of frames (including bodies)
shapes = w.getshapes() # ... named list of shapes
consts = w.getconstraints() # ... named list of constraints
ctrls = w.getcontrollers() # ... named list of controllers
# And that's all!
print("bodies", bodies)
print("joints", joints)
# These lists are named, so instances can be retrieved as follows:
body_Arm = bodies["Arm"]
# coding=utf-8
"""
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
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 testBallAndSocketConstraint1(self):
b0 = Body(mass=eye(6))
w = World()
w.add_link(w.ground, FreeJoint(), b0)
b1 = Body(mass=eye(6))
w.add_link(b0, FreeJoint(), b1)
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, -3.09167026e-22],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
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
#!/usr/bin/env python
"""
This example show how to add mesh from a collada file to another,
in order to display complex meshes during visualization.
"""
from arboris.core import World
from arboris.robots import simplearm
from arboris.homogeneousmatrix import transl
##### Create world
w = World()
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())
from arboris.robots import icub
from arboris.massmatrix import principalframe, transport
from arboris.homogeneousmatrix import inv as Hinv
from arboris.homogeneousmatrix import transl, roty, rotzyx, rotx
import numpy
numpy.set_printoptions(suppress=True)
from numpy import dot, eye, pi
################################################################################
#
# Create world with iCub
#
################################################################################
w = World()
icub.add(w)
w.update_dynamic()
jLim = icub.get_joint_limits(in_radian=True)
tauLim = icub.get_torque_limits()
# interesting values we want to obtain
H_body_com = {}
body_mass = {}
body_moment_of_inertia = {}
H_parentCoM_bCoM = {}
list_of_dof = {}
list_of_children = {}