def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False): """ Computes jacobian of frame n (with origin On in Oj) projected to frame i """ # symo.write_geom_param(robo, 'Jacobian') # TODO: Check projection frames, rewrite DGM call for higher efficiency J_col_list = [] if chain is None: chain = robo.chain(n) chain.reverse() # chain_ext = chain + [robo.ant[min(chain)]] # if not i in chain_ext: # i = min(chain_ext) # if not j in chain_ext: # j = max(chain_ext) kTj_dict = dgm(robo, symo, chain[0], j, key='left', trig_subs=trig_subs) kTj_tmp = dgm(robo, symo, chain[-1], j, key='left', trig_subs=trig_subs) kTj_dict.update(kTj_tmp) iTk_dict = dgm(robo, symo, i, chain[0], key='right', trig_subs=trig_subs) iTk_tmp = dgm(robo, symo, i, chain[-1], key='right', trig_subs=trig_subs) iTk_dict.update(iTk_tmp) for k in chain: kTj = kTj_dict[k, j] iTk = iTk_dict[i, k] isk, ink, iak = Transform.sna(iTk) sigm = robo.sigma[k] if sigm == 1: dvdq = iak J_col = dvdq.col_join(Matrix([0, 0, 0])) elif sigm == 0: dvdq = kTj[0, 3] * ink - kTj[1, 3] * isk J_col = dvdq.col_join(iak) else: J_col = Matrix([0, 0, 0, 0, 0, 0]) J_col_list.append(J_col.T) Jac = Matrix(J_col_list).T Jac = Jac.applyfunc(symo.simp) iRj = Transform.R(iTk_dict[i, j]) jTn = dgm(robo, symo, j, n, fast_form=False, trig_subs=trig_subs) jPn = Transform.P(jTn) L = -tools.skew(iRj * jPn) L = L.applyfunc(trigsimp) if forced: symo.mat_replace(Jac, 'J', '', forced) L = symo.mat_replace(L, 'L', '', forced) return Jac, L
def test_jac(self): print "######## test_jac ##########" kinematics.jacobian(self.robo, 6, 3, 6) for j in xrange(1, 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)