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. ] ])
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 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
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_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
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 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]])
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()
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())
##### 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)
# Given the dynamic equation of tree structure without control: # # M * d(gvel)/dt + (N + B) * gvel = gforce # # - dt is given by user. # - M, N, B, gvel, gforce are computed by world (depending on current state) M = w.mass N = w.nleffects B = w.viscosity gvel = w.gvel gforce = w.gforce # if the world state is modified (if joints positions or velocities change), # one has to update the kinematic and dynamic vectors and matrices w.update_geometric() # make forward kinematic w.update_dynamic() # make forward dynamic # Simulate the world is done with a loop containing these functions dt = .01 #s w.update_dynamic() # update dynamic of the world w.update_controllers(dt) # ...... generalized force of controllers w.update_constraints(dt) # ...... constraints for kinematic loops w.integrate(dt) # ...... the current state of the world ##### The Frame class ########################################################## # This class represents the bodies and the subframes in the world f = frames["EndEffector"] name = f.name body = f.body # return parent body (itself if f is body) pose = f.pose # return H_0_f (0 is world.ground)
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. ]])
# # M * d(gvel)/dt + (N + B) * gvel = gforce # # - dt is given by user. # - M, N, B, gvel, gforce are computed by world (depending on current state) M = w.mass N = w.nleffects B = w.viscosity gvel = w.gvel gforce = w.gforce # if the world state is modified (if joints positions or velocities change), # one has to update the kinematic and dynamic vectors and matrices w.update_geometric() # make forward kinematic w.update_dynamic() # make forward dynamic # Simulate the world is done with a loop containing these functions dt = .01 #s w.update_dynamic() # update dynamic of the world w.update_controllers(dt) # ...... generalized force of controllers w.update_constraints(dt) # ...... constraints for kinematic loops w.integrate(dt) # ...... the current state of the world ##### The Frame class ########################################################## # This class represents the bodies and the subframes in the world f = frames["EndEffector"]