def solve(robot, tr, q, mask, ilimit=1000, stol=1e-6, gamma=1): print(ilimit, stol, gamma) nm = inf count = 0 while nm > stol: e = multiply(tr2diff(fkine(robot, q.T), tr), mask) #dq = pinv(Jac.jacob0(robot, q.T)) * e dq = Jac.jacob0(robot, q.T).T * e q += gamma * dq nm = norm(e) count += 1 if count > ilimit: error("Solution wouldn't converge") print(count, 'iterations') return q
def solve(robot, tr, q, mask, ilimit=1000, stol=1e-6, gamma=1): print ilimit, stol, gamma nm = inf; count = 0 while nm > stol: e = multiply( tr2diff(fkine(robot, q.T),tr), mask ) #dq = pinv(Jac.jacob0(robot, q.T)) * e dq = Jac.jacob0(robot, q.T).T * e q += gamma*dq; nm = norm(e) count += 1 if count > ilimit: error("Solution wouldn't converge") print count, 'iterations' return q;
def cinertia(robot, q): """ Compute the Cartesian (operational space) manipulator inertia matrix m = cinertia(robot, q) Return the M{6 x 6} inertia matrix which relates Cartesian force/torque to Cartesian acceleration. @type q: n-vector @type q: Joint coordinate @type robot: Robot object, n-axes @param robot: The robot @rtype: M{6 x 6} matrix @return: Cartesian inertia matrix @bug: Should handle the case of q as a matrix @see: L{inertia}, L{rne}, L{robot} """ J = Jac.jacob0(robot, q) Ji = inv(J) M = inertia(robot, q) return Ji.T * M * Ji
def cinertia(robot, q): """ Compute the Cartesian (operational space) manipulator inertia matrix m = cinertia(robot, q) Return the M{6 x 6} inertia matrix which relates Cartesian force/torque to Cartesian acceleration. @type q: n-vector @type q: Joint coordinate @type robot: Robot object, n-axes @param robot: The robot @rtype: M{6 x 6} matrix @return: Cartesian inertia matrix @bug: Should handle the case of q as a matrix @see: L{inertia}, L{rne}, L{robot} """ J = Jac.jacob0(robot, q) Ji = inv(J) M = inertia(robot, q) return Ji.T*M*Ji
def ikine(robot, tr, q=None, m=None): """ Inverse manipulator kinematics. Computes the joint coordinates corresponding to the end-effector transform C{tr}. Typically invoked as - Q = IKINE(ROBOT, T) - Q = IKINE(ROBOT, T, Q) - Q = IKINE(ROBOT, T, Q, M) Uniqueness ========== Note that the inverse kinematic solution is generally not unique, and depends on the initial guess C{q} (which defaults to 0). Iterative solution ================== Solution is computed iteratively using the pseudo-inverse of the manipulator Jacobian. Such a solution is completely general, though much less efficient than specific inverse kinematic solutions derived symbolically. This approach allows a solution to obtained at a singularity, but the joint angles within the null space are arbitrarily assigned. Operation on a trajectory ========================= If C{tr} is a list of transforms (a trajectory) then the solution is calculated for each transform in turn. The return values is a matrix with one row for each input transform. The initial estimate for the iterative solution at each time step is taken as the solution from the previous time step. Fewer than 6DOF =============== If the manipulator has fewer than 6 DOF then this method of solution will fail, since the solution space has more dimensions than can be spanned by the manipulator joint coordinates. In such a case it is necessary to provide a mask matrix, C{m}, which specifies the Cartesian DOF (in the wrist coordinate frame) that will be ignored in reaching a solution. The mask matrix has six elements that correspond to translation in X, Y and Z, and rotation about X, Y and Z respectively. The value should be 0 (for ignore) or 1. The number of non-zero elements should equal the number of manipulator DOF. For instance with a typical 5 DOF manipulator one would ignore rotation about the wrist axis, that is, M = [1 1 1 1 1 0]. @type robot: Robot instance @param robot: The robot @type tr: homgeneous transformation @param tr: End-effector pose @type q: vector @param q: initial estimate of joint coordinate @type m: vector @param m: mask vector @rtype: vector @return: joint coordinate @see: L{fkine}, L{tr2diff}, L{jacbo0}, L{ikine560} """ #solution control parameters ilimit = 1000 stol = 1e-12 n = robot.n if q == None: q = mat(zeros((n,1))) else: q = mat(q).flatten().T if q != None and m != None: m = mat(m).flatten().T if len(m)!=6: error('Mask matrix should have 6 elements') if len(m.nonzero()[0].T)!=robot.n: error('Mask matrix must have same number of 1s as robot DOF') else: if n<6: print 'For a manipulator with fewer than 6DOF a mask matrix argument should be specified' m = mat(ones((6,1))) if isinstance(tr, list): #trajectory case qt = mat(zeros((0,n))) for T in tr: nm = 1 count = 0 while nm > stol: e = multiply(tr2diff( fkine(robot, q.T), T), m) dq = pinv(Jac.jacob0(robot,q.T))*e q += dq nm = norm(dq) count += 1 if count > ilimit: print 'i=',i,' nm=',nm error("Solution wouldn't converge") qt = vstack( (qt, q.T) ) return qt; elif ishomog(tr): #single xform case nm = 1 count = 0 while nm > stol: e = multiply( tr2diff(fkine(robot,q.T),tr), m ) dq = pinv(Jac.jacob0(robot, q.T)) * e q += dq; nm = norm(dq) count += 1 if count > ilimit: error("Solution wouldn't converge") qt = q.T return qt else: error('tr must be 4*4 matrix')
def ikine(robot, tr, q=None, m=None): """ Inverse manipulator kinematics. Computes the joint coordinates corresponding to the end-effector transform C{tr}. Typically invoked as - Q = IKINE(ROBOT, T) - Q = IKINE(ROBOT, T, Q) - Q = IKINE(ROBOT, T, Q, M) Uniqueness ========== Note that the inverse kinematic solution is generally not unique, and depends on the initial guess C{q} (which defaults to 0). Iterative solution ================== Solution is computed iteratively using the pseudo-inverse of the manipulator Jacobian. Such a solution is completely general, though much less efficient than specific inverse kinematic solutions derived symbolically. This approach allows a solution to obtained at a singularity, but the joint angles within the null space are arbitrarily assigned. Operation on a trajectory ========================= If C{tr} is a list of transforms (a trajectory) then the solution is calculated for each transform in turn. The return values is a matrix with one row for each input transform. The initial estimate for the iterative solution at each time step is taken as the solution from the previous time step. Fewer than 6DOF =============== If the manipulator has fewer than 6 DOF then this method of solution will fail, since the solution space has more dimensions than can be spanned by the manipulator joint coordinates. In such a case it is necessary to provide a mask matrix, C{m}, which specifies the Cartesian DOF (in the wrist coordinate frame) that will be ignored in reaching a solution. The mask matrix has six elements that correspond to translation in X, Y and Z, and rotation about X, Y and Z respectively. The value should be 0 (for ignore) or 1. The number of non-zero elements should equal the number of manipulator DOF. For instance with a typical 5 DOF manipulator one would ignore rotation about the wrist axis, that is, M = [1 1 1 1 1 0]. @type robot: Chain instance @param robot: The robot @type tr: homgeneous transformation @param tr: End-effector pose @type q: vector @param q: initial estimate of joint coordinate @type m: vector @param m: mask vector @rtype: vector @return: joint coordinate @see: L{fkine}, L{tr2diff}, L{jacbo0}, L{ikine560} """ #solution control parameters ilimit = 1000 stol = 1e-12 n = robot.n prevVal = [] if q == None: q = mat(zeros((n, 1))) else: q = mat(q).flatten().T if q != None and m != None: m = mat(m).flatten().T if len(m) != 6: error('Mask matrix should have 6 elements') if len(m.nonzero()[0].T) != robot.n: error('Mask matrix must have same number of 1s as robot DOF') else: if n < 6: print 'For a manipulator with fewer than 6DOF a mask matrix argument should be specified' m = mat(ones((6, 1))) if isinstance(tr, list): #trajectory case qt = mat(zeros((0, n))) for T in tr: nm = 1 count = 0 while nm > stol: e = multiply(tr2diff(fkine(robot, q.T), T), m) dq = pinv(Jac.jacob0(robot, q.T)) * e q += dq nm = norm(dq) count += 1 if [x for x in prevVal if (x == q).all()] != []: qt = vstack((qt, q.T)) return qt else: prevVal.append(q) print "in" if count > ilimit: print 'i=', i, ' nm=', nm error("Solution wouldn't converge") qt = vstack((qt, q.T)) return qt elif ishomog(tr): #single xform case nm = 1 count = 0 while nm > stol: e = multiply(tr2diff(fkine(robot, q.T), tr), m) dq = pinv(Jac.jacob0(robot, q.T)) * e q += dq nm = norm(dq) count += 1 prevVal.append(q.copy()) if count > ilimit: minDist = 100000000 for x in prevVal: dist = distance(xyz(fkine(robot, x.T)), xyz(tr)) if dist < minDist: minDist = dist minQ = x print "Found minimum dist %f of %d items!" % (minDist, len(prevVal)) return minQ.T #qt = q.T #return qt error("Solution wouldn't converge") qt = q.T return qt else: error('tr must be 4*4 matrix')