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 p11():
print('P11')
so3mat = np.array([[0, .5, -1],
[-.5, 0, 2],
[1, -2, 0]])
result = mr.MatrixExp3(so3mat)
print(result)
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 TrajectoryGenerator(Xstart, Xend, Tf, N, gripper_state, write):
"""Computes a trajectory as a list of N SE(3) matrices corresponding to
the straight line motion.
:param Xstart: The initial end-effector configuration
:param Xend: The final end-effector configuration
:param Tf: Total time of the motion in seconds from rest to rest
:param N: The number of points N > 1 (Start and stop) in the discrete
representation of the trajectory
:param gripper_state: 0- open, 1-close
:write: a csv_write object
:return: The discretized trajectory as a list of N matrices in SE(3)
separated in time by Tf/(N-1). The first in the list is Xstart
and the Nth is Xend. R is the rotation matrix in X, and p is the linear position part of X.
13-array: [9 R variables (from first row to last row), 3 P variables (from x to z), gripper_state ]
Example Input:
Xstart = np.array([[1, 0, 0, 1],
[0, 1, 0, 0],
[0, 0, 1, 1],
[0, 0, 0, 1]])
Xend = np.array([[0, 0, 1, 0.1],
[1, 0, 0, 0],
[0, 1, 0, 4.1],
[0, 0, 0, 1]])
Tf = 5
N = 4
gripper_state = 0
write = csv.writer(csv_file,delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
Output:
[1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.1992,0.0,0.7535,0.0]
"""
N = int(N)
timegap = Tf / (N - 1.0)
traj = [[None]] * N
for i in range(N):
s = mr.QuinticTimeScaling(Tf, timegap * i)
Rstart, pstart = mr.TransToRp(Xstart)
Rend, pend = mr.TransToRp(Xend)
traj[i] = np.r_[np.c_[np.dot(Rstart, \
mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(Rstart).T,Rend)) * s)), \
s * np.array(pend) + (1 - s) * np.array(pstart)], \
[[0, 0, 0, 1]]]
# traj[i] = np.dot(Xstart, mr.MatrixExp6(mr.MatrixLog6(np.dot(mr.TransInv(Xstart), Xend)) * s))
output = traj[i][:-1, :-1].flatten()
output = np.append(output, traj[i][:, -1][:-1].flatten())
output = np.append(output, gripper_state)
write.writerow(output)
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
def p2p_cart_cubic_decoupled(robot, T_end, ts):
T_start = robot.get_homogeneous()
pos_start = T_start[0:3, 3]
pos_end = T_end[0:3, 3]
rot_start = T_start[0:3, 0:3]
rot_end = T_end[0:3, 0:3]
a2 = 3 / (ts**2)
a3 = -2 / (ts**3)
t = 0
while t < ts:
t += robot.dt
s = a2 * t**2 + a3 * t**3
pos = pos_start + (pos_end - pos_start) * s
mat_rot = rot_start @ mr.MatrixExp3(
mr.MatrixLog3(rot_start.T @ rot_end) * s)
eul = rot2eul(mat_rot)
quat = p.getQuaternionFromEuler(eul)
joints = p.calculateInverseKinematics(
robot.robot_id,
endEffectorLinkIndex=robot.eef_link_idx,
targetPosition=pos,
targetOrientation=quat)
yield joints
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]])
w_hat_theta = mr.MatrixLog3(R)
print('w_hat_theta = ')
print(w_hat_theta)
print('\n')
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, # -1, 0, 0, 2
def p9():
print('P9')
omega = np.array([1, 2, 0])
so3mat = mr.VecToso3(omega)
result = mr.MatrixExp3(so3mat)
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
print 'Generating trajectory'
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")
MatLogR12 = mr.MatrixLog3(R12)
def TrajectoryGenerator(Xstart, Tsci, Tscf, Tceg, Tces, k):
"""
Generates end-effector trajectory between 9 points (8 transitions) and adds them to a csv file
The trajectory from the first to second standoff position is a screw instead of a Cartesian.
Xstart: initial configuration of end-effector in reference trajectory (Tsei)
Tsci: cube's initial configuration
Tscf: cube's final configuration
Tceg: end-effector position relative to cube when grasping
Tces: end-effector position above the cube before and after grasping
k: number of reference trajectory configurations per centisecond
"""
Tf = 3
method = 5
Xend = np.matmul(Tsci, Tces) #first standoff
FSL = np.matmul(Tsci, Tceg) #first standoff lowered
SS = np.matmul(Tscf, Tces) #second standoff
SSL = np.matmul(Tscf, Tceg) #second standoff lowered
Xtheend = np.matmul(Tscf, Tces)
N = Tf * 100 * k
N = int(N)
timegap = Tf / (N - 1.0)
traj = [[None]] * N
Rstart, pstart = mr.TransToRp(Xstart)
Rend, pend = mr.TransToRp(Xend)
for i in range(N):
if method == 3:
s = mr.CubicTimeScaling(Tf, timegap * i)
else:
s = mr.QuinticTimeScaling(Tf, timegap * i)
traj[i] \
= np.r_[np.c_[np.dot(Rstart, \
mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(Rstart).T,Rend)) * s)), \
s * np.array(pend) + (1 - s) * np.array(pstart)], \
[[0, 0, 0, 1]]]
row = []
for p in range(len(traj[i]) - 1):
for l in range(0, 3):
row.append(traj[i][p][l])
for m in range(len(traj[i]) - 1):
for n in range(0, 4):
if n == 3:
row.append(traj[i][m][n])
row.append(0)
with open('penultimate.csv', 'a') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(row)
csvFile.close()
N = int(N)
timegap = Tf / (N - 1.0)
traj = [[None]] * N
RstartFSL, pstartFSL = mr.TransToRp(Xend)
RendFSL, pendFSL = mr.TransToRp(FSL)
for i in range(N):
if method == 3:
s = mr.CubicTimeScaling(Tf, timegap * i)
else:
s = mr.QuinticTimeScaling(Tf, timegap * i)
traj[i] \
= np.r_[np.c_[np.dot(RstartFSL, \
mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartFSL).T,RendFSL)) * s)), \
s * np.array(pendFSL) + (1 - s) * np.array(pstartFSL)], \
[[0, 0, 0, 1]]]
row = []
for q in range(len(traj[i]) - 1):
for r in range(0, 3):
row.append(traj[i][q][r])
for s in range(len(traj[i]) - 1):
for t in range(0, 4):
if t == 3:
row.append(traj[i][s][t])
row.append(0)
with open('penultimate.csv', 'a') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(row)
csvFile.close()
N = int(N)
timegap = Tf / (N - 1.0)
traj = [[None]] * N
RstartFC, pstartFC = mr.TransToRp(FSL) #FC = FIRST CLOSE
RendFC, pendFC = mr.TransToRp(FSL)
for i in range(N):
if method == 3:
s = mr.CubicTimeScaling(Tf, timegap * i)
else:
s = mr.QuinticTimeScaling(Tf, timegap * i)
traj[i] \
= np.r_[np.c_[np.dot(RstartFC, \
mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartFC).T,RendFC)) * s)), \
s * np.array(pendFC) + (1 - s) * np.array(pstartFC)], \
[[0, 0, 0, 1]]]
row = []
for u in range(len(traj[i]) - 1):
for v in range(0, 3):
row.append(traj[i][u][v])
for w in range(len(traj[i]) - 1):
for x in range(0, 4):
if x == 3:
row.append(traj[i][w][x])
row.append(1)
with open('penultimate.csv', 'a') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(row)
csvFile.close()
N = int(N)
timegap = Tf / (N - 1.0)
traj = [[None]] * N
RstartFrise, pstartFrise = mr.TransToRp(FSL) #Frise = first rise
RendFrise, pendFrise = mr.TransToRp(Xend)
for i in range(N):
if method == 3:
s = mr.CubicTimeScaling(Tf, timegap * i)
else:
s = mr.QuinticTimeScaling(Tf, timegap * i)
traj[i] \
= np.r_[np.c_[np.dot(RstartFrise, \
mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartFrise).T,RendFrise)) * s)), \
s * np.array(pendFrise) + (1 - s) * np.array(pstartFrise)], \
[[0, 0, 0, 1]]]
row = []
for y in range(len(traj[i]) - 1):
for z in range(0, 3):
row.append(traj[i][y][z])
for aa in range(len(traj[i]) - 1):
for ab in range(0, 4):
if ab == 3:
row.append(traj[i][aa][ab])
row.append(1)
with open('penultimate.csv', 'a') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(row)
csvFile.close()
"""
N = int(N)
timegap = Tf / (N - 1.0)
traj = [[None]] * N
RstartSS, pstartSS = mr.TransToRp(Xend) #Frise = first rise
RendSS, pendSS = mr.TransToRp(SS)
for i in range(N):
if method == 3:
s = mr.CubicTimeScaling(Tf, timegap * i)
else:
s = mr.QuinticTimeScaling(Tf, timegap * i)
traj[i] \
= np.r_[np.c_[np.dot(RstartSS, \
mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartSS).T,RendSS)) * s)), \
s * np.array(pendSS) + (1 - s) * np.array(pstartSS)], \
[[0, 0, 0, 1]]]
"""
N = int(N)
timegap = Tf / (N - 1.0)
traj = [[None]] * N
for i in range(N):
if method == 3:
s = mr.CubicTimeScaling(Tf, timegap * i)
else:
s = mr.QuinticTimeScaling(Tf, timegap * i)
traj[i] \
= np.dot(Xend, mr.MatrixExp6(mr.MatrixLog6(np.dot(mr.TransInv(Xend), \
SS)) * s))
row = []
for ac in range(len(traj[i]) - 1):
for ad in range(0, 3):
row.append(traj[i][ac][ad])
for ae in range(len(traj[i]) - 1):
for af in range(0, 4):
if af == 3:
row.append(traj[i][ae][af])
row.append(1)
with open('penultimate.csv', 'a') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(row)
csvFile.close()
N = int(N)
timegap = Tf / (N - 1.0)
traj = [[None]] * N
RstartSSL, pstartSSL = mr.TransToRp(SS) #Frise = first rise
RendSSL, pendSSL = mr.TransToRp(SSL)
for i in range(N):
if method == 3:
s = mr.CubicTimeScaling(Tf, timegap * i)
else:
s = mr.QuinticTimeScaling(Tf, timegap * i)
traj[i] \
= np.r_[np.c_[np.dot(RstartSSL, \
mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartSSL).T,RendSSL)) * s)), \
s * np.array(pendSSL) + (1 - s) * np.array(pstartSSL)], \
[[0, 0, 0, 1]]]
row = []
for ag in range(len(traj[i]) - 1):
for ah in range(0, 3):
row.append(traj[i][ag][ah])
for ai in range(len(traj[i]) - 1):
for aj in range(0, 4):
if aj == 3:
row.append(traj[i][ai][aj])
row.append(1)
with open('penultimate.csv', 'a') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(row)
csvFile.close()
N = int(N)
timegap = Tf / (N - 1.0)
traj = [[None]] * N
RstartSSU, pstartSSU = mr.TransToRp(SSL) #Frise = first rise
RendSSU, pendSSU = mr.TransToRp(SSL)
for i in range(N):
if method == 3:
s = mr.CubicTimeScaling(Tf, timegap * i)
else:
s = mr.QuinticTimeScaling(Tf, timegap * i)
traj[i] \
= np.r_[np.c_[np.dot(RstartSSU, \
mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartSSU).T,RendSSU)) * s)), \
s * np.array(pendSSU) + (1 - s) * np.array(pstartSSU)], \
[[0, 0, 0, 1]]]
row = []
for ak in range(len(traj[i]) - 1):
for al in range(0, 3):
row.append(traj[i][ak][al])
for am in range(len(traj[i]) - 1):
for an in range(0, 4):
if an == 3:
row.append(traj[i][am][an])
row.append(0)
with open('penultimate.csv', 'a') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(row)
csvFile.close()
N = int(N)
timegap = Tf / (N - 1.0)
traj = [[None]] * N
RstartSSD, pstartSSD = mr.TransToRp(SSL) #Frise = first rise
RendSSD, pendSSD = mr.TransToRp(Xtheend)
for i in range(N):
if method == 3:
s = mr.CubicTimeScaling(Tf, timegap * i)
else:
s = mr.QuinticTimeScaling(Tf, timegap * i)
traj[i] \
= np.r_[np.c_[np.dot(RstartSSD, \
mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartSSD).T,RendSSD)) * s)), \
s * np.array(pendSSD) + (1 - s) * np.array(pstartSSD)], \
[[0, 0, 0, 1]]]
row = []
for ao in range(len(traj[i]) - 1):
for ap in range(0, 3):
row.append(traj[i][ao][ap])
for aq in range(len(traj[i]) - 1):
for ar in range(0, 4):
if ar == 3:
row.append(traj[i][aq][ar])
row.append(0)
with open('penultimate.csv', 'a') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(row)
csvFile.close()
return traj #maybe no return statement needed
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")
