def setUp(self): self.world = simplearm() a = WeightController() self.world.register(a) joints = self.world.getjoints() self.shoulder = joints['Shoulder'] c = JointLimits(self.shoulder, -3.14 / 2, 3.14 / 2) self.world.register(c)
def setUp(self): self.world = simplearm() a = WeightController() self.world.register(a) joints = self.world.getjoints() self.shoulder = joints['Shoulder'] c = JointLimits(self.shoulder, -3.14/2, 3.14/2) self.world.register(c)
def setUp(self): self.world = simplearm() joints = self.world.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 self.world.update_dynamic()
def testProportionalDerivativeController(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 pdc = ProportionalDerivativeController(joints,) w.register(pdc) w.init() w.update_dynamic() self.assertListsAlmostEqual(pdc.update(1/2)[0],([ 0., 0., 0.])) self.assertListsAlmostEqual(pdc.update(1/2)[1], [[ 0., 0., 0.],[ 0., 0., 0.],[ 0., 0., 0.]])
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.]])
def testJoinLimits(self): world = simplearm() a = WeightController() world.register(a) joints = world.getjoints() c = JointLimits(joints['Shoulder'], -3.14/2, 3.14/2) world.register(c) joints['Shoulder'].gpos[0] = 3.14/2 - 0.1 time = arange(0., 0.1, 1e-3) simulate(world, time) self.assertTrue(3.14/2 >= joints['Shoulder'].gpos[0]) joints['Shoulder'].gpos[0] = -3.14/2 + 0.1 time = arange(0., 0.1, 1e-3) simulate(world, time) self.assertTrue(-3.14/2 <= joints['Shoulder'].gpos[0])
def testBodies(self): w = simplearm() 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. ]])