Downloads: 1
Diff of
/URBasic/manipulation.py
[000000]
..
[c1b1c5]
Switch to side-by-side view
--- a +++ b/URBasic/manipulation.py @@ -0,0 +1,1212 @@ +import math +from numpy import * + +### HELPER FUNCTIONS ### +def randomVec(x): + ''' + Generates a vector of length x, each component being a random float b/w -10 and 10 + ''' + return random.uniform(-10,10,(x,1)) + + +def randomUnitAxisAngle(): + ''' + Generates a random unit axis and an angle + ''' + # Random longitude + u = random.uniform(0, 1) + longitude = 2*math.pi*u + + # Random latitude + v = random.uniform(0, 1) + latitude = 2*math.pi*v + + # Random unit rotation axis + axis = zeros((3,1)) + axis[0] = math.cos(latitude)*math.cos(longitude) + axis[1] = math.cos(latitude)*math.sin(longitude) + axis[2] = math.sin(latitude) + + # Random angle b/w 0 and 2pi + theta = 2*math.pi*random.uniform(0, 1) + + return axis, theta + + +def normalize(v): + ''' + Returns the normalized version of the vector v + ''' + norm = linalg.norm(v) + if norm == 0: + return v + return v/norm + + +def is_identity_matrix(M): + ''' + Returns True if input M is close to an identity matrix + ''' + if len(M) != len(M[0]): + return False + + c = list() + for i, row in enumerate(M): + for j, val in enumerate(row): + if i==j: + if val < 1.001 and val > 0.999: + c.append(True) + else: + c.append(False) + else: + if abs(val) < 0.001: + c.append(True) + else: + c.append(False) + + return all(c) + + +def is_rot_matrix(R): + ''' + Returns True if input R is a valid rotation matrix. + ''' + R = asarray(R) + return is_identity_matrix(dot(R.T,R)) and (abs(linalg.det(R)-1) < 0.001) + + +### MAIN FUNCTIONS ### +def RotInv(R): + ''' + Takes a rotation matrix belonging to SO(3) and returns its inverse. + Example: + + R = [[.707,-.707,0],[.707,.707,0],[0,0,1]] + RotInv(R) + >> array([[ 0.707, 0.707, 0. ], + [-0.707, 0.707, 0. ], + [ 0. , 0. , 1. ]]) + ''' + R = asarray(R) + assert is_rot_matrix(R), 'Not a valid rotation matrix' + + return R.T + + +def VecToso3(w): + ''' + Takes a 3-vector representing angular velocity and returns the 3x3 skew-symmetric matrix version, an element + of so(3). + Example: + + w = [2, 1, -4] + VecToso3(w) + >> array([[ 0, 4, 1], + [-4, 0, -2], + [-1, 2, 0]]) + ''' + w = asarray(w) + assert len(w) == 3, 'Not a 3-vector' + + w = w.flatten() + w_so3mat = array([[0, -w[2], w[1]], [w[2], 0, -w[0]], [-w[1], w[0], 0]]) + return w_so3mat + + +def so3ToVec(w_so3mat): + ''' + Takes a 3x3 skew-symmetric matrix (an element of so(3)) and returns the corresponding 3-vector. + Example: + + w_so3mat = [[ 0, 4, 1],[-4, 0, -2],[-1, 2, 0]] + so3ToVec(w_so3mat) + >> array([[ 2], + [ 1], + [-4]]) + ''' + w_so3mat = asarray(w_so3mat) + assert w_so3mat.shape == (3,3), 'Not a 3x3 matrix' + + w = array([[w_so3mat[2,1]], [w_so3mat[0,2]], [w_so3mat[1,0]]]) + return w + + +def AxisAng3(r): + ''' + Takes a 3-vector of exp coords r = w_unit*theta and returns w_unit and theta. + Example: + + r = [2, 1, -4] + w_unit, theta = AxisAng3(r) + w_unit + >> array([ 0.43643578, 0.21821789, -0.87287156]) + theta + >> 4.5825756949558398 + ''' + r = asarray(r) + assert len(r) == 3, 'Not a 3-vector' + + theta = linalg.norm(r) + w_unit = normalize(r) + + return w_unit, theta + + +def MatrixExp3(r): + ''' + Takes a 3-vector of exp coords r = w_unit*theta and returns the corresponding + rotation matrix R (an element of SO(3)). + Example: + + r = [2, 1, -4] + MatrixExp3(r) + >> array([[ 0.08568414, -0.75796072, -0.64664811], + [ 0.97309386, -0.07566572, 0.2176305 ], + [-0.21388446, -0.64789679, 0.73108357]]) + ''' + r = asarray(r) + assert len(r) == 3, 'Not a 3-vector' + + w_unit, theta = AxisAng3(r) + w_so3mat = VecToso3(w_unit) + + R = identity(3) + math.sin(theta)*w_so3mat + (1-math.cos(theta))*dot(w_so3mat, w_so3mat) + assert is_rot_matrix(R), 'Did not produce a valid rotation matrix' + return R + + +def MatrixLog3(R): + ''' + Takes a rotation matrix R and returns the corresponding 3-vector of exp coords r = w_unit*theta. + Example: + + R = [[.707,-.707,0],[.707,.707,0],[0,0,1]] + MatrixLog3(R) + >> array([[ 0. ], + [ 0. ], + [ 0.78554916]]) + ''' + R = asarray(R) + assert is_rot_matrix(R), 'Not a valid rotation matrix' + + if is_identity_matrix(R): + return zeros((3,1)) + + if trace(R) > -1.001 and trace(R) < -0.999: + theta = math.pi + c = max(diag(R)) + if c == R[2,2]: + w_unit = array([[R[0,2]],[R[1,2]],[1+c]])*1/((2*(1+c))**0.5) + return w_unit*theta + elif c == R[1,1]: + w_unit = array([[R[0,1]],[1+c],[R[2,1]]])*1/((2*(1+c))**0.5) + return w_unit*theta + elif c == R[0,0]: + w_unit = array([[1+c],[R[1,0]],[R[2,0]]])*1/((2*(1+c))**0.5) + return w_unit*theta + + theta = math.acos((trace(R)-1)/2) + w_so3mat = (R - R.T)/(2*math.sin(theta)) + w_unit = normalize(so3ToVec(w_so3mat)) + return w_unit*theta + + +def RpToTrans(R,p): + ''' + Takes a rotation matrix R and a point (3-vector) p, and returns the corresponding + 4x4 transformation matrix T, an element of SE(3). + Example: + + R = [[.707,-.707,0],[.707,.707,0],[0,0,1]] + p = [5,-4,9] + RpToTrans(R,p) + >> array([[ 0.707, -0.707, 0. , 5. ], + [ 0.707, 0.707, 0. , -4. ], + [ 0. , 0. , 1. , 9. ], + [ 0. , 0. , 0. , 1. ]]) + ''' + p = asarray(p) + R = asarray(R) + assert len(p) == 3, "Point not a 3-vector" + assert is_rot_matrix(R), "R not a valid rotation matrix" + + p.shape = (3,1) + T = vstack((hstack((R,p)), array([0,0,0,1]))) + return T + + +def TransToRp(T): + ''' + Takes a transformation matrix T and returns the corresponding R and p. + Example: + + T = [[0.707,-0.707,0,5],[0.707,0.707,0,-4],[0,0,1,9],[0,0,0,1]] + R, p = TransToRp(T) + R + >> array([[ 0.707, -0.707, 0. ], + [ 0.707, 0.707, 0. ], + [ 0. , 0. , 1. ]]) + p + >> array([[ 5.], + [-4.], + [ 9.]]) + ''' + T = asarray(T) + assert T.shape == (4,4), "Input not a 4x4 matrix" + + R = T[:3,:3] + assert is_rot_matrix(R), "Input not a valid transformation matrix" + + assert allclose([0,0,0,1],T[-1],atol=1e-03), "Last row of homogenous T matrix should be [0,0,0,1]" + + p = T[:3,-1] + p.shape = (3,1) + + return R, p + + +def TransInv(T): + ''' + Returns inverse of transformation matrix T. + Example: + + T = [[0.707,-0.707,0,5],[0.707,0.707,0,-4],[0,0,1,9],[0,0,0,1]] + TransInv(T) + >> array([[ 0.707, 0.707, 0. , -0.707], + [-0.707, 0.707, 0. , 6.363], + [ 0. , 0. , 1. , -9. ], + [ 0. , 0. , 0. , 1. ]]) + ''' + T = asarray(T) + R, p = TransToRp(T) + + T_inv = RpToTrans(RotInv(R), dot(-RotInv(R),p)) + return T_inv + + +def VecTose3(V): + ''' + Takes a 6-vector (representing spatial velocity) and returns the corresponding 4x4 matrix, + an element of se(3). + Example: + + V = [3,2,5,-3,7,0] + VecTose3(V) + >> array([[ 0., -5., 2., -3.], + [ 5., 0., -3., 7.], + [-2., 3., 0., 0.], + [ 0., 0., 0., 0.]]) + ''' + V = asarray(V) + assert len(V) == 6, "Input not a 6-vector" + + V.shape = (6,1) + w = V[:3] + w_so3mat = VecToso3(w) + v = V[3:] + V_se3mat = vstack((hstack((w_so3mat,v)), zeros(4))) + return V_se3mat + + +def se3ToVec(V_se3mat): + ''' + Takes an element of se(3) and returns the corresponding (6-vector) spatial velocity. + Example: + + V_se3mat = [[ 0., -5., 2., -3.], + [ 5., 0., -3., 7.], + [-2., 3., 0., 0.], + [ 0., 0., 0., 0.]] + se3ToVec(V_se3mat) + >> array([[ 3.], + [ 2.], + [ 5.], + [-3.], + [ 7.], + [ 0.]]) + ''' + V_se3mat = asarray(V_se3mat) + assert V_se3mat.shape == (4,4), "Matrix is not 4x4" + + w_so3mat = V_se3mat[:3,:3] + w = so3ToVec(w_so3mat) + + v = V_se3mat[:3,-1] + v.shape = (3,1) + + V = vstack((w,v)) + return V + + +def Adjoint(T): + ''' + Takes a transformation matrix T and returns the 6x6 adjoint representation [Ad_T] + Example: + + T = [[0.707,-0.707,0,5],[0.707,0.707,0,-4],[0,0,1,9],[0,0,0,1]] + Adjoint(T) + >> array([[ 0.707, -0.707, 0. , 0. , 0. , 0. ], + [ 0.707, 0.707, 0. , 0. , 0. , 0. ], + [ 0. , 0. , 1. , 0. , 0. , 0. ], + [-6.363, -6.363, -4. , 0.707, -0.707, 0. ], + [ 6.363, -6.363, -5. , 0.707, 0.707, 0. ], + [ 6.363, 0.707, 0. , 0. , 0. , 1. ]]) + ''' + T = asarray(T) + assert T.shape == (4,4), "Input not a 4x4 matrix" + + R, p = TransToRp(T) + p = p.flatten() + p_skew = array([[0, -p[2], p[1]], [p[2], 0, -p[0]], [-p[1], p[0], 0]]) + + ad1 = vstack((R, dot(p_skew,R))) + ad2 = vstack((zeros((3,3)),R)) + adT = hstack((ad1,ad2)) + return adT + + +def ScrewToAxis(q,s_hat,h): + ''' + Takes a point q (3-vector) on the screw, a unit axis s_hat (3-vector) in the direction of the screw, + and a screw pitch h (scalar), and returns the corresponding 6-vector screw axis S (a normalized + spatial velocity). + Example: + + q = [3,0,0] + s_hat = [0,0,1] + h = 2 + ScrewToAxis(q,s_hat,h) + >> array([[ 0], + [ 0], + [ 1], + [ 0], + [-3], + [ 2]]) + ''' + q = asarray(q) + s_hat = asarray(s_hat) + assert len(q) == len(s_hat) == 3, "q or s_hat not a 3-vector" + assert abs(linalg.norm(s_hat) - 1) < 0.001, "s_hat not a valid unit vector" + assert isscalar(h), "h not a scalar" + + q = q.flatten() + s_hat = s_hat.flatten() + + v_wnorm = -cross(s_hat, q) + h*s_hat + v_wnorm.shape = (3,1) + w_unit = s_hat + w_unit.shape = (3,1) + S = vstack((w_unit,v_wnorm)) + return S + + +def AxisAng6(STheta): + ''' + Takes a 6-vector of exp coords STheta and returns the screw axis S and the distance traveled along/ + about the screw axis theta. + Example: + + STheta = [0,0,1,0,-3,2] + S, theta = AxisAng6(STheta) + S + >> array([[ 0.], + [ 0.], + [ 1.], + [ 0.], + [-3.], + [ 2.]]) + theta + >> 1.0 + ''' + STheta = asarray(STheta) + assert len(STheta) == 6, 'Input not a 6-vector' + + w = STheta[:3] + v = STheta[3:] + + if linalg.norm(w) == 0: + theta = linalg.norm(v) + v_unit = normalize(v) + v_unit.shape = (3,1) + S = vstack((zeros((3,1)), v_unit)) + return S, theta + + theta = linalg.norm(w) + w_unit = normalize(w) + w_unit.shape = (3,1) + v_unit = v/theta + v_unit.shape = (3,1) + S = vstack((w_unit,v_unit)) + return S, theta + + +def MatrixExp6(STheta): + ''' + Takes a 6-vector of exp coords STheta and returns the corresponding 4x4 transformation matrix T. + Example: + + STheta = [0,0,1,0,-3,2] + MatrixExp6(STheta) + >> array([[ 0.54030231, -0.84147098, 0. , 1.37909308], + [ 0.84147098, 0.54030231, 0. , -2.52441295], + [ 0. , 0. , 1. , 2. ], + [ 0. , 0. , 0. , 1. ]]) + ''' + STheta = asarray(STheta) + assert len(STheta) == 6, 'Input not a 6-vector' + + S, theta = AxisAng6(STheta) + + w_unit = S[:3] + v_unit = S[3:] + + vTheta = STheta[3:] + vTheta.shape = (3,1) + + if linalg.norm(w_unit) == 0: + R = identity(3) + p = vTheta + T = RpToTrans(R,p) + return T + + r = w_unit*theta + R = MatrixExp3(r) + w_so3mat = VecToso3(w_unit) + p = dot((identity(3)*theta+(1-math.cos(theta))*w_so3mat+(theta-math.sin(theta))*dot(w_so3mat,w_so3mat)), v_unit) + T = RpToTrans(R,p) + return T + + +def MatrixLog6(T): + ''' + Takes a transformation matrix T and returns the corresponding 6-vector of exp coords STheta. + Example: + + T = [[ 0.54030231, -0.84147098, 0. , 1.37909308], + [ 0.84147098, 0.54030231, 0. , -2.52441295], + [ 0. , 0. , 1. , 2. ], + [ 0. , 0. , 0. , 1. ]] + MatrixLog6(T): + >> array([[ 0.00000000e+00], + [ 0.00000000e+00], + [ 9.99999995e-01], + [ 1.12156694e-08], + [ -2.99999999e+00], + [ 2.00000000e+00]]) + ''' + T = asarray(T) + assert T.shape == (4,4), "Input not a 4x4 matrix" + + R, p = TransToRp(T) + + if is_identity_matrix(R): + w_unit = zeros((3,1)) + vTheta = p + STheta = vstack((w_unit, p)) + return STheta + + if trace(R) > -1.001 and trace(R) < -0.999: + theta = math.pi + wTheta = MatrixLog3(R) + w_unit = wTheta/theta + w_so3mat = VecToso3(w_unit) + Ginv = identity(3)/theta - w_so3mat/2 + (1/theta - 1/(math.tan(theta/2)*2))*dot(w_so3mat,w_so3mat) + v_unit = dot(Ginv, p) + vTheta = v_unit*theta + STheta = vstack((wTheta, vTheta)) + return STheta + + theta = math.acos((trace(R)-1)/2) + w_so3mat = (R - R.T)/(2*math.sin(theta)) + Ginv = identity(3)/theta - w_so3mat/2 + (1/theta - 1/(math.tan(theta/2)*2))*dot(w_so3mat,w_so3mat) + wTheta = MatrixLog3(R) + v_unit = dot(Ginv, p) + vTheta = v_unit*theta + STheta = vstack((wTheta, vTheta)) + return STheta + + +def FKinFixed(M,Slist,thetalist): + ''' + Takes + - an element of SE(3): M representing the configuration of the end-effector frame when + the manipulator is at its home position (all joint thetas = 0), + - a list of screw axes Slist for the joints w.r.t fixed world frame, + - a list of joint coords thetalist, + and returns the T of end-effector frame w.r.t. fixed world frame when the joints are at + the thetas specified. + Example: + + S1 = [0,0,1,4,0,0] + S2 = [0,0,0,0,1,0] + S3 = [0,0,-1,-6,0,-0.1] + Slist = [S1, S2, S3] + thetalist = [math.pi/2, 3, math.pi] + M = [[-1, 0, 0, 0], + [ 0, 1, 0, 6], + [ 0, 0, -1, 2], + [ 0, 0, 0, 1]] + FKinFixed(M,Slist,thetalist) + >> array([[ -1.14423775e-17, 1.00000000e+00, 0.00000000e+00, -5.00000000e+00], + [ 1.00000000e+00, 1.14423775e-17, 0.00000000e+00, 4.00000000e+00], + [ 0.00000000e+00, 0.00000000e+00, -1.00000000e+00, 1.68584073e+00], + [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) + ''' + M = asarray(M) + R_M = TransToRp(M)[0] + assert M.shape == (4,4), "M not a 4x4 matrix" + assert len(Slist[0]) == 6, "Incorrect Screw Axis length" + Slist = asarray(Slist).T + + c = MatrixExp6(Slist[:,0]*thetalist[0]) + for i in range(len(thetalist)-1): + nex = MatrixExp6(Slist[:,i+1]*thetalist[i+1]) + c = dot(c, nex) + + T_se = dot(c, M) + return T_se + + +def FKinBody(M,Blist,thetalist): + ''' + Same as FKinFixed, except here the screw axes are expressed in the end-effector frame. + Example: + + B1 = [0,0,-1,2,0,0] + B2 = [0,0,0,0,1,0] + B3 = [0,0,1,0,0,0.1] + Blist = [S1b, S2b, S3b] + thetalist = [math.pi/2, 3, math.pi] + M = [[-1, 0, 0, 0], + [ 0, 1, 0, 6], + [ 0, 0, -1, 2], + [ 0, 0, 0, 1]] + FKinBody(M,Blist,thetalist) + >> array([[ -1.14423775e-17, 1.00000000e+00, 0.00000000e+00, -5.00000000e+00], + [ 1.00000000e+00, 1.14423775e-17, 0.00000000e+00, 4.00000000e+00], + [ 0.00000000e+00, 0.00000000e+00, -1.00000000e+00, 1.68584073e+00], + [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) + ''' + M = asarray(M) + R_M = TransToRp(M)[0] + assert M.shape == (4,4), "M not a 4x4 matrix" + assert len(Blist[0]) == 6, "Incorrect Screw Axis length" + Blist = asarray(Blist).T + + c = dot(M, MatrixExp6(Blist[:,0]*thetalist[0])) + for i in range(len(thetalist)-1): + nex = MatrixExp6(Blist[:,i+1]*thetalist[i+1]) + c = dot(c, nex) + + T_se = c + return T_se + + +### end of HW2 functions ############################# + +### start of HW3 functions ########################### + + +def FixedJacobian(Slist,thetalist): + ''' + Takes a list of joint angles (thetalist) and a list of screw axes (Slist) expressed in + fixed space frame, and returns the space Jacobian (a 6xN matrix, where N is the # joints). + Example: + + S1 = [0,0,1,4,0,0] + S2 = [0,0,0,0,1,0] + S3 = [0,0,-1,-6,0,-0.1] + Slist = [S1, S2, S3] + thetalist = [math.pi/2, 3, math.pi] + FixedJacobian(Slist,thetalist) + >> array([[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], + [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], + [ 1.00000000e+00, 0.00000000e+00, -1.00000000e+00], + [ 4.00000000e+00, -1.00000000e+00, -4.00000000e+00], + [ 0.00000000e+00, 1.11022302e-16, -5.00000000e+00], + [ 0.00000000e+00, 0.00000000e+00, -1.00000000e-01]]) + ''' + N = len(thetalist) + J = zeros((6,N)) + Slist = asarray(Slist).T + + J[:,0] = Slist[:,0] + for k in range(1,N): + c = MatrixExp6(Slist[:,0]*thetalist[0]) + for i in range(k-1): + nex = MatrixExp6(Slist[:,i+1]*thetalist[i+1]) + c = dot(c, nex) + J[:,k] = dot(Adjoint(c), Slist[:,k]) + + return J + + +def BodyJacobian(Blist,thetalist): + ''' + Takes a list of joint angles (thetalist) and a list of screw axes (Blist) expressed in + end-effector body frame, and returns the body Jacobian (a 6xN matrix, where N is the # joints). + Example: + + B1 = [0,0,-1,2,0,0] + B2 = [0,0,0,0,1,0] + B3 = [0,0,1,0,0,0.1] + Blist = [B1, B2, B3] + thetalist = [math.pi/2, 3, math.pi] + BodyJacobian(Blist,thetalist) + >> array([[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], + [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], + [ -1.00000000e+00, 0.00000000e+00, 1.00000000e+00], + [ -5.00000000e+00, 1.22464680e-16, 0.00000000e+00], + [ -6.12323400e-16, -1.00000000e+00, 0.00000000e+00], + [ 0.00000000e+00, 0.00000000e+00, 1.00000000e-01]]) + ''' + N = len(thetalist) + J = zeros((6,N)) + Blist = asarray(Blist).T + + J[:,N-1] = Blist[:,N-1] + for k in range(N-1): + c = MatrixExp6(-Blist[:,k+1]*thetalist[k+1]) + for i in range(k+2, len(thetalist)): + nex = MatrixExp6(-Blist[:,i]*thetalist[i]) + c = dot(nex, c) + J[:,k] = dot(Adjoint(c), Blist[:,k]) + + return J + + +def IKinBody(Blist, M, T_sd, thetalist_init, wthresh, vthresh): + ''' + A numerical inverse kinematics routine based on Newton-Raphson method. + Takes a list of screw axes (Blist) expressed in end-effector body frame, the end-effector zero + configuration (M), the desired end-effector configuration (T_sd), an initial guess of joint angles + (thetalist_init), and small positive scalar thresholds (wthresh, vthresh) controlling how close the + final solution thetas must be to the desired thetas. + Example: + + wthresh = 0.01 + vthresh = 0.001 + M = [[1,0,0,-.817],[0,0,-1,-.191],[0,1,0,-.006],[0,0,0,1]] + T_sd = [[0,1,0,-.6],[0,0,-1,.1],[-1,0,0,.1],[0,0,0,1]] + thetalist_init = [0]*6 + B1 = [0,1,0,.191,0,.817] + B2 = [0,0,1,.095,-.817,0] + B3 = [0,0,1,.095,-.392,0] + B4 = [0,0,1,.095,0,0] + B5 = [0,-1,0,-.082,0,0] + B6 = [0,0,1,0,0,0] + Blist = [B1,B2,B3,B4,B5,B6] + round(IKinBody(Blist, M, T_sd, thetalist_init, wthresh, vthresh), 3) + >> + array([[ 0. , 0. , 0. , 0. , 0. , 0. ], + [-0.356, -0.535, 0.468, 1.393, -0.356, -2.897], + [-0.399, -1.005, 1.676, -0.434, -0.1 , -1.801], + [-0.516, -1.062, 1.731, -1.63 , -0.502, -0.607], + [-0.493, -0.923, 1.508, -0.73 , -0.289, -1.42 ], + [-0.472, -0.818, 1.365, -0.455, -0.467, -1.662], + [-0.469, -0.834, 1.395, -0.561, -0.467, -1.571]]) + ''' + T_sd = asarray(T_sd) + assert T_sd.shape == (4,4), "T_sd not a 4x4 matrix" + + maxiterates = 100 + N = len(thetalist_init) + + jointAngles = asarray(thetalist_init).reshape(1,N) + + T_sb = FKinBody(M, Blist, thetalist_init) + Vb = MatrixLog6(dot(TransInv(T_sb), T_sd)) + wb = Vb[:3, 0] + vb = Vb[3:, 0] + + thetalist_i = asarray(thetalist_init) + + i = 0 + + while i<maxiterates and (linalg.norm(wb)>wthresh or linalg.norm(vb)>vthresh): + thetalist_next = thetalist_i.reshape(N,1) + dot(linalg.pinv(BodyJacobian(Blist,thetalist_i)), Vb) + # thetalist_next = (thetalist_next + pi)%(2*pi) - pi + jointAngles = vstack((jointAngles, thetalist_next.reshape(1,N))) + T_sb = FKinBody(M, Blist, thetalist_next.flatten()) + Vb = MatrixLog6(dot(TransInv(T_sb), T_sd)) + thetalist_i = thetalist_next.reshape(N,) + wb = Vb[:3, 0] + vb = Vb[3:, 0] + i += 1 + + return jointAngles + + +def IKinFixed(Slist, M, T_sd, thetalist_init, wthresh, vthresh): + ''' + Similar to IKinBody, except the screw axes are in the fixed space frame. + Example: + + M = [[1,0,0,0],[0,1,0,0],[0,0,1,0.910],[0,0,0,1]] + T_sd = [[1,0,0,.4],[0,1,0,0],[0,0,1,.4],[0,0,0,1]] + thetalist_init = [0]*7 + S1 = [0,0,1,0,0,0] + S2 = [0,1,0,0,0,0] + S3 = [0,0,1,0,0,0] + S4 = [0,1,0,-0.55,0,0.045] + S5 = [0,0,1,0,0,0] + S6 = [0,1,0,-0.85,0,0] + S7 = [0,0,1,0,0,0] + Slist = [S1,S2,S3,S4,S5,S6,S7] + round(IKinFixed(Slist, M, T_sd, thetas, wthresh, vthresh), 3) + >> + array([[ 0. , 0. , 0. , 0. , 0. , 0. , 0. ], + [ 0. , 4.471, -0. , -11.333, -0. , 6.863, -0. ], + [ -0. , 3.153, -0. , -5.462, -0. , 2.309, 0. ], + [ -0. , -1.006, 0. , 5.679, -0. , -4.673, 0. ], + [ -0. , 1.757, 0. , -0.953, 0. , -0.804, -0. ], + [ -0. , 1.754, 0. , -2.27 , -0. , 0.516, -0. ], + [ 0. , 1.27 , 0. , -1.85 , -0. , 0.58 , -0. ], + [ 0. , 1.367, 0. , -1.719, -0. , 0.351, -0. ], + [ 0. , 1.354, 0. , -1.71 , -0. , 0.356, -0. ]]) + ''' + T_sd = asarray(T_sd) + assert T_sd.shape == (4,4), "T_sd not a 4x4 matrix" + + maxiterates = 100 + + N = len(thetalist_init) + + jointAngles = asarray(thetalist_init).reshape(1,N) + + T_sb = FKinFixed(M, Slist, thetalist_init) + Vb = MatrixLog6(dot(TransInv(T_sb), T_sd)) + wb = Vb[:3, 0] + vb = Vb[3:, 0] + + thetalist_i = asarray(thetalist_init) + + i = 0 + + while i<maxiterates and (linalg.norm(wb)>wthresh or linalg.norm(vb)>vthresh): + Jb = dot(Adjoint(TransInv(T_sb)), FixedJacobian(Slist,thetalist_i)) + thetalist_next = thetalist_i.reshape(N,1) + dot(linalg.pinv(Jb), Vb) + jointAngles = vstack((jointAngles, thetalist_next.reshape(1,N))) + + T_sb = FKinFixed(M, Slist, thetalist_next.flatten()) + Vb = MatrixLog6(dot(TransInv(T_sb), T_sd)) + thetalist_i = thetalist_next.reshape(N,) + wb = Vb[:3, 0] + vb = Vb[3:, 0] + i += 1 + + return jointAngles + + +### end of HW3 functions ############################# + +### start of HW4 functions ########################### + + +def CubicTimeScaling(T,t): + ''' + Takes a total travel time T and the current time t satisfying 0 <= t <= T and returns + the path parameter s corresponding to a motion that begins and ends at zero velocity. + Example: + + CubicTimeScaling(10, 7) + >> 0.78399 + ''' + assert t >= 0 and t <= T, 'Invalid t' + + s = (3./(T**2))*(t**2) - (2./(T**3))*(t**3) + return s + + +def QuinticTimeScaling(T,t): + ''' + Takes a total travel time T and the current time t satisfying 0 <= t <= T and returns + the path parameter s corresponding to a motion that begins and ends at zero velocity + and zero acceleration. + Example: + + QuinticTimeScaling(10,7) + >> 0.83692 + ''' + assert t >= 0 and t <= T, 'Invalid t' + + s = (10./(T**3))*(t**3) + (-15./(T**4))*(t**4) + (6./(T**5))*(t**5) + return s + + +def JointTrajectory(thetas_start, thetas_end, T, N, method='cubic'): + ''' + Takes initial joint positions (n-dim) thetas_start, final joint positions thetas_end, the time of + the motion T in seconds, the number of points N >= 2 in the discrete representation of the trajectory, + and the time-scaling method (cubic or quintic) and returns a trajectory as a matrix with N rows, + where each row is an n-vector of joint positions at an instant in time. The trajectory is a straight-line + motion in joint space. + Example: + + thetas_start = [0.1]*6 + thetas_end = [pi/2]*6 + T = 2 + N = 5 + JointTrajectory(thetas_start, thetas_end, T, N, 'cubic') + >> + array([[ 0.1 , 0.1 , 0.1 , 0.1 , 0.1 , 0.1 ], + [ 0.33 , 0.33 , 0.33 , 0.33 , 0.33 , 0.33 ], + [ 0.835, 0.835, 0.835, 0.835, 0.835, 0.835], + [ 1.341, 1.341, 1.341, 1.341, 1.341, 1.341], + [ 1.571, 1.571, 1.571, 1.571, 1.571, 1.571]]) + ''' + assert len(thetas_start) == len(thetas_end), 'Incompatible thetas' + assert N >= 2, 'N must be >= 2' + assert isinstance(N, int), 'N must be an integer' + assert method == 'cubic' or method == 'quintic', 'Incorrect time-scaling method argument' + + trajectory = asarray(thetas_start) + + if method == 'cubic': + for i in range(1,N): + t = i*float(T)/(N-1) + s = CubicTimeScaling(T,t) + thetas_s = asarray(thetas_start)*(1-s) + asarray(thetas_end)*s + trajectory = vstack((trajectory, thetas_s)) + return trajectory + + elif method == 'quintic': + for i in range(1,N): + t = i*float(T)/(N-1) + s = QuinticTimeScaling(T,t) + thetas_s = asarray(thetas_start)*(1-s) + asarray(thetas_end)*s + trajectory = vstack((trajectory, thetas_s)) + return trajectory + + +def ScrewTrajectory(X_start, X_end, T, N, method='cubic'): + ''' + Similar to JointTrajectory, except that it takes the initial end-effector configuration X_start in SE(3), + the final configuration X_end, and returns the trajectory as a 4N x 4 matrix in which every 4 rows is an + element of SE(3) separated in time by T/(N-1). This represents a discretized trajectory of the screw motion + from X_start to X_end. + Example: + + thetas_start = [0.1]*6 + thetas_end = [pi/2]*6 + M_ur5 = [[1,0,0,-.817],[0,0,-1,-.191],[0,1,0,-.006],[0,0,0,1]] + S1_ur5 = [0,0,1,0,0,0] + S2_ur5 = [0,-1,0,.089,0,0] + S3_ur5 = [0,-1,0,.089,0,.425] + S4_ur5 = [0,-1,0,.089,0,.817] + S5_ur5 = [0,0,-1,.109,-.817,0] + S6_ur5 = [0,-1,0,-.006,0,.817] + Slist_ur5 = [S1_ur5,S2_ur5,S3_ur5,S4_ur5,S5_ur5,S6_ur5] + X_start = FKinFixed(M_ur5, Slist_ur5, thetas_start) + X_end = FKinFixed(M_ur5, Slist_ur5, thetas_end) + T = 2 + N = 3 + ScrewTrajectory(X_start, X_end, T, N, 'quintic') + >> + array([[ 0.922, -0.388, 0.004, -0.764], + [-0.007, -0.029, -1. , -0.268], + [ 0.388, 0.921, -0.03 , -0.124], + [ 0. , 0. , 0. , 1. ], + [ 0.534, -0.806, 0.253, -0.275], + [ 0.681, 0.234, -0.694, -0.067], + [ 0.5 , 0.543, 0.674, -0.192], + [ 0. , 0. , 0. , 1. ], + [ 0. , -1. , -0. , 0.109], + [ 1. , 0. , 0. , 0.297], + [-0. , -0. , 1. , -0.254], + [ 0. , 0. , 0. , 1. ]]) + ''' + R_start, p_start = TransToRp(X_start) #Just to ensure X_start is valid + R_end ,p_end = TransToRp(X_end) #Just to ensure X_end is valid + assert N >= 2, 'N must be >= 2' + assert isinstance(N, int), 'N must be an integer' + assert method == 'cubic' or method == 'quintic', 'Incorrect time-scaling method argument' + + trajectory = X_start + + if method == 'cubic': + for i in range(1,N): + t = i*float(T)/(N-1) + s = CubicTimeScaling(T,t) + X_s = X_start.dot(MatrixExp6(MatrixLog6(TransInv(X_start).dot(X_end))*s)) + trajectory = vstack((trajectory, X_s)) + return trajectory + + elif method == 'quintic': + for i in range(1,N): + t = i*float(T)/(N-1) + s = QuinticTimeScaling(T,t) + X_s = X_start.dot(MatrixExp6(MatrixLog6(TransInv(X_start).dot(X_end))*s)) + trajectory = vstack((trajectory, X_s)) + return trajectory + + +def CartesianTrajectory(X_start, X_end, T, N, method='cubic'): + ''' + Similar to ScrewTrajectory, except the origin of the end-effector frame follows a straight line, + decoupled from the rotational motion. + Example: + + thetas_start = [0.1]*6 + thetas_end = [pi/2]*6 + M_ur5 = [[1,0,0,-.817],[0,0,-1,-.191],[0,1,0,-.006],[0,0,0,1]] + S1_ur5 = [0,0,1,0,0,0] + S2_ur5 = [0,-1,0,.089,0,0] + S3_ur5 = [0,-1,0,.089,0,.425] + S4_ur5 = [0,-1,0,.089,0,.817] + S5_ur5 = [0,0,-1,.109,-.817,0] + S6_ur5 = [0,-1,0,-.006,0,.817] + Slist_ur5 = [S1_ur5,S2_ur5,S3_ur5,S4_ur5,S5_ur5,S6_ur5] + X_start = FKinFixed(M_ur5, Slist_ur5, thetas_start) + X_end = FKinFixed(M_ur5, Slist_ur5, thetas_end) + T = 2 + N = 3 + CartesianTrajectory(X_start, X_end, T, N, 'quintic') + >> + array([[ 0.922, -0.388, 0.004, -0.764], + [-0.007, -0.029, -1. , -0.268], + [ 0.388, 0.921, -0.03 , -0.124], + [ 0. , 0. , 0. , 1. ], + [ 0.534, -0.806, 0.253, -0.327], + [ 0.681, 0.234, -0.694, 0.014], + [ 0.5 , 0.543, 0.674, -0.189], + [ 0. , 0. , 0. , 1. ], + [ 0. , -1. , -0. , 0.109], + [ 1. , 0. , 0. , 0.297], + [-0. , -0. , 1. , -0.254], + [ 0. , 0. , 0. , 1. ]]) + + Notice the R of every T is same for ScrewTrajectory and CartesianTrajectory, but the translations + are different. + ''' + R_start, p_start = TransToRp(X_start) + R_end ,p_end = TransToRp(X_end) + assert N >= 2, 'N must be >= 2' + assert isinstance(N, int), 'N must be an integer' + assert method == 'cubic' or method == 'quintic', 'Incorrect time-scaling method argument' + + trajectory = X_start + + if method == 'cubic': + for i in range(1,N): + t = i*float(T)/(N-1) + s = CubicTimeScaling(T,t) + p_s = (1-s)*p_start + s*p_end + R_s = R_start.dot(MatrixExp3(MatrixLog3(RotInv(R_start).dot(R_end))*s)) + X_s = RpToTrans(R_s, p_s) + trajectory = vstack((trajectory, X_s)) + return trajectory + + elif method == 'quintic': + for i in range(1,N): + t = i*float(T)/(N-1) + s = QuinticTimeScaling(T,t) + p_s = (1-s)*p_start + s*p_end + R_s = R_start.dot(MatrixExp3(MatrixLog3(RotInv(R_start).dot(R_end))*s)) + X_s = RpToTrans(R_s, p_s) + trajectory = vstack((trajectory, X_s)) + return trajectory + + +### end of HW4 functions ############################# + +### start of HW5 functions ########################### + + +def LieBracket(V1, V2): + assert len(V1)==len(V2)==6, 'Needs two 6 vectors' + V1.shape = (6,1) + w1 = V1[:3,:] + v1 = V1[3:,:] + + w1_mat = VecToso3(w1) + v1_mat = VecToso3(v1) + col1 = vstack((w1_mat, v1_mat)) + col2 = vstack((zeros((3,3)), w1_mat)) + mat1 = hstack((col1, col2)) + + return mat1.dot(V2) + + +def TruthBracket(V1, V2): # Transposed version of Lie Bracket + assert len(V1)==len(V2)==6, 'Needs two 6 vectors' + V1.shape = (6,1) + w1 = V1[:3,:] + v1 = V1[3:,:] + + w1_mat = VecToso3(w1) + v1_mat = VecToso3(v1) + col1 = vstack((w1_mat, v1_mat)) + col2 = vstack((zeros((3,3)), w1_mat)) + mat1 = hstack((col1, col2)) + + return (mat1.T).dot(V2) + + +def InverseDynamics(thetas, thetadots, thetadotdots, g, Ftip, M_rels, Glist, Slist): + ''' + M1 = array(([1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,0.,.089159,1.])).T + M2 = array(([0.,0.,-1.,0.],[0.,1.,0.,0.],[1.,0.,0.,0.],[.28,.13585,.089159,1.])).T + M3 = array(([0.,0.,-1.,0.],[0.,1.,0.,0.],[1.,0.,0.,0.],[.675,.01615,.089159,1.])).T + M4 = array(([-1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,-1.,0.],[.81725,.01615,.089159,1.])).T + M5 = array(([-1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,-1.,0.],[.81725,.10915,.089159,1.])).T + M6 = array(([-1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,-1.,0.],[.81725,.10915,-.005491,1.])).T + M01 = array(([1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,0.,.089159,1.])).T + M12 = array(([0.,0.,-1.,0.],[0.,1.,0.,0.],[1.,0.,0.,0.],[.28,.13585,0.,1.])).T + M23 = array(([1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,-.1197,.395,1])).T + M34 = array(([0.,0.,-1.,0.],[0.,1.,0.,0.],[1.,0.,0.,0.],[0.,0.,.14225,1.])).T + M45 = array(([1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,.093,0.,1.])).T + M56 = array(([1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,0.,.09465,1.])).T + + G1 = array(([.010267,0.,0.,0.,0.,0.],[0.,.010267,0.,0.,0.,0.],[0.,0.,.00666,0.,0.,0.],[0.,0.,0.,3.7,0.,0.],[0.,0.,0.,0.,3.7,0.],[0.,0.,0.,0.,0.,3.7])) + G2 = array(([.22689,0.,0.,0.,0.,0.],[0.,.22689,0.,0.,0.,0.],[0.,0.,.0151074,0.,0.,0.],[0.,0.,0.,8.393,0.,0.],[0.,0.,0.,0.,8.393,0.],[0.,0.,0.,0.,0.,8.393])) + G3 = array(([.0494433,0.,0.,0.,0.,0.],[0.,.0494433,0.,0.,0.,0.],[0.,0.,.004095,0.,0.,0.],[0.,0.,0.,2.275,0.,0.],[0.,0.,0.,0.,2.275,0.],[0.,0.,0.,0.,0.,2.275])) + G4 = array(([.111172,0.,0.,0.,0.,0.],[0.,.111172,0.,0.,0.,0.],[0.,0.,.21942,0.,0.,0.],[0.,0.,0.,1.219,0.,0.],[0.,0.,0.,0.,1.219,0.],[0.,0.,0.,0.,0.,1.219])) + G5 = array(([.111172,0.,0.,0.,0.,0.],[0.,.111172,0.,0.,0.,0.],[0.,0.,.21942,0.,0.,0.],[0.,0.,0.,1.219,0.,0.],[0.,0.,0.,0.,1.219,0.],[0.,0.,0.,0.,0.,1.219])) + G6 = array(([.0171364,0.,0.,0.,0.,0.],[0.,.0171364,0.,0.,0.,0.],[0.,0.,.033822,.0,0.,0.],[0.,0.,0.,.1879,0.,0.],[0.,0.,0.,0.,.1879,0.],[0.,0.,0.,0.,0.,.1879])) + + Slist = [[0.,0.,1.,0.,0.,0.],[0.,1.,0.,-.089,0.,0.],[0.,1.,0.,-.089,0.,.425],[0.,1.,0.,-.089,0.,.817],[0.,0.,-1.,-.109,.817,.0],[0.,1.,0.,.006,0.,.817]] + + Glist = [G1,G2,G3,G4,G5,G6] + M_rels = [M01,M12,M23,M34,M45,M56] + Ftip = [0.,0.,0.,0.,0.,0.] + ''' + assert len(thetas) == len(thetadots) == len(thetadotdots), 'Joint inputs mismatch' + Slist = asarray(Slist) + N = len(thetas) # no. of links + Ftip = asarray(Ftip) + + # initialize iterables + Mlist = [0]*N + T_rels = [0]*N + Vlist = [0]*N + Vdotlist = [0]*N + Alist = [0]*N + Flist = [0]*N + Taulist = [0]*N + + + Mlist[0] = M_rels[0] + for i in range(1, N): + Mlist[i] = Mlist[i-1].dot(M_rels[i]) + + + for i in range(N): + Alist[i] = Adjoint(TransInv(Mlist[i])).dot(Slist[i]) + + + for i in range(N): + T_rels[i] = M_rels[i].dot(MatrixExp6(Alist[i]*thetas[i])) + + + V0 = zeros((6,1)) + Vlist[0] = V0 + + Vdot0 = -asarray([0,0,0]+g) + Vdotlist[0] = Vdot0 + + F_ip1 = asarray(Ftip).reshape(6,1) + + # forward iterations + for i in range(1, N): + # print i + T_i_im1 = TransInv(T_rels[i]) #### should it be i-1??? + + Vlist[i] = Adjoint(T_i_im1).dot(Vlist[i-1]) + (Alist[i]*thetadots[i]).reshape(6,1) + + Vdotlist[i] = Adjoint(T_i_im1).dot(Vdotlist[i-1]) + LieBracket(Vlist[i], Alist[i])*thetadots[i] + Alist[i]*thetadotdots[i] + + + # print T_rels + # print Vlist + # print Vdotlist + # print Alist + + # backward iterations + for i in range(N-1,-1,-1): + T_ip1_i = TransInv(T_rels[i]) + Flist[i] = (Adjoint(T_ip1_i).T).dot(F_ip1) + (Glist[i].dot(Vdotlist[i])).reshape(6,1) - TruthBracket(Vlist[i], Glist[i].dot(Vlist[i])) + Taulist[i] = (Flist[i].T).dot(Alist[i]) + + return asarray(Taulist).flatten() + + +def InertiaMatrix(thetas, M_rels, Glist, Slist): + Slist = asarray(Slist) + N = len(thetas) # no. of links + + M = zeros((N,N)) + + for i in range(N): + thetadotdots = [0]*N + thetadotdots[i] = 1 + M[:, i] = InverseDynamics(thetas, [0]*6, thetadotdots, [0]*3, [0]*6, M_rels, Glist, Slist) + + return M + + +def CoriolisForces(thetas, thetadots, M_rels, Glist, Slist): + C = InverseDynamics(thetas, thetadots, [0]*6, [0]*3, [0]*6, M_rels, Glist, Slist) + return C + + +def GravityForces(thetas, g, M_rels, Glist, Slist): + Gvec = InverseDynamics(thetas, [0]*6, [0]*6, g, [0]*6, M_rels, Glist, Slist) + return Gvec + + +def EndEffectorForces(Ftip, thetas, M_rels, Glist, Slist): + return InverseDynamics(thetas, [0]*6, [0]*6, [0]*3, Ftip, M_rels, Glist, Slist) + + +def ForwardDynamics(thetas, thetadots, taus, g, Ftip, M_rels, Glist, Slist): + M = InertiaMatrix(thetas, M_rels, Glist, Slist) + C = CoriolisForces(thetas, thetadots, M_rels, Glist, Slist) + Gvec = GravityForces(thetas, g, M_rels, Glist, Slist) + eeF = EndEffectorForces(Ftip, thetas, M_rels, Glist, Slist) + + thetadotdots = (linalg.pinv(M)).dot(taus - C - Gvec - eeF) + return thetadotdots + + +def EulerStep(thetas_t, thetadots_t, thetadotdots_t, delt): + thetas_t = asarray(thetas_t) + thetadots_t = asarray(thetadots_t) + thetadotdots_t = asarray(thetadotdots_t) + + thetas_next = thetas_t + delt*thetadots_t + thetadots_next = thetadots_t + delt*thetadotdots_t + + return thetas_next, thetadots_next + + +def InverseDynamicsTrajectory(thetas_traj, thetadots_traj, thetadotdots_traj, Ftip_traj, g, M_rels, Glist, Slist): + Np1 = len(thetas_traj) + # N = Np1-1 + idtraj = [] + for i in range(Np1): + taus = InverseDynamics(thetas_traj[i], thetadots_traj[i], thetadotdots_traj[i], g, Ftip_traj[i], M_rels, Glist, Slist) + idtraj.append(taus) + + return asarray(idtraj) + + +def ForwardDynamicsTrajectory(thetas_init, thetadots_init, tau_hist, delt, g, Ftip_traj, M_rels, Glist, Slist): + Np1 = len(tau_hist) + thetas_traj = asarray(thetas_init) + thetadots_traj = asarray(thetadots_init) + + for i in range(Np1): + thetadotdots_init = ForwardDynamics(thetas_init, thetadots_init, tau_hist[i], g, Ftip_traj[i], M_rels, Glist, Slist) + thetas_next, thetadots_next = EulerStep(thetas_init, thetadots_init, thetadotdots_init, delt) + # append new state + thetas_traj = hstack((thetas_traj, thetas_next)) + thetadots_traj = hstack((thetadots_traj, thetadots_next)) + # update initial conditions + thetas_init = thetas_next + thetadots_init = thetadots_next + + return thetas_traj.reshape(Np1+1, len(thetas_init)), thetadots_traj.reshape(Np1+1, len(thetas_init)) + + +### end of HW5 functions ############################# \ No newline at end of file