# differential kinematics jac = ChainJntToJacSolver(chain) jac2 = ChainJntToJacSolver(chain2) j = Jacobian(7) j2 = Jacobian(7) j2b = Jacobian(7) jx = Jacobian(7) jxb = Jacobian(7) kdl.SetToZero(j) kdl.SetToZero(j2) kdl.SetToZero(jx) jac.JntToJac(joints, j) jac2.JntToJac(joints, j2) ######################### fr2 = f*frame kdl.changeRefPoint(j, fr2.p - f.p, jx) assert(equal(j2, jx)) # transform to base frame kdl.changeRefPoint(j2, -f2.p, j2b) kdl.changeRefPoint(j, -f.p, jxb) assert(equal(j2b, jxb)) #########################
