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