def testSubFrames(self):
body = Body()
body.update_dynamic(self.alea_pose, self.alea_jac, self.alea_djac,
self.alea_twist)
sframe = SubFrame(body, Hg.rotz(3.14/3.), 'Brand New Frame')
self.assertListsAlmostEqual(sframe.bpose,
array([ [ 0.50045969, -0.86575984, 0. , 0. ],
[ 0.86575984, 0.50045969, 0. , 0. ],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ]]))
self.assertListsAlmostEqual(sframe.pose,
[ [ 4.3287992 , 2.50229845, 0., 0. ],
[ 2.16695503, -1.75051589, 0., 4.2 ],
[ 0. , 0. , 0., 0. ],
[ 7.79183856, 4.5041372 , 0., 3. ] ])
self.assertListsAlmostEqual(sframe.jacobian,
[ [ 2.10193069, 4.00367751, 0.50045969],
[-3.63619133, -6.92607872, -0.86575984],
[ 0. , 7. , 0. ],
[ 1.30119519, 0. , 0.43287992],
[-2.25097558, 0. , 0.25022984],
[ 8.8 , 0. , 2. ] ])
self.assertListsAlmostEqual(sframe.djacobian,
[ [ 0.50045969, 36.76228101, 0. ],
[ -0.86575984, 20.32669907, 0. ],
[ 0. , 4.2 , 2.6 ],
[ 0.50045969, 0.86575984, 0. ],
[ -0.86575984, 0.50045969, 0. ],
[ 7.1 , 6. , 0. ] ])
self.assertListsAlmostEqual(sframe.twist,
[ 0., 0., 7.1, 36.86237295, 20.1535471, 0. ])
