def matrixTrans(S, theta, Tmat):
I = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
w = np.array([S[0], S[1], S[2]])
v = np.array([S[3], S[4], S[5]])
#print w,v
skewW = ro.VecToso3(w)
skewV = ro.VecToso3(v)
#calculate terms of rodrigues form for exponential rotation matrix
term1 = np.multiply(I, theta)
term2 = np.multiply(1 - cos(theta), skewW)
term3l = theta - sin(theta)
term3r = np.matmul(skewW, skewW)
term3 = np.multiply(term3l, term3r)
R = term1 + term2 + term3 #Rotation matrix of exponential matrix
#print R
P = np.matmul(R, v) #column vector of exponential matrix
#print P
rT, pT = ro.TransToRp(Tmat) # R and P of given matrix T
#perform matrix transformation of form T_prime = ExpMatrix x T
rT_prime = np.matmul(R, rT)
pT_prime = np.matmul(R, pT) + P
return ro.RpToTrans(rT_prime,
pT_prime) #return transformation matrix of T prime
def milestone2_test():
"""
Milestone 2 : Test
The robot manipulator traectory generation for pick and place task
CoppeliaSim : Scene8_gripper_csv
"""
Tse_initial = [[1, 0, 0, 0.1662], [0, 1, 0, 0], [0, 0, 1, 0.5],
[0, 0, 0, 1]]
Tsc_initial = [[1, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0.025], [0, 0, 0, 1]]
Tsc_goal = [[0, 1, 0, 0], [-1, 0, 0, -1], [0, 0, 1, 0.025], [0, 0, 0, 1]]
# Generating stadoff and gripping transforms
theta = [0, 3 * math.pi / 4, 0]
R_so3 = mr.VecToso3(theta)
R = mr.MatrixExp3(R_so3)
position = [-0.01, 0, 0.20]
Tce_standoff = mr.RpToTrans(R, position)
position = [-0.01, 0, 0.01]
Tce_gripping = mr.RpToTrans(R, position)
trajectory = trajectory_planner.TrajectoryGenerator(
Tse_initial, Tsc_initial, Tsc_goal, Tce_gripping, Tce_standoff, 0.01,
10)
csv_writer.writeDataList(trajectory, 'milestone2_test')
def TAAtoTM(transaa):
transaa = np.vstack((transaa[0], transaa[1], transaa[2], transaa[3],
transaa[4], transaa[5]))
s = transaa.shape[1]
if s == 6:
transaa = (transaa).conj().transpose()
#
mres = (mr.MatrixExp3(mr.VecToso3(transaa[3:6])))
tm = np.vstack((np.hstack((mres, transaa[0:3])), np.array([0, 0, 0, 1])))
return tm
def youBot_Tbase(theta, x, y):
"""Computes the transform of youBot base given a base configuration
:param configuration: A 3-vector representing the current configuration of the robot base.
3 variables for the chassis configuration (theta, x, y),
:return: Transform of robot base in space frame
"""
rotation = [0, 0, theta]
position = [x, y, 0.0963]
R_so3 = mr.VecToso3(rotation)
R = mr.MatrixExp3(R_so3)
Tbase = mr.RpToTrans(R, position)
return Tbase
def poseToTransform(pose):
"""Converts a 3D pose (Wx, Wy, Wz, x, y, z) to a homogeneous transformation matrix
:param pose: 2D pose [Wx, Wy, Wz, x, y, z]
Wx, Wy, Wz : Rotation angles
x, y, z : Position
:return: Homogeneous Transformation matrix corresponding to given pose
"""
[Wx, Wy, Wz, x, y, z] = pose
rotation = [Wx, Wy, Wz]
position = [x, y, z]
R_so3 = mr.VecToso3(rotation)
R = mr.MatrixExp3(R_so3)
T = mr.RpToTrans(R, position)
return T
def youBot_Tse(configuration):
"""Computes the transform of youBot End effector given a robot configuration
:param configuration: A 12-vector representing the current configuration of the robot.
3 variables for the chassis configuration (theta, x, y),
5 variables for the arm configuration (J1, J2, J3, J4, J5),
and 4 variables for the wheel angles (W1, W2, W3, W4)
:return: Transform of End effector in space frame
"""
theta, x, y = configuration[0:3]
rotation = [0, 0, theta]
position = [x, y, 0.0963]
R_so3 = mr.VecToso3(rotation)
R = mr.MatrixExp3(R_so3)
Tsb = mr.RpToTrans(R, position)
Tbe = youBot_Tbe(configuration[3:8])
Tse = np.dot(Tsb, Tbe)
return Tse
print('Question 9: Calculate the matrix exponential\n')
term1 = np.identity(3)
term2 = np.dot(np.sin(theta), w_hat_skew)
term3_a = (1 - np.cos(np.sqrt(5)))
term3_b = matrix_power(w_hat_skew, 2)
term3 = np.dot(term3_a, term3_b)
R = term1 + term2 + term3
print('R =')
print(R)
print('\n')
# Question 10
print('Question 10: Create skew symmetric matrix using vector w.\n')
# np.array([1, 2, 3])
w = np.array([[1], [2], [0.5]])
w_skew = mr.VecToso3(w)
print('w_skew = ')
print(w_skew)
print('\n')
# Question 11
print('Question 11: Calculate rotation matrix using matrix exponential\n')
w_hat_theta = np.array([[0, 0.5, -1], [-0.5, 0, 2], [1, -2, 0]])
R = mr.MatrixExp3(w_hat_theta)
print('R =')
print(R)
print('\n')
# Question 12
print('Question 12: Calculate matrix logarithm given matrix exponential\n')
R = np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])
#Q5 V6 = np.array([0, 0, V[0], V[1], V[2], 0]) se3mat = mr.VecTose3(V6) T = mr.MatrixExp6(se3mat) R, p = mr.TransToRp(T) so3mat = mr.MatrixLog3(R) omega = mr.so3ToVec(so3mat) # print p # print omega #Q06 F = np.array([[0, 0], [0, 0], [-0.25, 0.25], [0.25, 0.25], [0, 0], [0, 0]]) # Finding Tbe omega = np.array([0, 0, math.pi / 2]) p = np.array([2, 3, 0]) so3mat = mr.VecToso3(omega) R = mr.MatrixExp3(so3mat) Tbe = mr.RpToTrans(R, p) Teb = np.linalg.inv(Tbe) Ad_Teb = mr.Adjoint(Teb) Jbase = np.dot(Ad_Teb, F) print Tbe print Jbase # #Q07 Jb = [0, 0, 1, -3, 0, 0] Jx = [0, 0, 1, 0, -2, 0] # print np.dot(Ad_Teb, Jx) # [0,-1,0,2;1,0,0,3;0,0,1,0;0,0,0,1] # 0, 1, 0, -3,
print(R_sa)
###############Question #2###########
print("Question #2\n")
invR_sb = Mr.RotInv(R_sb)
print(invR_sb)
###############Question #3###########
print("Question #3\n")
invR_sa = Mr.RotInv(R_sa)
R_ab = np.matmul(invR_sa, R_sb)
print(R_ab)
###############Question #5 ###########
print("Question #5\n")
p_b = [1, 2, 3]
p = np.matmul(R_sb, p_b)
print(p)
###############Question #7 ###########
print("Question #7\n")
w = np.array([3, 2, 1]).T
w_a = np.matmul(invR_sa, w)
print(w_a)
###############Question #8 ###########
print("Question #8\n")
w = np.array([3, 2, 1]).T
w_a = np.matmul(invR_sa, w)
print(w_a)
###############Question #9###########
print("Question #9\n")
omg = [1, 2, 0]
so3mat = Mr.VecToso3(omg)
print(so3mat)
def p9():
print('P9')
omega = np.array([1, 2, 0])
so3mat = mr.VecToso3(omega)
result = mr.MatrixExp3(so3mat)
print(result)
def p10():
print('P10')
omega = np.array([1, 2, .5])
result = mr.VecToso3(omega)
print(result)
Tse_refInitial = [[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0.5], [0, 0, 0, 1]]
gains = input("Enter Feedback Controller gains [kp, ki] : ")
# Program constants
timeStep = 0.01
velocityLimits = [10, 10, 10, 20, 20, 20, 20, 20, 10, 10, 10, 10]
K = 10
result_configuration = []
result_Xerr = []
# Generating stadoff and gripping transforms
theta = [0, 3 * math.pi/4, 0]
R_so3 = mr.VecToso3(theta)
R = mr.MatrixExp3(R_so3)
position = [0.00, 0, 0.20] #[-0.01, 0, 0.20]
Tce_standoff = mr.RpToTrans(R, position)
position = [0.00, 0, 0.00] #[-0.01, 0, 0.00]
Tce_gripping = mr.RpToTrans(R, position)
# Displaying initial End Effector configuration error
initialRef = kinematic_model.transformToPose(Tse_refInitial)
initialConf = kinematic_model.transformToPose(Tse_initial)
initialError = initialRef - initialConf
print 'Initial End Effector configuration Error (Theta_x, Theta_y, Theta_z, x, y, z):\n', np.around(initialError, 3)
## Trajectory generation
Tsa = np.array([[0, -1, 0, 0], [0, 0, -1, 0], [1, 0, 0, 1], [0, 0, 0, 1]])
Tsb = np.array([[1, 0, 0, 0], [0, 0, 1, 2], [0, -1, 0, 0], [0, 0, 0, 1]])
PointB = np.array([1, 2, 3, 1])
# Vs = ([3,2,1,-1,-2,-3])
# omegaTheta = mr.VecToso3(np.array([0,1,2]))
# Theta = 2.236067977
# print mr.MatrixExp3(omegaTheta)
# print ( np.identity(3) * Theta + (1-math.cos(Theta))*omegaTheta + (Theta-math.sin(Theta)) * (omegaTheta * omegaTheta)) * np.transpose(np.array([3,0,0]))
# print mr.MatrixExp6(mr.VecTose3(np.array([0,1,2,3,0,0])))
#R = mr.MatrixLog6(Tsa)
#R = mr.Adjoint(mr.TransInv(Tsa)) * np.transpose(Vs)
# R = Tsa * PointB
#R = mr.TransInv(Tsb)
#R = mr.MatrixLog3(so3mat)
# print R
omega = np.array([1 / math.sqrt(5), 2 / math.sqrt(5), 0])
theta = math.sqrt(5)
i = np.identity(3)
R = i + mr.VecToso3(omega) * math.sin(theta) + np.dot(
mr.VecToso3(omega), mr.VecToso3(omega)) * (1 - math.cos(theta))
print R
# [1,3,-2]
print("Ex7, wa:")
wa = Rsa * ws
print(wa)
# [1,3,2]
print("Ex8, theta:")
MatLogRsa = mr.MatrixLog3(Rsa)
vec = mr.so3ToVec(MatLogRsa)
theta = mr.AxisAng3(vec)[-1]
print(theta)
# 2.094395102393196
print("Ex9, Matrix exponential:")
skew = mr.VecToso3(wtheta)
MatExp = mr.MatrixExp3(skew)
print(MatExp)
# [[-0.2938183,0.64690915,0.70368982],[0.64690915,0.67654542,-0.35184491],[-0.70368982,0.35184491,-0.61727288]]
print("Ex10, skew.symmetric matrix:")
skewMat = mr.VecToso3(wskew)
print(skewMat)
# [[0,-0.5,2],[0.5,0,-1],[-2,1,0]]
print("Ex11, Rotation matrix:")
RotMat = mr.MatrixExp3(so3)
print(RotMat)
# [[0.60482045,0.796274,-0.01182979],[0.46830057,-0.34361048,0.81401868],[0.64411707,-0.49787504,-0.58071821]]
print("Ex12, Matrix logarithm")
def milestone3_test4():
"""
Milestone 3 : Test4
Check robot controller for generated tragectory (kp = 0, ki = 0)
"""
configuration = [
0.0, 0.0, 0.0, 0.0, -0.35, -0.698, -0.505, 0.0, 0.0, 0.0, 0.0, 0.0
]
Tsc_initial = [[1, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0.025], [0, 0, 0, 1]]
Tsc_goal = [[0, 1, 0, 0], [-1, 0, 0, -1], [0, 0, 1, 0.025], [0, 0, 0, 1]]
# Tsc_goal = [[1, 0, 0, 0.5], [0, 1, 0, 0], [0, 0, 1, 0.025], [0, 0, 0, 1]]
Tse_initial = kinematic_model.youBot_Tse(configuration)
result_configuration = []
result_Xerr = []
gains = [0, 0] # kp, ki
timeStep = 0.01
velocityLimits = [10, 10, 10, 20, 20, 20, 20, 20, 10, 10, 10, 10]
K = 1
# Generating stadoff and gripping transforms
theta = [0, 3 * math.pi / 4, 0]
R_so3 = mr.VecToso3(theta)
R = mr.MatrixExp3(R_so3)
position = [-0.01, 0, 0.20]
Tce_standoff = mr.RpToTrans(R, position)
position = [-0.005, 0, 0.005]
Tce_gripping = mr.RpToTrans(R, position)
# Trajectory generation
print 'Generating trajectory'
trajectory = trajectory_planner.TrajectoryGenerator(
Tse_initial, Tsc_initial, Tsc_goal, Tce_gripping, Tce_standoff,
timeStep, K)
print 'Trajectory generation successfull'
# Control generation
for x in range(len(trajectory) - 1):
Xd = kinematic_model.configurationToTransform(trajectory[x])
Xd_next = kinematic_model.configurationToTransform(trajectory[x + 1])
Tse = kinematic_model.youBot_Tse(configuration)
V, Xerr = controller.FeedbackControl(Tse, Xd, Xd_next, gains, timeStep)
# Calculate Je
Je = kinematic_model.youBot_Je(configuration)
while True:
# Calculating control
cmd = np.dot(np.linalg.pinv(Je), V)
# Control : omega (5 variables), wheel speeds u (4 variables)
control = np.concatenate((cmd[4:9], cmd[0:4]))
next_config = kinematic_simulator.NextState(
configuration, control, timeStep, velocityLimits)
# Joint limit checking
# status, jointVector = kinematic_model.youBot_testJointLimits(next_config[3:8])
# status = True # TODO
status = kinematic_model.youBot_selfCollisionCheck(
next_config[3:8])
jointVector = [0, 0, 0, 0, 0]
if status:
configuration = next_config
break
else:
Je[:, 4:9] = Je[:, 4:9] * jointVector
# Save configuration (Tse) and Xerr per every K iterations
if (x % K) == 0:
# Append with gripper state
gripper_state = trajectory[x][12]
youBot_configuration = np.concatenate(
(configuration, [gripper_state]))
result_configuration.append(youBot_configuration)
result_Xerr.append(Xerr)
completion = round(((x + 1) * 100.0 / len(trajectory)), 2)
sys.stdout.write("\033[F")
print('Generating youBot trajectory controls. Progress ' +
str(completion) + '%\r')
print "Final Error (Xerr) : \n", np.around(Xerr, 5)
configComments = [
"A 12-vector representing the current configuration of the robot",
"\t3 variables for the chassis configuration (theta, x, y)",
"\t5 variables for the arm configuration (J1, J2, J3, J4, J5)",
"\tand 4 variables for the wheel angles (W1, W2, W3, W4)"
]
csv_writer.writeDataList(result_configuration,
name="milestone3_configurations",
comments=configComments)
XerrComments = [
"A 6-vector representing the error twist",
"\t3 variables for the angular velocity error (Wx, Wy, Wz)",
"\t3 variables for the linear velocity error (Vx, Vy, Vz)"
]
csv_writer.writeDataList(result_Xerr,
name="milestone3_Xerr",
comments=XerrComments)
# Displaying Error Plots
labels = ['Wx', 'Wy', 'Wz', 'Vx', 'Vy', 'Vz']
for x in range(6):
plt.plot(np.array(result_Xerr)[:, x], label=labels[x])
plt.ylabel('Xerr')
plt.legend()
plt.show()
def milestone3_test4_debug():
"""
Milestone 3 : Test4
Check robot controller for generated tragectory (kp = 0, ki = 0)
"""
# configuration = [0, 0, 0, 0, -0.35, -0.698, -0.505, 0, 0, 0, 0, 0]
configuration = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
Tsc_initial = [[1, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0.025], [0, 0, 0, 1]]
Tsc_goal = [[0, 1, 0, 0], [-1, 0, 0, -1], [0, 0, 1, 0.025], [0, 0, 0, 1]]
Tse_initial = kinematic_model.youBot_Tse(configuration)
result_configuration = []
result_Xerr = []
gains = [0, 0] # kp, ki
timeStep = 0.5
K = 1
# Generating stadoff and gripping transforms
theta = [0, 3 * math.pi / 4, 0]
R_so3 = mr.VecToso3(theta)
R = mr.MatrixExp3(R_so3)
position = [-0.01, 0, 0.20]
Tce_standoff = mr.RpToTrans(R, position)
position = [-0.01, 0, 0.01]
Tce_gripping = mr.RpToTrans(R, position)
# Trajectory generation
# trajectory = trajectory_planner.TrajectoryGenerator(Tse_initial, Tsc_initial, Tsc_goal, Tce_gripping, Tce_standoff, timeStep, K )
Xstart = kinematic_model.youBot_Tse(configuration)
XendConfig = [0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Xend = kinematic_model.youBot_Tse(XendConfig)
trajectory = mr.ScrewTrajectory(Xstart, Xend, 3, 6, 3)
csv_writer.writeDataList(trajectory, "milestone3_trajectory")
# Control generation
for x in range(len(trajectory) - 1):
Xd = trajectory[x]
Xd_next = trajectory[x + 1]
# Xd = kinematic_model.configurationToTransform(trajectory[x])
# Xd_next = kinematic_model.configurationToTransform(trajectory[x+1])
Tse = kinematic_model.youBot_Tse(configuration)
print "####### Iteration #######"
print "Tse"
print np.around(Tse, 3).tolist()
print "Xd"
print np.around(Xd, 3).tolist()
print "Xd_next"
print np.around(Xd_next, 3).tolist()
V, Xerr = controller.FeedbackControl(Tse, Xd, Xd_next, gains, timeStep)
print "V"
print np.around(V, 3).tolist()
print "Xerr"
print np.around(Xerr, 3).tolist()
# Calculate Je
Je = kinematic_model.youBot_Je(configuration)
while True:
# Calculating control
cmd = np.dot(np.linalg.pinv(Je), V)
# Control : omega (5 variables), wheel speeds u (4 variables)
control = np.concatenate((cmd[4:9], cmd[0:4]))
print "control"
print np.around(control, 3).tolist()
next_config = kinematic_simulator.NextState(
configuration, control, timeStep)
print "next_config"
print np.around(next_config, 3).tolist()
# Joint limit checking
status, jointVector = kinematic_model.youBot_testJointLimits(
next_config[3:8])
if status:
configuration = next_config
break
else:
Je[:, 4:9] = Je[:, 4:9] * jointVector
# Save configuration (Tse) and Xerr per every K iterations
if (x % K) == 0:
result_configuration.append(configuration)
result_Xerr.append(Xerr)
# print np.around(configuration, 3).tolist()
csv_writer.writeDataList(result_configuration, "milestone3_configurations")