def p12():
print('P12')
R = np.array([[0, 0, 1],
[-1, 0, 0],
[0, -1, 0]])
result = mr.MatrixLog3(R)
print(result)
def p8():
print('P8')
Tsa = np.array([[0, -1, 0, 0],
[0, 0, -1, 0],
[1, 0, 0, 1],
[0, 0, 0, 1]])
Rsa = Tsa[:3, :3]
so3mat = mr.MatrixLog3(Rsa)
expc3 = mr.so3ToVec(so3mat)
omghat, theta = mr.AxisAng3(expc3)
print(theta)
def transformToPose(T):
"""Converts a homogeneous transformation matrix to a 3D pose (Wx, Wy, Wz, x, y, z) 6-vector
:param: Homogeneous Transformation matrix corresponding to given pose
:return pose: 2D pose [Wx, Wy, Wz, x, y, z]
Wx, Wy, Wz : Rotation angles
x, y, z : Position
"""
R, position = mr.TransToRp(T)
R_so3 = mr.MatrixLog3(R)
rotation = mr.so3ToVec(R_so3)
return np.concatenate((rotation, position))
def baseConfiguration(Tbase):
"""Computes the configuration of youBot base given a base transformation in space frame
:param configuration: Transform of robot base in space frame
:return: A 3-vector representing the current configuration of the robot base.
3 variables for the chassis configuration (theta, x, y),
"""
R, position = mr.TransToRp(Tbase)
R_so3 = mr.MatrixLog3(R)
rotation = mr.so3ToVec(R_so3)
theta = rotation[2]
x, y = position[0:2]
return theta, x, y
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 NextState(curConfig,controls,del_t,limits):
nextState = []
curJointConfig = np.array(curConfig[3:8])
q_cur= np.array(curConfig[0:3])
curWheelConfig = np.array(curConfig[8:])
jointSpeeds = np.array(controls[0:5])
u = np.array(controls[5:])
## checking limits
for i in range(len(controls)):
if controls[i] > limits: controls[i] = limits
elif controls[i] < -limits: controls[i] = -limits
## Euler step for joints and wheels
nextJointConfig = curJointConfig + jointSpeeds*del_t
nextWheelConfig = curWheelConfig + u*del_t
## YouBot Properties:
l = 0.47/2
w = 0.3/2
r = 0.0475
phi_k = q_cur[0]
## Odometry calculations for chassis Euler step
F = (r/4)*np.array([[-1/(l+w), 1/(l+w), 1/(l+w),-1/(l+w)],[1, 1, 1, 1],[-1, 1, -1, 1]])
del_theta = u*del_t
Vb = np.dot(F,del_theta)
Vb6 = np.array([0,0,Vb[0],Vb[1],Vb[2],0])
[R,p] = mr.TransToRp(mr.MatrixExp6(mr.VecTose3(Vb6)))
omg = mr.so3ToVec(mr.MatrixLog3(R))
del_x = p[0]
del_y = p[1]
del_w = omg[2]
if del_w == 0: del_qb = np.array([del_w,del_x,del_y])
else: del_qb = np.array([del_w, \
(del_x*m.sin(del_w) + del_y*(m.cos(del_w)-1))/del_w, \
(del_y*m.sin(del_w) + del_y*(1-m.cos(del_w)))/del_w])
del_q = np.dot(np.array([[1,0,0],[0,m.cos(phi_k),-m.sin(phi_k)],[0,m.sin(phi_k),m.cos(phi_k)]]), \
del_qb)
q_next = q_cur + del_q
nextConfig = [list(q_next),list(nextJointConfig),list(nextWheelConfig)]
nextState = list(itertools.chain(*nextConfig))
return nextState
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')
#Q4 H0 = np.array([[-5, 1, -1], [5, 1, 1], [5, 1, -1], [-5, 1, 1]]) u = np.array([-1.18, 0.68, 0.02, -0.52]) Hinverse = np.linalg.pinv(H0) V = np.dot(Hinverse, u) # print V #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)
r12 r23 = np.dot(r21, r13) r34 r45 = np.dot(r43, r35) r56 = np.dot(r51, r16) r6b = np.transpose(rb6) rsb = np.dot(rs6, r6b) # Getting angles of rotation """ * mr.MatrixLog3 takes R from SO3 to so3 (skew symmetric) in exponential coordinates * mr.so3ToVec converts 3x3 skew symmetric matrix to 3-vector * mr.AxisAng3 returns axis and rotation angle from 3-vector in exponential coordinates """ so3mat_s1 = mr.MatrixLog3(rs1) so3mat_12 = mr.MatrixLog3(r12) so3mat_23 = mr.MatrixLog3(r23) so3mat_34 = mr.MatrixLog3(r34) so3mat_45 = mr.MatrixLog3(r45) so3mat_56 = mr.MatrixLog3(r56) #so3mat_6b = mr.MatrixLog3(r6b) so3ToVec_s1 = mr.so3ToVec(so3mat_s1) so3ToVec_12 = mr.so3ToVec(so3mat_12) so3ToVec_23 = mr.so3ToVec(so3mat_23) so3ToVec_34 = mr.so3ToVec(so3mat_34) so3ToVec_45 = mr.so3ToVec(so3mat_45) so3ToVec_56 = mr.so3ToVec(so3mat_56) #so3ToVec_6b = mr.so3ToVec(so3mat_6b)
print(Rab)
# [[0,-1,0],[1,0,0],[0,0,1]]
print("Ex5, pb:")
Rsb = mr.RotInv(Rbs)
pb = Rsb * pb
print(pb)
# [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]]
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
