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)
