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)
Exemple #5
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()
    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)
Exemple #7
0
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()