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 set_pose_quat_target(self, pose): ############################ rospy.logwarn("set_pose_quat_target") # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" start_pose = self.arm.get_current_pose(self.end_effector_link) rospy.logwarn(self.end_effector_link) rospy.logwarn("start_pose:") rospy.logwarn(start_pose) rospy.logwarn("pose:") rospy.logwarn(pose) # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = self.end_effector_link orientation_constraint.orientation.x = start_pose.pose.orientation.x orientation_constraint.orientation.y = start_pose.pose.orientation.y orientation_constraint.orientation.z = start_pose.pose.orientation.z orientation_constraint.orientation.w = start_pose.pose.orientation.w orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) rospy.logwarn("constraints:") rospy.logwarn(constraints) # Set the path constraints on the right_arm self.arm.set_path_constraints(constraints) ############################ self.arm.set_pose_target(pose, self.end_effector_link)
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 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 get_ik(target): """ :param target: a PoseStamped give the desired position of the endeffector. """ pose = group.get_current_pose(group.get_end_effector_link()) constraints = Constraints() orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 2*pi current_orientation_list = [pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w] # get euler angle from quaternion (roll, pitch, yaw) = euler_from_quaternion(current_orientation_list) pitch = pi roll = 0 orientation_constraint.weight = 1 [orientation_constraint.orientation.x, orientation_constraint.orientation.y, orientation_constraint.orientation.z, orientation_constraint.orientation.w] = \ quaternion_from_euler(roll, pitch, yaw) constraints.orientation_constraints.append(orientation_constraint) # group.set_path_constraints(constraints) ##################################################################### rospy.wait_for_service('compute_ik') request_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) service_request = PositionIKRequest() service_request.group_name = 'robot' service_request.pose_stamped.header.frame_id = 'base_footprint' # service_request.pose_stamped = group.get_current_pose() service_request.robot_state = robot.get_current_state() service_request.ik_link_name = 'arm_link_5' # Set position service_request.pose_stamped.pose.position.x = target[0] service_request.pose_stamped.pose.position.y = target[1] service_request.pose_stamped.pose.position.z = target[2] service_request.pose_stamped.pose.orientation.w =1 service_request.constraints.orientation_constraints.append(orientation_constraint) service_request.timeout.secs= 4 service_request.attempts= 2 service_request.avoid_collisions = True resp = request_ik(service_request) return resp
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 set_upright_constraints(self,pose): upright_constraints = Constraints() orientation_constraint = OrientationConstraint() upright_constraints.name = "upright" orientation_constraint.header = pose.header orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 3.14 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 0.1 #orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis orientation_constraint.weight = 1 upright_constraints.orientation_constraints.append(orientation_constraint) group.set_path_constraints(upright_constraints)
def set_path_ori_constraints(self, stampedPoseList): self.__graspPathConstraints.orientation_constraints = [] ori_constraints = [] for stampedPose in stampedPoseList: constraint = OrientationConstraint() constraint.header = stampedPose.header constraint.orientation = deepcopy(stampedPose.pose.orientation) constraint.link_name = "world" constraint.absolute_x_axis_tolerance = 0.2 constraint.absolute_y_axis_tolerance = 0.2 constraint.absolute_z_axis_tolerance = 0.2 constraint.weight = 0.8 ori_constraints.append(constraint) # ipdb.set_trace() self.__graspPathConstraints.orientation_constraints = ori_constraints
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 addConstrains(): pose = group.get_current_pose() constraint = Constraints() constraint.name = "downRight" orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation 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.absolute_z_axis_tolerance = 3.14 #ignore this axis orientation_constraint.weight = 1 constraint.orientation_constraints.append(orientation_constraint) group.set_path_constraints(constraint)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_test') arm = MoveGroupCommander('arm') end_effector_link = arm.get_end_effector_link() arm.allow_replanning(True) arm.set_planning_time(5) target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_init_orientation = Quaternion() target_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0) self.setPose(target_pose, [0.5, 0.2, 0.5],list(target_init_orientation)) current_pose = arm.get_current_pose(end_effector_link) self.setPose(current_pose, [current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z], list(target_init_orientation)) arm.set_pose_target(current_pose) arm.go() rospy.sleep(2) constraints = Constraints() orientation_constraint = OrientationConstraint() constraints.name = 'gripper constraint' orientation_constraint.header = target_pose.header orientation_constraint.link_name = end_effector_link orientation_constraint.orientation.x = target_init_orientation[0] orientation_constraint.orientation.y = target_init_orientation[1] orientation_constraint.orientation.z = target_init_orientation[2] orientation_constraint.orientation.w = target_init_orientation[3] orientation_constraint.absolute_x_axis_tolerance = 0.03 orientation_constraint.absolute_y_axis_tolerance = 0.03 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) arm.set_path_constraints(constraints) arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def freeze_orientation(self): # get current pose cur_pose = self._group.get_current_pose() # create pose constraint constraints = Constraints() constraints.name = "orientation_constraints" orientation_constraint = OrientationConstraint() orientation_constraint.header = cur_pose.header orientation_constraint.header.stamp = rospy.Time.now() orientation_constraint.link_name = self._group.get_end_effector_link() orientation_constraint.orientation = cur_pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 0.5 orientation_constraint.absolute_y_axis_tolerance = 0.5 orientation_constraint.absolute_z_axis_tolerance = 0.5 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) self._group.set_path_constraints(constraints) self.orientation_frozen = True
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 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 create_move_group_pose_goal(self, goal_pose=Pose(), group="right_arm", end_link_name="arm_right_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 get_constraint(self, euler_orientation=[0, math.pi / 2, 0], tol=[3, 3, .5]): #method takes euler-angle inputs, this converts it to a quaternion q_orientation = tf.transformations.quaternion_from_euler( euler_orientation[0], euler_orientation[1], euler_orientation[2]) orientation_msg = Quaternion(q_orientation[0], q_orientation[1], q_orientation[2], q_orientation[3]) #defines a constraint that sets the end-effector so a cup in it's hand will be upright, or straight upside-down constraint = Constraints() constraint.name = 'upright_wrist' upright_orientation = OrientationConstraint() upright_orientation.link_name = self.group.get_end_effector_link() upright_orientation.orientation = orientation_msg upright_orientation.absolute_x_axis_tolerance = tol[0] upright_orientation.absolute_y_axis_tolerance = tol[1] upright_orientation.absolute_z_axis_tolerance = tol[2] upright_orientation.weight = 1.0 upright_orientation.header = self.start_pose.header constraint.orientation_constraints.append(upright_orientation) return (constraint)
def create_move_group_pose_goal(self, goal_pose=Pose(), group="manipulator", end_link_name=None, plan_only=True): 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 if end_link_name != None: position_c.link_name = end_link_name position_c.constraint_region.primitives.append( SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.1])) 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 = 0.01 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 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 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 __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_constraints_demo', anonymous=True) robot = RobotCommander() # Connect to the arm move group arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since constraint planning can take a while arm.set_planning_time(5) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow some leeway in position(meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file arm.set_named_target('l_arm_init') # Plan and execute a trajectory to the goal configuration arm.go() rospy.sleep(1) # Open the gripper gripper.set_joint_value_target(GRIPPER_NEUTRAL) gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.263803774718 target_pose.pose.position.y = 0.295405791959 target_pose.pose.position.z = 0.690438884208 q = quaternion_from_euler(0, 0, -1.57079633) target_pose.pose.orientation.x = q[0] target_pose.pose.orientation.y = q[1] target_pose.pose.orientation.z = q[2] target_pose.pose.orientation.w = q[3] # Set the start state and target pose, then plan and execute arm.set_start_state(robot.get_current_state()) arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(2) # Close the gripper gripper.set_joint_value_target(GRIPPER_CLOSED) gripper.go() rospy.sleep(1) # Store the current pose start_pose = arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 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 = 1.0 # q = quaternion_from_euler(0, 0, -1.57079633) # orientation_constraint.orientation.x = q[0] # orientation_constraint.orientation.y = q[1] # orientation_constraint.orientation.z = q[2] # orientation_constraint.orientation.w = q[3] # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the arm arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.39000848183 target_pose.pose.position.y = 0.185900663329 target_pose.pose.position.z = 0.732752341378 target_pose.pose.orientation.w = 1 # Set the start state and target pose, then plan and execute arm.set_start_state_to_current_state() arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) # Clear all path constraints arm.clear_path_constraints() # Open the gripper gripper.set_joint_value_target(GRIPPER_NEUTRAL) gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file arm.set_named_target('l_arm_init') # Plan and execute a trajectory to the goal configuration arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) robot = RobotCommander() # Connect to the right_arm move group right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since constraint planning can take a while right_arm.set_planning_time(15) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file right_arm.set_named_target('right_arm_zero') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.237012590198 target_pose.pose.position.y = -0.0747191267505 target_pose.pose.position.z = 0.901578401949 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state(robot.get_current_state()) right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(2) # Close the gripper right_gripper.set_joint_value_target(GRIPPER_CLOSED) right_gripper.go() rospy.sleep(1) # Store the current pose start_pose = right_arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = right_arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the right_arm right_arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.173187824708 target_pose.pose.position.y = -0.0159929871606 target_pose.pose.position.z = 0.692596608605 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state_to_current_state() right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(1) # Clear all path constraints right_arm.clear_path_constraints() # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file right_arm.set_named_target('right_arm_zero') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) robot = RobotCommander() # Connect to the right_arm move group right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since contraint planning can take a while right_arm.set_planning_time(15) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file right_arm.set_named_target('resting') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.237012590198 target_pose.pose.position.y = -0.0747191267505 target_pose.pose.position.z = 0.901578401949 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state(robot.get_current_state()) right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(2) # Close the gripper right_gripper.set_joint_value_target(GRIPPER_CLOSED) right_gripper.go() rospy.sleep(1) # Store the current pose start_pose = right_arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = right_arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the right_arm right_arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.173187824708 target_pose.pose.position.y = -0.0159929871606 target_pose.pose.position.z = 0.692596608605 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state_to_current_state() right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(1) # Clear all path constraints right_arm.clear_path_constraints() # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file right_arm.set_named_target('resting') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) robot = RobotCommander() # Connect to the right_arm move group right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_tolerance(0.00001) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file right_arm.set_named_target('right') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.4 target_pose.pose.position.y = 0.3 target_pose.pose.position.z = 0.4 target_pose.pose.orientation.x = 0.670820393249937 target_pose.pose.orientation.y = 0.223606797749979 target_pose.pose.orientation.z = 0.223606797749979 target_pose.pose.orientation.w = 0.670820393249937 # Set the start state and target pose, then plan and execute right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(2) # Store the current pose start_pose = right_arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = end_effector_link orientation_constraint.absolute_x_axis_tolerance = 0.00001 orientation_constraint.absolute_y_axis_tolerance = 3.14 orientation_constraint.absolute_z_axis_tolerance = 0.00001 orientation_constraint.weight = 1.0 orientation_constraint.orientation = start_pose.pose.orientation # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the right_arm right_arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = deepcopy(start_pose) target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.4 target_pose.pose.position.y = -0.3 target_pose.pose.position.z = 0.4 target_pose.pose.orientation.x = 0.670820393249937 target_pose.pose.orientation.y = -0.223606797749979 target_pose.pose.orientation.z = -0.223606797749979 target_pose.pose.orientation.w = 0.670820393249937 #target_pose.pose.orientation.x = 0.707107 #target_pose.pose.orientation.y = 0 #target_pose.pose.orientation.z = 0 #target_pose.pose.orientation.w = 0.707107 # Set the start state and target pose, then plan and execute right_arm.set_start_state_to_current_state() right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(1) # Clear all path constraints right_arm.clear_path_constraints() # Return to the "resting" configuration stored in the SRDF file right_arm.set_named_target('up') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0) def getOrientation(self, xyz): pass
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'
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" 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 global n # 设置动作对象目标位置姿态 if n == 1: pose_goal.orientation.x = -0.00825 pose_goal.orientation.y = 0.03556 pose_goal.orientation.z = 0.79932 pose_goal.orientation.w = 0.5998 pose_goal.position.x = 0.54425 pose_goal.position.y = -0.2208 pose_goal.position.z = 0.14822 elif n == 2: pose_goal.orientation.x = -0.00507 pose_goal.orientation.y = -0.012593 pose_goal.orientation.z = 0.98844 pose_goal.orientation.w = 0.15101 pose_goal.position.x = 0.55198 pose_goal.position.y = -0.06859 pose_goal.position.z = 0.17909 elif n == 3: pose_goal.orientation.x = -0.05578 pose_goal.orientation.y = 0.025377 pose_goal.orientation.z = 0.99365 pose_goal.orientation.w = -0.095267 pose_goal.position.x = 0.36478 pose_goal.position.y = -0.05781 pose_goal.position.z = 0.15907 n = 0 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): # Save original points list all_point_list = point_list pose = self.group.get_current_pose(self.group.get_end_effector_link()) constraints = Constraints() last_ee_pose = self.group.get_current_pose().pose #### 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) j = 0 # Record how many points has already finished finsih_num = 0 print_num = 0 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_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) 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) executing_state = 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 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 self.waypoint_execution_status == 4: # aborted state print 'stop and abort waypoint execution' self.group.stop() executing_state = 0 current_ee_pose = self.group.get_current_pose().pose self.printing_visualize((last_ee_pose.position.x, last_ee_pose.position.y, last_ee_pose.position.z), \ (current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z)) last_ee_pose = current_ee_pose 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 ]) self.printing_visualize((last_ee_pose.position.x, last_ee_pose.position.y, last_ee_pose.position.z), \ (current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z)) last_ee_pose = current_ee_pose executed_waypoint_index = self.check_executed_waypoint_index( success_planned_waypoint_array, current_ee_position_array) # print 'last_ee', (last_ee_pose.position.x,last_ee_pose.position.y), \ # 'current_ee', (current_ee_pose.position.x,current_ee_pose.position.y), \ # 'executed latest index', executed_waypoint_index # print(executed_waypoint_index) # print 'current number', finsih_num + executed_waypoint_index, 'printing number', print_num # if finsih_num + executed_waypoint_index - print_num <= 2 and finsih_num + executed_waypoint_index - print_num >=-2: # self.print_future_visualize(all_point_list, print_num) # print_num = finsih_num + executed_waypoint_index + 1 # # print(success_planned_waypoint_array.size) # print('printing') # print(finsih_num + executed_waypoint_index) # next_future_ob_index = finsih_num + executed_waypoint_index # if next_future_ob_index + 9 < len(all_point_list): # self.future_visualize(all_point_list[next_future_ob_index + 8], all_point_list[next_future_ob_index + 9]) ## Replan to check for dynamic obstacle waypoints = [] # Add the current pose to make sure the path is smooth, get latest pose current_ee_pose_smooth = self.group.get_current_pose().pose waypoints.append(copy.deepcopy(current_ee_pose_smooth)) # discard the executed waypoints new_point_list = point_list[executed_waypoint_index + 1:success_num] # Add future printing obstacle for k in new_point_list: (current_ee_pose_smooth.position.x, current_ee_pose_smooth.position.y, current_ee_pose_smooth.position.z) = k waypoints.append(copy.deepcopy(current_ee_pose_smooth)) (plan2, fraction2) = self.group.compute_cartesian_path( waypoints, 0.01, 0.00, path_constraints=constraints) if fraction2 < 1.0: ## new obstacle appear # print 'executed latest index', executed_waypoint_index # print 'fraction value', fraction,'\n' print 'new obstacle appeared along the planned path' self.group.stop() executing_state = 0 break j += 1 if j == 2: self.scene.addBox('boxb', 0.5, 0.5, 0.5, 2.5, 0.5, 0.15) rospy.sleep(0.01) if self.waypoint_execution_status == 3: # waypoint successfully printed # self.print_list_visualize(point_list[:success_num]) # print 'status 3', point_list[:success_num] del (point_list[0:success_num - 1]) finsih_num += success_num 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]) # print 'status 4', point_list[:executed_waypoint_index+1] del (point_list[:executed_waypoint_index + 1] ) # delete up till whatever is executed finsih_num += executed_waypoint_index + 1 self.msg_print.data = False self.group.set_path_constraints(None) print 'all finish'
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') scene = PlanningSceneInterface('world') self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) self.colors = dict() max_pick_attempts = 10 max_place_attempts = 10 rospy.sleep(1) arm = MoveGroupCommander('arm') end_effector_link = arm.get_end_effector_link() # arm.set_goal_position_tolerance(0.02) # arm.set_goal_orientation_tolerance(0.03) arm.allow_replanning(True) arm.set_planning_time(5) table_id = 'table' side_id = 'side' table2_id = 'table2' box1_id = 'box1' box2_id = 'box2' target_id = 'target' wall_id = 'wall' ground_id = 'ground' scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(table_id) scene.remove_world_object(table2_id) scene.remove_world_object(target_id) scene.remove_world_object(wall_id) scene.remove_world_object(ground_id) scene.remove_world_object(side_id) rospy.sleep(1) table_ground = 0.55 table_size = [0.2, 0.7, 0.01] table2_size = [0.25, 0.6, 0.01] box1_size = [0.02, 0.9, 0.6] box2_size = [0.05, 0.05, 0.15] target_size = [0.03, 0.06, 0.10] wall_size = [0.01, 0.9, 0.6] ground_size = [3, 3, 0.01] side_size = [0.01, 0.7, table_ground] table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME self.setPose(table_pose, [0.7, 0.0, table_ground + table_size[2] / 2.0]) scene.add_box(table_id, table_pose, table_size) side_pose = PoseStamped() side_pose.header.frame_id = REFERENCE_FRAME self.setPose(side_pose, [0.605, 0.0, side_size[2] / 2.0]) scene.add_box(side_id, side_pose, side_size) table2_pose = PoseStamped() table2_pose.header.frame_id = REFERENCE_FRAME self.setPose(table2_pose, [-0.325, 0.0, box1_size[2] + table2_size[2] / 2.0]) # scene.add_box(table2_id, table2_pose, table2_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME self.setPose(box1_pose, [-0.36, 0.0, box1_size[2] / 2.0]) # scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME self.setPose( box2_pose, [0.63, -0.12, table_ground + table_size[2] + box2_size[2] / 2.0]) # scene.add_box(box2_id, box2_pose, box2_size) target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME self.setPose( target_pose, [0.62, 0.0, table_ground + table_size[2] + target_size[2] / 2.0]) scene.add_box(target_id, target_pose, target_size) wall_pose = PoseStamped() wall_pose.header.frame_id = REFERENCE_FRAME self.setPose( wall_pose, [-0.405, 0.0, box1_size[2] + table2_size[2] + wall_size[2] / 2.0]) # scene.add_box(wall_id, wall_pose, wall_size) ground_pose = PoseStamped() ground_pose.header.frame_id = REFERENCE_FRAME self.setPose(ground_pose, [0.0, 0.0, -ground_size[2] / 2.0]) # scene.add_box(ground_id, ground_pose, ground_size) self.setColor(table_id, 0.8, 0.0, 0.0, 1.0) # self.setColor(table2_id, 0.8, 0.0, 0.0) # self.setColor(box1_id, 0.9, 0.9, 0.9) # self.setColor(box2_id, 0.8, 0.4, 1.0) self.setColor(target_id, 0.5, 0.4, 1.0) # self.setColor(wall_id, 0.9, 0.9, 0.9) self.setColor(ground_id, 0.3, 0.3, 0.3, 1.0) self.setColor(side_id, 0.8, 0.0, 0.0, 1.0) self.sendColors() constraints = Constraints() constraints.name = 'gripper constraint' orientation_constraint = OrientationConstraint() arm.set_support_surface_name(table_id) test_pose = PoseStamped() test_pose.header.frame_id = REFERENCE_FRAME test_orientation = Quaternion() test_orientation = quaternion_from_euler(0.0, 1.57, -3.14) self.setPose(test_pose, [ -0.15, -0.22, box1_size[2] + table2_size[2] + target_size[2] / 2.0 ], list(test_orientation)) place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME self.setPose( place_pose, [0.62, -0.22, table_ground + table_size[2] + target_size[2] / 2.0]) grasp_pose = target_pose grasp_init_orientation = Quaternion() grasp_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0) grasp_pose.pose.orientation.x = grasp_init_orientation[0] grasp_pose.pose.orientation.y = grasp_init_orientation[1] grasp_pose.pose.orientation.z = grasp_init_orientation[2] grasp_pose.pose.orientation.w = grasp_init_orientation[3] grasp_pose.pose.position.x -= 0.02 rospy.loginfo('quaterion: ' + str(grasp_pose.pose.orientation)) place_pose.pose.orientation.x = grasp_init_orientation[0] place_pose.pose.orientation.y = grasp_init_orientation[1] place_pose.pose.orientation.z = grasp_init_orientation[2] place_pose.pose.orientation.w = grasp_init_orientation[3] orientation_constraint.header = grasp_pose.header orientation_constraint.link_name = end_effector_link orientation_constraint.orientation.x = grasp_init_orientation[0] orientation_constraint.orientation.y = grasp_init_orientation[1] orientation_constraint.orientation.z = grasp_init_orientation[2] orientation_constraint.orientation.w = grasp_init_orientation[3] orientation_constraint.absolute_x_axis_tolerance = 0.03 orientation_constraint.absolute_y_axis_tolerance = 0.03 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) grasps = self.make_grasps(grasp_pose, [table_id], [0.05, 0.07, [1.0, 0.0, 0.0]], [0.22, 0.26, [-1.0, 0.0, 0.0]]) result = None n_attempts = 0 while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo('Pick attempt:' + str(n_attempts)) rospy.sleep(0.2) arm.set_path_constraints(constraints) arm.set_pose_target(place_pose) arm.go() ''' if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 places = self.make_places(place_pose, [table_id], [0.8, 0.13, [1.0, 0.0, 0.0]], [0.1, 0.12, [-1.0, 0.0, 0.0]]) while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: for place in places: result = arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.loginfo('Place attempt:' + str(n_attempts)) rospy.sleep(0.2) ''' ''' grasp_pose = place_pose grasp_pose.pose.orientation.x = grasp_init_orientation[0] grasp_pose.pose.orientation.y = grasp_init_orientation[1] grasp_pose.pose.orientation.z = grasp_init_orientation[2] grasp_pose.pose.orientation.w = grasp_init_orientation[3] grasp_pose.pose.position.x -= 0.02 grasps = self.make_grasps(grasp_pose, [table_id], [0.05, 0.07, [1.0, 0.0, 0.0]], [0.15, 0.20, [-1.0, 0.0, 0.0]]) result = None n_attempts = 0 while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo('Pick attempt:' + str(n_attempts)) rospy.sleep(0.2) ''' ''' arm.set_support_surface_name(table2_id) self.setPose(place_pose, [-0.26, -0.22, box1_size[2] + table2_size[2] + target_size[2]/2.0]) if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 places = self.make_places(place_pose, [table2_id], [0.03, 0.06, [-1.0, 0.0, 0.0]], [0.1, 0.15, [1.0, 0.0, 0.0]]) while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: for place in places: result = arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.loginfo('Place attempt:' + str(n_attempts)) rospy.sleep(0.2) ''' rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
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] - 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 # 设置动作组的两个目标点 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 print_pointlist(self, point_list, future_print_status=False): # Save original points list full_point_list = copy.deepcopy(point_list) full_point_array = np.delete(np.array(full_point_list), 2, axis=1) if future_print_status: self.print_future_visualize(full_point_list, 0, status=self.future_printing_status) self.future_printing_status = 1 # Initial the previous printing point last_ee_pose = point_list[0] # 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('plan', 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=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=True) executing_state = 1 break # Delete the points what already execute if success_num > 0: del (point_list[0:success_num - 1]) ## 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) if executing_state == 1: # if len(full_point_list) != len(point_list): self.future_printing_status = 1 # print 'success planned waypoint\n', success_planned_waypoint_array # print 'status', self.waypoint_execution_status while self.waypoint_execution_status != 3 and len( success_point_list) > 0: print(len(success_point_list)) success_planned_waypoint_array = np.delete( np.array(success_point_list), 2, axis=1) if self.waypoint_execution_status == 4 or self.move_group_execution_status == 4: # aborted state print 'stop and abort waypoint execution' self.group.stop() executing_state = 0 self.group.clear_pose_targets() break # if self.waypoint_execution_status == 1: # # 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') # self.group.stop() # executing_state = 0 # break # else: print('Check enclosure successful') 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 ]) # Add current printing obstacle # self.printing_visualize(last_ee_pose, (current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z), # name = 'printing point') # Update previous printing point last_ee_pose = (current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z) executed_waypoint_index = self.check_executed_waypoint_index( success_planned_waypoint_array, current_ee_position_array) del (point_list[:executed_waypoint_index + 1]) del (success_point_list[:executed_waypoint_index + 1]) index_check = self.check_executed_waypoint_index( full_point_array, current_ee_position_array) print 'index check:', index_check, executed_waypoint_index # print 'status:', self.waypoint_execution_status if future_print_status: if index_check >= self.further_printing_number: self.print_future_visualize( full_point_list, index_check, status=self.future_printing_status, point_num=index_check - self.further_printing_number) self.further_printing_number = index_check ## Replan to check for dynamic obstacle waypoints = [] # Add the current pose to make sure the path is smooth, get latest pose current_ee_pose_smooth = self.group.get_current_pose().pose waypoints.append(copy.deepcopy(current_ee_pose_smooth)) # discard the executed waypoints new_point_list = copy.deepcopy( success_point_list[executed_waypoint_index + 1:]) # Add future printing obstacle for k in new_point_list: (current_ee_pose_smooth.position.x, current_ee_pose_smooth.position.y, current_ee_pose_smooth.position.z) = k waypoints.append(copy.deepcopy(current_ee_pose_smooth)) (plan2, fraction2) = self.group.compute_cartesian_path( waypoints, 0.01, 0.00, path_constraints=constraints) if fraction2 < 0.95: ## new obstacle appear # print 'executed latest index', executed_waypoint_index # print 'fraction value', fraction,'\n' print 'new obstacle appeared along the planned path', 'fraction2:', fraction2 self.group.stop() executing_state = 0 break if self.waypoint_execution_status == 3: # waypoint successfully printed print 'status 3', executed_waypoint_index, point_list[: success_num] # del(point_list[0:executed_waypoint_index+1]) finsih_num += executed_waypoint_index + 1 elif self.waypoint_execution_status == 2 or 4: # state 2 = preempted, state 4 = aborted. print 'status 4', executed_waypoint_index, point_list[: executed_waypoint_index + 1] # del(point_list[:executed_waypoint_index+1]) # delete up till whatever is executed finsih_num += executed_waypoint_index + 1 self.group.clear_pose_targets() self.group.stop() executing_state = 0 print('point list left:', len(point_list)) self.msg_print.data = False self.group.set_path_constraints(None) print 'all finish'
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 print_pointlist(self, point_list, future_print_status=False): startime = datetime.datetime.now() # Record number of print part number_printing_part = 0 # 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)) # 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() # Way points plan_start_time = datetime.datetime.now() 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 plan_end_time = datetime.datetime.now() print 'first planing point time is', (plan_end_time - plan_start_time).seconds self.planning_time += max( (plan_end_time - plan_start_time).seconds, 1) ## 2nd loop ## always re-plan and check for obstacle while executing waypoints 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') self.group.stop() print('Removing future obstacle') self.future_printing_status = False self.enclosure(point_list[0]) break else: print('Check enclosure successful') 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: printing_start_time = datetime.datetime.now() 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 if self.waypoint_execution_status == 3: 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 = max( self.check_executed_waypoint_index( success_planned_waypoint_array, current_ee_position_array), executed_waypoint_index) # print 'executed latest index', executed_waypoint_index index_check = max( self.check_executed_waypoint_index( full_point_array, current_ee_position_array), index_check) 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') self.group.stop() print('Removing future obstacle') self.future_printing_status = False self.enclosure(full_point_list[index_check]) break 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 < 0.95: ## 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) printing_end_time = datetime.datetime.now() self.printing_time += (printing_end_time - printing_start_time).seconds number_printing_part += 1 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]) 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. 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('All time:', (finshtime - startime).seconds) print('Printing time:', self.printing_time) print('planning time:', self.planning_time) print('Travel time:', (finshtime - startime).seconds - self.printing_time - self.planning_time) print('number of printing:', number_printing_part) 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', linewidth=1.2) 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') plt3.savefig('experiment_data/' + str(self.name) + '/' + self.name + '_result.png', dpi=plt3.dpi) self.save_date() print('All finish')
def print_pointlist(self, point_list, future_print_status=True): # 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_extrude = 3.0 self.extruder_publisher.publish(self.msg_extrude) 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) self.msg_extrude = 3.0 self.extruder_publisher.publish(self.msg_extrude) 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 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.0: ## 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_extrude = 0.0 self.extruder_publisher.publish(self.msg_extrude) 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) print 'all finish'
def main(): roscpp_initialize(sys.argv) rospy.init_node('grasp_ur5', anonymous=True) robot = RobotCommander() scene = PlanningSceneInterface() arm_group = MoveGroupCommander("manipulator") q = quaternion_from_euler(1.5701, 0.0, -1.5701) pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = q[0] pose_target.orientation.y = q[1] pose_target.orientation.z = q[2] pose_target.orientation.w = q[3] pose_target.position.z = 0.23 #0.23 pose_target.position.y = 0.11 #0.11 pose_target.position.x = -0.45 #-0.45 arm_group.set_pose_target(pose_target) plan = arm_group.plan(pose_target) while plan[0]!= True: plan = arm_group.plan(pose_target) if plan[0]: traj = plan[1] arm_group.execute(traj, wait = True) arm_group.stop() arm_group.clear_pose_targets() # rospy.sleep(1) #FAKE PLAN WITHOUT RESTRICTIONS # # state = RobotState() # arm_group.set_start_state(state) pose_target.position.z = 0.77 arm_group.set_pose_target(pose_target) plan_fake = arm_group.plan(pose_target) while plan_fake[0] != True: plan_fake = arm_group.plan(pose_target) pose = arm_group.get_current_pose() constraint = Constraints() constraint.name = "restricao" orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = arm_group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 3.14 orientation_constraint.absolute_y_axis_tolerance = 3.14 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1 constraint.orientation_constraints.append(orientation_constraint) arm_group.set_path_constraints(constraint) # state = RobotState() # arm_group.set_start_state(state) pose_target.position.z = 0.77 # 0.77 # pose_target.position.y = -0.11 # -0.11 # pose_target.position.x = 0.31 # 0.31 arm_group.set_pose_target(pose_target) plan = arm_group.plan(pose_target) while plan[0]!= True: plan = arm_group.plan(pose_target) if plan[0]: traj = plan[1] arm_group.execute(traj, wait = True) arm_group.stop() arm_group.clear_pose_targets()