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 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 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 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 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 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)
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()
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()