def test_jac2(self): print "######## test_jac2 ##########" J, L = kinematics._jac(self.robo, self.symo, 6, 3, 3) jac63 = self.symo.gen_func('Jac1RX90', J, self.robo.q_vec) L63 = self.symo.gen_func('LRX90', L, self.robo.q_vec) J, L = kinematics._jac(self.robo, self.symo, 6, 3, 6) jac66 = self.symo.gen_func('Jac2RX90', J, self.robo.q_vec) for i in xrange(10): q = random.normal(size=6) j63 = matrix(jac63(q)) l63 = matrix(L63(q)) j66 = matrix(jac66(q)) X = eye(6) X[:3, 3:] = l63 self.assertLess(amax(j66 - X*j63), 1e-12)
def test_jac(self): print "######## test_jac ##########" kinematics.jacobian(self.robo, 6, 3, 6) for j in xrange(5, 7): print "######## Jac validation through DGM ##########" #compute Jac J, l = kinematics._jac(self.robo, self.symo, j, 0, j) jacj = self.symo.gen_func('JacRX90', J, self.robo.q_vec) #compute DGM T = geometry.dgm(self.robo, self.symo, 0, j, fast_form=True, trig_subs=True) T0j = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec) for i in xrange(10): dq = random.normal(size=6, scale=1e-7) q = random.normal(size=6) dX = matrix(jacj(q)) * matrix(dq[:j]).T T = (matrix(T0j(q+dq)) - T0j(q)) self.assertLess(amax(dX[:3] - trns.P(T)), 1e-12)