def append_traj_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None): """ Appends a trajectory_point to the given move_group goal, returns it appended""" if goal_to_append == None: rospy.logerr("append_trajectory_point_to_move_group_goal needs a goal!") return #goal_to_append = MoveGroupGoal() #rospy.logwarn("goal_to_append is: \n" + str(goal_to_append)) traj_c = TrajectoryConstraints() goal_c = Constraints() goal_c.name = "traj_constraint" # Position constraint position_c = PositionConstraint() position_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header position_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else link_name position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 2.0 goal_c.position_constraints.append(position_c) # Orientation constraint orientation_c = OrientationConstraint() orientation_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header orientation_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) traj_c.constraints.append(goal_c) goal_to_append.request.trajectory_constraints = traj_c return goal_to_append
def append_pose_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None): """ Appends a pose to the given move_group goal, returns it appended Goals for now are for the same link TODO: let it be for different links""" if goal_to_append == None: rospy.logerr("append_pose_to_move_group_goal needs a goal!") return goal_to_append = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = goal_to_append.request.goal_constraints[0].header position_c.link_name = goal_to_append.request.goal_constraints[0].link_name position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = goal_to_append.request.goal_constraints[0].header orientation_c.link_name = goal_to_append.request.goal_constraints[0].link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) goal_to_append.request.goal_constraints.append(goal_c) return goal_to_append
def create_move_group_pose_goal(self, goal_pose=Pose(), group="manipulator", end_link_name=None, plan_only=True): header = Header() header.frame_id = 'torso_base_link' header.stamp = rospy.Time.now() moveit_goal = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = header if end_link_name != None: position_c.link_name = end_link_name position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = header if end_link_name != None: orientation_c.link_name = end_link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) moveit_goal.request.goal_constraints.append(goal_c) moveit_goal.request.num_planning_attempts = 5 moveit_goal.request.allowed_planning_time = 5.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary moveit_goal.request.group_name = group return moveit_goal
def execute(self, userdata): # Prepare the position goal_pose = Pose() goal_pose.position = userdata.manip_goal_pose # Set the rotation of the tool link, all 0 means for the right hand palm looking left straight # roll = rotation over X, pitch = rotation over Y, yaw = rotation over Z quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw goal_pose.orientation = Quaternion(*quat.tolist()) header = Header() header.frame_id = 'base_link' header.stamp = rospy.Time.now() userdata.move_it_goal = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = header if userdata.manip_end_link != None: # For some groups the end_link_name can be deduced, but better add it manually position_c.link_name = userdata.manip_end_link # how big is the area where the end effector can be position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = header if userdata.manip_end_link != None: orientation_c.link_name = userdata.manip_end_link orientation_c.orientation = goal_pose.orientation # Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes orientation_c.absolute_x_axis_tolerance = 0.01 orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) userdata.move_it_goal.request.goal_constraints.append(goal_c) userdata.move_it_goal.request.num_planning_attempts = 5 # The number of times this plan is to be computed. Shortest solution will be reported. userdata.move_it_goal.request.allowed_planning_time = 5.0 userdata.move_it_goal.planning_options.plan_only = False # True: Plan-Only..... False : Plan + execute userdata.move_it_goal.planning_options.planning_scene_diff.is_diff = True # Necessary userdata.move_it_goal.request.group_name = userdata.manip_group return 'succeeded'
def create_move_group_pose_goal(goal_pose=Pose(), group="right_arm_torso", end_link_name=None, plan_only=True): """ Creates a move_group goal based on pose. @arg group string representing the move_group group to use @arg end_link_name string representing the ending link to use @arg goal_pose Pose() representing the goal pose @arg plan_only bool to for only planning or planning and executing @returns MoveGroupGoal with the data given on the arguments""" header = Header() header.frame_id = 'base_link' header.stamp = rospy.Time.now() # We are filling in the MoveGroupGoal a MotionPlanRequest and a PlanningOptions message # http://docs.ros.org/hydro/api/moveit_msgs/html/msg/MotionPlanRequest.html # http://docs.ros.org/hydro/api/moveit_msgs/html/msg/PlanningOptions.html moveit_goal = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = header if end_link_name != None: # For some groups the end_link_name can be deduced, but better add it manually position_c.link_name = end_link_name position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) # how big is the area where the end effector can be position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = header if end_link_name != None: orientation_c.link_name = end_link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 # Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) moveit_goal.request.goal_constraints.append(goal_c) moveit_goal.request.num_planning_attempts = 5 # The number of times this plan is to be computed. Shortest solution will be reported. moveit_goal.request.allowed_planning_time = 5.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary moveit_goal.request.group_name = group return moveit_goal
def create_move_group_pose_goal( self, goal_pose=Pose(), group="left_arm", end_link_name="arm_left_tool_link", plan_only=True ): """ Creates a move_group goal based on pose. @arg group string representing the move_group group to use @arg end_link_name string representing the ending link to use @arg goal_pose Pose() representing the goal pose""" # Specifying the header makes the planning fail... header = Header() header.frame_id = "base_link" header.stamp = rospy.Time.now() moveit_goal = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = header position_c.link_name = end_link_name position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = header orientation_c.link_name = end_link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) moveit_goal.request.goal_constraints.append(goal_c) moveit_goal.request.num_planning_attempts = 3 moveit_goal.request.allowed_planning_time = 1.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True moveit_goal.request.group_name = group return moveit_goal
def liftgripper(self, target_z=0.10): # approx centers of onions at 0.82, width of onion is 0.038 m. table is at 0.78 # length of gripper is 0.163 m The gripper should not go lower than # (height_z of table w.r.t base+gripper-height/2+tolerance) = 0.78-0.93+0.08+0.01=-0.24 # pnp._limb.endpoint_pose returns {'position': (x, y, z), 'orientation': (x, y, z, w)} # moving from z=-.02 to z=-0.1 group = self.group while self.target_location_x == -100: rospy.sleep(0.05) current_pose = group.get_current_pose().pose current_orientation = group.get_current_pose().pose.orientation curr_q = [ current_orientation.x, current_orientation.y, current_orientation.z, current_orientation.w ] position_constraint = PositionConstraint() position_constraint.target_point_offset.x = 0.1 position_constraint.target_point_offset.y = 0.2 position_constraint.target_point_offset.z = 0.3 position_constraint.weight = 0.8 position_constraint.link_name = group.get_end_effector_link() position_constraint.header.frame_id = group.get_planning_frame() orientation_constraint = OrientationConstraint() orientation_constraint.orientation = Quaternion(x=curr_q[0], y=curr_q[1], z=curr_q[2], w=curr_q[3]) orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 0.1 orientation_constraint.weight = 0.2 orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.header.frame_id = group.get_planning_frame() constraint = Constraints() constraint.orientation_constraints.append(orientation_constraint) constraint.position_constraints.append(position_constraint) group.set_path_constraints(constraint) allow_replanning = True planning_time = 5 threshold = 0.02 time_limit = 120 start_time = time.time() target_x = current_pose.position.x target_y = current_pose.position.y delta_z = (target_z - current_pose.position.z) / 4 print "Attempting to lift gripper" i = 1 # if (delta_z < threshold): # return True # while (current_pose.position.z < target_z) and ((target_z - current_pose.position.z) > 3*threshold) and ((time.time()-start_time) < time_limit): # status = self.go_to_pose_goal(curr_q[0], curr_q[1], curr_q[2], curr_q[3], # target_x, target_y, current_pose.position.z + i*delta_z, # allow_replanning, planning_time, thresh=threshold) # rospy.sleep(0.1) # current_pose = group.get_current_pose().pose # i += 1 status = self.go_to_pose_goal(curr_q[0], curr_q[1], curr_q[2], curr_q[3], target_x, target_y, target_z, allow_replanning, planning_time, thresh=threshold) rospy.sleep(0.25) print "Successfully lifted gripper to z: ", current_pose.position.z group.clear_path_constraints() # return ((target_z-current_pose.position.z) < 3*threshold or (current_pose.position.z > target_z)) return True
box_pose.pose.position.z = -0.25 box_pose.pose.orientation.w = 1.0 scene.add_box('box_object', box_pose, (0.4, 0.1, 0.5)) rospy.sleep(2) rospy.loginfo("Scene Objects : {}".format(scene.get_known_object_names())) # Set Path Constraint constraints = Constraints() constraints.name = "down" orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = group.get_planning_frame() orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.orientation = pose_target_1.orientation orientation_constraint.absolute_x_axis_tolerance = 3.1415 orientation_constraint.absolute_y_axis_tolerance = 0.05 orientation_constraint.absolute_z_axis_tolerance = 0.05 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) group.set_path_constraints(constraints) rospy.loginfo("Get Path Constraints:\n{}".format( group.get_path_constraints())) # Pose Target 2 rospy.loginfo("Start Pose Target 2") pose_target_2 = Pose() pose_target_2.position.x = 0.3
def both_arms_go_to_pose_goal(self): # 设置动作对象变量,此处为both_arms both_arms = self.both_arms # 获取当前各轴转角 axis_angle = both_arms.get_current_joint_values() # print axis_angle # 获取当前末端执行器位置姿态 right_pose_goal = both_arms.get_current_pose( end_effector_link="gripper_r_finger_r") left_pose_goal = both_arms.get_current_pose( end_effector_link="gripper_l_finger_r") print right_pose_goal # 限制末端夹具运动 right_joint_const = JointConstraint() right_joint_const.joint_name = "gripper_r_joint_r" if Rightfinger > -55: right_joint_const.position = 0.024 else: right_joint_const.position = 0 left_joint_const = JointConstraint() left_joint_const.joint_name = "gripper_l_joint_r" if Leftfinger > -32: left_joint_const.position = 0.024 else: left_joint_const.position = 0 # 添加末端姿态约束: right_orientation_const = OrientationConstraint() right_orientation_const.header = Header() right_orientation_const.orientation = right_pose_goal.pose.orientation right_orientation_const.link_name = "gripper_r_joint_r" right_orientation_const.absolute_x_axis_tolerance = 0.6 right_orientation_const.absolute_y_axis_tolerance = 0.6 right_orientation_const.absolute_z_axis_tolerance = 0.6 right_orientation_const.weight = 1 left_orientation_const = OrientationConstraint() left_orientation_const.header = Header() left_orientation_const.orientation = left_pose_goal.pose.orientation left_orientation_const.link_name = "gripper_l_joint_r" left_orientation_const.absolute_x_axis_tolerance = 0.6 left_orientation_const.absolute_y_axis_tolerance = 0.6 left_orientation_const.absolute_z_axis_tolerance = 0.6 left_orientation_const.weight = 1 # 施加全约束 consts = Constraints() consts.joint_constraints = [right_joint_const, left_joint_const] # consts.orientation_constraints = [right_orientation_const, left_orientation_const] both_arms.set_path_constraints(consts) # # 设置动作对象目标位置姿态 # # 右臂 # right_pose_goal.pose.orientation.x = Right_Qux # right_pose_goal.pose.orientation.y = Right_Quy # right_pose_goal.pose.orientation.z = Right_Quz # right_pose_goal.pose.orientation.w = Right_Quw # right_pose_goal.pose.position.x = Neurondata[5] # right_pose_goal.pose.position.y = Neurondata[3] # right_pose_goal.pose.position.z = Neurondata[4] # # 左臂 # left_pose_goal.pose.orientation.x = Left_Qux # left_pose_goal.pose.orientation.y = Left_Quy # left_pose_goal.pose.orientation.z = Left_Quz # left_pose_goal.pose.orientation.w = Left_Quw # left_pose_goal.pose.position.x = Neurondata[11] # left_pose_goal.pose.position.y = Neurondata[9] # left_pose_goal.pose.position.z = Neurondata[10] # # 右臂 # right_pose_goal.pose.orientation.x = Right_Qux # right_pose_goal.pose.orientation.y = Right_Quy # right_pose_goal.pose.orientation.z = Right_Quz # right_pose_goal.pose.orientation.w = Right_Quw # right_pose_goal.pose.position.x = (1266/740)*(Neurondata[5]+0.28)-0.495 # right_pose_goal.pose.position.y = (1295/780)*(Neurondata[3]+0.56)-0.754 # right_pose_goal.pose.position.z = (1355/776)*(Neurondata[4]-0.054)-0.184 # # 左臂 # left_pose_goal.pose.orientation.x = Left_Qux # left_pose_goal.pose.orientation.y = Left_Quy # left_pose_goal.pose.orientation.z = Left_Quz # left_pose_goal.pose.orientation.w = Left_Quw # left_pose_goal.pose.position.x = (1266/850)*(Neurondata[11]+0.33)-0.495 # left_pose_goal.pose.position.y = (1295/720)*(Neurondata[9]+0.19)-0.541 # left_pose_goal.pose.position.z = (1355/745)*(Neurondata[10]-0.055)-0.184 # 右臂 right_pose_goal.pose.orientation.x = Right_Qux right_pose_goal.pose.orientation.y = Right_Quy right_pose_goal.pose.orientation.z = Right_Quz right_pose_goal.pose.orientation.w = Right_Quw right_pose_goal.pose.position.x = (Neurondata[5] - 0.05) * 1.48 + 0.053 right_pose_goal.pose.position.y = (Neurondata[3] + 0.18) * 1.48 - 0.12 right_pose_goal.pose.position.z = (Neurondata[4] - 0.53) * 1.48 + 0.47 # 左臂 left_pose_goal.pose.orientation.x = Left_Qux left_pose_goal.pose.orientation.y = Left_Quy left_pose_goal.pose.orientation.z = Left_Quz left_pose_goal.pose.orientation.w = Left_Quw left_pose_goal.pose.position.x = (Neurondata[11] - 0.05) * 1.48 + 0.053 left_pose_goal.pose.position.y = (Neurondata[9] - 0.18) * 1.48 + 0.12 left_pose_goal.pose.position.z = (Neurondata[10] - 0.53) * 1.48 + 0.47 # # 右臂 # right_pose_goal.pose.orientation.x = right_pose_goal.pose.orientation.x # right_pose_goal.pose.orientation.y = right_pose_goal.pose.orientation.y # right_pose_goal.pose.orientation.z = right_pose_goal.pose.orientation.z # right_pose_goal.pose.orientation.w = right_pose_goal.pose.orientation.w # right_pose_goal.pose.position.x = right_pose_goal.pose.position.x # right_pose_goal.pose.position.y = right_pose_goal.pose.position.y # right_pose_goal.pose.position.z = right_pose_goal.pose.position.z # # 左臂 # left_pose_goal.pose.orientation.x = left_pose_goal.pose.orientation.x # left_pose_goal.pose.orientation.y = left_pose_goal.pose.orientation.y # left_pose_goal.pose.orientation.z = left_pose_goal.pose.orientation.z # left_pose_goal.pose.orientation.w = left_pose_goal.pose.orientation.w # left_pose_goal.pose.position.x = left_pose_goal.pose.position.x # left_pose_goal.pose.position.y = left_pose_goal.pose.position.y # left_pose_goal.pose.position.z = left_pose_goal.pose.position.z # 设置动作组的两个目标点 both_arms.set_pose_target(right_pose_goal, end_effector_link="gripper_r_finger_r") both_arms.set_pose_target(left_pose_goal, end_effector_link="gripper_l_finger_r") # 规划和输出动作 traj = both_arms.plan() both_arms.execute(traj, wait=False) # # 清除路径约束 both_arms.clear_path_constraints() # 确保输出停止 both_arms.stop()
def left_arm_go_to_pose_goal(self): # 设置动作对象变量,此处为arm left_arm = self.left_arm # 获取当前末端执行器位置姿态 pose_goal = left_arm.get_current_pose().pose # 限制末端夹具运动 left_joint_const = JointConstraint() left_joint_const.joint_name = "gripper_l_joint_r" if Leftfinger > -32: left_joint_const.position = 0.024 else: left_joint_const.position = 0 left_joint_const.weight = 1.0 # 限制末端位移 left_position_const = PositionConstraint() left_position_const.header = Header() left_position_const.link_name = "gripper_l_joint_r" left_position_const.target_point_offset.x = 0.5 left_position_const.target_point_offset.y = -0.5 left_position_const.target_point_offset.z = 1.0 left_position_const.weight = 1.0 # # 限制1轴转动 left_joint1_const = JointConstraint() left_joint1_const.joint_name = "yumi_joint_1_l" left_joint1_const.position = 0 left_joint1_const.tolerance_above = 1.76 left_joint1_const.tolerance_below = 0 left_position_const.weight = 1.0 # 限制2轴转动 left_joint2_const = JointConstraint() left_joint2_const.joint_name = "yumi_joint_2_l" left_joint2_const.position = 0 left_joint2_const.tolerance_above = 0 left_joint2_const.tolerance_below = 150 left_joint2_const.weight = 1.0 # 限制3轴转动 left_joint3_const = JointConstraint() left_joint3_const.joint_name = "yumi_joint_3_l" left_joint3_const.position = 0 left_joint3_const.tolerance_above = 35 left_joint3_const.tolerance_below = 55 left_joint3_const.weight = 1.0 # 限制4轴转动 left_joint4_const = JointConstraint() left_joint4_const.joint_name = "yumi_joint_4_l" left_joint4_const.position = 0 left_joint4_const.tolerance_above = 60 left_joint4_const.tolerance_below = 75 left_joint4_const.weight = 1.0 # 限制5轴转动 right_joint5_const = JointConstraint() right_joint5_const.joint_name = "yumi_joint_5_l" right_joint5_const.position = 40 right_joint5_const.tolerance_above = 50 right_joint5_const.tolerance_below = 20 right_joint5_const.weight = 1.0 # 限制6轴转动 left_joint6_const = JointConstraint() left_joint6_const.joint_name = "yumi_joint_6_l" left_joint6_const.position = 0 left_joint6_const.tolerance_above = 10 left_joint6_const.tolerance_below = 35 left_joint6_const.weight = 1.0 # 限制7轴转动 left_joint7_const = JointConstraint() left_joint7_const.joint_name = "yumi_joint_7_l" left_joint7_const.position = -10 left_joint7_const.tolerance_above = 0 left_joint7_const.tolerance_below = 160 left_joint7_const.weight = 1.0 # 限制末端位移 left_position_const = PositionConstraint() left_position_const.header = Header() left_position_const.link_name = "gripper_l_joint_r" left_position_const.target_point_offset.x = 0.5 left_position_const.target_point_offset.y = 0.25 left_position_const.target_point_offset.z = 0.5 left_position_const.weight = 1.0 # 添加末端姿态约束: left_orientation_const = OrientationConstraint() left_orientation_const.header = Header() left_orientation_const.orientation = pose_goal.orientation left_orientation_const.link_name = "gripper_l_finger_r" left_orientation_const.absolute_x_axis_tolerance = 0.5 left_orientation_const.absolute_y_axis_tolerance = 0.25 left_orientation_const.absolute_z_axis_tolerance = 0.5 left_orientation_const.weight = 1 # 施加全约束 consts = Constraints() consts.joint_constraints = [left_joint_const] # consts.orientation_constraints = [left_orientation_const] # consts.position_constraints = [left_position_const] left_arm.set_path_constraints(consts) # 设置动作对象目标位置姿态 pose_goal.orientation.x = Left_Qux pose_goal.orientation.y = Left_Quy pose_goal.orientation.z = Left_Quz pose_goal.orientation.w = Left_Quw pose_goal.position.x = (Neurondata[11] - 0.05) * 1.48 + 0.053 pose_goal.position.y = (Neurondata[9] - 0.18) * 1.48 + 0.12 pose_goal.position.z = (Neurondata[10] - 0.53) * 1.48 + 0.47 left_arm.set_pose_target(pose_goal) print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = pose_goal.orientation.x # pose_goal.orientation.y = pose_goal.orientation.y # pose_goal.orientation.z = pose_goal.orientation.z # pose_goal.orientation.w = pose_goal.orientation.w # pose_goal.position.x = pose_goal.position.x # pose_goal.position.y = pose_goal.position.y - 0.01 # pose_goal.position.z = pose_goal.position.z # left_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # 规划和输出动作 traj = left_arm.plan() left_arm.execute(traj, wait=False) # 动作完成后清除目标信息 left_arm.clear_pose_targets() # 清除路径约束 left_arm.clear_path_constraints() # 确保没有剩余未完成动作在执行 left_arm.stop()
def right_arm_go_to_pose_goal(self): # 设置动作对象变量,此处为arm right_arm = self.right_arm # 获取当前末端执行器位置姿态 pose_goal = right_arm.get_current_pose().pose # 限制末端夹具运动 right_joint_const = JointConstraint() right_joint_const.joint_name = "gripper_r_joint_r" if Rightfinger > -55: right_joint_const.position = 0.024 else: right_joint_const.position = 0 right_joint_const.weight = 1.0 # 限制1轴转动 right_joint1_const = JointConstraint() right_joint1_const.joint_name = "yumi_joint_1_r" right_joint1_const.position = 0 right_joint1_const.tolerance_above = 120 right_joint1_const.tolerance_below = 0 right_joint1_const.weight = 1.0 # 限制2轴转动 right_joint2_const = JointConstraint() right_joint2_const.joint_name = "yumi_joint_2_r" right_joint2_const.position = 0 right_joint2_const.tolerance_above = 0 right_joint2_const.tolerance_below = 150 right_joint2_const.weight = 1.0 # 限制3轴转动 right_joint3_const = JointConstraint() right_joint3_const.joint_name = "yumi_joint_3_r" right_joint3_const.position = 0 right_joint3_const.tolerance_above = 35 right_joint3_const.tolerance_below = 55 right_joint3_const.weight = 1.0 # 限制4轴转动 right_joint4_const = JointConstraint() right_joint4_const.joint_name = "yumi_joint_4_r" right_joint4_const.position = 0 right_joint4_const.tolerance_above = 60 right_joint4_const.tolerance_below = 75 right_joint4_const.weight = 1.0 # 限制5轴转动 right_joint5_const = JointConstraint() right_joint5_const.joint_name = "yumi_joint_5_r" right_joint5_const.position = 40 right_joint5_const.tolerance_above = 50 right_joint5_const.tolerance_below = 20 right_joint5_const.weight = 1.0 # 限制6轴转动 right_joint6_const = JointConstraint() right_joint6_const.joint_name = "yumi_joint_6_r" right_joint6_const.position = 0 right_joint6_const.tolerance_above = 10 right_joint6_const.tolerance_below = 35 right_joint6_const.weight = 1.0 # 限制7轴转动 right_joint7_const = JointConstraint() right_joint7_const.joint_name = "yumi_joint_7_r" right_joint7_const.position = -10 right_joint7_const.tolerance_above = 0 right_joint7_const.tolerance_below = 160 right_joint7_const.weight = 1.0 # 限制末端位移 right_position_const = PositionConstraint() right_position_const.header = Header() right_position_const.link_name = "gripper_r_joint_r" right_position_const.target_point_offset.x = 0.5 right_position_const.target_point_offset.y = -0.5 right_position_const.target_point_offset.z = 1.0 right_position_const.weight = 1.0 # 添加末端姿态约束: right_orientation_const = OrientationConstraint() right_orientation_const.header = Header() right_orientation_const.orientation = pose_goal.orientation right_orientation_const.link_name = "gripper_r_finger_r" right_orientation_const.absolute_x_axis_tolerance = 0.50 right_orientation_const.absolute_y_axis_tolerance = 0.25 right_orientation_const.absolute_z_axis_tolerance = 0.50 right_orientation_const.weight = 100 # 施加全约束 consts = Constraints() consts.joint_constraints = [right_joint_const] # consts.orientation_constraints = [right_orientation_const] # consts.position_constraints = [right_position_const] right_arm.set_path_constraints(consts) # 设置动作对象目标位置姿态 pose_goal.orientation.x = Right_Qux pose_goal.orientation.y = Right_Quy pose_goal.orientation.z = Right_Quz pose_goal.orientation.w = Right_Quw pose_goal.position.x = (Neurondata[5] - 0.05) * 1.48 + 0.053 pose_goal.position.y = (Neurondata[3] + 0.18) * 1.48 - 0.12 pose_goal.position.z = (Neurondata[4] - 0.53) * 1.48 + 0.47 right_arm.set_pose_target(pose_goal) print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = 0.1644 # pose_goal.orientation.y = 0.3111 # pose_goal.orientation.z = 0.9086 # pose_goal.orientation.w = 0.2247 # pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053 # pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12 # pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47 # right_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = pose_goal.orientation.x # pose_goal.orientation.y = pose_goal.orientation.y # pose_goal.orientation.z = pose_goal.orientation.z # pose_goal.orientation.w = pose_goal.orientation.w # pose_goal.position.x = pose_goal.position.x # pose_goal.position.y = pose_goal.position.y - 0.01 # pose_goal.position.z = pose_goal.position.z # right_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # 规划和输出动作 traj = right_arm.plan() right_arm.execute(traj, wait=False) # 动作完成后清除目标信息 right_arm.clear_pose_targets() # 清除路径约束 right_arm.clear_path_constraints() # 确保没有剩余未完成动作在执行 right_arm.stop()
def print_pointlist(self, point_list, future_print_status=False): startime = datetime.datetime.now() # Save original points list full_point_list = copy.deepcopy(point_list) full_point_array = np.delete(np.array(full_point_list), 2, axis=1) self.target_list = full_point_list if future_print_status: self.future_printing_status = True # Constraints pose = self.group.get_current_pose(self.group.get_end_effector_link()) constraints = Constraints() # joint constraints joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_1' joint_constraint.position = 169 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1.0 constraints.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_4' joint_constraint.position = 150 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1.0 constraints.joint_constraints.append(joint_constraint) self.group.set_path_constraints(constraints) # Orientation constrains orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = self.group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 2 * pi orientation_constraint.absolute_y_axis_tolerance = 2 * pi orientation_constraint.absolute_z_axis_tolerance = 2 * pi orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) # Record how many points has already finished finsih_num = 0 print_num = 0 index_check = 0 while len(point_list) > 0: print('New Plan, points left:', len(point_list), point_list) # Move the robot point to first point and find the height if len(point_list) > 1: initial_plan = self.move_to_initial(point_list[1]) else: initial_plan = self.move_to_initial(point_list[0]) # joint_goal = self.group.get_current_joint_values() head = initial_plan.joint_trajectory.header robot_state = self.robot.get_current_state() # print(robot_state.joint_state) robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \ tuple(robot_state.joint_state.position[7:]) # the joints for the wheel resp = self.request_fk(head, [self.group.get_end_effector_link()], robot_state) current_pose = self.group.get_current_pose().pose current_pose.orientation = resp.pose_stamped[0].pose.orientation (current_pose.position.x, current_pose.position.y, current_pose.position.z) = point_list[0] self.group.set_pose_target(current_pose) self.group.go() # Move the robot to the center of the striaght line to make sure Way point method can be executed # Way points waypoints = [] wpose = self.group.get_current_pose().pose # Add the current pose to make sure the path is smooth waypoints.append(copy.deepcopy(wpose)) success_num = 0 for point_num in range(len(point_list)): (wpose.position.x, wpose.position.y, wpose.position.z) = point_list[point_num] waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.00, path_constraints=constraints) print 'Adding the first planing point, and fraction is', fraction executing_state = 0 if fraction == 1: success_num += 1 execute_plan = plan if success_num == len(point_list): self.group.execute(execute_plan, wait=False) self.msg_print.data = True executing_state = 1 success_num += 1 elif success_num == 0: break else: # execute success plan self.msg_print.data = True self.group.execute(execute_plan, wait=False) executing_state = 1 break ## 2nd loop ## always re-plan and check for obstacle while executing waypoints executed_waypoint_index = 0 # initial value of nothing success_point_list = point_list[:success_num] print('when plan success, move_group_status:', self.move_group_execution_status, 'success_plan_number:', success_num) if executing_state == 1: success_planned_waypoint_array = np.delete(np.array( point_list[:success_num]), 2, axis=1) # print 'success planned waypoint\n', success_planned_waypoint_array print 'status', self.waypoint_execution_status while self.waypoint_execution_status != 3: if point_list == []: break if self.waypoint_execution_status == 4: # aborted state print 'stop and abort waypoint execution' self.msg_print.data = False self.group.stop() executing_state = 0 break current_ee_pose = self.group.get_current_pose().pose current_ee_position_array = np.array([ current_ee_pose.position.x, current_ee_pose.position.y ]) executed_waypoint_index = self.check_executed_waypoint_index( success_planned_waypoint_array, current_ee_position_array) # print 'executed latest index', executed_waypoint_index index_check = self.check_executed_waypoint_index( full_point_array, current_ee_position_array) self.further_printing_number = index_check print 'index:', index_check, 'way_point index', executed_waypoint_index if future_print_status == True: # Check for enclosure self.base_group.set_position_target( [0, 0, 0.05], self.base_group.get_end_effector_link()) result = self.base_group.plan() self.base_group.clear_pose_targets() if len(result.joint_trajectory.points) == 0: print('Check enclosure failed') else: print('Check enclosure successful') ## Replan to check for dynamic obstacle waypoints = [] # Add the current pose to make sure the path is smooth, get latest pose current_ee_pose = self.group.get_current_pose().pose waypoints.append(copy.deepcopy(current_ee_pose)) # discard the executed waypoints new_point_list = point_list[ executed_waypoint_index:success_num] for k in new_point_list: (current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z) = k waypoints.append(copy.deepcopy(current_ee_pose)) (plan2, fraction2) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.00, path_constraints=constraints) print 'Dynamic check fraction:', fraction2 if fraction2 < 1: ## new obstacle appear # print 'executed latest index', executed_waypoint_index # print 'fraction value', fraction,'\n' print 'new obstacle appeared to be in the path' self.group.stop() self.msg_print.data = False executing_state = 0 break rospy.sleep(2) print 'status:', self.waypoint_execution_status, 'executed_index:', executed_waypoint_index, 'success_num:', success_num if self.waypoint_execution_status == 3: # waypoint successfully printed # self.print_list_visualize(point_list[:success_num]) self.rviz_visualise_marker(point_list[:success_num]) del (point_list[:success_num - 1]) elif self.waypoint_execution_status == 2 or 4: # state 2 = preempted, state 4 = aborted. # only printed partial waypoint # self.print_list_visualize(point_list[:executed_waypoint_index+1]) if executed_waypoint_index > 0: # at index 0, it might have not print the point 0-1 edge successfully. self.rviz_visualise_marker( point_list[:executed_waypoint_index + 1]) del (point_list[:executed_waypoint_index] ) # delete up till whatever is executed self.msg_print.data = False if point_list == []: rospy.sleep(2) self.group.stop() # Delete the points what already execute # if success_num > 0: # Add obstacle after printing (need to revise) self.group.set_path_constraints(None) finshtime = datetime.datetime.now() print(finshtime - startime).seconds full_point_list_x = [base[0] for base in full_point_list] full_point_list_y = [base[1] for base in full_point_list] plt3 = plt.figure("Printing result", figsize=(6, 6)) plt.plot(self.re_position_x, self.re_position_y, 'b') plt.plot(full_point_list_x, full_point_list_y, 'ro') plt.xlim(min(full_point_list_x) - 0.1, max(full_point_list_x) + 0.1) plt.ylim(min(full_point_list_y) - 0.1, max(full_point_list_y) + 0.1) plt.legend(['Printing result', 'ground truth'], fontsize=8, bbox_to_anchor=(1.0, 1)) plt.xlabel("X axis (m)") plt.ylabel("y axis (m)") plt.title('Printing result') plt.show() plt3.savefig('result.png', dpi=plt3.dpi) print('All finish, time:', (finshtime - startime).seconds)
def __init__(self): self.bridge = CvBridge() joint_state_topic = ['joint_states:=/iiwa/joint_states'] moveit_commander.roscpp_initialize(joint_state_topic) rospy.Subscriber("/iiwa/joint_states", JointState, self.State_callback) # Instantiate a RobotCommander object. This object is # an interface to the robot as a whole. self.robot = moveit_commander.RobotCommander() self.group = moveit_commander.MoveGroupCommander("manipulator") # rospy.sleep(2) # self.scene = moveit_commander.PlanningSceneInterface('/iiwa/move_group/monitored_planning_scene') # box_pose = PoseStamped() # box_pose.header.frame_id = "world" # box_pose.pose.position.x = 1.0 # box_pose.pose.orientation.w = 1.0 # self.scene.add_box("test", box_pose, size=(0.1, 0.2, 0.3)) # while not rospy.is_shutdown(): # rospy.sleep(1.0) # for k in self.scene.__dict__.keys(): # print(k, self.scene.__dict__[k]) # # print(self.scene) # print(self.scene.get_known_object_names()) # print(self.scene.get_attached_objects()) # exit() self.group.set_max_velocity_scaling_factor(0.05) self.group.set_max_acceleration_scaling_factor(0.05) current_pose = self.group.get_current_pose(end_effector_link='iiwa_link_ee').pose self._joint_efforts = 0 self._joint_vel = 0 self._joint_name = 0 self._header = None pose = PoseStamped() self.upright_constraints = Constraints() self.upright_constraints.name = "upright" orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = self.group.get_planning_frame() orientation_constraint.link_name = self.group.get_end_effector_link() pose.pose.orientation.x = 1.0 pose.pose.orientation.y = 0.0 pose.pose.orientation.z = 0.0 pose.pose.orientation.w = 0.0 orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = .7#3.0 orientation_constraint.absolute_y_axis_tolerance = .7#3.0 orientation_constraint.absolute_z_axis_tolerance = 3.1415 #orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis orientation_constraint.weight = 1 self.upright_constraints.orientation_constraints.append(orientation_constraint) self.group.allow_replanning(True) self.group.allow_looking(True) workspace = [0.5,-0.3,0.15,0.7,0.2,0.25] # self.group.set_workspace(workspace) # self.group.set_path_constraints(self.upright_constraints) self.traj_num = -1 self.im_num = 0 self.MAX_PATH_LENGTH = 15
def place(self, target_name, place_position, pre_place_distance, post_place_retreat): limit = {'dist': 0.01, 'r': 0.10, 'p': 0.10, 'y': 0.10} constraints = Constraints() oc = OrientationConstraint() oc.header.frame_id = REFERENCE_FRAME oc.link_name = 'j2s7s300_end_effector' oc.absolute_x_axis_tolerance = limit['r'] # radian oc.absolute_y_axis_tolerance = limit['p'] oc.absolute_z_axis_tolerance = limit['y'] oc.weight = 0.85 oc.orientation = place_position.pose.orientation constraints.orientation_constraints.append(deepcopy(oc)) result = False replan_times = 1 replan_state = True while replan_state and replan_times <= 5: (place_path, fraction) = self.get_path(place_position.pose, 0.01, constraints=constraints) if fraction >= 0.95: print "Pre_place_approach..." self.arm.execute(place_path) result = self.check(place_position.pose, limit) replan_state = False replan_times += 1 if result: result = False print "Placing..." self.gripper.set_named_target("Open") self.gripper.go() self.arm.detach_object(target_name) rospy.sleep(1) post_place_position = self.get_retreat_point( place_position.pose, post_place_retreat) replan_state = True replan_times = 1 while replan_state and replan_times <= 5: (retreat_path, fraction) = self.get_path(post_place_position, 0.01, constraints=constraints) if fraction > 0.8: self.arm.execute(retreat_path) result = self.check(post_place_position, limit) replan_state = False replan_times += 1 if not result: self.state = STATE.PLACE_RETREAT_ERR else: self.state = STATE.PRE_PLACE_ERR if self._server.current_goal.get_goal_status( ).status == GoalStatus.PREEMPTING: rospy.logwarn("Can't cancel task after place action start!") if result: self.state = STATE.PLACE_FINISH self.arm.clear_path_constraints()
def pick(self, target_name, grasp_position, pre_grasp_distance, post_grasp_retreat): pre_grasp_posture = JointTrajectory() grasp_posture = JointTrajectory() pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) limit = {'dist': 0.01, 'r': 0.15, 'p': 0.15, 'y': 0.15} constraints = Constraints() oc = OrientationConstraint() oc.header.frame_id = REFERENCE_FRAME oc.link_name = 'j2s7s300_end_effector' oc.absolute_x_axis_tolerance = limit['r'] # radian oc.absolute_y_axis_tolerance = limit['p'] oc.absolute_z_axis_tolerance = limit['y'] oc.weight = 1.0 oc.orientation = grasp_position.pose.orientation constraints.orientation_constraints.append(deepcopy(oc)) result = False replan_times = 1 replan_state = True while replan_state and replan_times <= 5: (pre_grasp_path, fraction) = self.get_path(grasp_position.pose, 0.01, constraints=constraints) if self._server.current_goal.get_goal_status( ).status == GoalStatus.PREEMPTING: self.state = STATE.STOP return if fraction >= 0.92: print "Pre_grasp_approach..." self.arm.execute(pre_grasp_path) result = self.check(grasp_position.pose, limit) replan_state = False replan_times += 1 if result: result = False if self.arm.attach_object( target_name, self.arm.get_end_effector_link(), [ "j2s7s300_end_effector", "j2s7s300_link_finger_1", "j2s7s300_link_finger_tip_1", "j2s7s300_link_finger_2", "j2s7s300_link_finger_tip_2", "j2s7s300_link_finger_3", "j2s7s300_link_finger_tip_3", "j2s7s300_joint_finger_1", "j2s7s300_joint_finger_2", "j2s7s300_joint_finger_3" ]): rospy.logwarn("Attach request sent successfully!") else: raise Exception("Can't send attach request!") print "Grasping..." self.gripper.set_named_target("Close") if self._server.current_goal.get_goal_status( ).status == GoalStatus.PREEMPTING: self.arm.detach_object(target_name) self.back_to_init_pose() self.state = STATE.STOP return self.gripper.go() rospy.sleep(1) post_grasp_position = self.get_retreat_point( grasp_position.pose, post_grasp_retreat) print "post_grasp_position: %s\n" % post_grasp_position replan_times = 1 replan_state = True while replan_state and replan_times <= 5: (retreat_path, fraction) = self.get_path(post_grasp_position, 0.005, constraints=constraints) if self._server.current_goal.get_goal_status( ).status == GoalStatus.PREEMPTING: self.arm.detach_object(target_name) self.back_to_init_pose(state=1) self.state = STATE.STOP return if fraction > 0.8: print "Retreating..." self.arm.execute(retreat_path) result = self.check(post_grasp_position, limit) replan_state = False replan_times += 1 if not result: self.state = STATE.GRASP_RETREAT_ERR else: self.state = STATE.PRE_GRASP_ERR # pre_grasp planning failed if result: self.state = STATE.PICK_FINISH self.arm.clear_path_constraints()
def print_pointlist(self, point_list): # Get differential of way point list differential = self.get_way_point_differential(point_list) differential_num = [0] differential_num = differential_num + differential eef_rotation_change = [0, 0] for i in range(2, len(differential_num)): eef_rotation_change.append(differential_num[i] - differential_num[i - 1]) pose = self.group.get_current_pose(self.group.get_end_effector_link()) constraints = Constraints() #### joint constraints joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_1' joint_constraint.position = 169 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1 constraints.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_4' joint_constraint.position = 150 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1 constraints.joint_constraints.append(joint_constraint) self.group.set_path_constraints(constraints) # Orientation constrains # constraints = Constraints() orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = self.group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 2 * pi orientation_constraint.absolute_y_axis_tolerance = 2 * pi orientation_constraint.absolute_z_axis_tolerance = 2 * pi constraints.orientation_constraints.append(orientation_constraint) while len(point_list) > 1: # Move the robot point to first point and find the height initial_plan = self.move_to_initial(point_list[1]) joint_goal = self.group.get_current_joint_values() head = initial_plan.joint_trajectory.header robot_state = self.robot.get_current_state() # print(robot.get_current_state().joint_state.position) robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \ tuple(robot_state.joint_state.position[7:]) resp = self.request_fk(head, [self.group.get_end_effector_link()], robot_state) current_pose = self.group.get_current_pose().pose current_pose.orientation = resp.pose_stamped[0].pose.orientation (current_pose.position.x, current_pose.position.y, current_pose.position.z) = point_list[0] self.group.set_pose_target(current_pose) self.group.go() # Move the robot to the center of the striaght line to make sure Way point method can be executed # Way points waypoints = [] wpose = self.group.get_current_pose().pose # Add the current pose to make sure the path is smooth waypoints.append(copy.deepcopy(wpose)) success_num = 0 for point_num in range(len(point_list)): (wpose.position.x, wpose.position.y, wpose.position.z) = point_list[point_num] (roll, pitch, yaw) = euler_from_quaternion([ wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w ]) # [wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w] = \ # quaternion_from_euler(roll, pitch, yaw + eef_rotation_change[point_num]) # print(yaw + eef_rotation_change[point_num]) [wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w] = \ quaternion_from_euler(roll, pitch, yaw) waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.00, path_constraints=constraints) if fraction == 1: success_num += 1 execute_plan = plan if success_num == len(point_list): self.group.execute(execute_plan) self.print_list_visualize(point_list) elif success_num == 0: break else: # execute success plan self.msg_prit.data = True self.group.execute(execute_plan) self.print_list_visualize(point_list[:success_num]) break self.msg_prit.data = False # Delete the points what already execute if success_num > 0: del (point_list[0:success_num - 1]) del (eef_rotation_change[0:success_num - 1]) # Add obstacle after printing (need to revise) self.group.set_path_constraints(None) print 'all finish'