def callback(states): scene = PlanningSceneInterface() robot = RobotCommander() arm = MoveGroupCommander("manipulator") arm.set_planner_id("RRTConnectkConfigDefault") arm.set_num_planning_attempts(20) arm.allow_replanning(True) pose = copy.deepcopy(states.pose[-1]) temp = tf.transformations.euler_from_quaternion( (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)) quaternion = tf.transformations.quaternion_from_euler( math.pi, temp[1], temp[2]) pose.position.z += 0.22 pose.orientation.x = quaternion[0] pose.orientation.y = quaternion[1] pose.orientation.z = quaternion[2] pose.orientation.w = quaternion[3] print pose arm.set_pose_target(pose) move_plan = arm.plan() i = 0 while (not move_plan.joint_trajectory.joint_names): move_plan = arm.plan() i += 1 if (i == 5): break state = arm.execute(move_plan)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.pose_goal = Pose() rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): self.robot_arm.set_named_target("home") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to home 1 ======") rospy.sleep(0.5) # print("====== move plan go to up ======") self.robot_arm.set_named_target("up") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to up ======") rospy.sleep(0.5) self.robot_arm.set_named_target("home") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to home 1 ======") rospy.sleep(0.5) # robot_arm.set_named_target("up") # robot_arm.go(wait=True) robot_state = self.robot_arm.get_current_pose() robot_angle = self.robot_arm.get_current_joint_values() print(robot_state) def move_TF(self): self.ee_TF_list = [-2.046, 1.121, 0.575, 0.453, 0.561, -0.435, 0.538] self.pose_goal.position.x = self.ee_TF_list[0] self.pose_goal.position.y = self.ee_TF_list[1] self.pose_goal.position.z = self.ee_TF_list[2] self.pose_goal.orientation.x = self.ee_TF_list[3] self.pose_goal.orientation.y = self.ee_TF_list[4] self.pose_goal.orientation.z = self.ee_TF_list[5] self.pose_goal.orientation.w = self.ee_TF_list[6] self.robot_arm.set_pose_target(self.pose_goal) self.robot_arm.go(True)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.Pose_goal = Pose() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) # robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): # self.robot_arm.set_named_target("up") #go to goal state. # self.robot_arm.go() # print("====== move plan go to home 1 ======") # rospy.sleep(2) # print("====== move plan go to up ======") # self.robot_arm.set_named_target("zeros") #go to goal state. # self.robot_arm.go(wait=True) # print("====== move plan go to zeros ======") # rospy.sleep(1) # robot_arm.set_named_target("up") # robot_arm.go(wait=True) self.robot_arm.set_pose_target(self.Pose_goal) # go to goal state. self.robot_arm.go(True) print("====== move plan go to Pose_goal ======") #rospy.sleep(2) robot_state = self.robot_arm.get_current_pose(); robot_angle = self.robot_arm.get_current_joint_values(); print(robot_state) def callback(self, data): rospy.loginfo(rospy.get_caller_id() + "I heard %s", data) self.Pose_goal = data self.move_code() def listener(self): rospy.Subscriber("chatter", Pose, self.callback)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.01) arm.set_max_velocity_scaling_factor(0.8) arm.set_max_acceleration_scaling_factor(0.5) arm.set_planning_time(1) # 规划时间限制为2秒 arm.set_num_planning_attempts(2) # 规划两次 # 获取终端link的名称 eef_link = arm.get_end_effector_link() scene = moveit_commander.PlanningSceneInterface() print arm.get_current_pose(eef_link).pose sub = rospy.Subscriber('/detect_grasps_yolo/juggle_rects', Float64MultiArray, self.callback) self.juggle_rects = Float64MultiArray() self.box_name = '' self.scene = scene self.group = arm self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.move_distance = 0.1 self.back_distance = 0.15
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): self.robot_arm.set_named_target("home") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to home 1 ======") rospy.sleep(0.5) robot_state = self.robot_arm.get_current_pose() robot_angle = self.robot_arm.get_current_joint_values() print(robot_state) # print("====== move plan go to up ======") self.robot_arm.set_named_target("up") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to zero ======") rospy.sleep(0.5) robot_state = self.robot_arm.get_current_pose() robot_angle = self.robot_arm.get_current_joint_values() print(robot_state) self.robot_arm.set_named_target("home") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to home2 ======") rospy.sleep(0.5) # robot_arm.set_named_target("up") # robot_arm.go(wait=True) robot_state = self.robot_arm.get_current_pose() robot_angle = self.robot_arm.get_current_joint_values() print(robot_state)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) #self.clear_octomap() def ar_callback(self, msg): #marker = msg.markers[1] self.marker = msg.markers ml = len(self.marker) m_id = self.marker[0].id print "marker length: ", len(self.marker) print "marker id: ", m_id m_idd = [] ''' for ii in range(0, 2): print(ii) m_idd[ii] = marker[ii].id #m_id[ii] = marker[ii].id print(m_idd[ii]) ''' pos_x = self.marker[0].pose.pose.position.x pos_y = self.marker[0].pose.pose.position.y pos_z = self.marker[0].pose.pose.position.z dist = math.sqrt((pos_x * pos_x) + (pos_y * pos_y)) #ori_x = marker.pose.pose.orientation.x #ori_y = marker.pose.pose.orientation.y #ori_z = marker.pose.pose.orientation.z #ori_w = marker.pose.pose.orientation.w #print(m_id) print "===========" print 'id: ', m_id print 'pos: ', pos_x, pos_y, pos_z #print('ori: ', ori_x, ori_y, ori_z, ori_w) self.goal_x = pos_x - 1.0 self.goal_y = pos_y - pos_y self.goal_z = pos_z - pos_z def move_code(self): robot_state = self.robot_arm.get_current_pose() print "====== robot pose: \n", print robot_state.pose.position #marker_joint_goal = self.robot_arm.get_current_joint_values() marker_joint_goal = [ 0.07100913858593216, -1.8767615298285376, 2.0393206555899503, -1.8313959190971882, -0.6278395875738125, 1.6918219826764682 ] self.robot_arm.go(marker_joint_goal, wait=True) print "====== robot joint value: \n" print marker_joint_goal self.robot_arm.go(marker_joint_goal, wait=True) self.robot_arm.go(True) print "look at the markers" pose_goal = Pose() pose_goal.orientation.w = 0.0 pose_goal.position.x = 0.4 pose_goal.position.y = -0.4 pose_goal.position.z = 0.7 planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame #self.robot_arm.set_pose_target(pose_goal) #7self.robot_arm.go(True) def plan_cartesian_path(self, scale=1): waypoints = [] wpose = self.robot_arm.get_current_pose().pose wpose.position.z -= scale * 0.2 wpose.position.x -= scale * 0.2 waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def print_ar_pose(self): rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) rospy.loginfo("Maker messages detected. Starting followers...")
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") #self.listener = tf.TransformListener() self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.marker = [] self.position_list = [] self.orientation_list = [] self.m_idd = 0 self.m_pose_x = [] self.m_pose_y = [] self.m_pose_z = [] self.m_ori_w = [] self.m_ori_x = [] self.m_ori_y = [] self.m_ori_z = [] self.ar_pose = Pose() self.goalPoseFromAR = Pose() self.br = tf.TransformBroadcaster() #self.goalPose_from_arPose = Pose() self.trans = [] self.rot = [] self.calculed_coke_pose = Pose() #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print "====== current pose : ", self.wpose marker_joint_goal = [ 0.07100913858593216, -1.8767615298285376, 2.0393206555899503, -1.8313959190971882, -0.6278395875738125, 1.6918219826764682 ] self.robot_arm.go(marker_joint_goal, wait=True) def look_object(self): look_object = self.robot_arm.get_current_joint_values() ## wrist3 현재 상태가 0도 인지, 360도인지에 따라 90도의 움직임을 -로 할지, + 할지 고려함 print "wrist3 joint value(deg, rad): ", math.degrees( look_object[5]), look_object[5] look_object[5] = math.radians(90) ''' if look_object[5] > abs(math.radians(180)): if look_object[5] < 0: look_object[5] = look_object[5] + math.radians(90) # wrist3 elif look_object[5] > 0: look_object[5] = look_object[5] - math.radians(90) else: look_object[5] = look_object[5] - math.radians(90) # wrist3 ''' print "wrist3 joint value(deg, rad): ", math.degrees( look_object[5]), look_object[5] #look_object[3] = look_object[3] - (math.radians(00)) # wrist1 self.robot_arm.go(look_object, wait=True) #look_object[5] = look_object[5] + (math.radians(90)) # wrist3 #self.robot_arm.go(look_object, wait=True) def look_up_down(self): self.clear_octomap() print "======== clear_octomap... Please wait...." look_up_down = self.robot_arm.get_current_joint_values() #print "before: ", look_up_down look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) def plan_cartesian_path(self, x_offset, y_offset, scale=1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose print "===== robot arm pose: ", self.wpose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10 #print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.goal_y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def print_ar_pose(self): rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") print "======= pos(meter): ", self.position_list print "======= orientation: ", self.orientation_list def go_to_move(self, scale=1.0): # 로봇 팔: 마커 밑에 있는 물체 쪽으로 움직이기 #self.calculed_coke_pose = self.robot_arm.get_current_pose() planning_frame = self.robot_arm.get_planning_frame() print "========== robot arm plannig frame: \n", planning_frame self.calculed_coke_pose.position.x = ( scale * self.goal_x) + 0.10 # base_link to wrist2 x-offset self.calculed_coke_pose.position.y = (scale * self.goal_y) - 0.25 #self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 - 0.115 # world to base_link z-offset and coke can offset self.calculed_coke_pose.position.z = ( scale * self.goal_z ) + 0.72 + 0.2 # world to base_link z-offset real version offset self.calculed_coke_pose.orientation = Quaternion( *quaternion_from_euler(0, 0, 1.54)) print "========== coke_pose goal frame: ", self.calculed_coke_pose self.robot_arm.set_pose_target(self.calculed_coke_pose) tf_display_position = [ self.calculed_coke_pose.position.x, self.calculed_coke_pose.position.y, self.calculed_coke_pose.position.z ] tf_display_orientation = [ self.calculed_coke_pose.orientation.x, self.calculed_coke_pose.orientation.y, self.calculed_coke_pose.orientation.z, self.calculed_coke_pose.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() print "============ Press `Enter` to if plan is correct!! ..." raw_input() self.robot_arm.go(True) def ar_tf_listener(self, msg): try: self.marker = msg.markers ml = len(self.marker) target_id = 9 #self.m_idd = self.marker[0].id # 임시용 for ii in range(0, ml): # 0 <= ii < ml self.m_idd = self.marker[ii].id #print "checked all id: ", self.m_idd if self.m_idd != target_id: pass #target_id_flage = False elif self.m_idd == target_id: target_id_flage = True target_id = self.m_idd target_id_index = ii #print "target id: ", target_id_index, target_id, target_id_flage if target_id_flage == True: self.ar_pose.position.x = self.marker[ target_id_index].pose.pose.position.x self.ar_pose.position.y = self.marker[ target_id_index].pose.pose.position.y self.ar_pose.position.z = self.marker[ target_id_index].pose.pose.position.z self.ar_pose.orientation.x = self.marker[ target_id_index].pose.pose.orientation.x self.ar_pose.orientation.y = self.marker[ target_id_index].pose.pose.orientation.y self.ar_pose.orientation.z = self.marker[ target_id_index].pose.pose.orientation.z self.ar_pose.orientation.w = self.marker[ target_id_index].pose.pose.orientation.w self.goal_x = self.ar_pose.position.x self.goal_y = self.ar_pose.position.y self.goal_z = self.ar_pose.position.z self.position_list = [self.goal_x, self.goal_y, self.goal_z] self.orientation_list = [ self.ar_pose.orientation.x, self.ar_pose.orientation.y, self.ar_pose.orientation.z, self.ar_pose.orientation.w ] (self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion( self.orientation_list) #list form으로 넘겨주어야 함 #print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z #print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw #print "ar_pos(meter): \n", self.position_list #print "ar_orientation: \n", self.orientation_list except: return
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move',anonymous=True) self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) init_table_goal = self.robot_arm.get_current_joint_values() init_table_goal[0] = 0.2 init_table_goal[1] = -1.983025375996725 init_table_goal[2] = -2.4233086744891565 init_table_goal[3] = 0.9490636587142944 init_table_goal[4] = 1.4068996906280518 init_table_goal[5] = -3.060608450566427 self.robot_arm.go(init_table_goal, wait=True) rospy.sleep(0.5) self.clear_octomap() print("====== move plan go to init table goal ======") def move_code(self): self.robot_arm.set_named_target("home") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to home 1 ======") rospy.sleep(0.5) self.clear_octomap() print("===== octomap clear =====") # print("====== move plan go to up ======") ''' test code 04/20''' joint_goal = self.robot_arm.get_current_joint_values() joint_goal[0] = 0.2 joint_goal[1] = -pi/2 joint_goal[2] = 0 joint_goal[3] = -pi/2 joint_goal[4] = 0 joint_goal[5] = 0 self.robot_arm.go(joint_goal, wait=True) rospy.sleep(0.5) print("===== move plan test code =====") ''' ======================================= ''' self.clear_octomap() print("===== octomap clear =====") robot_state = self.robot_arm.get_current_pose(); robot_angle = self.robot_arm.get_current_joint_values(); last_goal = self.robot_arm.get_current_joint_values() last_goal = [0.2, -1.98, -2.4233, 0.9490, 1.4068, -3.0606] self.robot_arm.go(last_goal, wait=True) rospy.sleep(0.5) print(robot_state) print(robot_angle) self.clear_octomap() print("===== octomap clear =====")
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander(GROUP_NAME_ARM) right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) #right_arm.set_goal_position_tolerance(0.005) right_arm.set_goal_orientation_tolerance(0.005) right_arm.set_planning_time(5) right_arm.set_num_planning_attempts(5) eef = right_arm.get_end_effector_link() rospy.sleep(2) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) #scene.remove_attached_object(GRIPPER_FRAME, "part") # clean the scene #scene.remove_world_object("table") #scene.remove_world_object("part") right_arm.set_named_target("default") right_arm.go(wait=True) right_gripper.set_named_target("open") right_gripper.go(wait=True) rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add an object to be grasped p.pose.position.x = 0.170 p.pose.position.y = 0.04 p.pose.position.z = 0.3 #scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = -0.00756585784256 start_pose.pose.position.y = -0.225419849157 start_pose.pose.position.z = 0.117192693055 start_pose.pose.orientation.x = 0.95493721962 start_pose.pose.orientation.y = -0.0160209629685 start_pose.pose.orientation.z = -0.00497157918289 start_pose.pose.orientation.w = 0.296333402395 print("going to pick up pose") right_arm.set_pose_target(start_pose) right_gripper.set_named_target("close") right_arm.go(wait=True) right_gripper.go(wait=True) rospy.sleep(1) right_arm.set_named_target("default") right_arm.go(wait=True) next_pose = PoseStamped() next_pose.header.frame_id = FIXED_FRAME next_pose.pose.position.x = -0.100732862949 next_pose.pose.position.y = -0.210876911879 next_pose.pose.position.z = 0.244678631425 next_pose.pose.orientation.x = 0.784905433655 next_pose.pose.orientation.y = -0.177844554186 next_pose.pose.orientation.z = -0.131161093712 next_pose.pose.orientation.w = 0.578870952129 right_arm.set_pose_target(next_pose) right_gripper.set_named_target("open") right_arm.go(wait=True) right_gripper.go(wait=True) rospy.sleep(3) right_arm.set_named_target("default") right_arm.go(wait=True) rospy.spin() roscpp_shutdown()
GROUP_NAME_ARM = "manipulator" FIXED_FRAME = 'world' #GROUP_NAME_GRIPPER = "NAME OF GRIPPER" if __name__ == '__main__': roscpp_initialize(sys.argv) rospy.init_node('ur3_wrist3_tester', anonymous=True) scene = PlanningSceneInterface() robot_cmd = RobotCommander() robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) robot_arm.set_goal_orientation_tolerance(0.005) robot_arm.set_planning_time(5) robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution robot_arm.allow_replanning(True) robot_joints = robot_arm.get_current_joint_values() robot_joints[5] = math.radians(90) print "robot_joints wrist3: deg, rad", math.degrees( robot_joints[5]), robot_joints[5] robot_arm.go(robot_joints, wait=True) while not rospy.is_shutdown(): robot_joints = robot_arm.get_current_joint_values() if robot_joints[5] > abs(math.radians(170)):
class Move(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.br = tf.TransformBroadcaster() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def initial_code(self): planning_frame = self.robot_arm.get_planning_frame() print("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print("====== current pose : ", self.wpose) joint_goal = [ 2.3884003162384033, -1.6826804319964808, 0.7239289283752441, -2.2834256331073206, 4.623193740844727, -0.025881592427388966 ] print("init pose : ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait=True) def go_to_move(self): planning_frame = self.robot_arm.get_planning_frame() print("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print("====== current pose : ", self.wpose) joint_goal = [ 2.3886876106262207, -1.6729376951800745, 1.6410179138183594, -2.855138603840963, 4.623205661773682, -0.025869671498433888 ] print("move_pose : ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait=True) def go_to_tomato(self): planning_frame = self.robot_arm.get_planning_frame() print("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print("====== current pose : ", self.wpose) joint_goal = [ 2.238821506500244, -0.8660662809955042, 0.8210840225219727, -2.8785727659808558, 4.622917175292969, -0.025917832051412404 ] print("tomato_pose : ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait=True) def plan_cartesian_y(self, y_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.y = (scale * self.wpose.position.y) + y_offset waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.detected = {} self.detected_names = rospy.get_param( '/darknet_ros/yolo_model/detection_classes/names') self.object_pose_sub = rospy.Subscriber( '/cluster_decomposer/centroid_pose_array', PoseArray, self.collectJsk) self.listener = tf.TransformListener() display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.ii = 0 global goal_x global goal_y global goal_z global ori_w global ori_x global ori_y global ori_z self.distance = 0 self.tomato = [] self.position_list = [] self.orientation_list = [] self.tomato_pose = Pose() self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.calculated_tomato_coor = Pose() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(10) self.robot_arm.set_num_planning_attempts(10) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print "====== current pose : ", self.wpose marker_joint_goal = [ 2.6482303142547607, -0.3752959410296839, -2.1118043104754847, -0.4801228682147425, -1.4944542090045374, -4.647655431424276 ] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) def pose_subscriber(self): rospy.loginfo("waiting for pose topic...") rospy.get_param('/darknet_ros/yolo_model/detection_classes/names') rospy.Subscriber('/cluster_decomposer/centroid_pose_array', PoseArray, self.collectJsk) def go_to_goal_point(self, scale=1.0): planning_frame = self.robot_arm.get_planning_frame() print(">> robot arm planning frame: \n", planning_frame) self.calculated_tomato.position.x = (scale * goal_x) self.calculated_tomato.position.y = (scale * goal_y) self.calculated_tomato.position.z = (scale * goal_z) self.calculated_tomato.orientation.w = (scale * ori_w) self.calculated_tomato.orientation.x = (scale * ori_x) self.calculated_tomato.orientation.y = (scale * ori_y) self.calculated_tomato.orientation.z = (scale * ori_z) print(">> robot_pose goal frame: ", self.calculated_tomato) self.robot_arm.set_pose_target(self.calculated_tomato) tf_display_position = [ self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z ] tf_display_orientation = [ self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") tomato_waypoints = [] tomato_waypoints.append(copy.deepcopy(self.calculated_tomato)) (tomato_plan, tomato_fraction) = self.robot_arm.compute_cartesian_path( tomato_waypoints, 0.01, 0.0) self.display_trajectory(tomato_plan) print("======= Press `Enter` to if plan in correct!======") raw_input() self.robot_arm.go(True) ''' def go_to_designated_coordinate(self): desired_goal_pose = [-0.537,0.166, 0.248] Cplanning_frame = self.robot_arm.get_planning_frame() print(">> current planning frame: \n",Cplanning_frame) self.calculated_tomato_coor.position.x = desired_goal_pose[0] self.calculated_tomato_coor.position.y = desired_goal_pose[1] self.calculated_tomato_coor.position.z = desired_goal_pose[2] self.calculated_tomato_coor.orientation = Quaternion(*quaternion_from_euler(desired_goal_pose[0],desired_goal_pose[1],desired_goal_pose[2])) print(">> goal frame", self.calculated_tomato_coor) self.robot_arm.set_pose_target(self.calculated_tomato_coor) tf_display_position = [self.calculated_tomato_coor.position.x, self.calculated_tomato_coor.position.y, self.calculated_tomato_coor.position.z] tf_display_orientation = [self.calculated_tomato_coor.orientation.x, self.calculated_tomato_coor.orientation.y, self.calculated_tomato_coor.orientation.z, self.calculated_tomato_coor.orientation.w] jj = 0 while jj < 5: jj += 1 self.br.sendTransform( tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") tomato_waypoints = [] tomato_waypoints.append(copy.deepcopy(self.calculated_tomato_coor)) (plan, fraction) = self.robot_arm.compute_cartesian_path(tomato_waypoints,0.01,0.0) self.display_trajectory(plan) print("=========== Press `Enter` to if plan is correct!!...") raw_input() self.robot_arm.go(True) ''' def plan_cartesian_path(self, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x -= scale * 0.1 self.wpose.position.y += scale * 0.1 waypoints.append(copy.deepcopy(self.wpose)) ''' self.wpose.position.x -= scale*0.2 waypoints.append(copy.deepcopy(self.wpose)) ''' ''' self.wpose.position.x -= scale*0.1 waypoints.append(copy.deepcopy(self.wpose)) ''' (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def collectJsk(self, msg): global goal_x global goal_y global goal_z global ori_w global ori_x global ori_y global ori_z try: (trans, rot) = self.listener.lookupTransform('base_link', 'yolo_output00', rospy.Time(0)) goal_x = trans[0] goal_y = trans[1] goal_z = trans[2] ori_w = rot[0] ori_x = rot[1] ori_y = rot[2] ori_z = rot[3] print("==== trans[x,y,z]: ", trans) print("==== rot[x,y,z,w]: ", rot) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn('there is no tf')
class DaArmServer: """The basic, design problem/tui agnostic arm server """ gestures = {} pointing_height = 0.06 grasp_height = 0.05 drop_height = 0.07 cruising_height = 0.1 START_TOLERANCE = 0.05 # this is for moveit to check for change in joint angles before moving GOAL_TOLERANCE = 0.005 moving = False paused = False move_group_state = "IDLE" last_joint_trajectory_goal = "" last_joint_trajectory_result = "" def __init__(self, num_planning_attempts=100, safe_zone=None): rospy.init_node("daarm_server", anonymous=True) self.safe_zone = safe_zone # this is a fallback zone to drop a block on fail if nothing is passed: [{x,y},{xT,yT}] self.init_params() self.init_scene() self.init_publishers() self.init_subscribers() self.init_action_clients() self.init_service_clients() self.init_arm(num_planning_attempts) def init_arm(self, num_planning_attempts=700): rospy.set_param( "/move_group/trajectory_execution/allowed_start_tolerance", self.START_TOLERANCE) self.arm = MoveGroupCommander("arm") self.gripper = MoveGroupCommander("gripper") self.robot = RobotCommander() self.arm.set_num_planning_attempts(num_planning_attempts) self.arm.set_goal_position_tolerance(self.GOAL_TOLERANCE) self.arm.set_goal_orientation_tolerance(0.02) self.init_services() self.init_action_servers() def init_scene(self): world_objects = [ "table", "tui", "monitor", "overHead", "wall", "farWall", "frontWall", "backWall", "blockProtector", "rearCamera" ] self.robot = RobotCommander() self.scene = PlanningSceneInterface() for obj in world_objects: self.scene.remove_world_object(obj) rospy.sleep(0.5) self.tuiPose = PoseStamped() self.tuiPose.header.frame_id = self.robot.get_planning_frame() self.tuiPose.pose.position = Point(0.0056, -0.343, -0.51) self.tuiDimension = (0.9906, 0.8382, 0.8836) self.overHeadPose = PoseStamped() self.overHeadPose.header.frame_id = self.robot.get_planning_frame() self.overHeadPose.pose.position = Point(0.0056, 0.0, 0.97) self.overHeadDimension = (0.9906, 0.8382, 0.05) self.blockProtectorPose = PoseStamped() self.blockProtectorPose.header.frame_id = self.robot.get_planning_frame( ) self.blockProtectorPose.pose.position = Point( 0.0056, -0.343, -0.51 + self.cruising_height) self.blockProtectorDimension = (0.9906, 0.8382, 0.8636) self.wallPose = PoseStamped() self.wallPose.header.frame_id = self.robot.get_planning_frame() self.wallPose.pose.position = Point(-0.858, -0.343, -0.3048) self.wallDimension = (0.6096, 2, 1.35) self.farWallPose = PoseStamped() self.farWallPose.header.frame_id = self.robot.get_planning_frame() self.farWallPose.pose.position = Point(0.9, -0.343, -0.3048) self.farWallDimension = (0.6096, 2, 3.35) self.frontWallPose = PoseStamped() self.frontWallPose.header.frame_id = self.robot.get_planning_frame() self.frontWallPose.pose.position = Point(0.0056, -0.85, -0.51) self.frontWallDimension = (1, 0.15, 4) self.backWallPose = PoseStamped() self.backWallPose.header.frame_id = self.robot.get_planning_frame() self.backWallPose.pose.position = Point(0.0056, 0.55, -0.51) self.backWallDimension = (1, 0.005, 4) self.rearCameraPose = PoseStamped() self.rearCameraPose.header.frame_id = self.robot.get_planning_frame() self.rearCameraPose.pose.position = Point(0.65, 0.45, -0.51) self.rearCameraDimension = (0.5, 0.5, 2) rospy.sleep(0.5) self.scene.add_box("tui", self.tuiPose, self.tuiDimension) self.scene.add_box("wall", self.wallPose, self.wallDimension) self.scene.add_box("farWall", self.farWallPose, self.farWallDimension) self.scene.add_box("overHead", self.overHeadPose, self.overHeadDimension) self.scene.add_box("backWall", self.backWallPose, self.backWallDimension) self.scene.add_box("frontWall", self.frontWallPose, self.frontWallDimension) self.scene.add_box("rearCamera", self.rearCameraPose, self.rearCameraDimension) def raise_table(self): #raises the table obstacle to protect blocks on the table during transport self.scene.remove_world_object("blockProtector") self.scene.add_box("blockProtector", self.blockProtectorPose, self.blockProtectorDimension) def lower_table(self): #lowers the table to allow grasping into it self.scene.remove_world_object("blockProtector") def init_params(self): try: self.grasp_height = get_ros_param( "GRASP_HEIGHT", "Grasp height defaulting to 0.01") self.drop_height = get_ros_param("DROP_HEIGHT", "Drop height defaulting to 0.07") self.cruising_height = get_ros_param( "CRUISING_HEIGHT", "Cruising height defaulting to 0.1") self.pointing_height = get_ros_param( "POINT_HEIGHT", "Pointing height defaulting to 0.06") except ValueError as e: rospy.loginfo(e) def handle_param_update(self, message): self.init_params() def init_publishers(self): self.calibration_publisher = rospy.Publisher("/calibration_results", CalibrationParams, queue_size=1) self.action_belief_publisher = rospy.Publisher("/arm_action_beliefs", String, queue_size=1) rospy.sleep(0.5) def init_subscribers(self): self.joint_angle_subscriber = rospy.Subscriber( '/j2s7s300_driver/out/joint_angles', JointAngles, self.update_joints) self.joint_trajectory_subscriber = rospy.Subscriber( '/j2s7s300/follow_joint_trajectory/status', GoalStatusArray, self.update_joint_trajectory_state) self.joint_trajectory_goal_subscriber = rospy.Subscriber( '/j2s7s300/follow_joint_trajectory/goal', FollowJointTrajectoryActionGoal, self.update_joint_trajectory_goal) self.joint_trajectory_result_subscriber = rospy.Subscriber( '/j2s7s300/follow_joint_trajectory/result', FollowJointTrajectoryActionResult, self.update_joint_trajectory_result) self.finger_position_subscriber = rospy.Subscriber( '/j2s7s300_driver/out/finger_position', FingerPosition, self.update_finger_position) self.param_update_subscriber = rospy.Subscriber( "/param_update", String, self.handle_param_update) self.moveit_status_subscriber = rospy.Subscriber( '/move_group/status', GoalStatusArray, self.update_move_group_status) self.move_it_feedback_subscriber = rospy.Subscriber( '/move_group/feedback', MoveGroupActionFeedback, self.update_move_group_state) #Topic for getting joint torques rospy.Subscriber('/j2s7s300_driver/out/joint_torques', JointAngles, self.monitorJointTorques) #Topic for getting cartesian force on end effector rospy.Subscriber('/j2s7s300_driver/out/tool_wrench', geometry_msgs.msg.WrenchStamped, self.monitorToolWrench) def init_action_servers(self): self.calibration_server = actionlib.SimpleActionServer( "calibrate_arm", CalibrateAction, self.calibrate, auto_start=False) self.calibration_server.start() self.move_block_server = actionlib.SimpleActionServer( "move_block", MoveBlockAction, self.handle_move_block, auto_start=False) self.move_block_server.start() #self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm) self.move_pose_server = actionlib.SimpleActionServer( "move_pose", MovePoseAction, self.handle_move_pose, auto_start=False) self.move_pose_server.start() def init_services(self): self.home_arm_service = rospy.Service("/home_arm", ArmCommand, self.handle_home_arm) # emergency stop self.stop_arm_service = rospy.Service("/stop_arm", ArmCommand, self.handle_stop_arm) # stop and pause for a bit self.pause_arm_service = rospy.Service("/pause_arm", ArmCommand, self.handle_pause_arm) self.start_arm_service = rospy.Service("/restart_arm", ArmCommand, self.handle_restart_arm) def init_action_clients(self): # Action Client for joint control joint_action_address = '/j2s7s300_driver/joints_action/joint_angles' self.joint_action_client = actionlib.SimpleActionClient( joint_action_address, kinova_msgs.msg.ArmJointAnglesAction) rospy.loginfo('Waiting for ArmJointAnglesAction server...') self.joint_action_client.wait_for_server() rospy.loginfo('ArmJointAnglesAction Server Connected') # Service to move the gripper fingers finger_action_address = '/j2s7s300_driver/fingers_action/finger_positions' self.finger_action_client = actionlib.SimpleActionClient( finger_action_address, kinova_msgs.msg.SetFingersPositionAction) self.finger_action_client.wait_for_server() # def init_service_clients(self): self.is_simulation = False try: self.is_simulation = get_ros_param("IS_SIMULATION", "") except: self.is_simulation = False if self.is_simulation is True: # setup alternatives to jaco services for emergency stop, joint control, and finger control pass # Service to get TUI State rospy.wait_for_service('get_tui_blocks') self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState) # Service for homing the arm home_arm_service = '/j2s7s300_driver/in/home_arm' self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm) rospy.loginfo('Waiting for kinova home arm service') rospy.wait_for_service(home_arm_service) rospy.loginfo('Kinova home arm service server connected') # Service for emergency stop emergency_service = '/j2s7s300_driver/in/stop' self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop) rospy.loginfo('Waiting for Stop service') rospy.wait_for_service(emergency_service) rospy.loginfo('Stop service server connected') # Service for restarting the arm start_service = '/j2s7s300_driver/in/start' self.restart_arm = rospy.ServiceProxy(start_service, Start) rospy.loginfo('Waiting for Start service') rospy.wait_for_service(start_service) rospy.loginfo('Start service server connected') def handle_start_arm(self, message): return self.restart_arm() def handle_stop_arm(self, message): return self.stop_motion() def handle_pause_arm(self, message): self.stop_motion(home=True, pause=True) return str(self.paused) def handle_restart_arm(self, message): self.restart_arm() self.paused = False return str(self.paused) def handle_home_arm(self, message): try: status = self.home_arm() return json.dumps(status) except rospy.ServiceException as e: rospy.loginfo("Homing arm failed") def home_arm(self): # send the arm home # for now, let's just use the kinova home #self.home_arm_client() self.home_arm_kinova() return "done" def custom_home_arm(self): angles_set = [ 629.776062012, 150.076568694, -0.13603515923, 29.8505859375, 0.172727271914, 212.423721313, 539.743164062 ] goal = kinova_msgs.msg.ArmJointAnglesGoal() goal.angles.joint1 = angles_set[0] goal.angles.joint2 = angles_set[1] goal.angles.joint3 = angles_set[2] goal.angles.joint4 = angles_set[3] goal.angles.joint5 = angles_set[4] goal.angles.joint6 = angles_set[5] goal.angles.joint7 = angles_set[6] self.joint_action_client.send_goal(goal) def home_arm_kinova(self): """Takes the arm to the kinova default home if possible """ # self.arm.set_named_target("Home") angles_set = map(np.deg2rad, [ 629.776062012, 150.076568694, -0.13603515923, 29.8505859375, 0.172727271914, 212.423721313, 269.743164062 ]) self.arm.clear_pose_targets() try: self.arm.set_joint_value_target(angles_set) except MoveItCommanderException as e: pass #stupid bug in movegroupcommander wrapper throws an exception when trying to set joint angles try: self.arm.go() return "successful home" except: return "failed to home" # This callback function monitors the Joint Torques and stops the current execution if the Joint Torques exceed certain value def monitorJointTorques(self, torques): if abs(torques.joint1) > 1: return #self.emergency_stop() #Stop arm driver #rospy.sleep(1.0) #self.group.stop() #Stop moveit execution # This callback function monitors the Joint Wrench and stops the current # execution if the Joint Wrench exceeds certain value def monitorToolWrench(self, wrenchStamped): return #toolwrench = abs(wrenchStamped.wrench.force.x**2 + wrenchStamped.wrench.force.y**2 + wrenchStamped.wrench.force.z**2) ##print toolwrench #if toolwrench > 100: # self.emergency_stop() # Stop arm driver def move_fingers(self, finger1_pct, finger2_pct, finger3_pct): finger_max_turn = 6800 goal = kinova_msgs.msg.SetFingersPositionGoal() goal.fingers.finger1 = float((finger1_pct / 100.0) * finger_max_turn) goal.fingers.finger2 = float((finger2_pct / 100.0) * finger_max_turn) goal.fingers.finger3 = float((finger3_pct / 100.0) * finger_max_turn) self.finger_action_client.send_goal(goal) if self.finger_action_client.wait_for_result(rospy.Duration(5.0)): return self.finger_action_client.get_result() else: self.finger_action_client.cancel_all_goals() rospy.loginfo('the gripper action timed-out') return None def move_joint_angles(self, angle_set): goal = kinova_msgs.msg.ArmJointAnglesGoal() goal.angles.joint1 = angle_set[0] goal.angles.joint2 = angle_set[1] goal.angles.joint3 = angle_set[2] goal.angles.joint4 = angle_set[3] goal.angles.joint5 = angle_set[4] goal.angles.joint6 = angle_set[5] goal.angles.joint7 = angle_set[6] self.joint_action_client.send_goal(goal) if self.joint_action_client.wait_for_result(rospy.Duration(20.0)): return self.joint_action_client.get_result() else: print(' the joint angle action timed-out') self.joint_action_client.cancel_all_goals() return None def handle_move_block(self, message): """msg format: {id: int, source: Point {x: float,y: float}, target: Point {x: float, y: float} """ print(message) pick_x = message.source.x pick_y = message.source.y pick_x_threshold = message.source_x_tolerance pick_y_threshold = message.source_y_tolerance block_id = message.id place_x = message.target.x place_y = message.target.y place_x_threshold = message.target_x_tolerance place_y_threshold = message.target_y_tolerance self.move_block(block_id, pick_x, pick_y, pick_x_threshold, pick_y_threshold, place_x, place_y, place_x_threshold, place_y_threshold, message.block_size) def handle_pick_failure(self, exception): rospy.loginfo("Pick failed, going home.") self.open_gripper() self.home_arm() raise exception def handle_place_failure(self, safe_zone, block_size, exception): #should probably figure out if I'm holding the block first so it doesn't look weird #figure out how to drop the block somewhere safe #pass none and none if you are certain you haven't picked up a block yet if safe_zone is None and block_size is None: self.home_arm() raise exception rospy.loginfo("HANDLING PLACE FAILURE") block_response = json.loads(self.get_block_state('tuio').tui_state) current_block_state = block_response drop_location = self.calculate_drop_location( safe_zone[0]['x'], safe_zone[0]['y'], safe_zone[1]['x_tolerance'], safe_zone[1]['y_tolerance'], current_block_state, block_size, num_attempts=1000) try: arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y, get_tuio_bounds(), get_arm_bounds()) rospy.loginfo("panic arm drop: " + str(arm_drop_location)) self.place_block( Point(arm_drop_location[0], arm_drop_location[1], 0)) except Exception as e: rospy.loginfo( "ERROR: Cannot panic place the block! Get ready to catch it!") self.open_gripper() self.home_arm() raise exception def get_candidate_blocks(self, block_id, pick_x, pick_y, pick_x_tolerance, pick_y_tolerance): block_response = json.loads(self.get_block_state('tuio').tui_state) current_block_state = block_response candidate_blocks = [] print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance, pick_y_tolerance) # get candidate blocks -- blocks with the same id and within the error bounds/threshold given print(current_block_state) for block in current_block_state: print( block, self.check_block_bounds(block['x'], block['y'], pick_x, pick_y, pick_x_tolerance, pick_y_tolerance)) if block['id'] == block_id and self.check_block_bounds( block['x'], block['y'], pick_x, pick_y, pick_x_tolerance, pick_y_tolerance): candidate_blocks.append(block) return candidate_blocks def move_block(self, block_id, pick_x, pick_y, pick_x_tolerance, pick_y_tolerance, place_x, place_y, place_x_tolerance, place_y_tolerance, block_size=None, safe_zone=None, pick_tries=2, place_tries=1, point_at_block=True): if block_size is None: block_size = get_ros_param('DEFAULT_BLOCK_SIZE') block_response = json.loads(self.get_block_state('tuio').tui_state) current_block_state = block_response candidate_blocks = [] print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance, pick_y_tolerance) # get candidate blocks -- blocks with the same id and within the error bounds/threshold given print(current_block_state) # check for a drop location before trying to pick, do this before checking source to prevent cases where we go for a block user # moved while we are checking for a drop location drop_location = self.calculate_drop_location(place_x, place_y, place_x_tolerance, place_y_tolerance, current_block_state, block_size, num_attempts=2000) if drop_location == None: self.handle_place_failure( None, None, Exception("no room in the target zone to drop the block")) # here we are actually building a set of candidate blocks to pick for block in current_block_state: print( block, self.check_block_bounds(block['x'], block['y'], pick_x, pick_y, pick_x_tolerance, pick_y_tolerance)) if block['id'] == block_id and self.check_block_bounds( block['x'], block['y'], pick_x, pick_y, pick_x_tolerance, pick_y_tolerance): candidate_blocks.append(block) # select best block to pick and pick up pick_location = None if len(candidate_blocks) < 1: raise Exception("no block of id " + str(block_id) + " found within the source zone") elif len(candidate_blocks) == 1: pick_location = Point(candidate_blocks[0]['x'], candidate_blocks[0]['y'], 0) else: pick_location = Point(*self.find_most_isolated_block( candidate_blocks, current_block_state)) if point_at_block == True: try: arm_pick_location = tuio_to_arm(pick_location.x, pick_location.y, get_tuio_bounds(), get_arm_bounds()) arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y, get_tuio_bounds(), get_arm_bounds()) self.close_gripper() self.point_at_block(location=Point(arm_pick_location[0], arm_pick_location[1], 0)) self.point_at_block(location=Point(arm_drop_location[0], arm_drop_location[1], 0)) self.home_arm() except Exception as e: rospy.loginfo(str(e)) rospy.loginfo("failed trying to point at block. giving up.") self.home_arm() self.move_block_server.set_succeeded( MoveBlockResult(drop_location)) return for i in range(pick_tries): try: arm_pick_location = tuio_to_arm(pick_location.x, pick_location.y, get_tuio_bounds(), get_arm_bounds()) self.pick_block(location=Point(arm_pick_location[0], arm_pick_location[1], 0), check_grasp=True) break except Exception as e: if i < pick_tries - 1: rospy.loginfo("pick failed and trying again..." + str(e)) else: rospy.loginfo("pick failed and out of attempts..." + str(e)) self.handle_pick_failure(e) if safe_zone == None: if self.safe_zone == None: safe_zone = [{ 'x': pick_x, 'y': pick_y }, { 'x_tolerance': pick_x_tolerance, 'y_tolerance': pick_y_tolerance }] else: safe_zone = self.safe_zone # calculate drop location block_response = json.loads(self.get_block_state('tuio').tui_state) current_block_state = block_response drop_location = self.calculate_drop_location(place_x, place_y, place_x_tolerance, place_y_tolerance, current_block_state, block_size, num_attempts=2000) if drop_location == None: self.handle_place_failure( safe_zone, block_size, Exception("no room in the target zone to drop the block")) rospy.loginfo("tuio drop" + str(drop_location)) for i in range(place_tries): try: arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y, get_tuio_bounds(), get_arm_bounds()) rospy.loginfo("arm drop: " + str(arm_drop_location)) self.place_block( Point(arm_drop_location[0], arm_drop_location[1], 0)) break except Exception as e: if i < place_tries - 1: rospy.loginfo("place failed and trying again..." + str(e)) else: rospy.loginfo("place failed and out of attempts..." + str(e)) # check to see if we've defined a safe zone to drop the blocks self.handle_place_failure(safe_zone, block_size, e) # assume success if we made it this far self.move_block_server.set_succeeded(MoveBlockResult(drop_location)) # check if a certain x, y position is within the bounds of another x,y position @staticmethod def check_block_bounds(x, y, x_origin, y_origin, x_threshold, y_threshold): if x <= x_origin + x_threshold and x >= x_origin - x_threshold \ and y <= y_origin + y_threshold and y >= y_origin - y_threshold: return True return False # calculate the best location to drop the block def calculate_drop_location(self, x, y, x_threshold, y_threshold, blocks, block_size, num_attempts=1000): attempt = 0 x_original, y_original = x, y while (attempt < num_attempts): valid = True for block in blocks: if self.check_block_bounds(block['x'], block['y'], x, y, 0.8 * block_size, block_size): valid = False break if valid: return Point(x, y, 0) else: x = random.uniform(x_original - x_threshold, x_original + x_threshold) y = random.uniform(y_original - y_threshold, y_original + y_threshold) attempt += 1 #if none found in num_attempts, return none return None # candidates should have more than one element in it @staticmethod def find_most_isolated_block(candidates, all_blocks): print(candidates) min_distances = [] # tuples of candidate, distance to closest block for candidate in candidates: min_dist = -1 for block in all_blocks: if block['x'] == candidate['x'] and block['y'] == candidate[ 'y']: continue else: dist = DaArmServer.block_dist(candidate, block) if min_dist == -1 or dist < min_dist: min_dist = dist min_distances.append([candidate, min_dist]) most_isolated, _ = max(min_distances, key=lambda x: x[ 1]) # get most isolated candidate, and min_distance return most_isolated['x'], most_isolated['y'], 0 @staticmethod def block_dist(block_1, block_2): return np.sqrt((block_2['x'] - block_1['x'])**2 + (block_2['y'] - block_1['y'])**2) def update_finger_position(self, message): self.finger_positions = [ message.finger1, message.finger2, message.finger3 ] def check_grasp(self): closed_pos = 0.95 * 6800 distance_from_closed = 0 for fp in self.finger_positions: distance_from_closed += (closed_pos - fp)**2 if np.sqrt(distance_from_closed ) > 130: #this is just some magic number for now return True #if the fingers aren't fully closed, then grasp is good else: return False def open_gripper(self, delay=0): """open the gripper ALL THE WAY, then delay """ if self.is_simulation is True: self.gripper.set_named_target("Open") self.gripper.go() else: try: rospy.loginfo("Opening Gripper!!") self.move_fingers(50, 50, 50) except Exception as e: rospy.loginfo("Caught it!!" + str(e)) rospy.sleep(delay) def close_gripper(self, delay=0): """close the gripper ALL THE WAY, then delay """ # self.gripper.set_named_target("Close") # self.gripper.go() try: rospy.loginfo("Closing Gripper!!") self.move_fingers(95, 95, 95) except Exception as e: rospy.loginfo("Caught it!!" + str(e)) rospy.sleep(delay) def handle_gesture(self, gesture): # lookup the gesture from a table? or how about a message? # one possibility, object that contains desired deltas in pos/orientation # as well as specify the arm or gripper choice pass def handle_move_pose(self, message): # takes a geometry_msgs/Pose message self.move_arm_to_pose(message.target.position, message.target.orientation, action_server=self.move_pose_server) self.move_pose_server.set_succeeded() def check_plan_result(self, target_pose, cur_pose): #we'll do a very lenient check, this is to find failures, not error #also only checking position, not orientation rospy.loginfo("checking pose:" + str(target_pose) + str(cur_pose)) if np.abs(target_pose.pose.position.x - cur_pose.pose.position.x) > self.GOAL_TOLERANCE * 2: print("x error too far") return False if np.abs(target_pose.pose.position.y - cur_pose.pose.position.y) > self.GOAL_TOLERANCE * 2: print("y error too far") return False if np.abs(target_pose.pose.position.z - cur_pose.pose.position.z) > self.GOAL_TOLERANCE * 2: print("z error too far") return False print("tolerable error") return True # expects cooridinates for position to be in arm space def move_arm_to_pose(self, position, orientation, delay=0.5, waypoints=[], corrections=4, action_server=None): for i in range(corrections + 1): rospy.loginfo("TRY NUMBER " + str(i)) if len(waypoints) > 0 and i < 1: # this is good for doing gestures plan, fraction = self.arm.compute_cartesian_path( waypoints, eef_step=0.01, jump_threshold=0.0) else: p = self.arm.get_current_pose() p.pose.position = position p.pose.orientation = orientation rospy.loginfo("PLANNING TO " + str(p)) self.arm.set_pose_target(p) last_traj_goal = self.last_joint_trajectory_goal rospy.loginfo("EXECUTING!") plan = self.arm.go(wait=False) timeout = 5 / 0.001 while self.last_joint_trajectory_goal == last_traj_goal: rospy.sleep(0.001) timeout -= 1 if timeout <= 0: raise (Exception( "Timeout waiting for kinova to accept movement goal." )) rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL") current_goal = self.last_joint_trajectory_goal # then loop until a result for it gets published timeout = 90 / 0.001 while self.last_joint_trajectory_result != current_goal: rospy.sleep(0.001) timeout -= 1 if timeout <= 0: raise (Exception( "Motion took longer than 90 seconds. aborting...")) if self.paused is True: self.arm.stop() # cancel the moveit goals return rospy.loginfo("LEAVING THE WHILE LOOP") # for i in range(corrections): # rospy.loginfo("Correcting the pose") # self.move_joint_angles(angle_set) rospy.sleep(delay) if (self.check_plan_result(p, self.arm.get_current_pose())): break #we're there, no need to retry #rospy.loginfo("OK GOT THROUGH THE PLANNING PHASE") if False: # # get the last pose to correct if desired # ptPos = plan.joint_trajectory.points[-1].positions # # print "==================================" # # print "Last point of the current trajectory: " # angle_set = list() # for i in range(len(ptPos)): # tempPos = ptPos[i]*180/np.pi + int(round((self.joint_angles[i] - ptPos[i]*180/np.pi)/(360)))*360 # angle_set.append(tempPos) if action_server: pass #action_server.publish_feedback(CalibrateFeedback("Plan Found")) last_traj_goal = self.last_joint_trajectory_goal rospy.loginfo("EXECUTING!") self.arm.execute(plan, wait=False) # this is a bit naive, but I'm going to loop until a new trajectory goal gets published timeout = 5 / 0.001 while self.last_joint_trajectory_goal == last_traj_goal: rospy.sleep(0.001) timeout -= 1 if timeout <= 0: raise (Exception( "Timeout waiting for kinova to accept movement goal." )) rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL") current_goal = self.last_joint_trajectory_goal # then loop until a result for it gets published timeout = 15 / 0.001 while self.last_joint_trajectory_result != current_goal: rospy.sleep(0.001) timeout -= 1 if timeout <= 0: raise (Exception( "Motion took longer than 15 seconds. aborting...")) if self.paused is True: self.arm.stop() # cancel the moveit goals return rospy.loginfo("LEAVING THE WHILE LOOP") # for i in range(corrections): # rospy.loginfo("Correcting the pose") # self.move_joint_angles(angle_set) rospy.sleep(delay) else: if action_server: #action_server.publish_feedback(CalibrateFeedback("Planning Failed")) pass # Let's have the caller handle sequences instead. # def handle_move_sequence(self, message): # # if the move fails, do we cancel the sequence # cancellable = message.cancellable # moves = message.moves # for move in moves: # try: # self.handle_move_block(move) # except Exception: # if cancellable: # rospy.loginfo("Part of move failed, cancelling the rest.") # break def update_move_group_state(self, message): rospy.loginfo(message.feedback.state) self.move_group_state = message.feedback.state def update_move_group_status(self, message): if message.status_list: #rospy.loginfo("MoveGroup Status for "+str(message.status_list[0].goal_id.id)+": "+str(message.status_list[0].status)) self.move_group_status = message.status_list[0].status def update_joint_trajectory_state(self, message): # print(message.status_list) if len(message.status_list) > 0: self.joint_trajectory_state = message.status_list[0].status else: self.joint_trajectory_state = 0 def update_joint_trajectory_goal(self, message): #print("goalisasis", message.goal_id.id) self.last_joint_trajectory_goal = message.goal_id.id def update_joint_trajectory_result(self, message): #print("resultisasis", message.status.goal_id.id) self.last_joint_trajectory_result = message.status.goal_id.id def get_grasp_orientation(self, position): #return Quaternion(0, 0, 1/np.sqrt(2), 1/np.sqrt(2)) return Quaternion(-0.707388, -0.706825, 0.0005629, 0.0005633) def update_joints(self, joints): self.joint_angles = [ joints.joint1, joints.joint2, joints.joint3, joints.joint4, joints.joint5, joints.joint6, joints.joint7 ] def move_z_relative(self, distance): p = self.arm.get_current_pose() p.pose.position.z += distance self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0) def move_z_absolute(self, height): p = self.arm.get_current_pose() p.pose.position.z = height self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0) def pick_block(self, location, check_grasp=False, retry_attempts=0, delay=0, action_server=None): # go to a spot and pick up a block # if check_grasp is true, it will check torque on gripper and retry or fail if not holding # open the gripper # print('Position: ', position) self.open_gripper() position = Point(location.x, location.y, self.cruising_height) orientation = self.get_grasp_orientation(position) try: self.raise_table() self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) self.lower_table() position = Point(location.x, location.y, self.grasp_height) self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) except Exception as e: current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < 0: self.move_z_absolute(self.crusing_height) self.raise_table() raise ( e ) #problem because sometimes we get exception e.g. if we're already there # and it will skip the close if so. if action_server: action_server.publish_feedback() self.close_gripper() self.move_z_absolute(self.cruising_height) #try to wait until we're up to check grasp rospy.sleep(0.5) if check_grasp is True: if (self.check_grasp() is False): print("Grasp failed!") self.raise_table() raise (Exception("grasp failed!")) # for now, but we want to check finger torques # for i in range(retry_attempts): # self.move_z(0.3) self.raise_table() rospy.sleep(delay) def place_block(self, location, check_grasp=False, delay=0, action_server=None): # go to a spot an drop a block # if check_grasp is true, it will check torque on gripper and fail if not holding a block # print('Position: ', position) current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct self.move_z_absolute(self.cruising_height) position = Point(location.x, location.y, self.cruising_height) rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " + str(self.drop_height)) orientation = self.get_grasp_orientation(position) try: self.raise_table( ) # only do this as a check in case it isn't already raised self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) self.lower_table() self.move_z_absolute(self.drop_height) except Exception as e: current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < 0: self.move_z_absolute(self.cruising_height) self.raise_table() raise (e) if action_server: action_server.publish_feedback() self.open_gripper() self.move_z_absolute(self.cruising_height) self.raise_table() self.close_gripper() def point_at_block(self, location, delay=0, action_server=None): # go to a spot an drop a block # if check_grasp is true, it will check torque on gripper and fail if not holding a block # print('Position: ', position) current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct self.move_z_absolute(self.cruising_height) position = Point(location.x, location.y, self.cruising_height) rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " + str(self.drop_height)) orientation = self.get_grasp_orientation(position) try: self.raise_table( ) # only do this as a check in case it isn't already raised self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) self.lower_table() self.move_z_absolute(self.pointing_height) self.move_z_absolute(self.cruising_height) except Exception as e: current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < 0: self.move_z_absolute(self.cruising_height) self.raise_table() raise (e) if action_server: action_server.publish_feedback() self.raise_table() def stop_motion(self, home=False, pause=False): rospy.loginfo("STOPPING the ARM") # cancel the moveit_trajectory # self.arm.stop() # call the emergency stop on kinova self.emergency_stop() # rospy.sleep(0.5) if pause is True: self.paused = True if home is True: # self.restart_arm() self.home_arm() def calibrate(self, message): print("calibrating ", message) self.place_calibration_block() rospy.sleep(5) # five seconds to correct the drop if it bounced, etc. print("moving on...") self.calibration_server.publish_feedback( CalibrateFeedback("getting the coordinates")) params = self.record_calibration_params() self.set_arm_calibration_params(params[0], params[1]) self.calibration_publisher.publish( CalibrationParams(params[0], params[1])) self.calibration_server.set_succeeded(CalibrateResult(params)) def set_arm_calibration_params(self, arm_x_min, arm_y_min): rospy.set_param("ARM_X_MIN", arm_x_min) rospy.set_param("ARM_Y_MIN", arm_y_min) def place_calibration_block(self): # start by homing the arm (kinova home) self.calibration_server.publish_feedback(CalibrateFeedback("homing")) self.home_arm_kinova() # go to grab a block from a human self.open_gripper() self.close_gripper() self.open_gripper(4) self.close_gripper(2) # move to a predetermined spot self.calibration_server.publish_feedback( CalibrateFeedback("moving to drop")) try: self.move_arm_to_pose(Point(0.4, -0.4, 0.1), Quaternion(1, 0, 0, 0), corrections=0) except Exception as e: rospy.loginfo("THIS IS TH PRoblem" + str(e)) # drop the block self.open_gripper() self.calibration_server.publish_feedback( CalibrateFeedback("dropped the block")) calibration_block = {'x': 0.4, 'y': -0.4, 'id': 0} calibration_action_belief = { "action": "add", "block": calibration_block } self.action_belief_publisher.publish( String(json.dumps(calibration_action_belief))) rospy.loginfo("published arm belief") return def record_calibration_params(self): """ Call the block_tracker service to get the current table config. Find the calibration block and set the appropriate params. """ # make sure we know the dimensions of the table before we start # fixed table dimensions include tuio_min_x,tuio_min_y,tuio_dist_x,tuio_dist_y,arm_dist_x,arm_dist_y tuio_bounds = get_tuio_bounds() arm_bounds = get_arm_bounds(calibrate=False) try: block_state = json.loads(self.get_block_state("tuio").tui_state) except rospy.ServiceException as e: # self.calibration_server.set_aborted() raise (ValueError("Failed getting block state to calibrate: " + str(e))) if len(block_state) != 1: # self.calibration_server.set_aborted() raise (ValueError( "Failed calibration, either couldn't find block or > 1 block on TUI!" )) # if we made it here, let's continue! # start by comparing the reported tuio coords where we dropped the block # with the arm's localization of the end-effector that dropped it # (we assume that the block fell straight below the drop point) drop_pose = self.arm.get_current_pose() end_effector_x = drop_pose.pose.position.x end_effector_y = drop_pose.pose.position.y block_tuio_x = block_state[0]['x'] block_tuio_y = block_state[0]['y'] x_ratio, y_ratio = tuio_to_ratio(block_tuio_x, block_tuio_y, tuio_bounds) arm_x_min = end_effector_x - x_ratio * arm_bounds["x_dist"] arm_y_min = end_effector_y - y_ratio * arm_bounds["y_dist"] return [arm_x_min, arm_y_min]
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.detected = {} self.detected_names = rospy.get_param( '/darknet_ros/yolo_model/detection_classes/names') self.object_pose_sub = rospy.Subscriber( '/cluster_decomposer/centroid_pose_array', PoseArray, self.collectJsk) self.listener = tf.TransformListener() self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.position_list = [] self.orientation_list = [] self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.5) self.robot_arm.set_planning_time(10) self.robot_arm.set_num_planning_attempts(10) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): ''' planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print"====== current pose(1) : ", self.wpose ''' marker_joint_goal = [ 4.721744537353516, -0.7451499144183558, -1.6199515501605433, -1.2175200621234339, 1.6366002559661865, 3.1263363361358643 ] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) ''' self.wpose = self.robot_arm.get_current_pose() print"====== current pose(2) : ", self.wpose ''' def move_code2(self): planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print "====== current pose(1) : ", self.wpose marker_joint_goal = [ 4.768651485443115, -1.1165898481952112, -2.1672890822040003, 5.898628234863281, 1.7003079652786255, 3.2297513484954834 ] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) self.wpose = self.robot_arm.get_current_pose() print "====== current pose(2) : ", self.wpose def plan_cartesian_path(self, scale=1.0): waypoints = [] ''' self.wpose = self.robot_arm.get_current_pose() print"====== current pose(1) : ", self.wpose ''' self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x += scale * 0.009 self.wpose.position.y += scale * 0.07 self.wpose.position.z -= scale * 0.06 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) ''' self.wpose = self.robot_arm.get_current_pose() print"====== current pose(2) : ", self.wpose ''' return plan, fraction def pose_subscriber(self): rospy.loginfo("waiting for pose topic...") rospy.get_param('/darknet_ros/yolo_model/detection_classes/names') detection_start = time.time() rospy.Subscriber('/cluster_decomposer/centroid_pose_array', PoseArray, self.collectJsk) print("detection time", format(time.time() - detection_start)) detection_start = time.time() def collectJsk(self, msg): # detection_start = time.time() try: detection_start = time.time() (trans, rot) = self.listener.lookupTransform('base_link', 'yolo_output00', rospy.Time(0)) self.goal_x = trans[0] self.goal_y = trans[1] self.goal_z = trans[2] self.goal_ori_x = rot[0] self.goal_ori_y = rot[1] self.goal_ori_z = rot[2] self.goal_ori_w = rot[3] self.position_list = [self.goal_x, self.goal_y, self.goal_z] self.orientation_list = [ self.goal_ori_x, self.goal_ori_y, self.goal_ori_z, self.goal_ori_w ] (self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion(self.orientation_list) except: return print("detection time", format(time.time() - detection_start)) # print("detection time",format(time.time()-detection_start)) def go_to_move(self, scale=1.0): ''' planning_frame = self.robot_arm.get_planning_frame() self.wpose = self.robot_arm.get_current_pose() print"====== current pose(1) : ", self.wpose ''' self.calculated_tomato.position.x = (scale * self.goal_x) self.calculated_tomato.position.y = (scale * self.goal_y) self.calculated_tomato.position.z = (scale * self.goal_z) self.calculated_tomato.orientation = Quaternion( *quaternion_from_euler(3.14, 0, 1.57)) print("============ tomato_pose goal frame: ", self.calculated_tomato) self.robot_arm.set_pose_target(self.calculated_tomato) tf_display_position = [ self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z ] tf_display_orientation = [ self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") tomato_waypoints = [] tomato_waypoints.append(copy.deepcopy(self.calculated_tomato)) (tomato_plan, tomato_fraction) = self.robot_arm.compute_cartesian_path( tomato_waypoints, 0.01, 0.0) self.display_trajectory(tomato_plan) print("======= Press `Enter` to if plan in correct!======") raw_input() self.robot_arm.go(True) ''' self.wpose = self.robot_arm.get_current_pose() print"====== current pose(2) : ", self.wpose ''' def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) #self.listener = tf.TransformListener() self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.marker = [] self.position_list = [] self.orientation_list = [] self.m_idd = 0 self.m_pose_x = [] self.m_pose_y = [] self.m_pose_z = [] self.m_ori_w = [] self.m_ori_x = [] self.m_ori_y = [] self.m_ori_z = [] self.ar_pose = Pose() self.goalPoseFromAR = Pose() self.br = tf.TransformBroadcaster() #self.goalPose_from_arPose = Pose() self.trans = [] self.rot = [] self.target_ar_id = 9 self.calculed_coke_pose = Pose() #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): """ move to initial pose of the UR3 """ #self.clear_octomap() planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print "====== current pose : ", self.wpose marker_joint_goal = [ -0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982 ] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) def move_moveit_setting_pose(self, pose_name): """ move to one of the moveit_setup pose: "home", "zeros", "table" """ if pose_name == "home": self.robot_arm.set_named_target("home") elif pose_name == "zeros": self.robot_arm.set_named_target("zeros") elif pose_name == "table": self.robot_arm.set_named_target("table") #print "Press the Enter" #raw_input() self.robot_arm.go(wait=True) def go_to_move(self, scale=1.0): """ manipulator is moving to the target ar_marker. \n the start ar_marker id is 9, goal ar_marker id is 10. """ #self.calculed_coke_pose = self.robot_arm.get_current_pose() planning_frame = self.robot_arm.get_planning_frame() coke_offset = [0, -0.35, -0.1] #x y z # gazebo_coke_offset = [0, -0.2875, -0.23] gazebo 에서의 마커와 코크 캔의 offset, 바로 명령하면 해를 못 품. # linear offset = abs([0, 0.0625, 0.13]) robot_base_offset = 0.873 base_wrist2_offset = 0.1 #for avoiding link contact error if self.target_ar_id == 9: print ">> robot arm plannig frame: \n", planning_frame print ">> move mode id: ", self.target_ar_id self.calculed_coke_pose.position.x = ( scale * self.goal_x) # base_link to wrist2 x-offset self.calculed_coke_pose.position.y = (scale * self.goal_y) + coke_offset[1] #self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 + coke_offset# world to base_link z-offset self.calculed_coke_pose.position.z = ( scale * self.goal_z ) + robot_base_offset # world to base_link z-offset and coke can offset self.calculed_coke_pose.orientation = Quaternion( *quaternion_from_euler(3.14, 0, 1.57)) print "========== coke_pose goal frame: ", self.calculed_coke_pose self.robot_arm.set_pose_target(self.calculed_coke_pose) elif self.target_ar_id == 10: print ">> robot arm plannig frame: \n", planning_frame print ">> move mode id: ", self.target_ar_id self.calculed_coke_pose.position.x = (scale * self.goal_x) + coke_offset[1] self.calculed_coke_pose.position.y = (scale * self.goal_y) + 0 self.calculed_coke_pose.position.z = ( scale * self.goal_z ) + robot_base_offset # world to base_link z-offset and coke can offset self.calculed_coke_pose.orientation = Quaternion( *quaternion_from_euler(3.14, 0, 0)) print "========== coke_pose goal frame: ", self.calculed_coke_pose self.robot_arm.set_pose_target(self.calculed_coke_pose) tf_display_position = [ self.calculed_coke_pose.position.x, self.calculed_coke_pose.position.y, self.calculed_coke_pose.position.z ] tf_display_orientation = [ self.calculed_coke_pose.orientation.x, self.calculed_coke_pose.orientation.y, self.calculed_coke_pose.orientation.z, self.calculed_coke_pose.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz coke_waypoints = [] coke_waypoints.append(copy.deepcopy(self.calculed_coke_pose)) (coke_plan, coke_fraction) = self.robot_arm.compute_cartesian_path( coke_waypoints, 0.01, 0.0) self.display_trajectory(coke_plan) ## ## ## print "============ Press `Enter` to if plan is correct!! ..." raw_input() self.robot_arm.go(True) def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch, yaw): """ Manipulator is moving to the desired coordinate. \n Now move to the ar_10 marker. """ calculed_ar_id_10 = Pose() #desired_goal_pose = [0.171, -0.113, 1.039] #desired_goal_euler = [3.14, 0.17, 0] desired_goal_pose = [pose_x, pose_y, pose_z] desired_goal_euler = [roll, pitch, yaw] Cplanning_frame = self.robot_arm.get_planning_frame() print ">> current planning frame: \n", Cplanning_frame calculed_ar_id_10.position.x = desired_goal_pose[0] + 0.1 calculed_ar_id_10.position.y = desired_goal_pose[1] calculed_ar_id_10.position.z = desired_goal_pose[2] calculed_ar_id_10.orientation = Quaternion(*quaternion_from_euler( desired_goal_euler[0], desired_goal_euler[1], desired_goal_euler[2])) print ">>> ar id 10 goal frame: ", desired_goal_pose self.robot_arm.set_pose_target(calculed_ar_id_10) tf_display_position = [ calculed_ar_id_10.position.x, calculed_ar_id_10.position.y, calculed_ar_id_10.position.z ] tf_display_orientation = [ calculed_ar_id_10.orientation.x, calculed_ar_id_10.orientation.y, calculed_ar_id_10.orientation.z, calculed_ar_id_10.orientation.w ] jj = 0 while jj < 5: jj += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz ar_id_10_waypoints = [] ar_id_10_waypoints.append(copy.deepcopy(calculed_ar_id_10)) (ar_id_10_plan, ar_id_10_fraction) = self.robot_arm.compute_cartesian_path( ar_id_10_waypoints, 0.01, 0.0) self.display_trajectory(ar_id_10_plan) ## ## ## print "============ Press `Enter` to if plan is correct!! ..." raw_input() self.robot_arm.go(True) def display_trajectory(self, plan): """ Visualized trajectory of the manipulator """ display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0): """ Working on the "linear move": x-axis -> y-axis -> z-axis """ waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose #print "===== robot arm pose: ", self.wpose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10 #print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # waypoints.append(copy.deepcopy(self.wpose)) ii += 1 #print "waypoints:", waypoints (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_x(self, x_offset, scale=1.0): """ Working on the "linear move": x-axis """ waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose #print "===== robot arm pose: ", self.wpose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_y(self, y_offset, scale=1.0): """ Working on the "linear move": y-axis """ waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose #print "===== robot arm pose: ", self.wpose self.wpose.position.y = (scale * self.wpose.position.y) + y_offset #-0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_z(self, z_offset, scale=1.0): """ Working on the "linear move": z-axis """ waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose #print "===== robot arm pose: ", self.wpose self.wpose.position.z = (scale * self.wpose.position.z) + z_offset #-0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): """ execute planned "plan" trajectory. """ group = self.robot_arm group.execute(plan, wait=True) def ar_pose_subscriber(self): """ ar_maker subscriber """ rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") #print "======= pos(meter): ", self.position_list #print "======= orientation: ", self.orientation_list def ar_tf_listener(self, msg): """ Listening the ar_marker pose coordinate. """ try: self.marker = msg.markers ml = len(self.marker) target_start_point_id = self.target_ar_id #target_id = target_ar_id #self.m_idd = self.marker[0].id # 임시용 for ii in range(0, ml): # 0 <= ii < ml self.m_idd = self.marker[ii].id #print "checked all id: ", self.m_idd if self.m_idd != target_start_point_id: pass #target_id_flage = False elif self.m_idd == target_start_point_id: target_id_flage = True target_id = self.m_idd target_id_index = ii #print "target id: ", target_id_index, target_id, target_id_flage if target_id_flage == True: self.ar_pose.position.x = self.marker[ target_id_index].pose.pose.position.x self.ar_pose.position.y = self.marker[ target_id_index].pose.pose.position.y self.ar_pose.position.z = self.marker[ target_id_index].pose.pose.position.z self.ar_pose.orientation.x = self.marker[ target_id_index].pose.pose.orientation.x self.ar_pose.orientation.y = self.marker[ target_id_index].pose.pose.orientation.y self.ar_pose.orientation.z = self.marker[ target_id_index].pose.pose.orientation.z self.ar_pose.orientation.w = self.marker[ target_id_index].pose.pose.orientation.w self.goal_x = self.ar_pose.position.x self.goal_y = self.ar_pose.position.y self.goal_z = self.ar_pose.position.z self.position_list = [self.goal_x, self.goal_y, self.goal_z] self.orientation_list = [ self.ar_pose.orientation.x, self.ar_pose.orientation.y, self.ar_pose.orientation.z, self.ar_pose.orientation.w ] (self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion( self.orientation_list) #list form으로 넘겨주어야 함 #print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z #print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw #print "ar_pos(meter): \n", self.position_list #print "ar_orientation: \n", self.orientation_list except: return
class MoveItObstaclesDemo: def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group self.arm = MoveGroupCommander('manipulator') # 设置机械臂工作空间 self.arm.set_workspace([-100, -100, 0, 100, 0.3, 100]) # 设置机械臂最大速度 self.arm.set_max_velocity_scaling_factor(value=0.1) # 获取终端link的名称 self.end_effector_link = self.arm.get_end_effector_link() rospy.loginfo('end effector link: {}'.format(self.end_effector_link)) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 # self.arm.allow_replanning(True) self.arm.set_num_planning_attempts(10) # self.arm.allow_looking(True) # 设置目标位置所使用的参考坐标系 self.reference_frame = 'base_link' self.arm.set_pose_reference_frame(self.reference_frame) # 设置每次运动规划的时间限制:5s self.arm.set_planning_time(3) # 控制机械臂先回到初始化位置 self.arm.set_named_target('home') self.arm.go() rospy.sleep(2) def planning(self, start_point, end_point): """ 功能:动态避障 :param start_point: 起始点, type: dict :param end_point: 终点, type: dict :return: None """ # 移动机械臂到指定位置,并获取当前位姿数据为机械臂运动的起始位姿 if start_point: self.move_arm(p=start_point) self.start_pose = self.arm.get_current_pose(self.end_effector_link).pose # 使用moveit自带的求解器规划出A到B的离散路径点, 并存到列表way_points中 # way_points = self.get_way_points(start_point, end_point) while True: print('set planner id: RRT') self.arm.set_planner_id('TRRTkConfigDefault') self.arm.set_named_target('up') self.arm.go() rospy.sleep(5) print('set planner id: PRM') self.arm.set_planner_id('TRRTkConfigDefault') self.arm.set_named_target('home') self.arm.go() rospy.sleep(5) # ------------------------------------------------------------------- def get_way_points(self, a, b): way_points = [] # plan 1 self.arm.set_named_target('up') traj = self.arm.plan() if traj.joint_trajectory.points: # True if trajectory contains points rospy.loginfo("get trajectory success") else: rospy.logerr("Trajectory is empty. Planning false!") self.arm.clear_pose_targets() # plan 2 # traj = self.moveit_planning(p=b) for i, p in enumerate(traj.joint_trajectory.points): # rospy.loginfo('waypoint: {}'.format(i)) if i%2 == 0: point = { 'x': p.positions[0], 'y': p.positions[1], 'z': p.positions[2], 'ox': p.positions[3], 'oy': p.positions[4], 'oz': p.positions[5] } way_points.append(point) rospy.loginfo('waypoint: \n {}'.format(way_points)) rospy.loginfo(len(way_points)) return way_points def moveit_planning(self, p): """ 使用moveit求解器规划路径 :param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0} :return: """ rospy.loginfo('start moveit planning...') target_pose = PoseStamped() target_pose.header.frame_id = self.reference_frame target_pose.pose.position.x = p['x'] target_pose.pose.position.y = p['y'] target_pose.pose.position.z = p['z'] if 'ox' in p.keys() and p['ox']: target_pose.pose.orientation.x = p['ox'] if 'oy' in p.keys() and p['oy']: target_pose.pose.orientation.y = p['oy'] if 'oz' in p.keys() and p['oz']: target_pose.pose.orientation.z = p['oz'] if 'ow' in p.keys() and p['ow']: target_pose.pose.orientation.w = p['ow'] # 传入一个PoseStamped # self.arm.set_pose_target(target_pose, self.end_effector_link) # 尝试直接传入一个列表 self.arm.set_pose_target([p['x'], p['y'], p['z'], p['ox'], p['oy'], p['oz']], self.end_effector_link) traj = self.arm.plan() if traj.joint_trajectory.points: # True if trajectory contains points rospy.loginfo("get trajectory success") return traj else: rospy.logerr("Trajectory is empty. Planning false!") def move_arm(self, p): """ 移动机械臂到p点 :param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0} :return: """ rospy.loginfo('start arm moving...') traj = self.moveit_planning(p) self.arm.execute(traj) # 设置当前位置为起始位置 self.arm.set_start_state_to_current_state() rospy.sleep(1) def stop_arm(self): """ 急停 :return: """ pass def exist_danger_obstacle(self): """ 环境中是否存在危险的障碍物 :return: True or False """ return False def get_obstacle_octomap(self): """ 获取环境的octomap信息 :return: """ pass
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move',anonymous=True) self.listener = tf.TransformListener() self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_roll = 0 self.goal_pitch = 0 self.goal_yaw = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.wpose = [] self.marker = [] self.tf_list = [] self.m_idd = [] self.m_pose_x = [] self.m_pose_y = [] self.m_pose_z = [] self.m_ori_w = [] self.m_ori_x = [] self.m_ori_y = [] self.m_ori_z = [] self.trans = [] self.rot = [] self.pose_goal = Pose() #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def ar_callback(self, msg): #marker = msg.markers[1] self.marker = msg.markers ml = len(self.marker) m_id = self.marker[0].id #print "marker length: ", ml, ml-1 #print "marker id: ", m_id, self.marker[1].id #self.m_idd = [] #self.m_pose_x = [] #self.m_pose_y = [] #self.m_pose_z = [] #self.m_ori_w = [] #self.m_ori_x = [] #self.m_ori_y = [] #self.m_ori_z = [] for ii in range(0, ml): self.m_idd.append(self.marker[ii].id) self.m_pose_x.append(self.marker[ii].pose.pose.position.x) self.m_pose_y.append(self.marker[ii].pose.pose.position.y) self.m_pose_z.append(self.marker[ii].pose.pose.position.z) self.m_ori_w.append(self.marker[ii].pose.pose.orientation.w) self.m_ori_x.append(self.marker[ii].pose.pose.orientation.x) self.m_ori_y.append(self.marker[ii].pose.pose.orientation.y) self.m_ori_z.append(self.marker[ii].pose.pose.orientation.z) ''' print "id: ", self.m_idd print "pose_x: ", self.m_pose_x print "pose_y: ", self.m_pose_y print "pose_z: ", self.m_pose_z print "ori_w: ", self.m_ori_w print "ori_x: ", self.m_ori_x print "ori_y: ", self.m_ori_y print "ori_z: ", self.m_ori_z ''' m_pose_goal = Pose() m_pose_goal.orientation = Quaternion(*quaternion_from_euler(0, -1.5, 0)) m_pose_goal.position.x = self.m_pose_x[0] + 0.029# red line 0.2 0.2 m_pose_goal.position.y = self.m_pose_y[0] - 0.7 # green line 0.15 0.15 m_pose_goal.position.z = self.m_pose_z[0] + 0.3 # blue line # 0.35 0.6 ''' print "goal_pose: ", m_pose_goal self.robot_arm.set_pose_target(m_pose_goal) self.robot_arm.go(True) print "==== check check" marker_joint_goal = [0.07100913858593216, -1.8767615298285376, 2.0393206555899503, -1.8313959190971882, -0.6278395875738125, 1.6918219826764682] self.robot_arm.go(marker_joint_goal, wait=True) print "====== robot joint value: \n" print marker_joint_goal #self.robot_arm.go(marker_joint_goal, wait=True) pos_x = self.marker[0].pose.pose.position.x pos_y = self.marker[0].pose.pose.position.y pos_z = self.marker[0].pose.pose.position.z dist = math.sqrt( (pos_x*pos_x) + (pos_y * pos_y) ) ''' #ori_x = marker.pose.pose.orientation.x #ori_y = marker.pose.pose.orientation.y #ori_z = marker.pose.pose.orientation.z #ori_w = marker.pose.pose.orientation.w #print(m_id) #print "===========" #print 'id: ', m_id #print 'pos: ', pos_x, pos_y, pos_z #print('ori: ', ori_x, ori_y, ori_z, ori_w) def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print"====== current pose : ", self.wpose #self.pose_goal = self.robot_arm.get_current_pose() self.pose_goal.position.x = 0.062 self.pose_goal.position.y = 0.194 self.pose_goal.position.z = 0.878 self.pose_goal.orientation.x = 0.673 self.pose_goal.orientation.y = 0.673 self.pose_goal.orientation.z = -0.217 self.pose_goal.orientation.w = 0.217 #print "========== goal frame: ", self.pose_goal self.robot_arm.set_pose_target(self.pose_goal) self.robot_arm.go(True) def look_up_down(self): self.clear_octomap() print "======== clear_octomap" look_up_down = self.robot_arm.get_current_joint_values() #print "before: ", look_up_down look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) def plan_cartesian_path(self, scale = 1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose #self.wpose.position.z -= scale * (0.695) # First move up (z) #self.wpose.position.y += scale * (0.199) # and sideways (y) #waypoints.append(copy.deepcopy(self.wpose)) print "===== robot arm pose: ", self.wpose ''' # 19.06.11 10:45 분 코딩 print self.wpose.position.x," + (",scale," * ",self.goal_x,")" print self.wpose.position.y," + (",scale," * ",self.goal_y,")" self.wpose.position.x = self.wpose.position.x - (scale * self.goal_x) self.wpose.position.y = self.wpose.position.y + (scale * self.goal_y) print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ii += 1 print self.wpose.position.z," + (",scale," * ",self.goal_z,")" self.wpose.position.z += (scale * self.goal_z) print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ''' self.wpose.position.x = (scale * self.wpose.position.x) - 0.10 #self.wpose.position.y = (scale * self.goal_y) - 0.1 #print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ii += 1 #print self.wpose.position.z," + (",scale," * ",self.goal_z,")" #self.wpose.position.z = (scale * self.goal_z) + 0.1 self.wpose.position.y = (scale * self.goal_y) + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def print_ar_pose(self): rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) rospy.loginfo("Maker messages detected. Starting followers...") def go_to_move(self, scale = 1.0): self.wpose = self.robot_arm.get_current_pose() planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.pose_goal.position.x = (scale * self.goal_x) + 0.10 # base_link to wrist2 x-offset self.pose_goal.position.y = (scale * self.goal_y) - 0.25 self.pose_goal.position.z = (scale * self.goal_z) + 0.72 - 0.115 # world to base_link z-offset and coke can offset self.pose_goal.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.54)) ''' 19.06.1 15:58 코딩 self.pose_goal.orientation.x = self.rot[0] self.pose_goal.orientation.y = self.rot[1] self.pose_goal.orientation.z = self.rot[2] self.pose_goal.orientation.w = self.rot[3] ''' #self.pose_goal.orientation.x = self.goal_roll + 1.57 #self.pose_goal.orientation.y = self.goal_pitch + 1.57 #self.pose_goal.orientation.z = self.goal_yaw print "========== goal frame: ", self.pose_goal self.robot_arm.set_pose_target(self.pose_goal) #self.robot_arm.go(True) def tf_listener(self): # 수정사안: ar_pose id의 가져와서 위치 값을 읽어와야함. try: child_tf = '/ar_marker_9' parent_tf = '/base_link' (self.trans,self.rot) = self.listener.lookupTransform(parent_tf, child_tf, rospy.Time(0)) # rosrun tf tf_echo /base_link /ar_marker_9 랑 같은 역할 # transformation: linear transformation, rotation: quaternion print "======= trans[x, y, z]: ", self.trans print "======= rotat[x, y, z, w]: ", self.rot (self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion(self.rot) print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw #convert 확인용 #quat = quaternion_from_euler(goal_roll, goal_pitch, goal_yaw) #print quat self.goal_x = self.trans[0] self.goal_y = self.trans[1] self.goal_z = self.trans[2] except Exception as ex: print "======== tf_listener Exception!!" rospy.loginfo(ex) def look_object(self): try: look_object = self.robot_arm.get_current_joint_values() #look_object[4] = look_object[4] + (math.radians(90)) # wrist2 look_object[5] = look_object[5] + (3.1) # wrist3 #look_object[3] = look_object[3] - (math.radians(00)) # wrist1 self.robot_arm.go(look_object, wait=True) #look_object[5] = look_object[5] + (math.radians(90)) # wrist3 #self.robot_arm.go(look_object, wait=True) except Exception as ex: print "======== look_object Exception!!" rospy.loginfo(ex)
class Arm(): def __init__(self): self.p = Pose() self.gripper = MoveGroupCommander("onine_gripper") self.arm = MoveGroupCommander("onine_arm") self.arm.set_goal_tolerance(0.004) self.arm.allow_replanning(True) # self.arm.set_goal_position_tolerance(0.005) # self.arm.set_goal_orientation_tolerance(0.1) self.arm.set_num_planning_attempts(10) self.arm.set_planning_time(5) self.arm.set_planner_id("RRTkConfigDefault") def go(self, x, y, z, roll, pitch, yaw): self.p.position.x = x self.p.position.y = y self.p.position.z = z self.p.orientation = Quaternion( *quaternion_from_euler(roll, pitch, yaw)) self.arm.set_pose_target(self.p) os.system("rosservice call clear_octomap") rospy.loginfo("Moving to arm target") self.arm.go(wait=True) rospy.sleep(1) def get_valid_pose(self, x, y, z, distance): origin_translation = [0.095, 0.00, 0.00] delta_x = x - origin_translation[0] delta_y = y - origin_translation[1] theta = math.atan(delta_y / delta_x) grasp_yaw = theta grasp_x = x + (distance * math.cos(theta)) grasp_y = y + (distance * math.sin(theta)) return (grasp_x, grasp_y, z, grasp_yaw) def ready(self): self.arm.set_named_target("onine_ready") self.arm.go(wait=True) def home(self): self.arm.set_named_target("onine_home") self.arm.go(wait=True) def feed_pos(self): self.arm.set_named_target("feed_pos") self.arm.go(wait=True) def tilt_food(self): self.arm.set_named_target("tilt_food") self.arm.go(wait=True) def open_gripper(self): os.system("rostopic pub /onine_gripper std_msgs/Float64 0.085 -1") os.system("rosservice call clear_octomap") def close_gripper(self): os.system("rostopic pub /onine_gripper std_msgs/Float64 0.035 -1") os.system("rosservice call clear_octomap") def pickup_sim(self, x, y, z): # self.ready() self.open_gripper() (aim_x, aim_y, aim_z, aim_yaw) = self.get_valid_pose(x, y, z + 0.15, 0.00) self.go(aim_x, aim_y, aim_z, 0.0, 0.0, aim_yaw) (aim_x, aim_y, aim_z, aim_yaw) = self.get_valid_pose(x, y, z, 0.00) self.go(aim_x, aim_y, aim_z, 0.0, 0.0, aim_yaw) self.close_gripper()
class UR5_Gripped_Manipulator: """ Class Attributes Robot : Robot Commander Object Scene : Planning Scene Interface Object (current scene) Man : MoveGroupCommander object to control manipulator EEF : MoveGroupCommander object to control endeffector Target_poses : Pose list of created target objects Picked : List of part indexes to ensure to picked non-picked ones """ def __init__(self, manip_name="manipulator", eef_name="endeffector"): self.robot = RobotCommander() self.man = MoveGroupCommander(manip_name) self.eef = MoveGroupCommander(eef_name) self.target_poses = [] self.picked = [] """ ----------------Function Name: clean_scene---------------- Definition: Clears the target_poses and picked lists in order to establish a new execution. Necessary for test_simulation method """ def clear_poses(self): for i in xrange(len(self.target_poses)): self.target_poses.pop() for i in xrange(len(self.picked)): self.picked.pop() """ ----------------Function Name: clean_scene---------------- Definition : Clears all non-attached objects from planning scene Group : MoveGroupCommander object to control given group """ def default_state_gripper(self, grp): rospy.loginfo("Openning Gripper") joint_vals = grp.get_current_joint_values() joint_vals[0] = 0.0 grp.set_joint_value_target(joint_vals) init_plan = grp.plan() return grp.execute(init_plan) """ ----------------Function Name: closed_state_gripper---------------- Definition : Function that opens gripper and detachs the gripped object Obj : Name of the Object that is needed to detach """ def closed_state_gripper(self, curr_scene, obj): rospy.loginfo("Closing Gripper") def convert(width): return 0.77 - width / 0.15316 * 0.77 width = curr_scene.get_objects([obj])[obj].primitives[0].dimensions[0] width = convert(width) now = self.robot.endeffector.get_current_joint_values()[0] cnt = 0 while abs(now - width) > 0.05: now = self.robot.endeffector.get_current_joint_values()[0] cnt = cnt + 1 tmp = width - abs(now - width) / 2.0 self.robot.endeffector.set_joint_value_target('finger_joint', tmp) self.robot.endeffector.go() rospy.sleep(0.05) if cnt == 7: break rospy.sleep(1.0) ret = self.robot.manipulator.attach_object(obj) return ret """ ----------------Function Name: set_mid_state---------------- Definition : Motion planning function for defined mid state for manipulator, can be used for enhancing the performance if needed Group : MoveGroupCommander object to control given group """ def set_mid_state(self, group): rospy.loginfo("Going to Mid-State") pos = PoseStamped() pos.header.frame_id = self.robot.get_planning_frame() pos.pose.position.x = 0.45358 pos.pose.position.y = 0.188518 pos.pose.position.z = 0.473063 pos.pose.orientation.x = 0.0004738 pos.pose.orientation.y = 1 pos.pose.orientation.z = 0 pos.pose.orientation.w = 0.000155894 group.set_pose_target(pos) move_plan = group.plan() while (not move_plan.joint_trajectory.joint_names): move_plan = group.plan() return group.execute(move_plan) """ ----------------Function Name: add_object---------------- Name : Object Name Pose : Pose of the Object (x,y,z,xo,yo,zo,wo) Dimension : Dimensions of the Obhect (Tuple) (d1,d2,d3), Type : Box(0),Sphere(1) d1 is radius for sphere i.e typ==1, """ def add_object(self, name, x, y, z, xo=0.0, yo=0.0, zo=0.0, wo=0.0, d1=0.1, d2=0.1, d3=0.1, curr_scene=None, typ=0): pos = PoseStamped() pos.header.frame_id = self.robot.get_planning_frame() pos.pose.position.x = x pos.pose.position.y = y pos.pose.position.z = z pos.pose.orientation.x = xo pos.pose.orientation.y = yo pos.pose.orientation.z = zo pos.pose.orientation.w = wo rospy.loginfo("ADDING OBJECT") if (typ == 0): curr_scene.add_box(name, pos, (d1, d2, d3)) elif (typ == 1): curr_scene.add_sphere(name, pos, d1) else: print("ERROR in Type") return pos """ ----------------Function Name: clean_scene---------------- Definition: Clears all non-attached objects from planning scene """ def clean_scene(self, curr_scene): rospy.loginfo("Clearing the Scene") curr_scene.remove_world_object() """ ----------------Function Name: create_environment---------------- Definition : Creates the simulation environment with non-target collision objects, can be edited to create different default environment """ def create_environment(self, curr_scene): self.add_object(name="wall", x=0.0, y=0.8, z=0.5, d1=0.1, d2=0.35, d3=1, curr_scene=curr_scene, typ=0) self.add_object(name="wall_2", x=0.0, y=-0.8, z=0.5, d1=0.1, d2=0.35, d3=1, curr_scene=curr_scene, typ=0) self.add_object(name="table", x=0.0, y=0.0, z=-0.05, d1=2, d2=2, d3=0.0001, curr_scene=curr_scene, typ=0) """ ----------------Function Name: check_targets---------------- Definition : Creates given number of objects within the distance provided. It prevents collision object overlapping. It returns the pose list of the objects Number : Number of collision objects required to spawn Distance : Required minimum distance between each collision objects """ def check_targets(self, curr_scene, number, distance): rospy.loginfo("Creating Boxes!") def al(typ, x=0.0): if typ == 'x': return random.uniform(0.35, 0.65) elif typ == "y": rang = math.sqrt(0.7**2 - x**2) return random.uniform(-rang, rang) hemme = [] dims = [] cnt = 0 while len(hemme) != number: if cnt == 200: hemme = [] cnt = 0 cnt = cnt + 1 dim = (random.uniform(0.08, 0.12), random.uniform(0.08, 0.10), random.uniform(0.05, 0.2)) quaternion = tf.transformations.quaternion_from_euler( 0.0, 0.0, random.uniform(-math.pi, math.pi)) box = PoseStamped() box.header.frame_id = self.robot.get_planning_frame() box.pose.position.z = (-0.04 + (dim[2] / 2)) box.pose.position.x = al('x') box.pose.position.y = al('y', box.pose.position.x) box.pose.orientation.x = quaternion[0] box.pose.orientation.y = quaternion[1] box.pose.orientation.z = quaternion[2] box.pose.orientation.w = quaternion[3] flag = 1 for i in hemme: if abs(i.pose.position.x - box.pose.position.x) < distance or abs( i.pose.position.y - box.pose.position.y) < distance: flag = 0 if flag == 0: continue hemme.append(box) dims.append(dim) cnt = 0 names = [] for i in xrange(len(hemme)): now = "part" + str(cnt) cnt = cnt + 1 names.append(now) self.add_object(name=now, x=hemme[i].pose.position.x, y=hemme[i].pose.position.y, z=hemme[i].pose.position.z, xo=hemme[i].pose.orientation.x, yo=hemme[i].pose.orientation.y, zo=hemme[i].pose.orientation.z, wo=hemme[i].pose.orientation.w, d1=dims[i][0], d2=dims[i][1], d3=dims[i][2], curr_scene=curr_scene, typ=0) print("End Check!") return hemme """ ----------------Function Name: pick_object---------------- Definition : It moves the given group i.e robot to the collision object whose index is given and picks that object. Group : MoveGroupCommander object to control given group Part_index : Index of the target object to obtain pose """ def pick_object(self, curr_scene, group, part_index): rospy.loginfo("Pick Operation starts!") gripped_object = curr_scene.get_objects(["part" + str(part_index) ])["part" + str(part_index)] pos = copy.deepcopy(self.target_poses[part_index]) temp = tf.transformations.euler_from_quaternion( (pos.pose.orientation.x, pos.pose.orientation.y, pos.pose.orientation.z, pos.pose.orientation.w)) quaternion = tf.transformations.quaternion_from_euler( math.pi, temp[1], temp[2]) pos.pose.position.z += 0.17 + ( gripped_object.primitives[0].dimensions[2] / 2.0) pos.pose.orientation.y = 1 group.set_pose_target(pos) move_plan = group.plan() i = 0 while (not move_plan.joint_trajectory.joint_names): move_plan = group.plan() i += 1 if (i == 500): break state = group.execute(move_plan) rospy.loginfo("Execute operation for Object is %s" % str(part_index)) if (state): self.closed_state_gripper(curr_scene, "part" + str(part_index)) rospy.sleep(1.0) self.place_object(curr_scene, group, part_index) return else: return """ ----------------Function Name: place_object---------------- Definition : It places the gripped object to the target location Group : MoveGroupCommander object to control given group Part_index : Index of the target object to obtain pose """ def place_object(self, curr_scene, group, part_index): pos = PoseStamped() def al(typ, x=0.0): if typ == 'x': return random.uniform(-0.35, -0.6) elif typ == 'y': rang = math.sqrt(0.75**2 - x**2) x = random.uniform(-rang, rang) while abs(x) < 0.15: x = random.uniform(-rang, rang) return x pos = PoseStamped() pos.header.frame_id = robot.get_planning_frame() pos.pose.position.x = al("x") pos.pose.position.y = al("y", pos.pose.position.x) #This line makes placing possible by setting a valid z position for gripped object pos.pose.position.z = -0.04 + 0.17 + ((curr_scene.get_attached_objects( ["part" + str(part_index) ])["part" + str(part_index)]).object.primitives[0].dimensions[2]) pos.pose.orientation.y = 1.0 group.set_pose_target(pos) move_plan = group.plan() while (not move_plan.joint_trajectory.joint_names): move_plan = group.plan() state = group.execute(move_plan) if (state): detached = group.detach_object("part" + str(part_index)) rospy.sleep(1.5) if (detached): curr_scene.remove_world_object("part" + str(part_index)) rospy.sleep(1.5) self.default_state_gripper(self.eef) self.picked.append(part_index) else: self.default_state_gripper(robot.endeffector) group.detach_object("part" + str(part_index)) rospy.sleep(2) """ ----------------Function Name: execute_simulation---------------- Definition : Creates the specified environment , generates motion plans and does the pick and place operation based on these plans. To visualize RViz is needed. Planner ID can be changed based on MoveIT's supported OMPL. """ def execute_simulation(self, curr_scene=None, num_of_objects=3): is_success = False #Reset State of the Gripper self.default_state_gripper(self.eef) #### #Reset the position of the Arm # Will be implemented if needed #### #Clean the scene self.clean_scene(curr_scene=curr_scene) #Create environment self.create_environment(curr_scene=curr_scene) #Create targets self.target_poses = self.check_targets(curr_scene, num_of_objects, 0.10) rospy.sleep(5) #Planner setup self.man.set_planner_id("RRTConnectkConfigDefault") self.man.set_num_planning_attempts(20) self.man.allow_replanning(True) #Pick and place every object for i in xrange(len(self.target_poses)): if i not in self.picked: self.pick_object(curr_scene=curr_scene, group=self.man, part_index=i) rospy.loginfo("END OF PICK AND PLACE") if (len(self.picked) == len(self.target_poses)): is_success = True self.clear_poses() return is_success """ ----------------Function Name: execute_simulation---------------- Definition : Executes the simulation num_attempts times. Stores the success rate and writes into the specified document Num_Attempts : Limiting number for executing simulation. (Default Value: 10) File_Name : File name (with path if needed) to write test results """ def test_simulation(self, curr_scene=None, num_attempts=10, num_of_objects=3, file_name="ur5_pick_place_test.txt"): success_rate = 0 file = open(file_name, "a+") file.write("Start Test \n") for case in xrange(1, num_attempts + 1): state = self.execute_simulation(curr_scene=curr_scene, num_of_objects=num_of_objects) if (state): success_rate += 1 rospy.loginfo("Success Rate: " + str(success_rate) + "/" + str(case)) file.write("Test Case %s is successfull" % str(case)) else: rospy.loginfo("Success Rate: " + str(success_rate) + "/" + str(case)) file.write("Test Case %s is failedl" % str(case)) file.write("Success: %" + str(((success_rate / (case - 1) * 100.0)))) file.write("----END OF TEST----") file.close() return
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.detected = {} self.detected_names = rospy.get_param('/darknet_ros/yolo_model/detection_classes/names') self.object_pose_sub = rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk) self.listener = tf.TransformListener() # self.object_name_sub = rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect) self.tomatoboundingBox = BoundingBox() display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20) global goal_x global goal_y global goal_z global goal_roll global goal_pitch global goal_yaw self.distance = 0 self.trans = [0.0, 0.0, 0.0] # self.marker = [] self.tomato = [] self.position_list = [] self.orientation_list = [] self.tomato_pose = Pose() self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print ("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print ("====== current pose : ", self.wpose) joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982] print ("INIT POSE: ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait = True) def go_to_desired_coordinate(self): self.calculated_tomato.position.x = goal_x self.calculated_tomato.position.y = goal_y self.calculated_tomato.position.z = goal_z self.calculated_tomato.orientation.x = goal_roll self.calculated_tomato.orientation.y = goal_pitch self.calculated_tomato.orientation.z = goal_yaw tf_display_position = [self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z] tf_display_orientation = [self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w] ii = 0 while ii < 5: ii += 1 self.br.sendTransform( tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz tomato_id_goal_waypoints = [] tomato_id_goal_waypoints.append(copy.deepcopy(calculated_tomato)) (tomato_id_goal_plan, tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path(tomato_id_goal_waypoints, 0.01, 0.0) self.display_trajectory(tomato_id_goal_plan) print ("============ Press `Enter` to if plan is correct!! ...") raw_input() self.robot_arm.go(True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # - 0.10 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset waypoints.append(copy.deepcopy(self.wpose)) ii += 1 (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_x(self, x_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_y(self, y_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_z(self, z_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def pose_subscriber(self): rospy.loginfo("waiting for pose topic...") rospy.get_param('/darknet_ros/yolo_model/detection_classes/names') rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk) # rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect) def collectJsk(self, msg): global goal_x global goal_y global goal_z global goal_roll global goal_pitch global goal_yaw try: (trans, rot) = self.listener.lookupTransform('base_link','yolo_output00', rospy.Time(0)) goal_x = round(trans[0],2) goal_y = round(trans[1],2) goal_z = round(trans[2],2) print("====== trans [x, y, z]: ", trans) print("====== rotat [x, y, z, w]: ", rot) orientation = euler_from_quaternion(rot) goal_roll = orientation[0] goal_pitch = orientation[1] goal_yaw = orientation[2] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn('there is no tf')
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.detected = {} self.listener = tf.TransformListener() display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20) self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.position_list = [] self.orientation_list = [] self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.5) self.robot_arm.set_planning_time(10) self.robot_arm.set_num_planning_attempts(10) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print"====== current pose(1) : ", self.wpose marker_joint_goal = [4.721744537353516, -0.7451499144183558, -1.6199515501605433, -1.2175200621234339, 1.6366002559661865, 3.1263363361358643] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) self.wpose = self.robot_arm.get_current_pose() print"====== current pose(2) : ", self.wpose def execute_plan(self,plan): group = self.robot_arm group.execute(plan, wait=True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory)
#Reset the position of the Arm # Will be implemented if needed #### #Clean the scene clean_scene(scene) #Create environment create_environment(scene, robot) #Create target object target_poses = check_targets(robot, scene, 3, 0.10) f.write("Used Library: " + libraries[2]) rospy.loginfo("TEST NUMBER: " + str(tr)) rospy.sleep(1) # Planner setup arm.set_planner_id("RRTConnectkConfigDefault") arm.set_num_planning_attempts(20) arm.allow_replanning(True) #Pick and place every object for i in xrange(len(target_poses)): if i not in picked: pick_object(group=arm, part_index=i, robot=robot, scene=scene) #For testing if (len(picked) == len(target_poses)): success_rate += 1 rospy.loginfo("END OF PICK AND PLACE") f.write("After try " + str(tr) + str(success_rate) + "/" + str(tr)) rospy.loginfo("Success Rate: " + str(success_rate) + "/" + str(tr)) clear_poses() rospy.sleep(5.0)
class PlannerAnnotationParser(AnnotationParserBase): """ Parses the annotations files that contains the benchmarking information. """ def __init__(self, path_to_annotation, path_to_data): super(PlannerAnnotationParser, self).__init__(path_to_annotation, path_to_data) self.parse() self._load_scene() self._init_planning() self.benchmark() def check_results(self, results): """ Returns the results from the planner, checking them against any eventual validation data (no validation data in our case). """ return self.planner_data def _load_scene(self): """ Loads the proper scene for the planner. It can be either a python static scene or bag containing an occupancy map. """ scene = self._annotations["scene"] for element in scene: if element["type"] == "launch": self.play_launch(element["name"]) elif element["type"] == "python": self.load_python(element["name"]) elif element["type"] == "bag": self.play_bag(element["name"]) for _ in range(150): rospy.sleep(0.3) # wait for the scene to be spawned properly rospy.sleep(0.5) def _init_planning(self): """ Initialises the needed connections for the planning. """ self.group_id = self._annotations["group_id"] self.planners = self._annotations["planners"] self.scene = PlanningSceneInterface() self.robot = RobotCommander() self.group = MoveGroupCommander(self.group_id) self._marker_pub = rospy.Publisher('/visualization_marker_array', MarkerArray, queue_size=10, latch=True) self._planning_time_sub = rospy.Subscriber( '/move_group/result', MoveGroupActionResult, self._check_computation_time) rospy.sleep(1) self.group.set_num_planning_attempts( self._annotations["planning_attempts"]) self.group.set_goal_tolerance(self._annotations["goal_tolerance"]) self.group.set_planning_time(self._annotations["planning_time"]) self.group.allow_replanning(self._annotations["allow_replanning"]) self._comp_time = [] self.planner_data = [] def benchmark(self): for test_id, test in enumerate(self._annotations["tests"]): marker_position_1 = test["start_xyz"] marker_position_2 = test["goal_xyz"] self._add_markers(marker_position_1, "Start test \n sequence", marker_position_2, "Goal") # Start planning in a given joint position joints = test["start_joints"] current = RobotState() current.joint_state.name = self.robot.get_current_state( ).joint_state.name current_joints = list( self.robot.get_current_state().joint_state.position) current_joints[0:6] = joints current.joint_state.position = current_joints self.group.set_start_state(current) joints = test["goal_joints"] for planner in self.planners: if planner == "stomp": planner = "STOMP" elif planner == "sbpl": planner = "AnytimeD*" self.planner_id = planner self.group.set_planner_id(planner) self._plan_joints( joints, self._annotations["name"] + "-test_" + str(test_id)) return self.planner_data def _add_markers(self, point, text1, point_2, text2): # add marker for start and goal pose of EE marker_array = MarkerArray() marker_1 = Marker() marker_1.header.frame_id = "world" marker_1.header.stamp = rospy.Time.now() marker_1.type = Marker.SPHERE marker_1.scale.x = 0.04 marker_1.scale.y = 0.04 marker_1.scale.z = 0.04 marker_1.lifetime = rospy.Duration() marker_2 = deepcopy(marker_1) marker_1.color.g = 0.5 marker_1.color.a = 1.0 marker_1.id = 0 marker_1.pose.position.x = point[0] marker_1.pose.position.y = point[1] marker_1.pose.position.z = point[2] marker_2.color.r = 0.5 marker_2.color.a = 1.0 marker_2.id = 1 marker_2.pose.position.x = point_2[0] marker_2.pose.position.y = point_2[1] marker_2.pose.position.z = point_2[2] marker_3 = Marker() marker_3.header.frame_id = "world" marker_3.header.stamp = rospy.Time.now() marker_3.type = Marker.TEXT_VIEW_FACING marker_3.scale.z = 0.10 marker_3.lifetime = rospy.Duration() marker_4 = deepcopy(marker_3) marker_3.text = text1 marker_3.id = 2 marker_3.color.g = 0.5 marker_3.color.a = 1.0 marker_3.pose.position.x = point[0] marker_3.pose.position.y = point[1] marker_3.pose.position.z = point[2] + 0.15 marker_4.text = text2 marker_4.id = 3 marker_4.color.r = 0.5 marker_4.color.a = 1.0 marker_4.pose.position.x = point_2[0] marker_4.pose.position.y = point_2[1] marker_4.pose.position.z = point_2[2] + 0.15 marker_array.markers = [marker_1, marker_2, marker_3, marker_4] self._marker_pub.publish(marker_array) rospy.sleep(1) def _plan_joints(self, joints, test_name): # plan to joint target and determine success self.group.clear_pose_targets() group_variable_values = self.group.get_current_joint_values() group_variable_values[0:6] = joints[0:6] self.group.set_joint_value_target(group_variable_values) plan = self.group.plan() plan_time = "N/A" total_joint_rotation = "N/A" comp_time = "N/A" plan_success = self._check_plan_success(plan) if plan_success: plan_time = self._check_plan_time(plan) total_joint_rotation = self._check_plan_total_rotation(plan) while not self._comp_time: rospy.sleep(0.5) comp_time = self._comp_time.pop(0) self.planner_data.append([ self.planner_id, test_name, str(plan_success), plan_time, total_joint_rotation, comp_time ]) @staticmethod def _check_plan_success(plan): if len(plan.joint_trajectory.points) > 0: return True else: return False @staticmethod def _check_plan_time(plan): # find duration of successful plan number_of_points = len(plan.joint_trajectory.points) time = plan.joint_trajectory.points[number_of_points - 1].time_from_start.to_sec() return time @staticmethod def _check_plan_total_rotation(plan): # find total joint rotation in successful trajectory angles = [0, 0, 0, 0, 0, 0] number_of_points = len(plan.joint_trajectory.points) for i in range(number_of_points - 1): angles_temp = [ abs(x - y) for x, y in zip(plan.joint_trajectory.points[i + 1].positions, plan.joint_trajectory.points[i].positions) ] angles = [x + y for x, y in zip(angles, angles_temp)] total_angle_change = sum(angles) return total_angle_change def _check_computation_time(self, msg): # get computation time for successful plan to be found if msg.status.status == 3: self._comp_time.append(msg.result.planning_time) def _check_path_length(self, plan): # find distance travelled by end effector # not yet implemented return
class DaArmServer: """The basic, design problem/tui agnostic arm server """ gestures = {} grasp_height = 0.1 drop_height = 0.2 moving = False paused = False move_group_state = "IDLE" def __init__(self, num_planning_attempts=20): rospy.init_node("daarm_server", anonymous=True) self.init_params() self.init_scene() self.init_publishers() self.init_subscribers() self.init_action_clients() self.init_service_clients() self.init_arm(num_planning_attempts) def init_arm(self, num_planning_attempts=20): self.arm = MoveGroupCommander("arm") self.gripper = MoveGroupCommander("gripper") self.robot = RobotCommander() self.arm.set_num_planning_attempts(num_planning_attempts) self.arm.set_goal_tolerance(0.2) self.init_services() self.init_action_servers() def init_scene(self): world_objects = ["table", "tui", "monitor", "overhead", "wall"] self.robot = RobotCommander() self.scene = PlanningSceneInterface() for obj in world_objects: self.scene.remove_world_object(obj) rospy.sleep(0.5) self.tuiPose = PoseStamped() self.tuiPose.header.frame_id = self.robot.get_planning_frame() self.wallPose = PoseStamped() self.wallPose.header.frame_id = self.robot.get_planning_frame() self.tuiPose.pose.position = Point(0.3556, -0.343, -0.51) self.tuiDimension = (0.9906, 0.8382, 0.8636) self.wallPose.pose.position = Point(-0.508, -0.343, -0.3048) self.wallDimension = (0.6096, 2, 1.35) rospy.sleep(0.5) self.scene.add_box("tui", self.tuiPose, self.tuiDimension) self.scene.add_box("wall", self.wallPose, self.wallDimension) def init_params(self): try: self.grasp_height = get_ros_param("GRASP_HEIGHT", "Grasp height defaulting to 0.1") self.drop_height = get_ros_param("DROP_HEIGHT", "Drop height defaulting to 0.2") except ValueError as e: rospy.loginfo(e) def init_publishers(self): self.calibration_publisher = rospy.Publisher("/calibration_results", CalibrationParams) self.action_belief_publisher = rospy.Publisher("/arm_action_beliefs", String, queue_size=1) rospy.sleep(0.5) def init_subscribers(self): self.joint_angle_subscriber = rospy.Subscriber( '/j2s7s300_driver/out/joint_angles', JointAngles, self.update_joints) self.move_it_feedback_subscriber = rospy.Subscriber( '/move_group/feedback', MoveGroupActionFeedback, self.update_move_group_state) def init_action_servers(self): self.calibration_server = actionlib.SimpleActionServer("calibrate_arm", CalibrateAction, self.calibrate) self.move_block_server = actionlib.SimpleActionServer("move_block", MoveBlockAction, self.handle_move_block) #self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm) def init_services(self): self.home_arm_service = rospy.Service("/home_arm", ArmCommand, self.handle_home_arm) # emergency stop self.stop_arm_service = rospy.Service("/stop_arm", ArmCommand, self.handle_stop_arm) # stop and pause for a bit self.pause_arm_service = rospy.Service("/pause_arm", ArmCommand, self.handle_pause_arm) self.start_arm_service = rospy.Service("/restart_arm", ArmCommand, self.handle_restart_arm) def init_action_clients(self): # Action Client for joint control joint_action_address = '/j2s7s300_driver/joints_action/joint_angles' self.joint_action_client = actionlib.SimpleActionClient( joint_action_address, kinova_msgs.msg.ArmJointAnglesAction) rospy.loginfo('Waiting for ArmJointAnglesAction server...') self.joint_action_client.wait_for_server() rospy.loginfo('ArmJointAnglesAction Server Connected') # Service to move the gripper fingers finger_action_address = '/j2s7s300_driver/fingers_action/finger_positions' self.finger_action_client = actionlib.SimpleActionClient( finger_action_address, kinova_msgs.msg.SetFingersPositionAction) self.finger_action_client.wait_for_server() def init_service_clients(self): self.is_simulation = None try: self.is_simulation = get_ros_param("IS_SIMULATION", "") except: self.is_simulation = False if self.is_simulation is True: # setup alternatives to jaco services for emergency stop, joint control, and finger control pass # Service to get TUI State rospy.wait_for_service('get_tui_blocks') self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState) # Service for homing the arm home_arm_service = '/j2s7s300_driver/in/home_arm' self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm) rospy.loginfo('Waiting for kinova home arm service') rospy.wait_for_service(home_arm_service) rospy.loginfo('Kinova home arm service server connected') # Service for emergency stop emergency_service = '/j2s7s300_driver/in/stop' self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop) rospy.loginfo('Waiting for Stop service') rospy.wait_for_service(emergency_service) rospy.loginfo('Stop service server connected') # Service for restarting the arm start_service = '/j2s7s300_driver/in/start' self.restart_arm = rospy.ServiceProxy(start_service, Start) rospy.loginfo('Waiting for Start service') rospy.wait_for_service(start_service) rospy.loginfo('Start service server connected') def handle_start_arm(self, message): return self.restart_arm() def handle_stop_arm(self, message): return self.stop_motion() def handle_pause_arm(self, message): self.stop_motion(home=True, pause=True) return str(self.paused) def handle_restart_arm(self, message): self.restart_arm() self.paused = False return str(self.paused) def handle_home_arm(self, message): try: status = self.home_arm_kinova() return json.dumps(status) except rospy.ServiceException as e: rospy.loginfo("Homing arm failed") def home_arm(self): # send the arm home # for now, let's just use the kinova home self.home_arm_client() def home_arm_kinova(self): """Takes the arm to the kinova default home if possible """ self.arm.set_named_target("Home") try: self.arm.go() return "successful home" except: return "failed to home" def move_fingers(self, finger1_pct, finger2_pct, finger3_pct): finger_max_turn = 6800 goal = kinova_msgs.msg.SetFingersPositionGoal() goal.fingers.finger1 = float((finger1_pct/100.0)*finger_max_turn) goal.fingers.finger2 = float((finger2_pct/100.0)*finger_max_turn) goal.fingers.finger3 = float((finger3_pct/100.0)*finger_max_turn) self.finger_action_client.send_goal(goal) if self.finger_action_client.wait_for_result(rospy.Duration(5.0)): return self.finger_action_client.get_result() else: self.finger_action_client.cancel_all_goals() rospy.loginfo('the gripper action timed-out') return None def move_joint_angles(self, angle_set): goal = kinova_msgs.msg.ArmJointAnglesGoal() goal.angles.joint1 = angle_set[0] goal.angles.joint2 = angle_set[1] goal.angles.joint3 = angle_set[2] goal.angles.joint4 = angle_set[3] goal.angles.joint5 = angle_set[4] goal.angles.joint6 = angle_set[5] goal.angles.joint7 = angle_set[6] self.joint_action_client.send_goal(goal) if self.joint_action_client.wait_for_result(rospy.Duration(20.0)): return self.joint_action_client.get_result() else: print(' the joint angle action timed-out') self.joint_action_client.cancel_all_goals() return None def handle_move_block(self, message): """msg format: {id: int, source: Point {x: float,y: float}, target: Point {x: float, y: float} """ def open_gripper(self, delay=0): """open the gripper ALL THE WAY, then delay """ if self.is_simulation is True: self.gripper.set_named_target("Open") self.gripper.go() else: self.move_fingers(50, 50, 50) rospy.sleep(delay) def close_gripper(self, delay=0): """close the gripper ALL THE WAY, then delay """ self.gripper.set_named_target("Close") self.gripper.go() rospy.sleep(delay) def handle_gesture(self, gesture): # lookup the gesture from a table? or how about a message? # one possibility, object that contains desired deltas in pos/orientation # as well as specify the arm or gripper choice pass def move_arm_to_pose(self, position, orientation, delay=0, waypoints=[], corrections=1, action_server=None): if len(waypoints) > 0: # this is good for doing gestures plan, fraction = self.arm.compute_cartesian_path(waypoints, eef_step=0.01, jump_threshold=0.0) else: p = self.arm.get_current_pose() p.pose.position = position p.pose.orientation = orientation self.arm.set_pose_target(p) plan = self.arm.plan() if plan: # get the last pose to correct if desired ptPos = plan.joint_trajectory.points[-1].positions # print "==================================" # print "Last point of the current trajectory: " angle_set = list() for i in range(len(ptPos)): tempPos = ptPos[i]*180/np.pi + int(round((self.joint_angles[i] - ptPos[i]*180/np.pi)/(360)))*360 angle_set.append(tempPos) if action_server: action_server.publish_feedback(CalibrateFeedback("Plan Found")) self.arm.execute(plan, wait=False) while self.move_group_state is not "IDLE": rospy.sleep(0.001) print(self.move_group_state) if self.paused is True: self.arm.stop() return rospy.loginfo("LEAVING THE WHILE LOOP") print("LEAVING THE LOOOOOOOOOP!!!!") if corrections > 0: rospy.loginfo("Correcting the pose") self.move_joint_angles(angle_set) rospy.sleep(delay) else: if action_server: action_server.publish_feedback(CalibrateFeedback("Planning Failed")) # Let's have the caller handle sequences instead. # def handle_move_sequence(self, message): # # if the move fails, do we cancel the sequence # cancellable = message.cancellable # moves = message.moves # for move in moves: # try: # self.handle_move_block(move) # except Exception: # if cancellable: # rospy.loginfo("Part of move failed, cancelling the rest.") # break def update_move_group_state(self, message): rospy.loginfo(message.feedback.state) self.move_group_state = message.feedback.state def get_grasp_orientation(self, position): return Quaternion(1, 0, 0, 0) def update_joints(self, joints): self.joint_angles = [joints.joint1, joints.joint2, joints.joint3, joints.joint4, joints.joint5, joints.joint6, joints.joint7] def move_z(self, distance): p = self.arm.get_current_pose() p.pose.position.z += distance self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0) def pick_block(self, location, check_grasp=False, retry_attempts=0, delay=0, action_server=None): # go to a spot and pick up a block # if check_grasp is true, it will check torque on gripper and retry or fail if not holding # open the gripper self.open_gripper() position = Point(location.x, location.y, self.grasp_height) orientation = self.get_grasp_orientation(position) try: self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) except Exception as e: raise(e) if action_server: action_server.publish_feedback() if check_grasp is True: pass # for now, but we want to check finger torques # for i in range(retry_attempts): # self.move_z(0.3) self.close_gripper() self.move_z(0.1) rospy.sleep(delay) def place_block(self, location, check_grasp=False, delay=0, action_server=None): # go to a spot an drop a block # if check_grasp is true, it will check torque on gripper and fail if not holding a block position = Point(location.x, location.y, self.drop_height) orientation = self.get_grasp_orientation(position) try: self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) except Exception as e: raise(e) if action_server: action_server.publish_feedback() self.open_gripper() self.move_z(0.1) self.close_gripper() def stop_motion(self, home=False, pause=False): rospy.loginfo("STOPPING the ARM") # cancel the moveit_trajectory # self.arm.stop() # call the emergency stop on kinova # self.emergency_stop() # rospy.sleep(0.5) if pause is True: self.paused = True if home is True: # self.restart_arm() self.home_arm() def calibrate(self, message): print("calibrating ", message) self.place_calibration_block() rospy.sleep(5) # five seconds to correct the drop if it bounced, etc. self.calibration_server.publish_feedback(CalibrateFeedback("getting the coordinates")) params = self.record_calibration_params() self.set_arm_calibration_params(params[0], params[1]) self.calibration_publisher.publish(CalibrationParams(params[0], params[1])) self.calibration_server.set_succeeded(CalibrateResult(params)) def set_arm_calibration_params(self, arm_x_min, arm_y_min): rospy.set_param("ARM_X_MIN", arm_x_min) rospy.set_param("ARM_Y_MIN", arm_y_min) def place_calibration_block(self): # start by homing the arm (kinova home) self.calibration_server.publish_feedback(CalibrateFeedback("homing")) self.home_arm_kinova() # go to grab a block from a human self.open_gripper() self.close_gripper() self.open_gripper(4) self.close_gripper(2) # move to a predetermined spot self.calibration_server.publish_feedback(CalibrateFeedback("moving to drop")) self.move_arm_to_pose(Point(0.4, -0.4, 0.1), Quaternion(0, 1, 0, 0)) # drop the block self.open_gripper() self.calibration_server.publish_feedback(CalibrateFeedback("dropped the block")) calibration_block = {'x': 0.4, 'y': 0.4, 'id': 0} calibration_action_belief = {"action": "add", "block": calibration_block} self.action_belief_publisher.publish(String(json.dumps(calibration_action_belief))) rospy.loginfo("published arm belief") return def record_calibration_params(self): """ Call the block_tracker service to get the current table config. Find the calibration block and set the appropriate params. """ # make sure we know the dimensions of the table before we start # fixed table dimensions include tuio_min_x,tuio_min_y,tuio_dist_x,tuio_dist_y,arm_dist_x,arm_dist_y tuio_bounds = get_tuio_bounds() arm_bounds = get_arm_bounds(calibrate=False) try: block_state = json.loads(self.get_block_state("tuio").tui_state) except rospy.ServiceException as e: # self.calibration_server.set_aborted() raise(ValueError("Failed getting block state to calibrate: "+str(e))) if len(block_state) != 1: # self.calibration_server.set_aborted() raise(ValueError("Failed calibration, either couldn't find block or > 1 block on TUI!")) # if we made it here, let's continue! # start by comparing the reported tuio coords where we dropped the block # with the arm's localization of the end-effector that dropped it # (we assume that the block fell straight below the drop point) drop_pose = self.arm.get_current_pose() end_effector_x = drop_pose.pose.position.x end_effector_y = drop_pose.pose.position.y block_tuio_x = block_state[0]['x'] block_tuio_y = block_state[0]['y'] x_ratio, y_ratio = tuio_to_ratio(block_tuio_x, block_tuio_y, tuio_bounds) arm_x_min = end_effector_x - x_ratio*arm_bounds["x_dist"] arm_y_min = end_effector_y - y_ratio*arm_bounds["y_dist"] return [arm_x_min, arm_y_min]
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) rospy.Subscriber('camera/depth_registered/points', PointCloud2, point_callback) rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes, bbox_callback) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.ii = 0 global goal_x global goal_y global goal_z # self.listener = tf.TransformListener() #self.goal_x = 0 #self.goal_y = 0 #self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 # self.marker = [] self.tomato = [] self.position_list = [] self.orientation_list = [] self.t_idd = 0 self.t_pose_x = [] self.t_pose_y = [] self.t_pose_z = [] self.t_ori_w = [] self.t_ori_x = [] self.t_ori_y = [] self.t_ori_z = [] self.tomato_pose = Pose() self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print("====== current pose : ", self.wpose) joint_goal = [ -0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982 ] print("INIT POSE: ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait=True) def go_to_move(self, scale=1.0): planning_frame = self.robot_arm.get_planning_frame() tomato_offset = [0, -0.35, -0.1] robot_base_offset = 0.873 base_wrist2_offset = 0.1 print(">> robot arm planning frame: \n", planning_frame) ''' self.calculated_tomato.position.x = (scale*self.goal_x) self.calculated_tomato.position.y = (scale*self.goal_y) self.calculated_tomato.position.z = (scale*self.goal_z) ''' self.calculated_tomato.position.x = (scale * goal_x) self.calculated_tomato.position.y = (scale * goal_y) self.calculated_tomato.position.z = (scale * goal_z) self.calculated_tomato.orientation = Quaternion( *quaternion_from_euler(3.14, 0.1, 1.57)) print("=== robot_pose goal frame: ", self.calculated_tomato) self.robot_arm.set_pose_target(self.calculated_tomato) tf_display_position = [ self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z ] tf_display_orientation = [ self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() tomato_waypoints = [] tomato_waypoints.append(copy.deepcopy(self.calculated_tomato)) (tomato_plan, tomato_offset) = self.robot_arm.compute_cartesian_path( tomato_waypoints, 0.01, 0.0) self.display_trajectory(tomato_plan) print("=== Press `Enter` to if plan is correct!! ...") raw_input() self.robot_arm.go(True) def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch, yaw): ''' Manipulator is moving to the desired coordinate Now move to the calculated_tomato_id_goal ''' calculated_tomato_id_goal = Pose() desired_goal_pose = [pose_x, pose_y, pose_z] desired_goal_euler = [roll, pitch, yaw] Cplanning_frame = self.robot_arm.get_planning_frame() print(">> current planning frame: \n", Cplanning_frame) calculated_tomato_id_goal.position.x = desired_goal_pose[0] calculated_tomato_id_goal.position.y = desired_goal_pose[1] calculated_tomato_id_goal.position.z = desired_goal_pose[2] calculated_tomato_id_goal.orientation = Quaternion( *quaternion_from_euler(desired_goal_euler[0], desired_goal_euler[1], desired_goal_euler[2])) print(">> tomato_id_goal frame: ", desired_goal_pose) self.robot_arm.set_pose_target(calculated_tomato_id_goal) tf_display_position = [ calculated_tomato_id_goal.position.x, calculated_tomato_id_goal.position.x, calculated_tomato_id_goal.position.z ] tf_display_orientation = [ calculated_tomato_id_goal.orientation.x, calculated_tomato_id_goal.orientation.y, calculated_tomato_id_goal.orientation.z, calculated_tomato_id_goal.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz tomato_id_goal_waypoints = [] tomato_id_goal_waypoints.append( copy.deepcopy(calculated_tomato_id_goal)) (tomato_id_goal_plan, tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path( tomato_id_goal_waypoints, 0.01, 0.0) self.display_trajectory(tomato_id_goal_plan) print("============ Press `Enter` to if plan is correct!! ...") raw_input() self.robot_arm.go(True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # - 0.10 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset waypoints.append(copy.deepcopy(self.wpose)) ii += 1 (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_x(self, x_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_y(self, y_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_z(self, z_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def pose_subscriber(self): rospy.loginfo("waiting for pose topic...") rospy.Subscriber('camera/depth_registered/points', PointCloud2, point_callback) rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes, bbox_callback)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move',anonymous=True) rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) #self.listener = tf.TransformListener() self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.marker = [] self.position_list = [] self.orientation_list = [] self.m_idd = 0 self.m_pose_x = [] self.m_pose_y = [] self.m_pose_z = [] self.m_ori_w = [] self.m_ori_x = [] self.m_ori_y = [] self.m_ori_z = [] self.ar_pose = Pose() self.goalPoseFromAR = Pose() self.br = tf.TransformBroadcaster() #self.goalPose_from_arPose = Pose() self.trans = [] self.rot = [] self.calculed_coke_pose = Pose() #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): self.clear_octomap() planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print"====== current pose : ", self.wpose marker_joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) def look_object(self): look_object = self.robot_arm.get_current_joint_values() ## wrist3 현재 상태가 0도 인지, 360도인지에 따라 90도의 움직임을 -로 할지, + 할지 고려함 print "wrist3 joint value(deg, rad): ", math.degrees(look_object[5]), look_object[5] #look_object[5] = math.radians(90) look_object[5] = 0 ''' if look_object[5] > abs(math.radians(180)): if look_object[5] < 0: look_object[5] = look_object[5] + math.radians(90) # wrist3 elif look_object[5] > 0: look_object[5] = look_object[5] - math.radians(90) else: look_object[5] = look_object[5] - math.radians(90) # wrist3 ''' print "wrist3 joint value(deg, rad): ", math.degrees(look_object[5]), look_object[5] #look_object[3] = look_object[3] - (math.radians(00)) # wrist1 self.robot_arm.go(look_object, wait=True) #look_object[5] = look_object[5] + (math.radians(90)) # wrist3 #self.robot_arm.go(look_object, wait=True) def look_up_down(self): self.clear_octomap() print "======== clear_octomap... Please wait...." look_up_down = self.robot_arm.get_current_joint_values() #print "before: ", look_up_down look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory); def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose print "===== robot arm pose: ", self.wpose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10 #print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # waypoints.append(copy.deepcopy(self.wpose)) ii += 1 print "waypoints:", waypoints (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def print_ar_pose(self): rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") print "======= pos(meter): ", self.position_list print "======= orientation: ", self.orientation_list def go_to_move(self, scale = 1.0): # 로봇 팔: 마커 밑에 있는 물체 쪽으로 움직이기 #self.calculed_coke_pose = self.robot_arm.get_current_pose() planning_frame = self.robot_arm.get_planning_frame() coke_offset = [0, -0.35, -0.1] #x y z # gazebo_coke_offset = [0, -0.2875, -0.23] gazebo 에서의 마커와 코크 캔의 offset, 바로 명령하면 해를 못 품. # linear offset = abs([0, 0.0625, 0.13]) robot_base_offset = 0.873 base_wrist2_offset = 0.1 #for avoiding link contact error print "========== robot arm plannig frame: \n", planning_frame self.calculed_coke_pose.position.x = (scale * self.goal_x) + base_wrist2_offset # base_link to wrist2 x-offset self.calculed_coke_pose.position.y = (scale * self.goal_y) + coke_offset[1] #self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 + coke_offset# world to base_link z-offset self.calculed_coke_pose.position.z = (scale * self.goal_z) + robot_base_offset # world to base_link z-offset and coke can offset self.calculed_coke_pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.54)) print "========== coke_pose goal frame: ", self.calculed_coke_pose self.robot_arm.set_pose_target(self.calculed_coke_pose) tf_display_position = [self.calculed_coke_pose.position.x, self.calculed_coke_pose.position.y, self.calculed_coke_pose.position.z] tf_display_orientation = [self.calculed_coke_pose.orientation.x, self.calculed_coke_pose.orientation.y, self.calculed_coke_pose.orientation.z, self.calculed_coke_pose.orientation.w] ii = 0 while ii < 5: ii += 1 self.br.sendTransform( tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz coke_waypoints = [] coke_waypoints.append(copy.deepcopy(self.calculed_coke_pose)) (coke_plan, coke_fraction) = self.robot_arm.compute_cartesian_path(coke_waypoints, 0.01, 0.0) self.display_trajectory(coke_plan) ## ## ## print "============ Press `Enter` to if plan is correct!! ..." raw_input() self.robot_arm.go(True) def ar_tf_listener(self, msg): try: self.marker = msg.markers ml = len(self.marker) target_id = 9 #self.m_idd = self.marker[0].id # 임시용 for ii in range(0, ml): # 0 <= ii < ml self.m_idd = self.marker[ii].id #print "checked all id: ", self.m_idd if self.m_idd != target_id: pass #target_id_flage = False elif self.m_idd == target_id: target_id_flage = True target_id = self.m_idd target_id_index = ii #print "target id: ", target_id_index, target_id, target_id_flage if target_id_flage == True: self.ar_pose.position.x = self.marker[target_id_index].pose.pose.position.x self.ar_pose.position.y = self.marker[target_id_index].pose.pose.position.y self.ar_pose.position.z = self.marker[target_id_index].pose.pose.position.z self.ar_pose.orientation.x = self.marker[target_id_index].pose.pose.orientation.x self.ar_pose.orientation.y = self.marker[target_id_index].pose.pose.orientation.y self.ar_pose.orientation.z = self.marker[target_id_index].pose.pose.orientation.z self.ar_pose.orientation.w = self.marker[target_id_index].pose.pose.orientation.w self.goal_x = self.ar_pose.position.x self.goal_y = self.ar_pose.position.y self.goal_z = self.ar_pose.position.z self.position_list = [self.goal_x, self.goal_y, self.goal_z] self.orientation_list = [self.ar_pose.orientation.x, self.ar_pose.orientation.y, self.ar_pose.orientation.z, self.ar_pose.orientation.w] (self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion(self.orientation_list) #list form으로 넘겨주어야 함 #print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z #print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw #print "ar_pos(meter): \n", self.position_list #print "ar_orientation: \n", self.orientation_list except: return
class SrRobotCommander(object): """ Base class for hand and arm commanders """ def __init__(self, name): """ Initialize MoveGroupCommander object @param name - name of the MoveIt group """ self._name = name self._move_group_commander = MoveGroupCommander(name) self._robot_commander = RobotCommander() self._robot_name = self._robot_commander._r.get_robot_name() self.refresh_named_targets() self._warehouse_name_get_srv = rospy.ServiceProxy( "get_robot_state", GetState) self._planning_scene = PlanningSceneInterface() self._joint_states_lock = threading.Lock() self._joint_states_listener = \ rospy.Subscriber("joint_states", JointState, self._joint_states_callback, queue_size=1) self._joints_position = {} self._joints_velocity = {} self._joints_effort = {} self._joints_state = None self._clients = {} self.__plan = None self._controllers = {} rospy.wait_for_service('compute_ik') self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK) controller_list_param = rospy.get_param("/move_group/controller_list") # create dictionary with name of controllers and corresponding joints self._controllers = { item["name"]: item["joints"] for item in controller_list_param } self._set_up_action_client(self._controllers) self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer) threading.Thread(None, rospy.spin) def set_planner_id(self, planner_id): self._move_group_commander.set_planner_id(planner_id) def set_num_planning_attempts(self, num_planning_attempts): self._move_group_commander.set_num_planning_attempts( num_planning_attempts) def set_planning_time(self, seconds): self._move_group_commander.set_planning_time(seconds) def get_end_effector_pose_from_named_state(self, name): state = self._warehouse_name_get_srv(name, self._robot_name).state return self.get_end_effector_pose_from_state(state) def get_end_effector_pose_from_state(self, state): header = Header() fk_link_names = [self._move_group_commander.get_end_effector_link()] header.frame_id = self._move_group_commander.get_pose_reference_frame() response = self._forward_k(header, fk_link_names, state) return response.pose_stamped[0] def get_planning_frame(self): return self._move_group_commander.get_planning_frame() def set_pose_reference_frame(self, reference_frame): self._move_group_commander.set_pose_reference_frame(reference_frame) def get_group_name(self): return self._name def refresh_named_targets(self): self._srdf_names = self.__get_srdf_names() self._warehouse_names = self.__get_warehouse_names() def set_max_velocity_scaling_factor(self, value): self._move_group_commander.set_max_velocity_scaling_factor(value) def set_max_acceleration_scaling_factor(self, value): self._move_group_commander.set_max_acceleration_scaling_factor(value) def allow_looking(self, value): self._move_group_commander.allow_looking(value) def allow_replanning(self, value): self._move_group_commander.allow_replanning(value) def execute(self): """ Executes the last plan made. """ if self.check_plan_is_valid(): self._move_group_commander.execute(self.__plan) self.__plan = None else: rospy.logwarn("No plans were made, not executing anything.") def execute_plan(self, plan): if self.check_given_plan_is_valid(plan): self._move_group_commander.execute(plan) self.__plan = None else: rospy.logwarn("Plan is not valid, not executing anything.") def move_to_joint_value_target(self, joint_states, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self._move_group_commander.go(wait=wait) def plan_to_joint_value_target(self, joint_states, angle_degrees=False): """ Set target of the robot's links and plans. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param angle_degrees - are joint_states in degrees or not This is a blocking method. """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self.__plan = self._move_group_commander.plan() return self.__plan def check_plan_is_valid(self): """ Checks if current plan contains a valid trajectory """ return (self.__plan is not None and len(self.__plan.joint_trajectory.points) > 0) def check_given_plan_is_valid(self, plan): """ Checks if given plan contains a valid trajectory """ return (plan is not None and len(plan.joint_trajectory.points) > 0) def get_robot_name(self): return self._robot_name def named_target_in_srdf(self, name): return name in self._srdf_names def set_named_target(self, name): if name in self._srdf_names: self._move_group_commander.set_named_target(name) elif (name in self._warehouse_names): response = self._warehouse_name_get_srv(name, self._robot_name) active_names = self._move_group_commander._g.get_active_joints() joints = response.state.joint_state.name positions = response.state.joint_state.position js = {} for n, this_name in enumerate(joints): if this_name in active_names: js[this_name] = positions[n] self._move_group_commander.set_joint_value_target(js) else: rospy.logerr("Unknown named state '%s'..." % name) return False return True def get_named_target_joint_values(self, name): output = dict() if (name in self._srdf_names): output = self._move_group_commander.\ _g.get_named_target_values(str(name)) elif (name in self._warehouse_names): js = self._warehouse_name_get_srv( name, self._robot_name).state.joint_state for x, n in enumerate(js.name): if n in self._move_group_commander._g.get_joints(): output[n] = js.position[x] else: rospy.logerr("No target named %s" % name) return None return output def get_end_effector_link(self): return self._move_group_commander.get_end_effector_link() def get_current_pose(self, reference_frame=None): """ Get the current pose of the end effector. @param reference_frame - The desired reference frame in which end effector pose should be returned. If none is passed, it will use the planning frame as reference. @return geometry_msgs.msg.Pose() - current pose of the end effector """ if reference_frame is not None: try: trans = self.tf_buffer.lookup_transform( reference_frame, self._move_group_commander.get_end_effector_link(), rospy.Time(0), rospy.Duration(5.0)) current_pose = geometry_msgs.msg.Pose() current_pose.position.x = trans.transform.translation.x current_pose.position.y = trans.transform.translation.y current_pose.position.z = trans.transform.translation.z current_pose.orientation.x = trans.transform.rotation.x current_pose.orientation.y = trans.transform.rotation.y current_pose.orientation.z = trans.transform.rotation.z current_pose.orientation.w = trans.transform.rotation.w return current_pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logwarn( "Couldn't get the pose from " + self._move_group_commander.get_end_effector_link() + " in " + reference_frame + " reference frame") return None else: return self._move_group_commander.get_current_pose().pose def get_current_state(self): """ Get the current joint state of the group being used. @return a dictionary with the joint names as keys and current joint values """ joint_names = self._move_group_commander._g.get_active_joints() joint_values = self._move_group_commander._g.get_current_joint_values() return dict(zip(joint_names, joint_values)) def get_current_state_bounded(self): """ Get the current joint state of the group being used, enforcing that they are within each joint limits. @return a dictionary with the joint names as keys and current joint values """ current = self._move_group_commander._g.get_current_state_bounded() names = self._move_group_commander._g.get_active_joints() output = {n: current[n] for n in names if n in current} return output def get_robot_state_bounded(self): return self._move_group_commander._g.get_current_state_bounded() def move_to_named_target(self, name, wait=True): """ Set target of the robot's links and moves to it @param name - name of the target pose defined in SRDF @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self._move_group_commander.go(wait=wait) def plan_to_named_target(self, name): """ Set target of the robot's links and plans This is a blocking method. @param name - name of the target pose defined in SRDF """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self.__plan = self._move_group_commander.plan() def __get_warehouse_names(self): try: list_srv = rospy.ServiceProxy("list_robot_states", ListStates) return list_srv("", self._robot_name).states except rospy.ServiceException as exc: rospy.logwarn("Couldn't access warehouse: " + str(exc)) return list() def _reset_plan(self): self.__plan = None def _set_plan(self, plan): self.__plan = plan def __get_srdf_names(self): return self._move_group_commander._g.get_named_targets() def get_named_targets(self): """ Get the complete list of named targets, from SRDF as well as warehouse poses if available. @return list of strings containing names of targets. """ return self._srdf_names + self._warehouse_names def get_joints_position(self): """ Returns joints position @return - dictionary with joints positions """ with self._joint_states_lock: return self._joints_position def get_joints_velocity(self): """ Returns joints velocities @return - dictionary with joints velocities """ with self._joint_states_lock: return self._joints_velocity def _get_joints_effort(self): """ Returns joints effort @return - dictionary with joints efforts """ with self._joint_states_lock: return self._joints_effort def get_joints_state(self): """ Returns joints state @return - JointState message """ with self._joint_states_lock: return self._joints_state def run_joint_trajectory(self, joint_trajectory): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. """ plan = RobotTrajectory() plan.joint_trajectory = joint_trajectory self._move_group_commander.execute(plan) def make_named_trajectory(self, trajectory): """ Makes joint value trajectory from specified by named poses (either from SRDF or from warehouse) @param trajectory - list of waypoints, each waypoint is a dict with the following elements (n.b either name or joint_angles is required) - name -> the name of the way point - joint_angles -> a dict of joint names and angles - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp - degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent. """ current = self.get_current_state_bounded() joint_trajectory = JointTrajectory() joint_names = current.keys() joint_trajectory.joint_names = joint_names start = JointTrajectoryPoint() start.positions = current.values() start.time_from_start = rospy.Duration.from_sec(0.001) joint_trajectory.points.append(start) time_from_start = 0.0 for wp in trajectory: joint_positions = None if 'name' in wp.keys(): joint_positions = self.get_named_target_joint_values( wp['name']) elif 'joint_angles' in wp.keys(): joint_positions = copy.deepcopy(wp['joint_angles']) if 'degrees' in wp.keys() and wp['degrees']: for joint, angle in joint_positions.iteritems(): joint_positions[joint] = radians(angle) if joint_positions is None: rospy.logerr( "Invalid waypoint. Must contain valid name for named target or dict of joint angles." ) return None new_positions = {} for n in joint_names: new_positions[n] = joint_positions[ n] if n in joint_positions else current[n] trajectory_point = JointTrajectoryPoint() trajectory_point.positions = [ new_positions[n] for n in joint_names ] current = new_positions time_from_start += wp['interpolate_time'] trajectory_point.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(trajectory_point) if 'pause_time' in wp and wp['pause_time'] > 0: extra = JointTrajectoryPoint() extra.positions = trajectory_point.positions time_from_start += wp['pause_time'] extra.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(extra) return joint_trajectory def send_stop_trajectory_unsafe(self): """ Sends a trajectory of all active joints at their current position. This stops the robot. """ current = self.get_current_state_bounded() trajectory_point = JointTrajectoryPoint() trajectory_point.positions = current.values() trajectory_point.time_from_start = rospy.Duration.from_sec(0.1) trajectory = JointTrajectory() trajectory.points.append(trajectory_point) trajectory.joint_names = current.keys() self.run_joint_trajectory_unsafe(trajectory) def run_named_trajectory_unsafe(self, trajectory, wait=False): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory directly via contoller. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory_unsafe(joint_trajectory, wait) def run_named_trajectory(self, trajectory): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory via moveit. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory(joint_trajectory) def move_to_position_target(self, xyz, end_effector_link="", wait=True): """ Specify a target position for the end-effector and moves to it @param xyz - new position of end-effector @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_position_target(self, xyz, end_effector_link=""): """ Specify a target position for the end-effector and plans. This is a blocking method. @param xyz - new position of end-effector @param end_effector_link - name of the end effector link """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self.__plan = self._move_group_commander.plan() def move_to_pose_target(self, pose, end_effector_link="", wait=True): """ Specify a target pose for the end-effector and moves to it @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_pose_target(pose, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_pose_target(self, pose, end_effector_link="", alternative_method=False): """ Specify a target pose for the end-effector and plans. This is a blocking method. @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param alternative_method - use set_joint_value_target instead of set_pose_target """ self._move_group_commander.set_start_state_to_current_state() if alternative_method: self._move_group_commander.set_joint_value_target( pose, end_effector_link) else: self._move_group_commander.set_pose_target(pose, end_effector_link) self.__plan = self._move_group_commander.plan() return self.__plan def _joint_states_callback(self, joint_state): """ The callback function for the topic joint_states. It will store the received joint position, velocity and efforts information into dictionaries @param joint_state - the message containing the joints data. """ with self._joint_states_lock: self._joints_state = joint_state self._joints_position = { n: p for n, p in zip(joint_state.name, joint_state.position) } self._joints_velocity = { n: v for n, v in zip(joint_state.name, joint_state.velocity) } self._joints_effort = { n: v for n, v in zip(joint_state.name, joint_state.effort) } def _set_up_action_client(self, controller_list): """ Sets up an action client to communicate with the trajectory controller """ self._action_running = {} for controller_name in controller_list.keys(): self._action_running[controller_name] = False service_name = controller_name + "/follow_joint_trajectory" self._clients[controller_name] = SimpleActionClient( service_name, FollowJointTrajectoryAction) if self._clients[controller_name].wait_for_server( timeout=rospy.Duration(4)) is False: err_msg = 'Failed to connect to action server ({}) in 4 sec'.format( service_name) rospy.logwarn(err_msg) def move_to_joint_value_target_unsafe(self, joint_states, time=0.002, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param time - time in s (counting from now) for the robot to reach the target (it needs to be greater than 0.0 for it not to be rejected by the trajectory controller) @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ # self._update_default_trajectory() # self._set_targets_to_default_trajectory(joint_states) goals = {} joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [] point = JointTrajectoryPoint() point.positions = [] for x in joint_states_cpy.keys(): if x in controller_joints: goal.trajectory.joint_names.append(x) point.positions.append(joint_states_cpy[x]) point.time_from_start = rospy.Duration.from_sec(time) goal.trajectory.points = [point] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def action_is_running(self, controller=None): if controller is not None: return self._action_running[controller] for controller_running in self._action_running.values(): if controller_running: return True return False def _action_done_cb(self, controller, terminal_state, result): self._action_running[controller] = False def _call_action(self, goals): for client in self._clients: self._action_running[client] = True self._clients[client].send_goal( goals[client], lambda terminal_state, result: self._action_done_cb( client, terminal_state, result)) def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. @param wait - should method wait for movement end or not """ goals = {} for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory = copy.deepcopy(joint_trajectory) indices_of_joints_in_this_controller = [] for i, joint in enumerate(joint_trajectory.joint_names): if joint in controller_joints: indices_of_joints_in_this_controller.append(i) goal.trajectory.joint_names = [ joint_trajectory.joint_names[i] for i in indices_of_joints_in_this_controller ] for point in goal.trajectory.points: if point.positions: point.positions = [ point.positions[i] for i in indices_of_joints_in_this_controller ] if point.velocities: point.velocities = [ point.velocities[i] for i in indices_of_joints_in_this_controller ] if point.effort: point.effort = [ point.effort[i] for i in indices_of_joints_in_this_controller ] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def plan_to_waypoints_target(self, waypoints, reference_frame=None, eef_step=0.005, jump_threshold=0.0): """ Specify a set of waypoints for the end-effector and plans. This is a blocking method. @param reference_frame - the reference frame in which the waypoints are given @param waypoints - an array of poses of end-effector @param eef_step - configurations are computed for every eef_step meters @param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path """ old_frame = self._move_group_commander.get_pose_reference_frame() if reference_frame is not None: self.set_pose_reference_frame(reference_frame) (self.__plan, fraction) = self._move_group_commander.compute_cartesian_path( waypoints, eef_step, jump_threshold) self.set_pose_reference_frame(old_frame) def set_teach_mode(self, teach): """ Activates/deactivates the teach mode for the robot. Activation: stops the the trajectory controllers for the robot, and sets it to teach mode. Deactivation: stops the teach mode and starts trajectory controllers for the robot. Currently this method blocks for a few seconds when called on a hand, while the hand parameters are reloaded. @param teach - bool to activate or deactivate teach mode """ if teach: mode = RobotTeachModeRequest.TEACH_MODE else: mode = RobotTeachModeRequest.TRAJECTORY_MODE self.change_teach_mode(mode, self._name) def move_to_trajectory_start(self, trajectory, wait=True): """ Make and execute a plan from the current state to the first state in an pre-existing trajectory @param trajectory - moveit_msgs/JointTrajectory @param wait - Bool to specify if movement should block untill finished. """ if len(trajectory.points) <= 0: rospy.logerr("Trajectory has no points in it, can't reverse...") return None first_point = trajectory.points[0] end_state = dict(zip(trajectory.joint_names, first_point.positions)) self.move_to_joint_value_target(end_state, wait=wait) @staticmethod def change_teach_mode(mode, robot): teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode) req = RobotTeachModeRequest() req.teach_mode = mode req.robot = robot try: resp = teach_mode_client(req) if resp.result == RobotTeachModeResponse.ERROR: rospy.logerr("Failed to change robot %s to mode %d", robot, mode) else: rospy.loginfo("Changed robot %s to mode %d Result = %d", robot, mode, resp.result) except rospy.ServiceException: rospy.logerr("Failed to call service teach_mode") def get_ik(self, target_pose, avoid_collisions=False, joint_states=None): """ Computes the inverse kinematics for a given pose. It returns a JointState @param target_pose - A given pose of type PoseStamped @param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false """ service_request = PositionIKRequest() service_request.group_name = self._name service_request.ik_link_name = self._move_group_commander.get_end_effector_link( ) service_request.pose_stamped = target_pose service_request.timeout.secs = 0.5 service_request.avoid_collisions = avoid_collisions if joint_states is None: service_request.robot_state.joint_state = self.get_joints_state() else: service_request.robot_state.joint_state = joint_states try: resp = self._compute_ik(ik_request=service_request) # Check if error_code.val is SUCCESS=1 if resp.error_code.val != 1: if resp.error_code.val == -10: rospy.logerr("Unreachable point: Start state in collision") elif resp.error_code.val == -12: rospy.logerr("Unreachable point: Goal state in collision") elif resp.error_code.val == -31: rospy.logerr("Unreachable point: No IK solution") else: rospy.logerr("Unreachable point (error: %s)" % resp.error_code) return else: return resp.solution.joint_state except rospy.ServiceException, e: rospy.logerr("Service call failed: %s" % e)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.robot_arm.remember_joint_values("zeros", None) rospy.sleep(2) # Allow replanning to increase the odds of a solution #self.robot_arm.allow_replanning(True) #scene = moveit_commander.PlanningSceneInterface() #robot = moveit_commander.RobotCommander() #rospy.sleep(2) #add Table p = PoseStamped() p.header.frame_id = FIXED_FRAME p.pose.position.x = 0.0 p.pose.position.y = 0.2 p.pose.position.z = 0.1 self.scene.add_box("table", p, (0.3, 0.3, 0.3)) def move_code(self): self.robot_arm.set_named_target("home") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to home 1 ======") rospy.sleep(1) self.robot_arm.attach_object("table") pose_goal.orientation.w = 0.0 pose_goal.position.x = 0.4 # red line 0.2 0.2 pose_goal.position.y = 0.15 # green line 0.15 0.15 pose_goal.position.z = 0.2 # blue line # 0.35 0.6 self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) #if (input == 'w') #pose_goal.position.x += 0.1 # red line 0.2 0.2 print("====== move plan go to up ======") self.robot_arm.set_named_target("zeros") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to zeros ======") # rospy.sleep(1) #self.robot_arm.place("table", pose_goal) # robot_arm.set_named_target("up") # robot_arm.go(wait=True) robot_state = self.robot_arm.get_current_pose() robot_angle = self.robot_arm.get_current_joint_values() self.robot_arm.detach_object("table") pose_goal.orientation.w = 0.0 pose_goal.position.x = 0.2 # red line 0.2 0.2 pose_goal.position.y = 0.3 # green line 0.15 0.15 pose_goal.position.z = 0.1 # blue line # 0.35 0.6 self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) print(robot_state)