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 #############################
