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 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 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