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 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 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 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.")