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')
