class PlanAffordanceState(EventState): ''' Plans for a given affordance without collision avoidance. -- vel_scaling float Scales the velocity of the planned trajectory, lower values for slower trajectories. -- planner_id string Sets the ID of the planner to use (MoveIt planner id or "drake" - default = "RRTConnectkConfigDefault") -- drake_sample_rate float Sample rate for Drake's result trajectory (in Hz, default = 4.0) -- drake_orientation_type int How should the eef-orientation be handled (see ExtendedPlanningOptions, 0 = full, 1 = axis only, 2 = ignore) -- drake_link_axis float[] Target axis to keep (only for drake_orientation_type == ORIENTATION_AXIS_ONLY ># affordance Affordance A message as defined in vigir_object_template_msgs containing all required information. ># hand string The hand which should execute the given affordance. ># reference_point PoseStamped A point of interest on the template w.r.t. which planning will take place (defaults to palm if not set). A template's 'usability' pose would go here. #> joint_trajectory JointTrajectory Trajectory of the end effector to perform the requested motion. #> plan_fraction float Fraction of the requested motion which could be planned. A value below 1 indicates a partial motion. <= done Successfully planned the given affordance. <= failed Failed to plan the affordance. ''' LEFT_HAND = 'left' RIGHT_HAND = 'right' def __init__(self, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault", drake_sample_rate=4.0, drake_orientation_type=2, drake_link_axis=[0, 0, 1]): '''Constructor''' super(PlanAffordanceState, self).__init__( outcomes=['done', 'incomplete', 'failed'], input_keys=['affordance', 'hand', 'reference_point'], output_keys=['joint_trajectory', 'plan_fraction']) self._client = ProxyMoveitClient() try: self.set_up_affordance_service() # topics and clients are set here except Exception as e: Logger.logerr( 'Failed to set up GetAffordanceInWristFrame service: \n%s' % str(e)) return self._vel_scaling = vel_scaling self._planner_id = planner_id self._drake_sample_rate = drake_sample_rate self._drake_orientation_type = drake_orientation_type self._drake_link_axis = Point(x=drake_link_axis[0], y=drake_link_axis[1], z=drake_link_axis[2]) self._failed = False self._done = False self._incomplete = False def execute(self, userdata): '''Wait for result and determine outcome based on fraction thresholds''' fail_threshold = 0.05 success_threshold = 0.95 if self._failed: return 'failed' if self._done: return 'done' if self._incomplete: return 'incomplete' if self._client.finished(): userdata.joint_trajectory = self._client.get_plan() if self._client.success(): plan_fraction = self._client.get_plan_fraction() Logger.loginfo('Plan completion fraction = %f%%' % (plan_fraction * 100)) userdata.plan_fraction = plan_fraction if plan_fraction < fail_threshold: Logger.logwarn( 'Failure! The plan fraction was below %d%%' % (fail_threshold * 100)) self._failed = True return 'failed' elif plan_fraction > success_threshold: Logger.loginfo( 'Success! The plan fraction was above %d%%' % (success_threshold * 100)) self._done = True return 'done' else: Logger.logwarn( 'Partial success.The plan fraction was between %d%% and %d%%' % (fail_threshold * 100, success_threshold * 100)) self._incomplete = True return 'incomplete' else: Logger.logwarn('MoveIt failed with the following error: %s' % self._client.error_msg()) self._failed = True return 'failed' def on_enter(self, userdata): '''Handle cartesian/circular affordance, set options, and send goal.''' self._failed = False self._done = False self._incomplete = False planning_group_str = 'l_arm_group' if userdata.hand == PlanAffordanceState.LEFT_HAND else 'r_arm_group' affordance = userdata.affordance print affordance #TODO: remove temp debug statement # Create the manipulation action goal self._client.new_goal(move_group=planning_group_str) if affordance.type == "circular": self._client.set_circular_motion(angle=affordance.displacement, pitch=affordance.pitch) for waypoint in affordance.waypoints: # There should only be one self._client.add_endeffector_pose(waypoint.pose, waypoint.header.frame_id) print waypoint #TODO: remove temp debug statement elif affordance.type == "cartesian": self._client.set_cartesian_motion() try: srv_request = GetAffordanceInWristFrameRequest(affordance) srv_result = self._srv.call( self._service_topics[userdata.hand], srv_request) wrist_affordance = srv_result.wrist_affordance # PoseStamped print wrist_affordance #TODO: remove temp debug statement self._client.add_endeffector_pose( wrist_affordance.pose, wrist_affordance.header.frame_id) except Exception as e: Logger.logwarn( 'Failed to transform affordance to wrist frame: \n%s' % str(e)) self._failed = True return else: Logger.logwarn("Unknown affordance type: %s" % affordance.type) self._failed = True return # Set planning options self._client.set_planner_id(self._planner_id) self._client.set_velocity_scaling(self._vel_scaling) self._client.set_collision_avoidance(True) self._client.set_execute_incomplete_plans(True) self._client.set_keep_orientation(affordance.keep_orientation) #Set reference_point and frame, if provided. Defaults to palm. if userdata.reference_point is not None: self._client.set_reference_point( pose=userdata.reference_point.pose, frame_id=userdata.reference_point.header.frame_id) #Set Drake specific options self._client.set_drake_target_orientation_type( self._drake_orientation_type) self._client.set_drake_target_link_axis(self._drake_link_axis) self._client.set_drake_trajectory_sample_rate(self._drake_sample_rate) try: self._client.start_planning() except Exception as e: Logger.logwarn('Could not request a plan for the affordance!\n%s' % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.finished(): self._client.cancel() Logger.loginfo("Cancelled active action goal.") ############################################################################ def set_up_affordance_service(self): hands_in_use = self.determine_hands_in_use() # Set up services for transforming cartesian affordances to wrist frame self._service_topics = dict() for hand in hands_in_use: topic = '/manipulation_control/%s_hand/wrist_affordance_srv' % hand[ 0] self._service_topics[hand] = topic self._srv = ProxyServiceCaller({ t: GetAffordanceInWristFrame for t in self._service_topics.values() }) def determine_hands_in_use(self): '''Determine which hand types and sides have been sourced''' if not rospy.has_param("behavior/hand_type_prefix"): Logger.logerr( "Need to specify behavior/parameter hand_type_prefix at the parameter server" ) return hand_type_prefix = rospy.get_param("behavior/hand_type_prefix") hands_in_use = list() LEFT_HAND = os.environ[hand_type_prefix + 'LEFT_HAND_TYPE'] RIGHT_HAND = os.environ[hand_type_prefix + 'RIGHT_HAND_TYPE'] if LEFT_HAND not in ['l_stump', 'l_hook']: hands_in_use.append('left') if RIGHT_HAND not in ['r_stump', 'r_hook']: hands_in_use.append('right') if len(hands_in_use) == 0: Logger.logerr( 'No hands seem to be in use:\nLEFT_HAND = %s\nRIGHT_HAND = %s' % (LEFT_HAND, RIGHT_HAND)) return hands_in_use
class PlanEndeffectorPoseState(EventState): ''' Plans to reach the given pose with the specified endeffector. -- ignore_collisions boolean Should collisions be ignored? Only pass True if you are sure that it is safe. -- include_torso boolean Should the torso be included in the trajectory or stay fixed? -- allowed_collisions tuple[] List of 2-tuples (strings) for each pair of links which is allowed to collide. Set the second value to an empty string if collision to any other link is allowed. -- planner_id string Sets the ID of the planner to use (MoveIt planner id or "drake" - default = "RRTConnectkConfigDefault") ># target_pose PoseStamped Target pose to reach. ># hand string One of the class constants determining the hand to be moved. #> joint_trajectory JointTrajectory Trajectory of the end effector to perform the requested motion. <= planned Was able to generate a valid motion plan. <= failed Failed to create a motion plan at all. ''' LEFT_HAND = 'left' RIGHT_HAND = 'right' def __init__(self, ignore_collisions = False, include_torso = False, allowed_collisions = [], planner_id = "RRTConnectkConfigDefault"): ''' Constructor ''' super(PlanEndeffectorPoseState, self).__init__(outcomes=['planned', 'failed'], input_keys=['target_pose', 'hand'], output_keys=['joint_trajectory']) self._client = ProxyMoveitClient() self._ignore_collisions = ignore_collisions self._include_torso = include_torso self._allowed_collisions = allowed_collisions self._failed = False self._planner_id = planner_id def execute(self, userdata): ''' Execute this state ''' if self._failed: return 'failed' if self._client.finished(): userdata.joint_trajectory = self._client.get_plan() if self._client.success(): return 'planned' else: Logger.logwarn('MoveIt failed with the following error: %s' % self._client.error_msg()) self._failed = True return 'failed' def on_enter(self, userdata): if not hasattr(userdata, 'target_pose') or userdata.target_pose is None: self._failed = True Logger.logwarn('Userdata "target_pose" of state %s does not exist or is currently undefined!' % self.name) return if not hasattr(userdata, 'hand') or userdata.hand is None: self._failed = True Logger.logwarn('Userdata "hand" of state %s does not exist or is currently undefined!' % self.name) return self._failed = False planning_group_str = \ ('l' if userdata.hand == self.LEFT_HAND else 'r') + \ '_arm' + \ ('_with_torso' if self._include_torso else '') + \ '_group' # create the motion goal self._client.new_goal(planning_group_str) self._client.add_endeffector_pose(userdata.target_pose.pose, userdata.target_pose.header.frame_id) self._client.set_collision_avoidance(self._ignore_collisions) self._client.set_planner_id(self._planner_id) for link, target in self._allowed_collisions: self._client.set_allowed_collision(link, target) # for later use #for link, target in self._allowed_collisions: Logger.loginfo('Sending planning request to reach point (%f, %f, %f) in frame %s...' % (userdata.target_pose.pose.position.x, userdata.target_pose.pose.position.y, userdata.target_pose.pose.position.z, userdata.target_pose.header.frame_id)) try: self._client.start_planning() except Exception as e: Logger.logwarn('Could not request a plan!\n%s' % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.finished(): self._client.cancel() Logger.loginfo("Cancelled active action goal.")
class PlanEndeffectorCircularMotionState(EventState): ''' Plans a circular motion of the endeffector. -- ignore_collisions boolean Should collisions be ignored? Only pass True if you are sure that it is safe. -- include_torso boolean Should the torso be included in the trajectory or stay fixed? -- keep_endeffector_orientation boolean Should the initial orientation of the endeffector be kept? -- planner_id string Sets the ID of the planner to use (MoveIt planner id or "drake" - default = "RRTConnectkConfigDefault") ># hand string One of the class constants determining the hand to be moved. ># rotation_axis PoseStamped Pose in the center of the rotation, can be retrieved from a template pose. ># rotation_angle float Angle of the rotation in radians. #> joint_trajectory JointTrajectory Trajectory of the endeffector to perform the requested motion. #> plan_fraction float Fraction of the requested motion which could be planned. A value below 1 indicates a partial motion. <= planned Was able to generate a valid motion plan. This plan is not necessarily complete. <= failed Failed to create a motion plan at all. ''' LEFT_HAND = 'left' RIGHT_HAND = 'right' def __init__(self, ignore_collisions = False, include_torso = False, keep_endeffector_orientation = False, planner_id = "RRTConnectkConfigDefault"): ''' Constructor ''' super(PlanEndeffectorCircularMotionState, self).__init__(outcomes=['planned', 'failed'], input_keys=['hand', 'rotation_axis', 'rotation_angle'], output_keys=['joint_trajectory', 'plan_fraction']) self._client = ProxyMoveitClient() self._ignore_collisions = ignore_collisions self._include_torso = include_torso self._keep_endeffector_orientation = keep_endeffector_orientation self._failed = False self._planner_id = planner_id def execute(self, userdata): ''' Execute this state ''' if self._failed: return 'failed' if self._client.finished(): userdata.joint_trajectory = self._client.get_plan() if self._client.success(): userdata.plan_fraction = 1 return 'planned' else: Logger.logwarn('MoveIt failed with the following error: %s' % self._client.error_msg()) self._failed = True return 'failed' def on_enter(self, userdata): if not hasattr(userdata, 'rotation_axis') or userdata.rotation_axis is None \ or not hasattr(userdata, 'hand') or userdata.hand is None \ or not hasattr(userdata, 'rotation_angle') or userdata.rotation_angle is None: self._failed = True Logger.logwarn('Userdata key of state %s does not exist or is currently undefined!' % self.name) return center_pose = userdata.rotation_axis planning_group_str = \ ('l' if userdata.hand == self.LEFT_HAND else 'r') + \ '_arm' + \ ('_with_torso' if self._include_torso else '') + \ '_group' # create the motion goal self._client.new_goal(planning_group_str) self._client.set_collision_avoidance(self._ignore_collisions) self._client.set_circular_motion(userdata.rotation_angle) self._client.set_keep_orientation(self._keep_endeffector_orientation) self._client.add_endeffector_pose(userdata.rotation_axis.pose, userdata.rotation_axis.header.frame_id) # rotation center self._client.set_planner_id(self._planner_id) Logger.loginfo('Sending planning request for circular motion of %f rad...' % userdata.rotation_angle) try: self._client.start_planning() except Exception as e: Logger.logwarn('Could not request a plan!\n%s' % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.finished(): self._client.cancel() Logger.loginfo("Cancelled active action goal.")
class MoveitPlanningState(EventState): """ Uses moveit to plan to the given joint configuration. -- planning_group string Name of the planning group. -- vel_scaling string Scales the velocity of the planned trajectory, lower values for slower trajectories. ># joint_values float[] Target joint values to plan to. #> joint_trajectory JointTrajectory Planned joint trajectory to reach the goal. <= planned Requested joint trajectory has been plannd. <= failed Failed to plan to the requested joint configuration. """ def __init__(self, planning_group, vel_scaling=0.1): """Constructor""" super(MoveitPlanningState, self).__init__(outcomes=['planned', 'failed'], input_keys=['joint_values'], output_keys=['joint_trajectory']) self._client = ProxyMoveitClient() self._planning_group = planning_group self._vel_scaling = vel_scaling self._failed = False def execute(self, userdata): """Execute this state""" if self._failed: return 'failed' if self._client.finished(): if self._client.success(): userdata.joint_trajectory = self._client.get_plan() return 'planned' else: Logger.logwarn('MoveIt failed with the following error: %s' % self._client.error_msg()) self._failed = True return 'failed' def on_enter(self, userdata): self._failed = False # create the motion goal self._client.new_goal(self._planning_group) self._client.add_joint_values(userdata.joint_values) self._client.set_velocity_scaling(self._vel_scaling) # execute the motion try: self._client.start_planning() except Exception as e: Logger.logwarn('Was unable to execute move group request:\n%s' % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.finished(): self._client.cancel() Logger.loginfo("Cancelled active action goal.")
class PlanAffordanceState(EventState): ''' Plans for a given affordance without collision avoidance. -- vel_scaling float Scales the velocity of the planned trajectory, lower values for slower trajectories. -- planner_id string Sets the ID of the planner to use (MoveIt planner id or "drake" - default = "RRTConnectkConfigDefault") -- drake_sample_rate float Sample rate for Drake's result trajectory (in Hz, default = 4.0) -- drake_orientation_type int How should the eef-orientation be handled (see ExtendedPlanningOptions, 0 = full, 1 = axis only, 2 = ignore) -- drake_link_axis float[] Target axis to keep (only for drake_orientation_type == ORIENTATION_AXIS_ONLY ># affordance Affordance A message as defined in vigir_object_template_msgs containing all required information. ># hand string The hand which should execute the given affordance. ># reference_point PoseStamped A point of interest on the template w.r.t. which planning will take place (defaults to palm if not set). A template's 'usability' pose would go here. #> joint_trajectory JointTrajectory Trajectory of the end effector to perform the requested motion. #> plan_fraction float Fraction of the requested motion which could be planned. A value below 1 indicates a partial motion. <= done Successfully planned the given affordance. <= failed Failed to plan the affordance. ''' LEFT_HAND = 'left' RIGHT_HAND = 'right' def __init__(self, vel_scaling = 0.1, planner_id = "RRTConnectkConfigDefault", drake_sample_rate = 4.0, drake_orientation_type = 2, drake_link_axis = [0, 0, 1]): '''Constructor''' super(PlanAffordanceState, self).__init__(outcomes = ['done', 'incomplete', 'failed'], input_keys = ['affordance', 'hand', 'reference_point'], output_keys = ['joint_trajectory', 'plan_fraction']) self._client = ProxyMoveitClient() try: self.set_up_affordance_service() # topics and clients are set here except Exception as e: Logger.logerr('Failed to set up GetAffordanceInWristFrame service: \n%s' % str(e)) return self._vel_scaling = vel_scaling self._planner_id = planner_id self._drake_sample_rate = drake_sample_rate self._drake_orientation_type = drake_orientation_type self._drake_link_axis = Point( x = drake_link_axis[0], y = drake_link_axis[1], z = drake_link_axis[2] ) self._failed = False self._done = False self._incomplete = False def execute(self, userdata): '''Wait for result and determine outcome based on fraction thresholds''' fail_threshold = 0.05 success_threshold = 0.95 if self._failed: return 'failed' if self._done: return 'done' if self._incomplete: return 'incomplete' if self._client.finished(): userdata.joint_trajectory = self._client.get_plan() if self._client.success(): plan_fraction = self._client.get_plan_fraction() Logger.loginfo('Plan completion fraction = %f%%' % (plan_fraction*100)) userdata.plan_fraction = plan_fraction if plan_fraction < fail_threshold: Logger.logwarn('Failure! The plan fraction was below %d%%' % (fail_threshold*100)) self._failed = True return 'failed' elif plan_fraction > success_threshold: Logger.loginfo('Success! The plan fraction was above %d%%' % (success_threshold*100)) self._done = True return 'done' else: Logger.logwarn('Partial success.The plan fraction was between %d%% and %d%%' % (fail_threshold*100, success_threshold*100)) self._incomplete = True return 'incomplete' else: Logger.logwarn('MoveIt failed with the following error: %s' % self._client.error_msg()) self._failed = True return 'failed' def on_enter(self, userdata): '''Handle cartesian/circular affordance, set options, and send goal.''' self._failed = False self._done = False self._incomplete = False planning_group_str = 'l_arm_group' if userdata.hand == PlanAffordanceState.LEFT_HAND else 'r_arm_group' affordance = userdata.affordance print affordance #TODO: remove temp debug statement # Create the manipulation action goal self._client.new_goal(move_group = planning_group_str) if affordance.type == "circular": self._client.set_circular_motion(angle = affordance.displacement, pitch = affordance.pitch) for waypoint in affordance.waypoints: # There should only be one self._client.add_endeffector_pose(waypoint.pose, waypoint.header.frame_id) print waypoint #TODO: remove temp debug statement elif affordance.type == "cartesian": self._client.set_cartesian_motion() try: srv_request = GetAffordanceInWristFrameRequest(affordance) srv_result = self._srv.call(self._service_topics[userdata.hand], srv_request) wrist_affordance = srv_result.wrist_affordance # PoseStamped print wrist_affordance #TODO: remove temp debug statement self._client.add_endeffector_pose(wrist_affordance.pose, wrist_affordance.header.frame_id) except Exception as e: Logger.logwarn('Failed to transform affordance to wrist frame: \n%s' % str(e)) self._failed = True return else: Logger.logwarn("Unknown affordance type: %s" % affordance.type) self._failed = True return # Set planning options self._client.set_planner_id(self._planner_id) self._client.set_velocity_scaling(self._vel_scaling) self._client.set_collision_avoidance(True) self._client.set_execute_incomplete_plans(True) self._client.set_keep_orientation(affordance.keep_orientation) #Set reference_point and frame, if provided. Defaults to palm. if userdata.reference_point is not None: self._client.set_reference_point( pose = userdata.reference_point.pose, frame_id = userdata.reference_point.header.frame_id) #Set Drake specific options self._client.set_drake_target_orientation_type(self._drake_orientation_type) self._client.set_drake_target_link_axis(self._drake_link_axis) self._client.set_drake_trajectory_sample_rate(self._drake_sample_rate) try: self._client.start_planning() except Exception as e: Logger.logwarn('Could not request a plan for the affordance!\n%s' % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.finished(): self._client.cancel() Logger.loginfo("Cancelled active action goal.") ############################################################################ def set_up_affordance_service(self): hands_in_use = self.determine_hands_in_use() # Set up services for transforming cartesian affordances to wrist frame self._service_topics = dict() for hand in hands_in_use: topic = '/manipulation_control/%s_hand/wrist_affordance_srv' % hand[0] self._service_topics[hand] = topic self._srv = ProxyServiceCaller( {t: GetAffordanceInWristFrame for t in self._service_topics.values()}) def determine_hands_in_use(self): '''Determine which hand types and sides have been sourced''' if not rospy.has_param("behavior/hand_type_prefix"): Logger.logerr("Need to specify behavior/parameter hand_type_prefix at the parameter server") return hand_type_prefix = rospy.get_param("behavior/hand_type_prefix") hands_in_use = list() LEFT_HAND = os.environ[hand_type_prefix + 'LEFT_HAND_TYPE'] RIGHT_HAND = os.environ[hand_type_prefix + 'RIGHT_HAND_TYPE'] if LEFT_HAND not in ['l_stump', 'l_hook']: hands_in_use.append('left') if RIGHT_HAND not in ['r_stump', 'r_hook']: hands_in_use.append('right') if len(hands_in_use) == 0: Logger.logerr('No hands seem to be in use:\nLEFT_HAND = %s\nRIGHT_HAND = %s' % (LEFT_HAND, RIGHT_HAND)) return hands_in_use
class PlanEndeffectorCartesianWaypointsState(EventState): ''' Plans a cartesian endeffector trajectory passing all given waypoints. -- ignore_collisions boolean Should collisions be ignored? Only pass True if you are sure that it is safe. -- include_torso boolean Should the torso be included in the trajectory or stay fixed? -- keep_endeffector_orientation boolean Should the initial orientation of the endeffector be kept? -- allow_incomplete_plans boolean Should incomplete plans be allowed? -- vel_scaling string Scales the velocity of the motion, lower values for slower trajectories. -- planner_id string Sets the ID of the planner to use (MoveIt planner id or "drake" - default = "RRTConnectkConfigDefault") ># waypoints Pose[] Waypoints for the endeffector motion. ># hand string One of the class constants determining the hand to be moved. ># frame_id string Frame in which the waypoints are given. #> joint_trajectory JointTrajectory Trajectory of the endeffector to perform the requested motion. #> plan_fraction float Fraction of the requested motion which could be planned. A value below 1 indicates a partial motion. <= planned Was able to generate a valid motion plan. This plan is not necessarily complete. <= failed Failed to create a motion plan at all. ''' LEFT_HAND = 'left' RIGHT_HAND = 'right' def __init__(self, ignore_collisions = False, include_torso = False, keep_endeffector_orientation = False, allow_incomplete_plans = False, vel_scaling = 0.1, planner_id = "RRTConnectkConfigDefault"): '''Constructor''' super(PlanEndeffectorCartesianWaypointsState, self).__init__(outcomes=['planned', 'incomplete', 'failed'], input_keys=['waypoints', 'hand', 'frame_id'], output_keys=['joint_trajectory', 'plan_fraction']) self._client = ProxyMoveitClient() self._ignore_collisions = ignore_collisions self._include_torso = include_torso self._keep_endeffector_orientation = keep_endeffector_orientation self._allow_incomplete_plans = allow_incomplete_plans self._vel_scaling = vel_scaling self._planner_id = planner_id self._failed = False self._done = False self._incomplete = False def execute(self, userdata): ''' Execute this state ''' fail_threshold = 0.05 success_threshold = 0.95 if self._failed: return 'failed' if self._done: return 'planned' if self._incomplete: return 'incomplete' if self._client.finished(): userdata.joint_trajectory = self._client.get_plan() if self._client.success(): # Incomplete plans are considered a SUCCESS, if we allow them plan_fraction = self._client.get_plan_fraction() Logger.loginfo('Plan completion fraction = %.2f%%' % (plan_fraction*100)) userdata.plan_fraction = plan_fraction if plan_fraction < fail_threshold: rospy.logwarn('Failure! The plan fraction was below %d%%' % (fail_threshold*100)) self._failed = True return 'failed' elif plan_fraction > success_threshold: rospy.loginfo('Success! The plan fraction was above %d%%' % (success_threshold*100)) self._done = True return 'planned' else: rospy.logwarn('Partial success.The plan fraction was between %d%% and %d%%' % (fail_threshold*100, success_threshold*100)) self._incomplete = True return 'incomplete' else: Logger.logwarn('MoveIt failed with the following error: %s' % self._client.error_msg()) self._failed = True return 'failed' def on_enter(self, userdata): '''...''' if not hasattr(userdata, 'waypoints') or userdata.waypoints is None \ or not hasattr(userdata, 'hand') or userdata.hand is None \ or not hasattr(userdata, 'frame_id') or userdata.frame_id is None: self._failed = True Logger.logwarn('Userdata key of state %s does not exist or is currently undefined!' % self.name) return self._failed = False self._done = False self._incomplete = False planning_group_str = \ ('l' if userdata.hand == self.LEFT_HAND else 'r') + \ '_arm' + \ ('_with_torso' if self._include_torso else '') + \ '_group' # create the motion goal self._client.new_goal(planning_group_str) self._client.set_collision_avoidance(self._ignore_collisions) self._client.set_cartesian_motion() self._client.set_keep_orientation(self._keep_endeffector_orientation) self._client.set_execute_incomplete_plans(self._allow_incomplete_plans) self._client.set_velocity_scaling(self._vel_scaling) self._client.set_planner_id(self._planner_id) for pose in userdata.waypoints: self._client.add_endeffector_pose(pose, userdata.frame_id) rospy.loginfo('Sending planning request to pass %d waypoints in frame %s...' % (len(userdata.waypoints), userdata.frame_id)) try: self._client.start_planning() except Exception as e: Logger.logwarn('Could not request a plan!\n%s' % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.finished(): self._client.cancel() Logger.loginfo("Cancelled active action goal.")
class PlanEndeffectorCartesianWaypointsState(EventState): ''' Plans a cartesian endeffector trajectory passing all given waypoints. -- ignore_collisions boolean Should collisions be ignored? Only pass True if you are sure that it is safe. -- include_torso boolean Should the torso be included in the trajectory or stay fixed? -- keep_endeffector_orientation boolean Should the initial orientation of the endeffector be kept? -- allow_incomplete_plans boolean Should incomplete plans be allowed? -- vel_scaling string Scales the velocity of the motion, lower values for slower trajectories. -- planner_id string Sets the ID of the planner to use (MoveIt planner id or "drake" - default = "RRTConnectkConfigDefault") ># waypoints Pose[] Waypoints for the endeffector motion. ># hand string One of the class constants determining the hand to be moved. ># frame_id string Frame in which the waypoints are given. #> joint_trajectory JointTrajectory Trajectory of the endeffector to perform the requested motion. #> plan_fraction float Fraction of the requested motion which could be planned. A value below 1 indicates a partial motion. <= planned Was able to generate a valid motion plan. This plan is not necessarily complete. <= failed Failed to create a motion plan at all. ''' LEFT_HAND = 'left' RIGHT_HAND = 'right' def __init__(self, ignore_collisions=False, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=False, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"): '''Constructor''' super(PlanEndeffectorCartesianWaypointsState, self).__init__(outcomes=['planned', 'incomplete', 'failed'], input_keys=['waypoints', 'hand', 'frame_id'], output_keys=['joint_trajectory', 'plan_fraction']) self._client = ProxyMoveitClient() self._ignore_collisions = ignore_collisions self._include_torso = include_torso self._keep_endeffector_orientation = keep_endeffector_orientation self._allow_incomplete_plans = allow_incomplete_plans self._vel_scaling = vel_scaling self._planner_id = planner_id self._failed = False self._done = False self._incomplete = False def execute(self, userdata): ''' Execute this state ''' fail_threshold = 0.05 success_threshold = 0.95 if self._failed: return 'failed' if self._done: return 'planned' if self._incomplete: return 'incomplete' if self._client.finished(): userdata.joint_trajectory = self._client.get_plan() if self._client.success(): # Incomplete plans are considered a SUCCESS, if we allow them plan_fraction = self._client.get_plan_fraction() Logger.loginfo('Plan completion fraction = %.2f%%' % (plan_fraction * 100)) userdata.plan_fraction = plan_fraction if plan_fraction < fail_threshold: rospy.logwarn('Failure! The plan fraction was below %d%%' % (fail_threshold * 100)) self._failed = True return 'failed' elif plan_fraction > success_threshold: rospy.loginfo('Success! The plan fraction was above %d%%' % (success_threshold * 100)) self._done = True return 'planned' else: rospy.logwarn( 'Partial success.The plan fraction was between %d%% and %d%%' % (fail_threshold * 100, success_threshold * 100)) self._incomplete = True return 'incomplete' else: Logger.logwarn('MoveIt failed with the following error: %s' % self._client.error_msg()) self._failed = True return 'failed' def on_enter(self, userdata): '''...''' if not hasattr(userdata, 'waypoints') or userdata.waypoints is None \ or not hasattr(userdata, 'hand') or userdata.hand is None \ or not hasattr(userdata, 'frame_id') or userdata.frame_id is None: self._failed = True Logger.logwarn( 'Userdata key of state %s does not exist or is currently undefined!' % self.name) return self._failed = False self._done = False self._incomplete = False planning_group_str = \ ('l' if userdata.hand == self.LEFT_HAND else 'r') + \ '_arm' + \ ('_with_torso' if self._include_torso else '') + \ '_group' # create the motion goal self._client.new_goal(planning_group_str) self._client.set_collision_avoidance(self._ignore_collisions) self._client.set_cartesian_motion() self._client.set_keep_orientation(self._keep_endeffector_orientation) self._client.set_execute_incomplete_plans(self._allow_incomplete_plans) self._client.set_velocity_scaling(self._vel_scaling) self._client.set_planner_id(self._planner_id) for pose in userdata.waypoints: self._client.add_endeffector_pose(pose, userdata.frame_id) rospy.loginfo( 'Sending planning request to pass %d waypoints in frame %s...' % (len(userdata.waypoints), userdata.frame_id)) try: self._client.start_planning() except Exception as e: Logger.logwarn('Could not request a plan!\n%s' % str(e)) self._failed = True def on_exit(self, userdata): if not self._client.finished(): self._client.cancel() Logger.loginfo("Cancelled active action goal.")