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)
Beispiel #6
0
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), [])
Beispiel #7
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 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]])
Beispiel #10
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())
Beispiel #13
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.])
Beispiel #14
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())
Beispiel #16
0
 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()
Beispiel #17
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
Beispiel #18
0

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
Beispiel #26
0
 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]])
Beispiel #28
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
Beispiel #29
0
#!/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 = {}