def append_pose_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None): """ Appends a pose to the given move_group goal, returns it appended Goals for now are for the same link TODO: let it be for different links""" if goal_to_append == None: rospy.logerr("append_pose_to_move_group_goal needs a goal!") return goal_to_append = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = goal_to_append.request.goal_constraints[0].header position_c.link_name = goal_to_append.request.goal_constraints[0].link_name position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = goal_to_append.request.goal_constraints[0].header orientation_c.link_name = goal_to_append.request.goal_constraints[0].link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) goal_to_append.request.goal_constraints.append(goal_c) return goal_to_append
def create_move_group_pose_goal(self, goal_pose=Pose(), group="manipulator", end_link_name=None, plan_only=True): header = Header() header.frame_id = 'torso_base_link' header.stamp = rospy.Time.now() moveit_goal = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = header if end_link_name != None: position_c.link_name = end_link_name position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = header if end_link_name != None: orientation_c.link_name = end_link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) moveit_goal.request.goal_constraints.append(goal_c) moveit_goal.request.num_planning_attempts = 5 moveit_goal.request.allowed_planning_time = 5.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary moveit_goal.request.group_name = group return moveit_goal
def 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 createPositionConstraint(): position_constraints = [] p = PositionConstraint() p.header.frame_id = 'base_link' p.link_name = 'arm_right_tool_link' p.target_point_offset.x = 0.5 p.target_point_offset.y = -0.5 p.target_point_offset.z = 1.0 p.weight = 1.0 position_constraints.append(p) return position_constraints
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 _to_pose_constraint(pose, reference_frame, link_name, position_tolerance=_DEFAULT_POSITION_TOLERANCE): """Returns an position constraint suitable for ActionGoal's.""" pos_con = PositionConstraint() pos_con.header.frame_id = reference_frame pos_con.link_name = link_name pos_con.constraint_region.primitive_poses.append(pose) pos_con.weight = 1 region = shape_msgs.SolidPrimitive() region.type = shape_msgs.SolidPrimitive.SPHERE region.dimensions.append(position_tolerance) pos_con.constraint_region.primitives.append(region) return pos_con
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 = 1 # The number of times this plan is to be computed. Shortest solution will be reported. moveit_goal.request.allowed_planning_time = 5.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary moveit_goal.request.group_name = group return moveit_goal
def create_move_group_pose_goal(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 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 make_default_action_goal(object, pose, orientation=True): action_goal = MoveGroupGoal() action_goal.request.group_name = object._move_group action_goal.request.start_state.is_diff = True action_goal.request.planner_id = "PRMkConfigDefault" goal_constraint = Constraints() goal_constraint.position_constraints.append(PositionConstraint()) goal_constraint.position_constraints[ 0].header.frame_id = object.planning_frame goal_constraint.position_constraints[0].link_name = object.eef_link bounding_volume = BoundingVolume() solid_primitive = SolidPrimitive() solid_primitive.dimensions = [object._tolerance * object._tolerance] solid_primitive.type = solid_primitive.SPHERE bounding_volume.primitives.append(solid_primitive) bounding_volume.primitive_poses.append(pose) goal_constraint.position_constraints[0].constraint_region = bounding_volume goal_constraint.position_constraints[0].weight = 1.0 if orientation is not False: orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = object.planning_frame orientation_constraint.orientation = pose.orientation orientation_constraint.link_name = object.eef_link orientation_constraint.absolute_x_axis_tolerance = object._tolerance orientation_constraint.absolute_y_axis_tolerance = object._tolerance orientation_constraint.absolute_z_axis_tolerance = object._tolerance orientation_constraint.weight = 1.0 goal_constraint.orientation_constraints.append(orientation_constraint) action_goal.request.goal_constraints.append(goal_constraint) action_goal.request.num_planning_attempts = 5 action_goal.request.allowed_planning_time = 5 action_goal.request.workspace_parameters.header.frame_id = object.planning_frame action_goal.request.workspace_parameters.min_corner.x = -20 action_goal.request.workspace_parameters.min_corner.y = -20 action_goal.request.workspace_parameters.min_corner.z = -20 action_goal.request.workspace_parameters.max_corner.x = 20 action_goal.request.workspace_parameters.max_corner.y = 20 action_goal.request.workspace_parameters.min_corner.z = 20 action_goal.planning_options.look_around = False action_goal.planning_options.replan = True return action_goal
def motionPlanToPose(self, pose, tolerance=0.01, wait=True, **kwargs): ''' Move the arm to set of joint position goals :param joints: joint names for which the target position is specified. :param positions: target joint positions :param tolerance: allowed tolerance in the final joint positions. :param wait: if enabled, makes the fuctions wait until the target joint position is reached :type joints: list of string element type :type positions: list of float element type :type tolerance: float :type wait: bool ''' # Check arguments supported_args = ("max_velocity_scaling_factor", "max_acceleration_scaling_factor", "planner_id", "planning_scene_diff", "planning_time", "plan_only", "start_state") for arg in kwargs.keys(): if not arg in supported_args: rospy.loginfo("motionPlanToPose: unsupported argument: %s", arg) # Create goal g = MotionPlanRequest() # 1. fill in request workspace_parameters # 2. fill in request start_state try: g.start_state = kwargs["start_state"] except KeyError: g.start_state.is_diff = True # 3. fill in request goal_constraints c1 = Constraints() c1.position_constraints.append(PositionConstraint()) c1.position_constraints[0].header.frame_id = self._fixed_frame c1.position_constraints[0].link_name = self._gripper_frame # c1.position_constraints[0].target_point_offset b = BoundingVolume() s = SolidPrimitive() s.dimensions = [tolerance] s.type = s.SPHERE b.primitives.append(s) b.primitive_poses.append(pose) c1.position_constraints[0].constraint_region = b c1.position_constraints[0].weight = 1.0 c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.frame_id = self._fixed_frame c1.orientation_constraints[0].orientation = pose.orientation c1.orientation_constraints[0].link_name = self._gripper_frame c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance c1.orientation_constraints[0].weight = 1.0 g.goal_constraints.append(c1) # 4. fill in request path constraints # 5. fill in request trajectory constraints # 6. fill in request planner id try: g.planner_id = kwargs["planner_id"] except KeyError: if self.planner_id: g.planner_id = self.planner_id # 7. fill in request group name g.group_name = self._group # 8. fill in request number of planning attempts try: g.num_planning_attempts = kwargs["num_attempts"] except KeyError: g.num_planning_attempts = 1 # 9. fill in request allowed planning time try: g.allowed_planning_time = kwargs["planning_time"] except KeyError: g.allowed_planning_time = self.planning_time # 10. Fill in velocity scaling factor try: g.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"] except KeyError: pass # do not fill in at all # 11. Fill in acceleration scaling factor try: g.max_velocity_scaling_factor = kwargs["max_acceleration_scaling_factor"] except KeyError: pass # do not fill in at all result = self._mp_service(g) traj = result.motion_plan_response.trajectory.joint_trajectory.points if len(traj) < 1: rospy.logwarn('No motion plan found.') return None return traj
def moveToPose(self, pose_stamped, gripper_frame, tolerance=0.01, wait=True, **kwargs): # Check arguments supported_args = ("max_velocity_scaling_factor", "planner_id", "planning_time", "plan_only", "start_state") for arg in kwargs.keys(): if not arg in supported_args: rospy.loginfo("moveToJointPosition: unsupported argument: %s", arg) # Create goal g = MoveGroupGoal() pose_transformed = self._listener.transformPose(self._fixed_frame, pose_stamped) # 1. fill in request workspace_parameters # 2. fill in request start_state try: g.request.start_state = kwargs["start_state"] except KeyError: g.request.start_state.is_diff = True # 3. fill in request goal_constraints c1 = Constraints() c1.position_constraints.append(PositionConstraint()) c1.position_constraints[0].header.frame_id = self._fixed_frame c1.position_constraints[0].link_name = gripper_frame b = BoundingVolume() s = SolidPrimitive() s.dimensions = [tolerance * tolerance] s.type = s.SPHERE b.primitives.append(s) b.primitive_poses.append(pose_transformed.pose) c1.position_constraints[0].constraint_region = b c1.position_constraints[0].weight = 1.0 c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.frame_id = self._fixed_frame c1.orientation_constraints[0].orientation = pose_transformed.pose.orientation c1.orientation_constraints[0].link_name = gripper_frame c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance c1.orientation_constraints[0].weight = 1.0 g.request.goal_constraints.append(c1) # 4. fill in request path constraints # 5. fill in request trajectory constraints # 6. fill in request planner id try: g.request.planner_id = kwargs["planner_id"] except KeyError: if self.planner_id: g.request.planner_id = self.planner_id # 7. fill in request group name g.request.group_name = self._group # 8. fill in request number of planning attempts try: g.request.num_planning_attempts = kwargs["num_attempts"] except KeyError: g.request.num_planning_attempts = 1 # 9. fill in request allowed planning time try: g.request.allowed_planning_time = kwargs["planning_time"] except KeyError: g.request.allowed_planning_time = self.planning_time # Fill in velocity scaling factor try: g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"] except KeyError: pass # do not fill in at all # 10. fill in planning options diff g.planning_options.planning_scene_diff.is_diff = True g.planning_options.planning_scene_diff.robot_state.is_diff = True # 11. fill in planning options plan only try: g.planning_options.plan_only = kwargs["plan_only"] except KeyError: g.planning_options.plan_only = self.plan_only # 12. fill in other planning options g.planning_options.look_around = False g.planning_options.replan = False # 13. send goal self._action.send_goal(g) if wait: self._action.wait_for_result() return self._action.get_result() else: return None
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 build(self, tf_timeout=rospy.Duration(5.0)): """Builds the goal message. To set a pose or joint goal, call set_pose_goal or set_joint_goal before calling build. To add a path orientation constraint, call add_path_orientation_constraint first. Args: tf_timeout: rospy.Duration. How long to wait for a TF message. Only used with pose goals. Returns: moveit_msgs/MoveGroupGoal """ goal = MoveGroupGoal() # Set start state goal.request.start_state = copy.deepcopy(self.start_state) # Set goal constraint if self._pose_goal is not None: self._tf_listener.waitForTransform(self._pose_goal.header.frame_id, self.fixed_frame, rospy.Time.now(), tf_timeout) try: pose_transformed = self._tf_listener.transformPose( self.fixed_frame, self._pose_goal) except (tf.LookupException, tf.ConnectivityException): return None c1 = Constraints() c1.position_constraints.append(PositionConstraint()) c1.position_constraints[0].header.frame_id = self.fixed_frame c1.position_constraints[0].link_name = self.gripper_frame b = BoundingVolume() s = SolidPrimitive() s.dimensions = [self.tolerance * self.tolerance] s.type = s.SPHERE b.primitives.append(s) b.primitive_poses.append(self._pose_goal.pose) c1.position_constraints[0].constraint_region = b c1.position_constraints[0].weight = 1.0 c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.frame_id = self.fixed_frame c1.orientation_constraints[ 0].orientation = pose_transformed.pose.orientation c1.orientation_constraints[0].link_name = self.gripper_frame c1.orientation_constraints[ 0].absolute_x_axis_tolerance = self.tolerance c1.orientation_constraints[ 0].absolute_y_axis_tolerance = self.tolerance c1.orientation_constraints[ 0].absolute_z_axis_tolerance = self.tolerance c1.orientation_constraints[0].weight = 1.0 goal.request.goal_constraints.append(c1) elif self._joint_names is not None: c1 = Constraints() for i in range(len(self._joint_names)): c1.joint_constraints.append(JointConstraint()) c1.joint_constraints[i].joint_name = self._joint_names[i] c1.joint_constraints[i].position = self._joint_positions[i] c1.joint_constraints[i].tolerance_above = self.tolerance c1.joint_constraints[i].tolerance_below = self.tolerance c1.joint_constraints[i].weight = 1.0 goal.request.goal_constraints.append(c1) # Set path constraints goal.request.path_constraints.orientation_constraints = self._orientation_contraints # Set trajectory constraints # Set planner ID (name of motion planner to use) goal.request.planner_id = self.planner_id # Set group name goal.request.group_name = self.group_name # Set planning attempts goal.request.num_planning_attempts = self.num_planning_attempts # Set planning time goal.request.allowed_planning_time = self.allowed_planning_time # Set scaling factors goal.request.max_acceleration_scaling_factor = self.max_acceleration_scaling_factor goal.request.max_velocity_scaling_factor = self.max_velocity_scaling_factor # Set planning scene diff goal.planning_options.planning_scene_diff = copy.deepcopy( self.planning_scene_diff) # Set is plan only goal.planning_options.plan_only = self.plan_only # Set look around goal.planning_options.look_around = self.look_around # Set replanning options goal.planning_options.replan = self.replan goal.planning_options.replan_attempts = self.replan_attempts goal.planning_options.replan_delay = self.replan_delay return goal
def deliver_sample(move_arm, args): """ :type move_arm: class 'moveit_commander.move_group.MoveGroupCommander' :type args: List[bool, float, float, float] """ move_arm.set_planner_id("RRTstar") x_delivery = args[1] y_delivery = args[2] z_delivery = args[3] # after sample collect mypi = 3.14159 d2r = mypi / 180 r2d = 180 / mypi goal_pose = move_arm.get_current_pose().pose # position was found from rviz tool goal_pose.position.x = x_delivery goal_pose.position.y = y_delivery goal_pose.position.z = z_delivery r = -179 p = -20 y = -90 q = quaternion_from_euler(r * d2r, p * d2r, y * d2r) goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) move_arm.set_pose_target(goal_pose) plan = move_arm.plan() if len(plan.joint_trajectory.points) == 0: # If no plan found, abort return False plan = move_arm.go(wait=True) move_arm.stop() # rotate scoop to deliver sample at current location... # adding position constraint on the solution so that the tip doesnot diverge to get to the solution. pos_constraint = PositionConstraint() pos_constraint.header.frame_id = "base_link" pos_constraint.link_name = "l_scoop" pos_constraint.target_point_offset.x = 0.1 pos_constraint.target_point_offset.y = 0.1 # rotate scoop to deliver sample at current location begin pos_constraint.target_point_offset.z = 0.1 pos_constraint.constraint_region.primitives.append( SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) pos_constraint.weight = 1 # using euler angles for own verification.. r = +180 p = 90 # 45 worked get y = -90 q = quaternion_from_euler(r * d2r, p * d2r, y * d2r) goal_pose = move_arm.get_current_pose().pose rotation = (goal_pose.orientation.x, goal_pose.orientation.y, goal_pose.orientation.z, goal_pose.orientation.w) euler_angle = euler_from_quaternion(rotation) goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) move_arm.set_pose_target(goal_pose) plan = move_arm.plan() if len(plan.joint_trajectory.points) == 0: # If no plan found, abort return False plan = move_arm.go(wait=True) move_arm.stop() move_arm.clear_pose_targets() move_arm.set_planner_id("RRTconnect") return True
def on_enter(self, userdata): self._planning_failed = False self._control_failed = False self._success = False # TODO check userdata # if not isinstance(userdata.pose, PoseStamped): #Logger.logwarn('userdata.pose must be geomery_msgs.msg.PoseStamped. `%s` received' % str(type(userdata.pose))) #self._planning_failed = True #return check_type('pose', 'geometry_msgs/PoseStamped', userdata.pose) # request planing and execution action_goal = MoveGroupGoal() # set palnning options action_goal.planning_options.plan_only = self._plan_only action_goal.planning_options.replan = False # action_goal.planning_options.planning_scene_diff.is_diff = True # action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True # setup request action_goal.request.group_name = self._move_group action_goal.request.num_planning_attempts = 3 action_goal.request.allowed_planning_time = 1.0 action_goal.request.max_velocity_scaling_factor = 1.0 action_goal.request.max_acceleration_scaling_factor = 1.0 # start pose action_goal.request.start_state.is_diff = True # pose constraint goal_constraint = Constraints(name='') # set target position constraint = PositionConstraint() constraint.header = Header(frame_id=userdata.pose.header.frame_id) constraint.link_name = self._end_effector constraint.constraint_region = BoundingVolume() constraint.constraint_region.primitives = [ SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[self._position_tolerance]) ] constraint.constraint_region.primitive_poses = [ Pose(position=userdata.pose.pose.position) ] constraint.weight = 1.0 goal_constraint.position_constraints.append(constraint) # set target orientation constraint = OrientationConstraint() constraint.header = Header(frame_id=userdata.pose.header.frame_id) constraint.link_name = self._end_effector constraint.orientation = userdata.pose.pose.orientation constraint.absolute_x_axis_tolerance = self._orientation_tolerance constraint.absolute_y_axis_tolerance = self._orientation_tolerance constraint.absolute_z_axis_tolerance = self._orientation_tolerance constraint.weight = 0.1 goal_constraint.orientation_constraints.append(constraint) # add goal_constraint action_goal.request.goal_constraints.append(goal_constraint) try: self._client.send_goal('move_group', action_goal) except Exception as e: Logger.logwarn( 'MoveitToPose: Failed to send MoveGroupAction goal for group: %s\n%s' % (self._move_group, str(e))) self._planning_failed = True
def dip_incrementally(self, z_pose): # time limited and incremental movement group = self.group position_constraint = PositionConstraint() position_constraint.target_point_offset.x = 0.1 position_constraint.target_point_offset.y = 0.1 position_constraint.target_point_offset.z = 0.3 position_constraint.weight = 0.75 position_constraint.link_name = group.get_end_effector_link() position_constraint.header.frame_id = group.get_planning_frame() constraint = Constraints() constraint.position_constraints.append(position_constraint) group.set_path_constraints(constraint) current_pose = group.get_current_pose().pose current_orientation = group.get_current_pose().pose.orientation curr_q = [ current_orientation.x, current_orientation.y, current_orientation.z, current_orientation.w ] allow_replanning = False planning_time = 5 print "Now performing dip" tolerance = 0.01 time_limit = 120 start_time = time.time() offset_x = 0.0 target_x = current_pose.position.x - offset_x status = self.go_to_pose_goal(curr_q[0], curr_q[1], curr_q[2], curr_q[3], target_x, current_pose.position.y, current_pose.position.z, allow_replanning, planning_time, thresh=tolerance) while (current_pose.position.z - z_pose) > 2 * tolerance and ( time.time() - start_time) < time_limit: status = self.go_to_pose_goal(curr_q[0], curr_q[1], curr_q[2], curr_q[3], target_x, current_pose.position.y, current_pose.position.z - 0.01, allow_replanning, planning_time, thresh=tolerance) current_pose = group.get_current_pose().pose rospy.sleep(0.1) group.clear_path_constraints() rospy.sleep(0.05) dip = (current_pose.position.z - z_pose) < 2 * tolerance print("dip_incrementally: dip successful? " + str(dip)) return dip
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()
print('eef_link:', eef_link) # ('eef_link:', 'panda_link8') tolerance = 0.01 pose_goal = Pose() pose_goal.orientation.w = 1.0 pose_goal.position.x = 1.0 pose_goal.position.y = 0.15 pose_goal.position.z = 0.4 action_goal = MoveGroupGoal() action_goal.request.group_name = move_group goal_constraint = Constraints() goal_constraint.position_constraints.append(PositionConstraint()) goal_constraint.position_constraints[0].header.frame_id = planning_frame goal_constraint.position_constraints[0].link_name = eef_link solid_primitive = SolidPrimitive() solid_primitive.dimensions = [tolerance * tolerance] solid_primitive.type = solid_primitive.SPHERE bounding_volume = BoundingVolume() bounding_volume.primitives.append(solid_primitive) bounding_volume.primitive_poses.append(pose_goal) goal_constraint.position_constraints[0].constraint_region = bounding_volume goal_constraint.orientation_constraints.append(OrientationConstraint()) goal_constraint.orientation_constraints[0].header.frame_id = planning_frame goal_constraint.orientation_constraints[0].link_name = eef_link
def moveToPose(self, pose, tolerance=0.01, wait=True, **kwargs): ''' Move the arm, based on a goal pose_stamped for the end effector. :param pose: target pose to which we want to move specified link to :param tolerance: allowed tolerance in the final joint positions. :param wait: if enabled, makes the fuctions wait until the target joint position is reached :type pose_stamped: ros message object of type PoseStamped :type gripper_frame: string :type tolerance: float :type wait: bool ''' # Check arguments supported_args = ("max_velocity_scaling_factor", "planner_id", "planning_time", "plan_only", "start_state") for arg in kwargs.keys(): if not arg in supported_args: rospy.loginfo("moveToPose: unsupported argument: %s", arg) # Create goal g = MoveGroupGoal() # 1. fill in request workspace_parameters # 2. fill in request start_state try: g.request.start_state = kwargs["start_state"] except KeyError: g.request.start_state.is_diff = True # 3. fill in request goal_constraints c1 = Constraints() c1.position_constraints.append(PositionConstraint()) c1.position_constraints[0].header.frame_id = self._fixed_frame c1.position_constraints[0].link_name = self._gripper_frame # c1.position_constraints[0].target_point_offset b = BoundingVolume() s = SolidPrimitive() s.dimensions = [tolerance] s.type = s.SPHERE b.primitives.append(s) b.primitive_poses.append(pose) c1.position_constraints[0].constraint_region = b c1.position_constraints[0].weight = 1.0 c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.frame_id = self._fixed_frame c1.orientation_constraints[0].orientation = pose.orientation c1.orientation_constraints[0].link_name = self._gripper_frame c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance c1.orientation_constraints[0].weight = 1.0 g.request.goal_constraints.append(c1) # 4. fill in request path constraints # 5. fill in request trajectory constraints # 6. fill in request planner id try: g.request.planner_id = kwargs["planner_id"] except KeyError: if self.planner_id: g.request.planner_id = self.planner_id # 7. fill in request group name g.request.group_name = self._group # 8. fill in request number of planning attempts try: g.request.num_planning_attempts = kwargs["num_attempts"] except KeyError: g.request.num_planning_attempts = 1 # 9. fill in request allowed planning time try: g.request.allowed_planning_time = kwargs["planning_time"] except KeyError: g.request.allowed_planning_time = self.planning_time # Fill in velocity scaling factor try: g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"] except KeyError: pass # do not fill in at all # 10. fill in planning options diff g.planning_options.planning_scene_diff.is_diff = True g.planning_options.planning_scene_diff.robot_state.is_diff = True # 11. fill in planning options plan only try: g.planning_options.plan_only = kwargs["plan_only"] except KeyError: g.planning_options.plan_only = self.plan_only # 12. fill in other planning options g.planning_options.look_around = False g.planning_options.replan = False # 13. send goal self._action.send_goal(g) if wait: self._action.wait_for_result() result = self._action.get_result() return processResult(result) else: rospy.loginfo('Failed while waiting for action result.') return False
def make_default_action_goal(object, pose, orientation=True): action_goal = MoveGroupGoal() action_goal.request.group_name = object._move_group action_goal.request.start_state.is_diff = True action_goal.request.planner_id = "RRTkConfigDefault" # PRMkConfigDefault, PRMstarkConfigDefault, RRTkConfigDefault, RRTstarkConfigDefault goal_constraint = Constraints() goal_constraint.position_constraints.append(PositionConstraint()) goal_constraint.position_constraints[ 0].header.frame_id = object.planning_frame goal_constraint.position_constraints[0].link_name = object.eef_link bounding_volume = BoundingVolume() solid_primitive = SolidPrimitive() solid_primitive.dimensions = [object._tolerance] solid_primitive.type = solid_primitive.SPHERE bounding_volume.primitives.append(solid_primitive) # if object.selected_object_name[:8] == "postcard": # p_update = pose # p_update.position.z = 0.75 # bounding_volume.primitive_poses.append(p_update) #elif pose.position.z <= 0.1: #地面に当たらないように # if pose.position.z <= 0.1: #地面に当たらないように # p_update = pose # p_update.position.z = 0.13 # bounding_volume.primitive_poses.append(p_update) # else: bounding_volume.primitive_poses.append(pose) goal_constraint.position_constraints[0].constraint_region = bounding_volume goal_constraint.position_constraints[0].weight = 1.0 if orientation is not False: orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = object.planning_frame if pose.position.z <= 0.2: # Stage 2 (Grasp) rospy.loginfo("Planning num:0-0 (Grasp)") orientation_constraint.orientation.x = 0.675 orientation_constraint.orientation.y = 0.737 orientation_constraint.orientation.z = 0.018 orientation_constraint.orientation.w = -0.019 #goal_constraint.position_constraints[0].constraint_region.primitive_poses[0].position.z -= 0.01 # Stage1 # elif object.selected_object_name[:8] == "postcard": # #elif pose.position.z < 0.02: # Stage 1 # object. # orientation_constraint.orientation.x = 0.0#0.904 # orientation_constraint.orientation.y = 0.0#-0.166 # orientation_constraint.orientation.z = -0.676#-0.068 # orientation_constraint.orientation.w = 0.736#0.387 # #orientation_constraint.link_name = "hand_palm_link" elif object.count_num == 1: # Stage 2 (Sofa) rospy.loginfo("Planning num:1 (Put on sofa)") orientation_constraint.orientation.x = -0.019 orientation_constraint.orientation.y = -0.707 orientation_constraint.orientation.z = -0.202 orientation_constraint.orientation.w = 0.706 elif object.count_num == 2: # Stage 2 (Work desk, Shelf) rospy.loginfo("Planning num:2 (Put on shelf or desk)") orientation_constraint.orientation.x = 0.51 orientation_constraint.orientation.y = -0.489 orientation_constraint.orientation.z = 0.509 orientation_constraint.orientation.w = 0.489 elif object.count_num == 3: # Stage 2 (White table) rospy.loginfo("Planning num:3 (Put on table)") orientation_constraint.orientation.x = 0.51 orientation_constraint.orientation.y = 0.489 orientation_constraint.orientation.z = 0.501 orientation_constraint.orientation.w = -0.497 else: rospy.loginfo("Planning num:0-1 (Put)") orientation_constraint.orientation = pose.orientation orientation_constraint.link_name = object.eef_link orientation_constraint.absolute_x_axis_tolerance = object._tolerance * 100 orientation_constraint.absolute_y_axis_tolerance = object._tolerance * 100 orientation_constraint.absolute_z_axis_tolerance = object._tolerance * 100 orientation_constraint.weight = 1.0 goal_constraint.orientation_constraints.append(orientation_constraint) action_goal.request.goal_constraints.append(goal_constraint) action_goal.request.num_planning_attempts = 5 action_goal.request.allowed_planning_time = 10.0 action_goal.request.workspace_parameters.header.frame_id = object.planning_frame action_goal.request.workspace_parameters.min_corner.y = -5.0 #-20.0 action_goal.request.workspace_parameters.min_corner.x = -5.0 #-20.0 action_goal.request.workspace_parameters.min_corner.z = -5.0 #-20.0 action_goal.request.workspace_parameters.max_corner.x = 5.0 #20.0 action_goal.request.workspace_parameters.max_corner.y = 5.0 #20.0 action_goal.request.workspace_parameters.min_corner.z = 5.0 #20.0 action_goal.planning_options.look_around = False action_goal.planning_options.replan = True action_goal.planning_options.replan_attempts = 5 action_goal.planning_options.replan_delay = 10.0 return action_goal
def deliver_sample(move_arm, robot, moveit_fk, args): """ :type move_arm: class 'moveit_commander.move_group.MoveGroupCommander' :type args: List[bool, float, float, float] """ move_arm.set_planner_id("RRTstar") robot_state = robot.get_current_state() move_arm.set_start_state(robot_state) x_delivery = args.delivery.x y_delivery = args.delivery.y z_delivery = args.delivery.z # after sample collect mypi = 3.14159 d2r = mypi / 180 r2d = 180 / mypi goal_pose = move_arm.get_current_pose().pose # position was found from rviz tool goal_pose.position.x = x_delivery goal_pose.position.y = y_delivery goal_pose.position.z = z_delivery r = -179 p = -20 y = -90 q = quaternion_from_euler(r * d2r, p * d2r, y * d2r) goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) move_arm.set_pose_target(goal_pose) _, plan_a, _, _ = move_arm.plan() if len(plan_a.joint_trajectory.points) == 0: # If no plan found, abort return False # rotate scoop to deliver sample at current location... # adding position constraint on the solution so that the tip doesnot diverge to get to the solution. pos_constraint = PositionConstraint() pos_constraint.header.frame_id = "base_link" pos_constraint.link_name = "l_scoop" pos_constraint.target_point_offset.x = 0.1 pos_constraint.target_point_offset.y = 0.1 # rotate scoop to deliver sample at current location begin pos_constraint.target_point_offset.z = 0.1 pos_constraint.constraint_region.primitives.append( SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) pos_constraint.weight = 1 # using euler angles for own verification.. r = +180 p = 90 # 45 worked get y = -90 q = quaternion_from_euler(r * d2r, p * d2r, y * d2r) cs, start_state, goal_pose = calculate_joint_state_end_pose_from_plan_arm( robot, plan_a, move_arm, moveit_fk) move_arm.set_start_state(cs) goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) move_arm.set_pose_target(goal_pose) _, plan_b, _, _ = move_arm.plan() if len(plan_b.joint_trajectory.points ) == 0: # If no plan found, send the previous plan only return plan_a deliver_sample_traj = cascade_plans(plan_a, plan_b) # move_arm.set_planner_id("RRTconnect") return deliver_sample_traj
def liftgripper(self, target_z=0.10): # approx centers of onions at 0.82, width of onion is 0.038 m. table is at 0.78 # length of gripper is 0.163 m The gripper should not go lower than # (height_z of table w.r.t base+gripper-height/2+tolerance) = 0.78-0.93+0.08+0.01=-0.24 # pnp._limb.endpoint_pose returns {'position': (x, y, z), 'orientation': (x, y, z, w)} # moving from z=-.02 to z=-0.1 group = self.group while self.target_location_x == -100: rospy.sleep(0.05) current_pose = group.get_current_pose().pose current_orientation = group.get_current_pose().pose.orientation curr_q = [ current_orientation.x, current_orientation.y, current_orientation.z, current_orientation.w ] position_constraint = PositionConstraint() position_constraint.target_point_offset.x = 0.1 position_constraint.target_point_offset.y = 0.2 position_constraint.target_point_offset.z = 0.3 position_constraint.weight = 0.8 position_constraint.link_name = group.get_end_effector_link() position_constraint.header.frame_id = group.get_planning_frame() orientation_constraint = OrientationConstraint() orientation_constraint.orientation = Quaternion(x=curr_q[0], y=curr_q[1], z=curr_q[2], w=curr_q[3]) orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 0.1 orientation_constraint.weight = 0.2 orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.header.frame_id = group.get_planning_frame() constraint = Constraints() constraint.orientation_constraints.append(orientation_constraint) constraint.position_constraints.append(position_constraint) group.set_path_constraints(constraint) allow_replanning = True planning_time = 5 threshold = 0.02 time_limit = 120 start_time = time.time() target_x = current_pose.position.x target_y = current_pose.position.y delta_z = (target_z - current_pose.position.z) / 4 print "Attempting to lift gripper" i = 1 # if (delta_z < threshold): # return True # while (current_pose.position.z < target_z) and ((target_z - current_pose.position.z) > 3*threshold) and ((time.time()-start_time) < time_limit): # status = self.go_to_pose_goal(curr_q[0], curr_q[1], curr_q[2], curr_q[3], # target_x, target_y, current_pose.position.z + i*delta_z, # allow_replanning, planning_time, thresh=threshold) # rospy.sleep(0.1) # current_pose = group.get_current_pose().pose # i += 1 status = self.go_to_pose_goal(curr_q[0], curr_q[1], curr_q[2], curr_q[3], target_x, target_y, target_z, allow_replanning, planning_time, thresh=threshold) rospy.sleep(0.25) print "Successfully lifted gripper to z: ", current_pose.position.z group.clear_path_constraints() # return ((target_z-current_pose.position.z) < 3*threshold or (current_pose.position.z > target_z)) return True
def flip(cls): ''' rotate gripper 180 degree about y :return: ''' # return ManipulatorActions.flip_kinova() mg = ManipulatorActions.get_move_group() ManipulatorActions.with_planning_scene() pose = mg.get_current_pose() pose.pose.position.z = 0.4 quat = [ pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w ] current_euler = euler_from_quaternion(quat) pose.pose.orientation = ManipulatorActions.getGraspOrientation( 0, 100, 0, return_msg=True) mg.set_pose_target(pose) from moveit_msgs.msg import Constraints, PositionConstraint eef_on_line = PositionConstraint() eef_on_line.header.frame_id = 'base_link' eef_on_line.link_name = mg.get_end_effector_link() eef_on_line.target_point_offset.x = 0.0 eef_on_line.target_point_offset.y = 0.0 eef_on_line.target_point_offset.z = 0.0 eef_on_line.weight = 1 tolerance = 0.2 line = SolidPrimitive() line.type = SolidPrimitive.SPHERE line.dimensions.append(0.5) # line.dimensions.append(2.1) # line.dimensions.append(2.1) eef_on_line.constraint_region.primitives.append(line) box_pose = mg.get_current_pose().pose box_pose.orientation = Quaternion() box_pose.orientation.w = 1 eef_on_line.constraint_region.primitive_poses.append(box_pose) eef_on_line.header.frame_id = mg.get_planning_frame() flip_ct = Constraints() flip_ct.position_constraints.append(eef_on_line) # mg.set_path_constraints(flip_ct) goal_tol = mg.get_goal_position_tolerance() goal_tol_rot = mg.get_goal_orientation_tolerance() mg.set_goal_position_tolerance(0.3) rospy.sleep(0.5) mg.set_goal_orientation_tolerance(0.3) mg.set_planning_time(10.0) rospy.sleep(0.5) plan = mg.plan() mg.set_goal_position_tolerance(goal_tol) mg.set_goal_orientation_tolerance(goal_tol_rot) mg.set_planning_time(5.0) if len(plan.joint_trajectory.points) > 1: mg.execute(plan) return True, 'flip successful' else: return False, 'flip failed' pass