def testWeightController(self):
w = simplearm()
joints = w.getjoints()
joints['Shoulder'].gpos[0] = 3.14/4
joints['Elbow'].gpos[0] = 3.14/4
joints['Wrist'].gpos[0] = 3.14/4
c = WeightController()
w.register(c)
w.init()
w.update_dynamic() #TODO change for update_kinematic
self.assertListsAlmostEqual(c.update()[0],
([ 7.69376549, 2.49329922, 0.13889997]))
self.assertListsAlmostEqual(c.update()[1],
[[ 0., 0., 0.],[ 0., 0., 0.],[ 0., 0., 0.]])