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 youBot_selfCollisionCheck(thetaList):
"""Checks if the provided robot arm configuration is in self collision with youBot
:param configuration: A 5-vector representing the current configuration of the robot arm
5 variables for the arm configuration (J1, J2, J3, J4, J5),
:return: A boolean value, status
True - No self collision.
False - Self collision
This funtion checks whether the end effector of the youBot arm is not in restricted / self colliding
areas. The restricted area is designed such that, if the end effector position is in a non restricted area,
the arm will not be in self collision with the robot or the arm links themselves.
The restricted area is modeled as a combination of a cuboid (representing robot base)
and a cylinder (representing arm base)
"""
noCollision = True
T0e = mr.FKinBody(M, Blist, thetaList)
r, [x, y, z] = mr.TransToRp(T0e)
# T0e position must not be in the cylinder region of the robot arm base.
# Cylinder region radius : 0.1562m. Height 0.3604m
min_radius = 0.1562
min_height = 0.3604
radial_distance = math.sqrt(math.pow(x, 2) + math.pow(y, 2))
if (radial_distance < min_radius) and (z < min_height):
noCollision = False
return noCollision
Tbe = np.dot(Tb0, T0e)
r, [x, y, z] = mr.TransToRp(T0e)
# Tbe position must not be in the cuboid region of the youBot base
# Cuboid region coordinates:
x_min = -0.35
x_max = +0.35
y_min = -0.336
y_max = +0.336
z_min = 0
z_max = 0.175
if ((x_min < x) and
(x < x_max)) and ((y_min < y) and
(y < y_max)) and ((z_min < z and z < z_max)):
noCollision = False
return noCollision
return noCollision
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 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 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 forward_kinematics(joints):
# input: joint angles [joint1, joint2, joint3, joint4]
# output: the position of end effector [x, y, z]
L = 1
joint1 = joints[0]
joint2 = joints[1]
joint3 = joints[2]
joint4 = joints[3]
if joint3 > 1.2 or joint3 < 0:
raise ValueError(
"Invalid value for joint3, must be between 0 and 2 meters")
#precalculated M, [S3], [S2] and [S1]
M = np.array([[1, 0, 0, 0], [0, 1, 0, 3 * L], [0, 0, 1, 3 * L],
[0, 0, 0, 1]])
S4_bracket = np.array([[0, 0, 0, 0], [0, 0, -1, 3 * L], [0, 1, 0, -2 * L],
[0, 0, 0, 0]])
S3_bracket = np.array([[0, 0, 0, 0], [0, 0, 0, 1], [0, 0, 0, 0],
[0, 0, 0, 0]])
S2_bracket = np.array([[0, 0, 0, 0], [0, 0, -1, 3 * L], [0, 1, 0, 0],
[0, 0, 0, 0]])
S1_bracket = np.array([[0, -1, 0, 0], [1, 0, 0, 0], [0, 0, 0, 0],
[0, 0, 0, 0]])
#exponential term
S4_theta = joint4 * S4_bracket
S3_theta = joint3 * S3_bracket
S2_theta = joint2 * S2_bracket
S1_theta = joint1 * S1_bracket
#matrix exponential
S4_exp = mr.MatrixExp6(S4_theta)
S3_exp = mr.MatrixExp6(S3_theta)
S2_exp = mr.MatrixExp6(S2_theta)
S1_exp = mr.MatrixExp6(S1_theta)
#left-multiplying
T04 = np.dot(S4_exp, M)
T03 = np.dot(S3_exp, T04)
T02 = np.dot(S2_exp, T03)
T01 = np.dot(S1_exp, T02)
R, p = mr.TransToRp(T01)
x = p[0]
y = p[1]
z = p[2]
print[x, y, z]
return [x, y, z]
#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)
def moveBaseXYPhi(self,
endBaseConfig,
pid,
velocity_factor=0.1,
time_scaling_method=MOVEMENT_TIME_SCALING_METHOD_QUINTIC,
dryrun=True):
'''
Given a target body config vector (endBaseConfig : X, Y, phi), move the bot from the current position
to this new position
endBaseConfig = x, y, phi
type : float
velocity_factor = 1 = 100%, 0 = 0%
type : float
dryrun = true : will not drive the robot. False will
'''
# Adjust movement speed
self.current_omega = self.max_omega * velocity_factor
self.current_V = self.max_V * velocity_factor
# Calculate coordinates in space frame (fixed frame starting at robot init)
target_x, target_y, target_phi = endBaseConfig
R = np.array([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]])
p = np.array((target_x, target_y, 0))
Tdest = mr.RpToTrans(R, p)
# Convert rotation + translation from base frame in original space frame
Ts_dest = self.Tsb@Tdest
# use coordinates in space frame to perform move.
R, p = mr.TransToRp(Ts_dest)
target_x, target_y = p[0], p[1]
# calculate spin angle (check for singularity (if theta = -180 or pi or -pi), or if
# there's only a spin angle, no movement in X or Y)
# arctan is defined between -pi/2 and pi/2... so target_x has to be larger
if target_x == self.currentX and target_y == self.currentY:
phi = 0
elif target_x >= self.currentX: # quadrants 1 and 4ar
phi = arctan(np.float64(target_y - self.currentY) / np.float64(target_x - self.currentX))
if np.isnan(phi):
phi = 0
elif target_y >= self.currentY: # arccos is defined between 0 and pi (quadrant 2)
phi = arccos(np.float64(target_x - self.currentX) / (np.sqrt(np.power(target_x - self.currentX, 2) + np.power(target_y - self.currentY, 2))))
elif target_y < self.currentY and target_x < self.currentX: # below the X axis, 3th quadrant
phi = pi + arctan(np.float64(target_y - self.currentY) / np.float64(target_x - self.currentX))
else:
log.info(LOGGER_BASE_MOVEXYPHI, 'We have a problem... check for singularity configurations')
# subtract current angle
phi -= self.currentPhi
# First Spin
self.rotateBasePhi(phi=phi,
pid=pid,
velocity_factor=velocity_factor,
method=time_scaling_method,
dryrun=dryrun)
# Translate
distance = np.linalg.norm((np.array([target_x, target_y]) - np.array([self.currentX, self.currentY])))
self.moveBaseX(distance=distance,
pid=pid,
velocity_factor=velocity_factor,
method=time_scaling_method,
dryrun=dryrun)
# Last spin for final orientation
self.rotateBasePhi(phi=target_phi,
pid=pid,
velocity_factor=velocity_factor,
method=time_scaling_method,
dryrun=dryrun)
def main():
n = rospy.init_node('teleoperation_node')
teleoperation = Teleoperation(n)
# wait for a little bit time for everything to be ready
while not teleoperation.is_master_pose_received:
print("Waiting for master pose message...")
rospy.sleep(0.5)
teleoperation.set_master_home_pose(teleoperation.master_pose_original)
r = rospy.Rate(5) # 50 Hz
cnt = 0
rospy.wait_for_service('move_to_pose')
while not rospy.is_shutdown():
try:
req = MoveToPoseRequest()
#
# q_rot1 = Quaternion(degrees=90, axis=(0.0, 0.0, 1.0))
# q_rot2 = Quaternion(degrees=-90, axis=(1.0, 0.0, 0.0))
#
# r_gripper2tool = np.matrix([[0, 0, -1.0], [0, -1.0, 0], [-1.0, 0, 0]])
# q_gripper2tool = Quaternion(matrix=r_gripper2tool)
# # q_d = q_gripper2tool*teleoperation.q_original
# print(teleoperation.q_original.rotation_matrix)
# q_d = q_rot1*q_rot2*teleoperation.q_original
# #q_d = q_rot1 * q_rot2 * teleoperation.q_original
# #print(teleoperation.q_original.rotation_matrix)
# #print(q_d[0], q_d[1], q_d[2], q_d[3])
# q_home_tool = Quaternion(w=0.707, x=0.0, y =0.707, z=0.0)
# print(teleoperation.master_pose_original)
# T_master_current2home = np.matmul(teleoperation.master_pose_original, mr.TransInv(teleoperation.master_home_pose))
master_R_original, master_p_original = mr.TransToRp(
teleoperation.master_pose_original)
master_R_home, master_p_home = mr.TransToRp(
teleoperation.master_home_pose)
master_p_current2home = master_p_original - master_p_home
master_R_current2home = np.matmul(master_R_original,
mr.RotInv(master_R_home))
# print(master_R_current2home, master_p_current2home)
# R_z_90 = np.zeros((3,3))
# R_z_90[0, 1] = 1.0
# R_z_90[1, 0] = -1.0
# R_z_90[2, 2] = 1.0
#
# master_R_current2home = np.matmul(R_z_90, master_R_current2home)
# master_p_current2home = np.matmul(R_z_90, master_p_current2home)
slave_R_home, slave_p_home = mr.TransToRp(
teleoperation.slave_home_pose)
slave_R_des = np.matmul(master_R_current2home, slave_R_home)
slave_p_des = master_p_current2home * teleoperation.position_scale + slave_p_home
T_slave_des = mr.RpToTrans(slave_R_des, slave_p_des)
#print(T_slave_des)
slave_pose_msg = pose_msg_from_matrix(T_slave_des)
print(slave_pose_msg)
req.pose = slave_pose_msg
# dis = Quaternion.distance(q_d, q_home_tool)
# print(master_R_current2home[2, 2])
# if master_R_current2home[2, 2] < 0.8:
# req.pose.orientation.w = 0.707#q_d[0]
# req.pose.orientation.x = 0.0#q_d[1]
# req.pose.orientation.y = 0.707#q_d[2]
# req.pose.orientation.z = 0.0#q_d[3]
# # # print(dis)
# # # print(q_d)
# #
# #
# # # req.pose.orientation.w = q_d[0]
# # # req.pose.orientation.x = q_d[1]
# # # req.pose.orientation.y = q_d[2]
# # # req.pose.orientation.z = q_d[3]
# #
# p_original = teleoperation.current_position - teleoperation.position_offset
#
# p_d = np.matmul(q_rot1.rotation_matrix, p_original) * teleoperation.position_scale + teleoperation.p_rcm
# p_d[2] -= teleoperation.z_tip_offset
#
# req.pose.position.x = p_d[0]
# req.pose.position.y = p_d[1]
# req.pose.position.z = p_d[2]
#
if teleoperation.gripper_close:
req.jaw_angle = 0
else:
req.jaw_angle = np.pi / 3
req.secs = 0.1
# # print(req)
#
move_to_pose = rospy.ServiceProxy('move_to_pose', MoveToPose)
res = move_to_pose(req)
# if res.success:
# print('Successfully moved to the area above pad')
# else:
# print('Failed to move to the area above pad')
except rospy.ServiceException, e:
print "Service call failed: %s" % e
cnt += 1
print(cnt)
rospy.sleep(0.01)
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
async def asyncProcessMQTTMessages(self, loopDelay):
"""
This function receives the messages from MQTT to move the robot.
It is implemented in another thread so that the killswitch can kill
the thread without knowing which specific
"""
while True:
try:
await asyncio.sleep(loopDelay)
if self.mqttMoveQueue.empty() is False:
try:
if self.currentVoltage < LOW_VOLTAGE_THRESHOLD:
raise LowVoltageException
except LowVoltageException:
log.warning(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Low voltage threshold '
f'{float(LOW_VOLTAGE_THRESHOLD):.2} '
f'reached. Current voltage = '
f'{self.currentVoltage:.2f} .'
f'Robot will not move, but MQTT '
f'message will attempt to process. '
f'Consider charging the battery, '
f'or find some kind of power supply')
# Remove the first in the list, will pause until there
# is something
currentMQTTMoveMessage = self.mqttMoveQueue.get()
# Decode message received
msgdict = json.loads(
currentMQTTMoveMessage.payload.decode('utf-8'))
# Verify if need to urgently overried current movement
# and queue
if 'override' in msgdict:
if msgdict['override']:
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Overriding all motion - calling '
f'killswitch to empty queue')
# Urgently stop the current movement (and empty
# movement queue - done in killSwitch)
self.killSwitch()
# Default motion configuration
pid = False if 'pid' not in msgdict else msgdict["pid"]
velocity_factor = 0.1 if 'velocity_factor' not \
in msgdict else msgdict["velocity_factor"]
dryrun = False if 'dryrun' not in msgdict else \
msgdict["dryrun"]
inter_wheel_distance = None if 'inter_wheel_distance' not\
in msgdict else msgdict["inter_wheel_distance"]
wheel_radius = None if 'wheel_radius' not in msgdict \
else msgdict["wheel_radius"]
time_scaling_method = MOVEMENT_TIME_SCALING_METHOD_DEFAULT
if currentMQTTMoveMessage.topic == 'bot/killSwitch':
self.killSwitch()
elif currentMQTTMoveMessage.topic == \
'bot/base/reset/position':
log.warning(
LOGGER_DDR_IMPLMOVELOOP,
msg=f'Invoking reset body configuration')
self.base.resetBodyConfiguration()
elif currentMQTTMoveMessage.topic == \
'bot/base/move/xyphi':
self.eventLoop.run_in_executor(
self.eventLoopBaseExecutors,
functools.partial(self.base.moveBaseXYPhi,
endBaseConfig=(msgdict["x"],
msgdict["y"],
msgdict["phi"]),
pid=pid,
velocity_factor=velocity_factor,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/base/move/x':
self.eventLoop.run_in_executor(
self.eventLoopBaseExecutors,
functools.partial(self.base.moveBaseX,
distance=msgdict["distance"],
pid=pid,
velocity_factor=velocity_factor,
wheel_radius=wheel_radius,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/base/turn/phi':
self.eventLoop.run_in_executor(
self.eventLoopBaseExecutors,
functools.partial(
self.base.rotateBasePhi,
phi=msgdict["phi"],
pid=pid,
velocity_factor=velocity_factor,
inter_wheel_distance=inter_wheel_distance,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/base/move/home':
# Calculate where to go from there (not avoiding
# obstacles...)
_, (x, y, _) = mr.TransToRp(
mr.TransInv(self.base.Tsb))
# pi - self.base.currentPhi
phi = - 1 * self.base.currentPhi
self.base.moveBaseXYPhi(
endBaseConfig=(x, y, phi),
pid=pid,
velocity_factor=velocity_factor,
time_scaling_method=time_scaling_method,
dryrun=dryrun)
elif currentMQTTMoveMessage.topic == \
'bot/arm/move/rest':
self.eventLoop.run_in_executor(
self.eventLoopArmExecutors,
functools.partial(
self.arm.moveToAngle,
armConfigVector=self.arm.armRestPosition,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/arm/move/zero':
self.eventLoop.run_in_executor(
self.eventLoopArmExecutors,
functools.partial(
self.arm.moveToAngle,
armConfigVector=self.arm.armZeroPosition,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/arm/move/theta':
self.eventLoop.run_in_executor(
self.eventLoopArmExecutors,
functools.partial(
self.arm.moveToAngle,
armConfigVector=[msgdict["theta1"],
msgdict["theta2"]],
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/gripper/open':
# Already an async call (non blocking)
self.arm.openEndEffector()
elif currentMQTTMoveMessage.topic == \
'bot/gripper/close':
# Already an async call (non blocking)
self.arm.closeEndEffector()
elif currentMQTTMoveMessage.topic == \
'bot/gripper/position':
# Call function to move this thing
Pco = np.array(msgdict["P"])
angle = msgdict["grip_angle"]
Toe_grip = mr.RpToTrans(
R=np.array([[cos(angle), 0, sin(angle)],
[0, 1, 0],
[-sin(angle), 0, cos(angle)]]),
p=[0, 0, 0])
Rco, _ = mr.TransToRp(Toe_grip) # Rco=camera-object
# Rco, _ = mr.TransToRp(self.Toe_grip)
Tco_desired = mr.RpToTrans(R=Rco, p=Pco)
Tbo_desired = self.Tbc @ Tco_desired
# Initial guess - based on the angle received
thetalist0 = (0, 0, pi / 8, pi / 4) # pi / 4, pi / 2)
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Running inverse kinematics to get '
f'required joint angles.')
thetalist, success = mr.IKinBody(
Blist=self.arm.bList,
M=self.M0e,
T=Tbo_desired,
thetalist0=thetalist0,
eomg=0.02, # 0.08 rad = 5 deg uncerainty
ev=0.025) # 4cm error?
if success:
thetalist = thetalist.round(6)
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'IK Solved with joint thetas '
f'found {thetalist}')
# 0. Open gripper
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Opening or ensuring end effector '
f'is opened.')
self.arm.openEndEffector()
# 1. Move arm to 0 position - launch async.
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving arm to driving'
f'position')
self.arm.moveToAngle(
armConfigVector=self.arm.armDrivePosition,
dryrun=dryrun)
# 2. Move base by X first
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving body by X '
f'{thetalist[0]:.2f}')
self.base.moveBaseX(
distance=thetalist[0],
pid=pid,
velocity_factor=velocity_factor,
dryrun=dryrun)
# 3. Rotate base by Phi
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Rotating base by phi '
f'{thetalist[1]:.2f}')
self.base.rotateBasePhi(
phi=thetalist[1],
pid=pid,
velocity_factor=velocity_factor,
dryrun=dryrun)
# 4a. Move to standoff position
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving arm joints to standoff '
f'position')
# 4. Move other joint in position - need to block
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving arm joints into position '
f'for picking up object j3 = '
f'{degrees(thetalist[2]):.2f}, '
f'j4 = {degrees(thetalist[3]):.2f}')
self.arm.moveToAngle(armConfigVector=thetalist[2:],
dryrun=dryrun)
# 5. Grab object
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Closing end effector')
self.arm.closeEndEffector()
# Rest b/c the previous call is non blocking,
# and blocking causes issues on the wait
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Async Sleep for 1 second...')
await asyncio.sleep(1)
# 6. Set to driving position
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving arm to driving position.')
self.arm.moveToAngle(
armConfigVector=self.arm.armDrivePosition,
dryrun=dryrun)
log.debug(LOGGER_DDR_IMPLMOVELOOP, f'Done!')
else:
log.warning(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Unable to calculate joint and '
f'wheel angles to achieve the '
f'required position.')
elif currentMQTTMoveMessage.topic == \
'bot/logger':
# Changing the logging level on the fly...
log.setLevel(msgdict['logger'], lvl=msgdict['level'])
elif currentMQTTMoveMessage.topic == \
'bot/logger/multiple':
# Changing the logging level on the fly for multiple loggers at a time
for logger, level in msgdict.items():
log.setLevel(logger, level)
else:
raise NotImplementedError
except NotImplementedError:
log.warning(LOGGER_DDR_IMPLMOVELOOP,
msg=f'MQTT message not implemented.')
except asyncio.futures.CancelledError:
log.warning(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Cancelled the MQTT dequeing task.')
except:
log.error(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Error : {traceback.print_exc()}')