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 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 __init__(self, limb, tf_listener): self._limb = limb self._tf_listener = tf_listener # h: hand, c: camera try: self._tf_listener.waitForTransform( '/' + self._limb + '_hand', '/' + self._limb + '_hand_camera', rospy.Time(), rospy.Duration(4.0)) (self._t_hc, self._R_hc) = self._tf_listener.lookupTransform( '/' + self._limb + '_hand', '/' + self._limb + '_hand_camera', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print 'Error! Cannot find [{}_hand_camera] to [{}_hand] tf.'.format( self._limb, self._limb) sys.exit(0) self._R_hc = quaternion_matrix(self._R_hc) # frame transforms from camera to hand, only look up once self._T_hc = modern_robotics.RpToTrans(self._R_hc[:3, :3], self._t_hc) self._Ad_hc = modern_robotics.Adjoint(self._T_hc) # frame transforms from hand to body, look up in every loop self._T_bh = np.zeros((4, 4)) self._Ad_bh = np.zeros((6, 6)) self._arm = baxter_interface.limb.Limb(self._limb) self._kin = baxter_kinematics(self._limb)
def youBot_Tbase(theta, x, y): """Computes the transform of youBot base given a base configuration :param configuration: A 3-vector representing the current configuration of the robot base. 3 variables for the chassis configuration (theta, x, y), :return: Transform of robot base in space frame """ rotation = [0, 0, theta] position = [x, y, 0.0963] R_so3 = mr.VecToso3(rotation) R = mr.MatrixExp3(R_so3) Tbase = mr.RpToTrans(R, position) return Tbase
def poseToTransform(pose): """Converts a 3D pose (Wx, Wy, Wz, x, y, z) to a homogeneous transformation matrix :param pose: 2D pose [Wx, Wy, Wz, x, y, z] Wx, Wy, Wz : Rotation angles x, y, z : Position :return: Homogeneous Transformation matrix corresponding to given pose """ [Wx, Wy, Wz, x, y, z] = pose rotation = [Wx, Wy, Wz] position = [x, y, z] R_so3 = mr.VecToso3(rotation) R = mr.MatrixExp3(R_so3) T = mr.RpToTrans(R, position) return T
def youBot_Tse(configuration): """Computes the transform of youBot End effector given a robot configuration :param configuration: A 12-vector representing the current configuration of the robot. 3 variables for the chassis configuration (theta, x, y), 5 variables for the arm configuration (J1, J2, J3, J4, J5), and 4 variables for the wheel angles (W1, W2, W3, W4) :return: Transform of End effector in space frame """ theta, x, y = configuration[0:3] rotation = [0, 0, theta] position = [x, y, 0.0963] R_so3 = mr.VecToso3(rotation) R = mr.MatrixExp3(R_so3) Tsb = mr.RpToTrans(R, position) Tbe = youBot_Tbe(configuration[3:8]) Tse = np.dot(Tsb, Tbe) return Tse
def get_ik_thetas(S, M, p, init_thetas, alpha=None, beta=None, gamma=None): R = rot_mat(175.48, -1.59, 0) Xe = mr.RpToTrans(R, p) Xs = M print(T) #print(M) #print(S) eomg = 10**-2 ev = 10**-2 thetalist0 = np.zeros(6) success = 0 while not success: [thetalist, success] = mr.IKinSpace(S, M, T, thetalist0, eomg, ev) thetalist0 = np.random.randn(6) % np.pi return thetalist % (2 * np.pi), success
def update_hand_to_body_transforms(self): ''' Update frame transforms for transformation matrix and twist ''' try: self._tf_listener.waitForTransform('/base', '/' + self._limb + '_hand', rospy.Time(), rospy.Duration(4.0)) (t, R) = self._tf_listener.lookupTransform('/base', '/' + self._limb + '_hand', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print 'Warning! Cannot find [{}_hand] tf to [base].'.format( self._limb) R = quaternion_matrix(R) self._T_bh = modern_robotics.RpToTrans(R[:3, :3], t) self._Ad_bh = modern_robotics.Adjoint(self._T_bh)
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 main(): clientID = startSimulation() # Handles joint_handles = [-1, -1, -1, -1, -1, -1] for i in range(0, 6): joint_handles[i] = sim.simxGetObjectHandle(clientID, 'UR3_joint' + str(i + 1), sim.simx_opmode_blocking)[1] base_handle = sim.simxGetObjectHandle(clientID, "UR3", sim.simx_opmode_blocking)[1] end_effector_handle = sim.simxGetObjectHandle(clientID, "UR3_link7_visible", sim.simx_opmode_blocking)[1] end_effector_wrt_base = sim.simxGetObjectPosition( clientID, end_effector_handle, base_handle, sim.simx_opmode_streaming)[1] end_effector_quat_wrt_base = sim.simxGetObjectQuaternion( clientID, end_effector_handle, base_handle, sim.simx_opmode_streaming)[1] time.sleep(0.5) # Get end effector position and rotation wrt base end_effector_wrt_base = sim.simxGetObjectPosition( clientID, end_effector_handle, base_handle, sim.simx_opmode_oneshot_wait)[1] end_effector_quat_wrt_base = sim.simxGetObjectQuaternion( clientID, end_effector_handle, base_handle, sim.simx_opmode_oneshot_wait)[1] R_end_effector_wrt_base = Rotation.from_quat( end_effector_quat_wrt_base).as_matrix() P_end_effector_wrt_base = end_effector_wrt_base M = mr.RpToTrans(R_end_effector_wrt_base, P_end_effector_wrt_base) print(M) # Forward Kinematics Slist = getSlist(clientID, joint_handles, base_handle) theta = [ degToRad(180), degToRad(0), degToRad(0), degToRad(0), degToRad(0), degToRad(0) ] T_endeff_in_base = mr.FKinSpace(M, Slist.T, theta) print(T_endeff_in_base) success = False while (not success): theta_new, success = mr.IKinSpace(Slist.T, M, T_endeff_in_base, [ random.random(), random.random(), random.random(), random.random(), random.random(), random.random() ], 0.01, 0.01) time.sleep(1) move.setTargetPosition(clientID, joint_handles, theta_new) errorCode = 1 while (errorCode): errorCode, end_effector_wrt_base = sim.simxGetObjectPosition( clientID, end_effector_handle, base_handle, sim.simx_opmode_oneshot_wait) print(end_effector_wrt_base) time.sleep(1) move.setTargetPosition(clientID, joint_handles, [0, 0, 0, 0, 0, 0]) endSimulation(clientID) return 1
[0, 0, 0, 0]] shat = [1, 0, 0] p = [0, 0, 2] h = 1 pb = np.array([1, 2, 3]).T vs = np.array([3, 2, 1, -1, -2, -3]).T stheta = np.array([0, 1, 2, 3, 0, 0]).T fb = np.array([1, 0, 0, 2, 1, 0]).T v = np.array([1, 0, 0, 0, 2, 3]).T print("Ex1, Tsa:") Tsa = mr.RpToTrans(Rsa, pVa) print(Tsa) # [[0,-1,0,0],[0,0,-1,0],[1,0,0,1],[0,0,0,1]] print("Ex2, Inverse of Tsb:") Tsb = mr.RpToTrans(Rsb, pVb) Tbs = mr.TransInv(Tsb) print(Tbs) # [[1,0,0,0],[0,0,-1,0],[0,1,0,-2],[0,0,0,1]] print("Ex3, Tab:") Tas = mr.TransInv(Tsa) Tab = np.dot(Tas, Tsb) print(Tab) # [[0,-1,0,-1],[-1,0,0,0],[0,0,-1,-2],[0,0,0,1]]
Blist = np.array( [[0, 0, 0], [0, 0, 0], [1, 1, 1], [2 * homeydist, homeydist, homeydist], [0, -homexdist, homexdist], [0, 0, 0]] ) thetalist0 = np.array([np.pi/4, np.pi/8, -np.pi/4]) thetalist = [] for i in range(len(waypoints)): p = waypoints[i] Tsb = mr.RpToTrans(R, p) [th, success] = mr.IKinBody(Blist, M, Tsb, thetalist0, eomg, ev) thetalist.append(th) f = open("joint_angles.txt", "w") for i in range(len(thetalist)): angle = thetalist[i] f.write(str(angle)+"\n") f.close() # ============= Search ============== # def find_available(node): available = [] if (node[0] + 0 and node[1]+50) not in available:
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)
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 # 0, 0, 1, 0,
# 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' trajectory = trajectory_planner.TrajectoryGenerator(Tse_refInitial, Tsc_initial, Tsc_goal, Tce_gripping, Tce_standoff, timeStep, K ) print 'Trajectory generation successfull'
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)
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()}')
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")
def __init__(self, robot_config_file, initRobotConfig, timeStep, odometryTimer=0.1, defaultLogging=30): """ Creates the main bot class. """ if not os.path.exists(robot_config_file): raise ValueError(f'Cannot find configuration file ' f'"{robot_config_file}"') with open(robot_config_file, 'r') as f: self.robotConfiguration = json.load(f) # Adjust JSON just read self.__adjustConfigDict(self.robotConfiguration) # Exception Queue for inter threads exception communications self.exceptionQueue = queue.Queue() self.mqttMoveQueue = queue.Queue() self.baseMovementLock = asyncio.locks.Lock() self.timeStep = timeStep self.powerSupply = PowerSupply() # region sensor Init self.sensors = {} for sensor in self.robotConfiguration["sensors"]: sensorDict = self.robotConfiguration["sensors"][sensor] legoPort = LegoPort(sensorDict["input"]) # Applies to both I2C and SDK controled sensors if legoPort.mode != sensorDict["mode"]: legoPort.mode = sensorDict["mode"] # Only applies to sensors controled by the python SDK if "set_device" in sensorDict: legoPort.set_device = sensorDict["set_device"] time.sleep(1) address = sensorDict["input"] self.sensors[sensor] = sensorDict["sensorType"](address=address) # endregion # base Init self.base = DiffDriveBase(self.robotConfiguration["base"], initRobotConfig, odometryTimer) # arm/endeffector Init self.arm = Arm(armLinkDefinitions=self.robotConfiguration["arm"]) # Other matrix init self.M0e = mr.RpToTrans( R=np.array( self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["rotationMatrix"]).T, # noqa F501 p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["p"]) # noqa F501 self.Toe_grip = mr.RpToTrans( R=np.array( self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["rotationMatrix"]).T, # noqa F501 p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["p"]) # noqa F501 self.Tbc = mr.RpToTrans( R=np.array( self.robotConfiguration["cameraConfiguration"]["Tbc"]["rotationMatrix"]).T, # noqa F501 p=self.robotConfiguration["cameraConfiguration"]["Tbc"]["p"]) self.Tcb = mr.TransInv(self.Tbc) # Reset all motors at init self.urgentStop = False log.info(LOGGER_DDR_MAIN, 'Done main robot init.')