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 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 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 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 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, 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
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 goal_constraint.orientation_constraints[0].orientation = pose_goal.orientation goal_constraint.orientation_constraints[ 0].absolute_x_axis_tolerance = tolerance goal_constraint.orientation_constraints[ 0].absolute_y_axis_tolerance = tolerance goal_constraint.orientation_constraints[ 0].absolute_z_axis_tolerance = tolerance
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