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()}')