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 __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 __init__(self, planning_group):
        """Constructor"""
        super(MoveitMoveGroupState,
              self).__init__(outcomes=['reached', 'failed'],
                             input_keys=['target_joint_config'])

        self._client = ProxyMoveitClient()

        self._failed = False
        self._planning_group = planning_group
Ejemplo n.º 4
0
    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
class MoveitMoveGroupState(EventState):
    """
	Uses moveit to plan to the given joint configuration and executes the resulting trajctory.

	-- planning_group 		string 		Name of the planning group.

	># target_joint_config	float[] 	Target joint values to plan and move to.

	<= reached 							Requested joint configuration has been reached successfully.
	<= failed 							Failed to reach requested joint configuration.

	"""
    def __init__(self, planning_group):
        """Constructor"""
        super(MoveitMoveGroupState,
              self).__init__(outcomes=['reached', 'failed'],
                             input_keys=['target_joint_config'])

        self._client = ProxyMoveitClient()

        self._failed = False
        self._planning_group = planning_group

    def execute(self, userdata):
        """Execute this state"""
        if self._failed:
            return 'failed'

        if self._client.finished():
            if self._client.success():
                return 'reached'
            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.target_joint_config)

        # execute the motion
        try:
            Logger.loginfo("Moving %s to: %s" %
                           (self._planning_group, ", ".join(
                               map(str, userdata.target_joint_config))))
            self._client.start_execution()
        except Exception as e:
            Logger.logwarn('Was unable to execute move group request:\n%s' %
                           str(e))
            self._failed = True
	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
class MoveitMoveGroupState(EventState):
	"""
	Uses moveit to plan to the given joint configuration and executes the resulting trajctory.

	-- planning_group 		string 		Name of the planning group.

	># target_joint_config	float[] 	Target joint values to plan and move to.

	<= reached 							Requested joint configuration has been reached successfully.
	<= failed 							Failed to reach requested joint configuration.

	"""
	
	def __init__(self, planning_group):
		"""Constructor"""
		super(MoveitMoveGroupState, self).__init__(outcomes=['reached', 'failed'],
												input_keys=['target_joint_config'])

		self._client = ProxyMoveitClient()

		self._failed = False
		self._planning_group = planning_group


	def execute(self, userdata):
		"""Execute this state"""
		if self._failed:
			return 'failed'

		if self._client.finished():
			if self._client.success():
				return 'reached'
			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.target_joint_config)

		# execute the motion
		try: 
			Logger.loginfo("Moving %s to: %s" % (self._planning_group, ", ".join(map(str, userdata.target_joint_config))))
			self._client.start_execution()
		except Exception as e:
			Logger.logwarn('Was unable to execute move group request:\n%s' % str(e))
			self._failed = True
	def __init__(self, planning_group):
		"""Constructor"""
		super(MoveitMoveGroupState, self).__init__(outcomes=['reached', 'failed'],
												input_keys=['target_joint_config'])

		self._client = ProxyMoveitClient()

		self._failed = False
		self._planning_group = planning_group
	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 __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 __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 __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
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.")
	def __init__(self, target_pose,
					   vel_scaling = 0.1,
					   ignore_collisions = False,
					   link_paddings = {},
					   is_cartesian = False):
		"""Constructor"""
		super(MoveitPredefinedPoseState, self).__init__(outcomes=['done', 'failed'],
														input_keys=['side'])

		if not rospy.has_param("behavior/robot_namespace"):
			Logger.logerr("Need to specify parameter behavior/robot_namespace at the parameter server")
			return
		
		self._robot = rospy.get_param("behavior/robot_namespace")

		self._poses = dict()
		self._poses['flor'] = dict()
		# Position mode widget: src/vigir_ocs_eui/vigir_rqt/vigir_rqt_position_mode/launch
		self._poses['flor']['left'] =  {
				 # Joint names: l_arm_shz, l_arm_shx, l_arm_ely, l_arm_elx, l_arm_wry, l_arm_wrx, l_arm_wry2
			1:   {'group': 'l_arm_group',	 'joints': [-0.25, -1.34, 1.88, 0.49,  0.0,   0.0, 0.0]},
			3: 	 {'group': 'l_arm_group', 	 'joints': [+0.72, -0.95, 2.7,  0.95,  0.0,  -0.4, -0.50]},
			10:  {'group': 'l_arm_group', 	 'joints': [-1.0,   0.28,  1.2,   1.6,   0.3,    0.5  ,   0.0]},
			21:  {'group': 'torso_group', 	 'joints': [+0.20, 0.00, 0.00]},
			22:  {'group': 'torso_group', 	 'joints': [+0.40, 0.00, 0.00]},
			23:  {'group': 'torso_group', 	 'joints': [+0.55, 0.00, 0.00]},
			112: {'group': 'l_leg_group', 	 'joints': [+0.00, +0.00, -1.60, +1.40, -0.50, 0.00]}, # Safety pose
			114: {'group': 'l_arm_group', 	 'joints': [+0.76, -0.94, 0.80, 2.00, +1.00, -0.20, -1.35]}, # pre_drive
			115: {'group': 'l_arm_group', 	 'joints': [+0.11, -0.16, 1.75, 1.60, +1.00, -0.90, -1.00]}, # drive
			116: {'group': 'l_arm_group', 	 'joints': []}, # We use the right hand for the camera!
			131: {'group': 'l_arm_group', 	 'joints': [-0.29, -0.22, 1.87, 2.17, -0.17, 0.81, 0.12]},
			132: {'group': 'l_arm_group', 	 'joints': [-0.70, -0.09, 1.93, 0.66, -0.15, 1.52, 0.12]},
			133: {'group': 'l_arm_group', 	 'joints': [-1.38, -0.16, 1.82, 0.57, -0.19, 1.52, 0.12]},
			134: {'group': 'l_arm_group', 	 'joints': []}, # Most probably will never be used
			151: {'group': 'l_arm_group', 	 'joints': [-1.01, -0.43, +1.32, +1.67, -0.91, +1.46, +0.98]},
			155: {'group': 'l_arm_group', 	 'joints': [0.0,  -0.37,  2.65,  1.4,  -0.2,    0.9  ,  -1.54]},
			156: {'group': 'l_arm_group', 	 'joints': [-0.32, -0.9,   2.2,   1.3,   0.5,    1.0  ,  -1.8]},
			157: {'group': 'l_arm_group', 	 'joints': [-0.45, -1.0,   2.1,   1.3,   0.5,    0.8  ,  -0.8]}
		}
		self._poses['flor']['same'] =  {
			0:   {'group': 'both_arms_group', 'joints': [-0.25, -1.34, +1.88, +0.49, +0.00, +0.00, +0.00, \
														 +0.25, +1.34, +1.88, -0.49, +0.00, +0.00, +0.00]},
			2:   {'group': 'both_arms_group', 'joints': [+0.72, -0.95,  2.7,   0.95,  0.0,   -0.4  ,  -0.5, \
														 -0.72,  0.95,  2.7,  -0.95,  0.0,    0.4  ,  -0.5]},
			19:  {'group': 'both_arms_group', 'joints': [-1.5,  -1.5,  +0.30, +0.50,  +0.0,  +0.8,  +0.00, \
														 +1.5,  +1.5,  +0.30, -0.50,  +0.0,  -0.8,  +0.00]},
			20:  {'group': 'torso_group', 	  'joints': [+0.00, 0.00, 0.00]},
			111: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, +0.00, 0.0, \
														 +0.00, +1.50, +0.00, -1.72, 0.00, +0.00, 0.0]},
			113: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, -1.57, 0.0, \
														 +0.00, +1.50, +0.00, -1.72, 0.00, +1.57, 0.0]},
			181: {'group': 'both_arms_group', 'joints': [-1.53, -0.69, +0.12, +1.47, 0.00, +0.88, 0.00, \
														 +1.53, +0.69, +0.12, -1.47, 0.00, -0.88, 0.00]}
		}
		self._poses['flor']['right'] =  {
				 # Joint names: r_arm_shz, r_arm_shx, r_arm_ely, r_arm_elx, r_arm_wry, r_arm_wrx, r_arm_wry2
			1:   {'group': 'r_arm_group',	 'joints': [+0.25, 1.34, 1.88, -0.49,  0.0,   0.0,  0.0]},
			3: 	 {'group': 'r_arm_group', 	 'joints': [-0.72, 0.95, 2.7,  -0.95,  0.0,   0.4, -0.50]},
			10:  {'group': 'r_arm_group', 	 'joints': [+1.0,  -0.28,  1.2,  -1.6,   0.3,   -0.5  ,   0.0]},
			21:  {'group': 'torso_group', 	 'joints': [-0.20, 0.00, 0.00]},
			22:  {'group': 'torso_group', 	 'joints': [-0.40, 0.00, 0.00]},
			23:  {'group': 'torso_group', 	 'joints': [-0.55, 0.00, 0.00]},
			112: {'group': 'r_leg_group', 	 'joints': [+0.00, +0.00, -1.34, +1.23, 0.00, 0.00]},
			115: {'group': 'r_arm_group', 	 'joints': []}, # Driving is done with the left arm!
			116: {'group': 'r_arm_group', 	 'joints': [+0.90, 0.17, 0.50, -1.58, -0.70, +1.50, -0.34]}, # CarCamera
			131: {'group': 'r_arm_group', 	 'joints': [+0.29, 0.22, 1.87, -2.17, -0.17, -0.81, 0.12]},
			132: {'group': 'r_arm_group', 	 'joints': [+0.70, 0.09, 1.93, -0.66, -0.15, -1.52, 0.12]},
			133: {'group': 'r_arm_group', 	 'joints': [+1.38, 0.16, 1.82, -0.57, -0.19, -1.52, 0.12]},
			134: {'group': 'r_arm_group', 	 'joints': [+0.00, +0.54, +0.94, -1.04,  0.80,    0.5,   0.7]},
			151: {'group': 'r_arm_group', 	 'joints': [+1.01, +0.43, +1.32, -1.67, -0.91, -1.46, +0.98]},
			155: {'group': 'r_arm_group', 	 'joints': [+0.00, +0.37, +2.65, -1.40, -0.20, -0.90, -1.54]},
			156: {'group': 'r_arm_group', 	 'joints': [+0.32, +0.90, +2.20, -1.30, +0.50, -1.00, -1.80]},
			157: {'group': 'r_arm_group', 	 'joints': [0.45,  1.0,   2.1,  -1.3,   0.5,   -0.8  ,  -0.8]}
		}

		self._poses['thor_mang'] = dict()
		self._poses['thor_mang']['left'] = {
			1:   {'group': 'l_arm_group',	 'joints': [0.785385646194622, -0.281153767716932, 0.000600782658167331, -1.57080884130538, -0.25205140042963, 0.01563815008, 0]},
			3:   {'group': 'l_arm_group',	 'joints': [0.77, -0.27, 0.02, -1.21, -0.25, 0.02, 0]},
			4:   {'group': 'l_arm_group',	 'joints': [0.785385646194622, -0.281153767716932, 0.000600782658167331, -1.97080884130538, -0.25205140042963, 0.01563815008, 0]},
			21:  {'group': 'torso_group', 	 'joints': [+0.30, 0.03]},
			22:  {'group': 'torso_group', 	 'joints': [+0.60, 0.03]},
			23:  {'group': 'torso_group', 	 'joints': [+1.02, 0.03]},
			31:  {'group': 'head_group', 	 'joints': [1.57, 0.00]},
			131: {'group': 'l_arm_group', 	 'joints': [-1.73, -0.69, 1.75, -1.86, 0.04, -0.72, 1.63]},
			132: {'group': 'l_arm_group', 	 'joints': [-1.76, -1.13, 1.68, -0.55, 0.02, -1.81, 1.63]},
			133: {'group': 'l_arm_group', 	 'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]},
			134: {'group': 'l_arm_group', 	 'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]}, # placeholder
			136: {'group': 'l_arm_group', 	 'joints': [-2.31, -2.2, 1.83, -1.22, 4.1, 0.14, -2.08]},
			138: {'group': 'torso_group', 	 'joints': [0.92, 0]},
			139: {'group': 'torso_group', 	 'joints': [1.3, 0]},
			151: {'group': 'l_arm_group', 	 'joints': [-1.45, -1.26, 1.10, -2.02, -0.19, 0.04, -2.86]},
			155: {'group': 'l_arm_group',	 'joints': [-0.25, -0.75, 0.02, -1.21, -0.38, -1.36, -0.9]},
			156: {'group': 'l_arm_group',	 'joints': [-0.25, -0.25, 0.02, -1.21, -0.38, -1.36, -0.9]},
			157: {'group': 'l_arm_group',	 'joints': [-0.25, -0.13, 0.02, -1.21, -0.13, -1.55, 0.41]}
		}
		self._poses['thor_mang']['same'] = {
			0:   {'group': 'both_arms_group', 'joints': [0.79, -0.27, 0, -1.57, 1.55, 0, 0, -0.79, 0.27, 0, 1.57, -1.55, 0, 0]},
			2:   {'group': 'both_arms_group', 'joints': [-0.37, 0.0, 1.57, 0, 0, 0, 0, 0.37, 0.0, 1.57, 0, 0, 0, 0]},
			20:  {'group': 'torso_group', 	  'joints': [0.00, 0.03]},
			30:  {'group': 'head_group', 	  'joints': [0.00, 0.00]},
			181: {'group': 'both_arms_group', 'joints': [0.785385646194622, -0.281153767716932, 0.000600782658167331, -1.57080884130538, -0.25205140042963, 0.01563815008, 0, -0.785385646194622, 0.281153767716932, -0.000600782658167331, 1.57080884130538, 0.25205140042963, -0.01563815008, 0]}
		}
		self._poses['thor_mang']['right'] = {
			1:   {'group': 'r_arm_group',	 'joints': [-0.785385646194622, 0.281153767716932, -0.000600782658167331, 1.57080884130538, 0.25205140042963, -0.01563815008, 0]},
			3:   {'group': 'r_arm_group',	 'joints': [-0.77, 0.27, -0.02, 1.21, 0.25, -0.02, 0]},
			4:   {'group': 'r_arm_group',	 'joints': [-0.785385646194622, 0.281153767716932, -0.000600782658167331, 1.97080884130538, 0.25205140042963, -0.01563815008, 0]},
			21:  {'group': 'torso_group', 	 'joints': [-0.30, 0.03]},
			22:  {'group': 'torso_group', 	 'joints': [-0.60, 0.03]},
			23:  {'group': 'torso_group', 	 'joints': [-1.02, 0.03]},
			31:  {'group': 'head_group', 	 'joints': [-1.57, 0.00]},
			131: {'group': 'r_arm_group', 	 'joints': [1.73, 0.69, -1.75, 1.86, -0.04, 0.72, -1.63]},
			132: {'group': 'r_arm_group', 	 'joints': [1.76, 1.13, -1.68, 0.55, -0.02, 1.81, -1.63]},
			133: {'group': 'r_arm_group', 	 'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]},
			134: {'group': 'r_arm_group', 	 'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]}, # placeholder
			136: {'group': 'r_arm_group', 	 'joints': [2.31, 2.2, -1.83, 1.22, -4.1, -0.14, 2.08]},
			138: {'group': 'torso_group', 	 'joints': [-0.92, 0]},
			139: {'group': 'torso_group', 	 'joints': [-1.3, 0]},
			151: {'group': 'r_arm_group', 	 'joints': [1.45, 1.26, -1.10, 2.02, 0.19, -0.04, 2.86]},
			155: {'group': 'r_arm_group',	 'joints': [0.25, 0.75, -0.02, 1.21, 0.38, 1.36, 0.9]},
			156: {'group': 'r_arm_group',	 'joints': [0.25, 0.25, -0.02, 1.21, 0.38, 1.36, 0.9]},
			157: {'group': 'r_arm_group',	 'joints': [0.25, 0.13, -0.02, 1.21, 0.13, 1.55, -0.41]}
		}
		
		self._target_pose = target_pose
		self._vel_scaling = vel_scaling
		self._ignore_collisions = ignore_collisions
		self._link_paddings = link_paddings
		self._is_cartesian = is_cartesian

		self._client = ProxyMoveitClient()
		
		self._failed = False
class MoveitPredefinedPoseState(EventState):
	"""
	Uses moveit to go to one of the pre-defined poses.

	-- target_pose 			int 		Identifier of the pre-defined pose to be used.
	-- vel_scaling  		float 		Scales the velocity of the motion.
										Lower values for slower trajectories.
	-- ignore_collisions  	boolean 	Should collisions be ignored? Only pass True if you are sure that it is safe.
	-- link_paddings 		dict 		link_name (str) : padding (float) pairs
	-- is_cartesian 		boolean 	Execute as cartesian motion

	># side 				string 		Arm side, turning direction, etc.
										Possible values: {left, right, same}
 
	<= done 							Successfully executed the motion.
	<= failed 							Failed to execute the motion.

	"""

	# Arms
	STAND_POSE = 0
	SINGLE_ARM_STAND = 1 # for preventing unconstrained motion of the other
	BOTH_ARMS_SIDES = 2  # for safely holding the tool at the robot's side
	SINGLE_ARM_SIDE = 3
	STAND_POSE_UP = 4
	CALIBRATE_ARMS = 10  # for checking arm calibration with the head camera 
	WALK_POSE = 19

	# Torso
	TURN_TORSO_CENTER_POSE = 20
	TURN_TORSO_SLIGHTLY = 21
	TURN_TORSO_MORE = 22
	TURN_TORSO_FULL = 23

	# Head
	HEAD_CENTER = 30
	HEAD_SIDE = 31

	# Task-specific poses
	# Second digit is task number. Comment is pose in the Position widget.
	CAR_ENTRY_ARMS_POSE = 111 	# CarEntry
	CAR_ENTRY_LEGS_POSE = 112 	# CarEntry
	CAR_ENTRY_FORE_POSE = 113 	# CarEntryFore
	CAR_PREDRIVE_LARM_POSE = 114 	# pre_drive
	CAR_DRIVE_LARM_POSE = 115 		# drive
	CAR_DRIVE_CAMERA_POSE = 116 	# CarCamera
	DOOR_READY_POSE = 131
	DOOR_OPEN_POSE_TURNED = 132
	DOOR_OPEN_POSE_STRAIGHT = 133
	DOOR_OPEN_POSE_SIDE = 134 # push_door_3
	DOOR_PUSH_SIDE = 136
	DOOR_OPEN_TURN_MANIPULATE = 138
	DOOR_OPEN_TURN_LIDAR = 139


	POKE_READY_POSE = 151
	PREP_CUT_WALL_1 = 155
	PREP_CUT_WALL_2 = 156
	PREP_CUT_WALL_3 = 157

	LADDER_READY_POSE = 181

	
	def __init__(self, target_pose,
					   vel_scaling = 0.1,
					   ignore_collisions = False,
					   link_paddings = {},
					   is_cartesian = False):
		"""Constructor"""
		super(MoveitPredefinedPoseState, self).__init__(outcomes=['done', 'failed'],
														input_keys=['side'])

		if not rospy.has_param("behavior/robot_namespace"):
			Logger.logerr("Need to specify parameter behavior/robot_namespace at the parameter server")
			return
		
		self._robot = rospy.get_param("behavior/robot_namespace")

		self._poses = dict()
		self._poses['flor'] = dict()
		# Position mode widget: src/vigir_ocs_eui/vigir_rqt/vigir_rqt_position_mode/launch
		self._poses['flor']['left'] =  {
				 # Joint names: l_arm_shz, l_arm_shx, l_arm_ely, l_arm_elx, l_arm_wry, l_arm_wrx, l_arm_wry2
			1:   {'group': 'l_arm_group',	 'joints': [-0.25, -1.34, 1.88, 0.49,  0.0,   0.0, 0.0]},
			3: 	 {'group': 'l_arm_group', 	 'joints': [+0.72, -0.95, 2.7,  0.95,  0.0,  -0.4, -0.50]},
			10:  {'group': 'l_arm_group', 	 'joints': [-1.0,   0.28,  1.2,   1.6,   0.3,    0.5  ,   0.0]},
			21:  {'group': 'torso_group', 	 'joints': [+0.20, 0.00, 0.00]},
			22:  {'group': 'torso_group', 	 'joints': [+0.40, 0.00, 0.00]},
			23:  {'group': 'torso_group', 	 'joints': [+0.55, 0.00, 0.00]},
			112: {'group': 'l_leg_group', 	 'joints': [+0.00, +0.00, -1.60, +1.40, -0.50, 0.00]}, # Safety pose
			114: {'group': 'l_arm_group', 	 'joints': [+0.76, -0.94, 0.80, 2.00, +1.00, -0.20, -1.35]}, # pre_drive
			115: {'group': 'l_arm_group', 	 'joints': [+0.11, -0.16, 1.75, 1.60, +1.00, -0.90, -1.00]}, # drive
			116: {'group': 'l_arm_group', 	 'joints': []}, # We use the right hand for the camera!
			131: {'group': 'l_arm_group', 	 'joints': [-0.29, -0.22, 1.87, 2.17, -0.17, 0.81, 0.12]},
			132: {'group': 'l_arm_group', 	 'joints': [-0.70, -0.09, 1.93, 0.66, -0.15, 1.52, 0.12]},
			133: {'group': 'l_arm_group', 	 'joints': [-1.38, -0.16, 1.82, 0.57, -0.19, 1.52, 0.12]},
			134: {'group': 'l_arm_group', 	 'joints': []}, # Most probably will never be used
			151: {'group': 'l_arm_group', 	 'joints': [-1.01, -0.43, +1.32, +1.67, -0.91, +1.46, +0.98]},
			155: {'group': 'l_arm_group', 	 'joints': [0.0,  -0.37,  2.65,  1.4,  -0.2,    0.9  ,  -1.54]},
			156: {'group': 'l_arm_group', 	 'joints': [-0.32, -0.9,   2.2,   1.3,   0.5,    1.0  ,  -1.8]},
			157: {'group': 'l_arm_group', 	 'joints': [-0.45, -1.0,   2.1,   1.3,   0.5,    0.8  ,  -0.8]}
		}
		self._poses['flor']['same'] =  {
			0:   {'group': 'both_arms_group', 'joints': [-0.25, -1.34, +1.88, +0.49, +0.00, +0.00, +0.00, \
														 +0.25, +1.34, +1.88, -0.49, +0.00, +0.00, +0.00]},
			2:   {'group': 'both_arms_group', 'joints': [+0.72, -0.95,  2.7,   0.95,  0.0,   -0.4  ,  -0.5, \
														 -0.72,  0.95,  2.7,  -0.95,  0.0,    0.4  ,  -0.5]},
			19:  {'group': 'both_arms_group', 'joints': [-1.5,  -1.5,  +0.30, +0.50,  +0.0,  +0.8,  +0.00, \
														 +1.5,  +1.5,  +0.30, -0.50,  +0.0,  -0.8,  +0.00]},
			20:  {'group': 'torso_group', 	  'joints': [+0.00, 0.00, 0.00]},
			111: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, +0.00, 0.0, \
														 +0.00, +1.50, +0.00, -1.72, 0.00, +0.00, 0.0]},
			113: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, -1.57, 0.0, \
														 +0.00, +1.50, +0.00, -1.72, 0.00, +1.57, 0.0]},
			181: {'group': 'both_arms_group', 'joints': [-1.53, -0.69, +0.12, +1.47, 0.00, +0.88, 0.00, \
														 +1.53, +0.69, +0.12, -1.47, 0.00, -0.88, 0.00]}
		}
		self._poses['flor']['right'] =  {
				 # Joint names: r_arm_shz, r_arm_shx, r_arm_ely, r_arm_elx, r_arm_wry, r_arm_wrx, r_arm_wry2
			1:   {'group': 'r_arm_group',	 'joints': [+0.25, 1.34, 1.88, -0.49,  0.0,   0.0,  0.0]},
			3: 	 {'group': 'r_arm_group', 	 'joints': [-0.72, 0.95, 2.7,  -0.95,  0.0,   0.4, -0.50]},
			10:  {'group': 'r_arm_group', 	 'joints': [+1.0,  -0.28,  1.2,  -1.6,   0.3,   -0.5  ,   0.0]},
			21:  {'group': 'torso_group', 	 'joints': [-0.20, 0.00, 0.00]},
			22:  {'group': 'torso_group', 	 'joints': [-0.40, 0.00, 0.00]},
			23:  {'group': 'torso_group', 	 'joints': [-0.55, 0.00, 0.00]},
			112: {'group': 'r_leg_group', 	 'joints': [+0.00, +0.00, -1.34, +1.23, 0.00, 0.00]},
			115: {'group': 'r_arm_group', 	 'joints': []}, # Driving is done with the left arm!
			116: {'group': 'r_arm_group', 	 'joints': [+0.90, 0.17, 0.50, -1.58, -0.70, +1.50, -0.34]}, # CarCamera
			131: {'group': 'r_arm_group', 	 'joints': [+0.29, 0.22, 1.87, -2.17, -0.17, -0.81, 0.12]},
			132: {'group': 'r_arm_group', 	 'joints': [+0.70, 0.09, 1.93, -0.66, -0.15, -1.52, 0.12]},
			133: {'group': 'r_arm_group', 	 'joints': [+1.38, 0.16, 1.82, -0.57, -0.19, -1.52, 0.12]},
			134: {'group': 'r_arm_group', 	 'joints': [+0.00, +0.54, +0.94, -1.04,  0.80,    0.5,   0.7]},
			151: {'group': 'r_arm_group', 	 'joints': [+1.01, +0.43, +1.32, -1.67, -0.91, -1.46, +0.98]},
			155: {'group': 'r_arm_group', 	 'joints': [+0.00, +0.37, +2.65, -1.40, -0.20, -0.90, -1.54]},
			156: {'group': 'r_arm_group', 	 'joints': [+0.32, +0.90, +2.20, -1.30, +0.50, -1.00, -1.80]},
			157: {'group': 'r_arm_group', 	 'joints': [0.45,  1.0,   2.1,  -1.3,   0.5,   -0.8  ,  -0.8]}
		}

		self._poses['thor_mang'] = dict()
		self._poses['thor_mang']['left'] = {
			1:   {'group': 'l_arm_group',	 'joints': [0.785385646194622, -0.281153767716932, 0.000600782658167331, -1.57080884130538, -0.25205140042963, 0.01563815008, 0]},
			3:   {'group': 'l_arm_group',	 'joints': [0.77, -0.27, 0.02, -1.21, -0.25, 0.02, 0]},
			4:   {'group': 'l_arm_group',	 'joints': [0.785385646194622, -0.281153767716932, 0.000600782658167331, -1.97080884130538, -0.25205140042963, 0.01563815008, 0]},
			21:  {'group': 'torso_group', 	 'joints': [+0.30, 0.03]},
			22:  {'group': 'torso_group', 	 'joints': [+0.60, 0.03]},
			23:  {'group': 'torso_group', 	 'joints': [+1.02, 0.03]},
			31:  {'group': 'head_group', 	 'joints': [1.57, 0.00]},
			131: {'group': 'l_arm_group', 	 'joints': [-1.73, -0.69, 1.75, -1.86, 0.04, -0.72, 1.63]},
			132: {'group': 'l_arm_group', 	 'joints': [-1.76, -1.13, 1.68, -0.55, 0.02, -1.81, 1.63]},
			133: {'group': 'l_arm_group', 	 'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]},
			134: {'group': 'l_arm_group', 	 'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]}, # placeholder
			136: {'group': 'l_arm_group', 	 'joints': [-2.31, -2.2, 1.83, -1.22, 4.1, 0.14, -2.08]},
			138: {'group': 'torso_group', 	 'joints': [0.92, 0]},
			139: {'group': 'torso_group', 	 'joints': [1.3, 0]},
			151: {'group': 'l_arm_group', 	 'joints': [-1.45, -1.26, 1.10, -2.02, -0.19, 0.04, -2.86]},
			155: {'group': 'l_arm_group',	 'joints': [-0.25, -0.75, 0.02, -1.21, -0.38, -1.36, -0.9]},
			156: {'group': 'l_arm_group',	 'joints': [-0.25, -0.25, 0.02, -1.21, -0.38, -1.36, -0.9]},
			157: {'group': 'l_arm_group',	 'joints': [-0.25, -0.13, 0.02, -1.21, -0.13, -1.55, 0.41]}
		}
		self._poses['thor_mang']['same'] = {
			0:   {'group': 'both_arms_group', 'joints': [0.79, -0.27, 0, -1.57, 1.55, 0, 0, -0.79, 0.27, 0, 1.57, -1.55, 0, 0]},
			2:   {'group': 'both_arms_group', 'joints': [-0.37, 0.0, 1.57, 0, 0, 0, 0, 0.37, 0.0, 1.57, 0, 0, 0, 0]},
			20:  {'group': 'torso_group', 	  'joints': [0.00, 0.03]},
			30:  {'group': 'head_group', 	  'joints': [0.00, 0.00]},
			181: {'group': 'both_arms_group', 'joints': [0.785385646194622, -0.281153767716932, 0.000600782658167331, -1.57080884130538, -0.25205140042963, 0.01563815008, 0, -0.785385646194622, 0.281153767716932, -0.000600782658167331, 1.57080884130538, 0.25205140042963, -0.01563815008, 0]}
		}
		self._poses['thor_mang']['right'] = {
			1:   {'group': 'r_arm_group',	 'joints': [-0.785385646194622, 0.281153767716932, -0.000600782658167331, 1.57080884130538, 0.25205140042963, -0.01563815008, 0]},
			3:   {'group': 'r_arm_group',	 'joints': [-0.77, 0.27, -0.02, 1.21, 0.25, -0.02, 0]},
			4:   {'group': 'r_arm_group',	 'joints': [-0.785385646194622, 0.281153767716932, -0.000600782658167331, 1.97080884130538, 0.25205140042963, -0.01563815008, 0]},
			21:  {'group': 'torso_group', 	 'joints': [-0.30, 0.03]},
			22:  {'group': 'torso_group', 	 'joints': [-0.60, 0.03]},
			23:  {'group': 'torso_group', 	 'joints': [-1.02, 0.03]},
			31:  {'group': 'head_group', 	 'joints': [-1.57, 0.00]},
			131: {'group': 'r_arm_group', 	 'joints': [1.73, 0.69, -1.75, 1.86, -0.04, 0.72, -1.63]},
			132: {'group': 'r_arm_group', 	 'joints': [1.76, 1.13, -1.68, 0.55, -0.02, 1.81, -1.63]},
			133: {'group': 'r_arm_group', 	 'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]},
			134: {'group': 'r_arm_group', 	 'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]}, # placeholder
			136: {'group': 'r_arm_group', 	 'joints': [2.31, 2.2, -1.83, 1.22, -4.1, -0.14, 2.08]},
			138: {'group': 'torso_group', 	 'joints': [-0.92, 0]},
			139: {'group': 'torso_group', 	 'joints': [-1.3, 0]},
			151: {'group': 'r_arm_group', 	 'joints': [1.45, 1.26, -1.10, 2.02, 0.19, -0.04, 2.86]},
			155: {'group': 'r_arm_group',	 'joints': [0.25, 0.75, -0.02, 1.21, 0.38, 1.36, 0.9]},
			156: {'group': 'r_arm_group',	 'joints': [0.25, 0.25, -0.02, 1.21, 0.38, 1.36, 0.9]},
			157: {'group': 'r_arm_group',	 'joints': [0.25, 0.13, -0.02, 1.21, 0.13, 1.55, -0.41]}
		}
		
		self._target_pose = target_pose
		self._vel_scaling = vel_scaling
		self._ignore_collisions = ignore_collisions
		self._link_paddings = link_paddings
		self._is_cartesian = is_cartesian

		self._client = ProxyMoveitClient()
		
		self._failed = False


	def execute(self, userdata):
		"""Execute this state"""
		if self._failed:
			return 'failed'

		if self._client.finished():
			if self._client.success():
				return 'done'
			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

		planning_group = ""
		joint_values = []
		if userdata.side == 'left' and self._target_pose in self._poses[self._robot]['left'].keys():
			planning_group = self._poses[self._robot]['left'][self._target_pose]['group']
			joint_values = self._poses[self._robot]['left'][self._target_pose]['joints']
		elif userdata.side == 'right' and self._target_pose in self._poses[self._robot]['right'].keys():
			planning_group = self._poses[self._robot]['right'][self._target_pose]['group']
			joint_values = self._poses[self._robot]['right'][self._target_pose]['joints']
		elif self._target_pose in self._poses[self._robot]['same'].keys():
			planning_group = self._poses[self._robot]['same'][self._target_pose]['group']
			joint_values = self._poses[self._robot]['same'][self._target_pose]['joints']
		else:
			Logger.logwarn('Specified target pose %d is not specified for %s side' % (self._target_pose, userdata.side))
			self._failed = True
			return

		# create the motion goal
		self._client.new_goal(planning_group)
		self._client.add_joint_values(joint_values)
		self._client.set_velocity_scaling(self._vel_scaling)
		self._client.set_collision_avoidance(self._ignore_collisions)
		self._client.add_link_padding(link_paddings = self._link_paddings) #dict
		if self._is_cartesian:
			self._client.set_cartesian_motion()

		# execute the motion
		try: 
			self._client.start_execution()
		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 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.")
    def __init__(self,
                 target_pose,
                 vel_scaling=0.1,
                 ignore_collisions=False,
                 link_paddings={},
                 is_cartesian=False):
        """Constructor"""
        super(MoveitPredefinedPoseState,
              self).__init__(outcomes=['done', 'failed'], input_keys=['side'])

        if not rospy.has_param("behavior/robot_namespace"):
            Logger.logerr(
                "Need to specify parameter behavior/robot_namespace at the parameter server"
            )
            return

        self._robot = rospy.get_param("behavior/robot_namespace")

        self._poses = dict()
        self._poses['flor'] = dict()
        # Position mode widget: src/vigir_ocs_eui/vigir_rqt/vigir_rqt_position_mode/launch
        self._poses['flor']['left'] = {
            # Joint names: l_arm_shz, l_arm_shx, l_arm_ely, l_arm_elx, l_arm_wry, l_arm_wrx, l_arm_wry2
            1: {
                'group': 'l_arm_group',
                'joints': [-0.25, -1.34, 1.88, 0.49, 0.0, 0.0, 0.0]
            },
            3: {
                'group': 'l_arm_group',
                'joints': [+0.72, -0.95, 2.7, 0.95, 0.0, -0.4, -0.50]
            },
            10: {
                'group': 'l_arm_group',
                'joints': [-1.0, 0.28, 1.2, 1.6, 0.3, 0.5, 0.0]
            },
            21: {
                'group': 'torso_group',
                'joints': [+0.20, 0.00, 0.00]
            },
            22: {
                'group': 'torso_group',
                'joints': [+0.40, 0.00, 0.00]
            },
            23: {
                'group': 'torso_group',
                'joints': [+0.55, 0.00, 0.00]
            },
            112: {
                'group': 'l_leg_group',
                'joints': [+0.00, +0.00, -1.60, +1.40, -0.50, 0.00]
            },  # Safety pose
            114: {
                'group': 'l_arm_group',
                'joints': [+0.76, -0.94, 0.80, 2.00, +1.00, -0.20, -1.35]
            },  # pre_drive
            115: {
                'group': 'l_arm_group',
                'joints': [+0.11, -0.16, 1.75, 1.60, +1.00, -0.90, -1.00]
            },  # drive
            116: {
                'group': 'l_arm_group',
                'joints': []
            },  # We use the right hand for the camera!
            131: {
                'group': 'l_arm_group',
                'joints': [-0.29, -0.22, 1.87, 2.17, -0.17, 0.81, 0.12]
            },
            132: {
                'group': 'l_arm_group',
                'joints': [-0.70, -0.09, 1.93, 0.66, -0.15, 1.52, 0.12]
            },
            133: {
                'group': 'l_arm_group',
                'joints': [-1.38, -0.16, 1.82, 0.57, -0.19, 1.52, 0.12]
            },
            134: {
                'group': 'l_arm_group',
                'joints': []
            },  # Most probably will never be used
            151: {
                'group': 'l_arm_group',
                'joints': [-1.01, -0.43, +1.32, +1.67, -0.91, +1.46, +0.98]
            },
            155: {
                'group': 'l_arm_group',
                'joints': [0.0, -0.37, 2.65, 1.4, -0.2, 0.9, -1.54]
            },
            156: {
                'group': 'l_arm_group',
                'joints': [-0.32, -0.9, 2.2, 1.3, 0.5, 1.0, -1.8]
            },
            157: {
                'group': 'l_arm_group',
                'joints': [-0.45, -1.0, 2.1, 1.3, 0.5, 0.8, -0.8]
            }
        }
        self._poses['flor']['same'] =  {
         0:   {'group': 'both_arms_group', 'joints': [-0.25, -1.34, +1.88, +0.49, +0.00, +0.00, +0.00, \
                     +0.25, +1.34, +1.88, -0.49, +0.00, +0.00, +0.00]},
         2:   {'group': 'both_arms_group', 'joints': [+0.72, -0.95,  2.7,   0.95,  0.0,   -0.4  ,  -0.5, \
                     -0.72,  0.95,  2.7,  -0.95,  0.0,    0.4  ,  -0.5]},
         19:  {'group': 'both_arms_group', 'joints': [-1.5,  -1.5,  +0.30, +0.50,  +0.0,  +0.8,  +0.00, \
                     +1.5,  +1.5,  +0.30, -0.50,  +0.0,  -0.8,  +0.00]},
         20:  {'group': 'torso_group',    'joints': [+0.00, 0.00, 0.00]},
         111: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, +0.00, 0.0, \
                     +0.00, +1.50, +0.00, -1.72, 0.00, +0.00, 0.0]},
         113: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, -1.57, 0.0, \
                     +0.00, +1.50, +0.00, -1.72, 0.00, +1.57, 0.0]},
         181: {'group': 'both_arms_group', 'joints': [-1.53, -0.69, +0.12, +1.47, 0.00, +0.88, 0.00, \
                     +1.53, +0.69, +0.12, -1.47, 0.00, -0.88, 0.00]}
        }
        self._poses['flor']['right'] = {
            # Joint names: r_arm_shz, r_arm_shx, r_arm_ely, r_arm_elx, r_arm_wry, r_arm_wrx, r_arm_wry2
            1: {
                'group': 'r_arm_group',
                'joints': [+0.25, 1.34, 1.88, -0.49, 0.0, 0.0, 0.0]
            },
            3: {
                'group': 'r_arm_group',
                'joints': [-0.72, 0.95, 2.7, -0.95, 0.0, 0.4, -0.50]
            },
            10: {
                'group': 'r_arm_group',
                'joints': [+1.0, -0.28, 1.2, -1.6, 0.3, -0.5, 0.0]
            },
            21: {
                'group': 'torso_group',
                'joints': [-0.20, 0.00, 0.00]
            },
            22: {
                'group': 'torso_group',
                'joints': [-0.40, 0.00, 0.00]
            },
            23: {
                'group': 'torso_group',
                'joints': [-0.55, 0.00, 0.00]
            },
            112: {
                'group': 'r_leg_group',
                'joints': [+0.00, +0.00, -1.34, +1.23, 0.00, 0.00]
            },
            115: {
                'group': 'r_arm_group',
                'joints': []
            },  # Driving is done with the left arm!
            116: {
                'group': 'r_arm_group',
                'joints': [+0.90, 0.17, 0.50, -1.58, -0.70, +1.50, -0.34]
            },  # CarCamera
            131: {
                'group': 'r_arm_group',
                'joints': [+0.29, 0.22, 1.87, -2.17, -0.17, -0.81, 0.12]
            },
            132: {
                'group': 'r_arm_group',
                'joints': [+0.70, 0.09, 1.93, -0.66, -0.15, -1.52, 0.12]
            },
            133: {
                'group': 'r_arm_group',
                'joints': [+1.38, 0.16, 1.82, -0.57, -0.19, -1.52, 0.12]
            },
            134: {
                'group': 'r_arm_group',
                'joints': [+0.00, +0.54, +0.94, -1.04, 0.80, 0.5, 0.7]
            },
            151: {
                'group': 'r_arm_group',
                'joints': [+1.01, +0.43, +1.32, -1.67, -0.91, -1.46, +0.98]
            },
            155: {
                'group': 'r_arm_group',
                'joints': [+0.00, +0.37, +2.65, -1.40, -0.20, -0.90, -1.54]
            },
            156: {
                'group': 'r_arm_group',
                'joints': [+0.32, +0.90, +2.20, -1.30, +0.50, -1.00, -1.80]
            },
            157: {
                'group': 'r_arm_group',
                'joints': [0.45, 1.0, 2.1, -1.3, 0.5, -0.8, -0.8]
            }
        }

        self._poses['thor_mang'] = dict()
        self._poses['thor_mang']['left'] = {
            1: {
                'group':
                'l_arm_group',
                'joints': [
                    0.785385646194622, -0.281153767716932,
                    0.000600782658167331, -1.57080884130538, -0.25205140042963,
                    0.01563815008, 0
                ]
            },
            3: {
                'group': 'l_arm_group',
                'joints': [0.77, -0.27, 0.02, -1.21, -0.25, 0.02, 0]
            },
            4: {
                'group':
                'l_arm_group',
                'joints': [
                    0.785385646194622, -0.281153767716932,
                    0.000600782658167331, -1.97080884130538, -0.25205140042963,
                    0.01563815008, 0
                ]
            },
            21: {
                'group': 'torso_group',
                'joints': [+0.30, 0.03]
            },
            22: {
                'group': 'torso_group',
                'joints': [+0.60, 0.03]
            },
            23: {
                'group': 'torso_group',
                'joints': [+1.02, 0.03]
            },
            31: {
                'group': 'head_group',
                'joints': [1.57, 0.00]
            },
            131: {
                'group': 'l_arm_group',
                'joints': [-1.73, -0.69, 1.75, -1.86, 0.04, -0.72, 1.63]
            },
            132: {
                'group': 'l_arm_group',
                'joints': [-1.76, -1.13, 1.68, -0.55, 0.02, -1.81, 1.63]
            },
            133: {
                'group': 'l_arm_group',
                'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]
            },
            134: {
                'group': 'l_arm_group',
                'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]
            },  # placeholder
            136: {
                'group': 'l_arm_group',
                'joints': [-2.31, -2.2, 1.83, -1.22, 4.1, 0.14, -2.08]
            },
            138: {
                'group': 'torso_group',
                'joints': [0.92, 0]
            },
            139: {
                'group': 'torso_group',
                'joints': [1.3, 0]
            },
            151: {
                'group': 'l_arm_group',
                'joints': [-1.45, -1.26, 1.10, -2.02, -0.19, 0.04, -2.86]
            },
            155: {
                'group': 'l_arm_group',
                'joints': [-0.25, -0.75, 0.02, -1.21, -0.38, -1.36, -0.9]
            },
            156: {
                'group': 'l_arm_group',
                'joints': [-0.25, -0.25, 0.02, -1.21, -0.38, -1.36, -0.9]
            },
            157: {
                'group': 'l_arm_group',
                'joints': [-0.25, -0.13, 0.02, -1.21, -0.13, -1.55, 0.41]
            }
        }
        self._poses['thor_mang']['same'] = {
            0: {
                'group':
                'both_arms_group',
                'joints': [
                    0.79, -0.27, 0, -1.57, 1.55, 0, 0, -0.79, 0.27, 0, 1.57,
                    -1.55, 0, 0
                ]
            },
            2: {
                'group':
                'both_arms_group',
                'joints':
                [-0.37, 0.0, 1.57, 0, 0, 0, 0, 0.37, 0.0, 1.57, 0, 0, 0, 0]
            },
            20: {
                'group': 'torso_group',
                'joints': [0.00, 0.03]
            },
            30: {
                'group': 'head_group',
                'joints': [0.00, 0.00]
            },
            181: {
                'group':
                'both_arms_group',
                'joints': [
                    0.785385646194622, -0.281153767716932,
                    0.000600782658167331, -1.57080884130538, -0.25205140042963,
                    0.01563815008, 0, -0.785385646194622, 0.281153767716932,
                    -0.000600782658167331, 1.57080884130538, 0.25205140042963,
                    -0.01563815008, 0
                ]
            }
        }
        self._poses['thor_mang']['right'] = {
            1: {
                'group':
                'r_arm_group',
                'joints': [
                    -0.785385646194622, 0.281153767716932,
                    -0.000600782658167331, 1.57080884130538, 0.25205140042963,
                    -0.01563815008, 0
                ]
            },
            3: {
                'group': 'r_arm_group',
                'joints': [-0.77, 0.27, -0.02, 1.21, 0.25, -0.02, 0]
            },
            4: {
                'group':
                'r_arm_group',
                'joints': [
                    -0.785385646194622, 0.281153767716932,
                    -0.000600782658167331, 1.97080884130538, 0.25205140042963,
                    -0.01563815008, 0
                ]
            },
            21: {
                'group': 'torso_group',
                'joints': [-0.30, 0.03]
            },
            22: {
                'group': 'torso_group',
                'joints': [-0.60, 0.03]
            },
            23: {
                'group': 'torso_group',
                'joints': [-1.02, 0.03]
            },
            31: {
                'group': 'head_group',
                'joints': [-1.57, 0.00]
            },
            131: {
                'group': 'r_arm_group',
                'joints': [1.73, 0.69, -1.75, 1.86, -0.04, 0.72, -1.63]
            },
            132: {
                'group': 'r_arm_group',
                'joints': [1.76, 1.13, -1.68, 0.55, -0.02, 1.81, -1.63]
            },
            133: {
                'group': 'r_arm_group',
                'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]
            },
            134: {
                'group': 'r_arm_group',
                'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]
            },  # placeholder
            136: {
                'group': 'r_arm_group',
                'joints': [2.31, 2.2, -1.83, 1.22, -4.1, -0.14, 2.08]
            },
            138: {
                'group': 'torso_group',
                'joints': [-0.92, 0]
            },
            139: {
                'group': 'torso_group',
                'joints': [-1.3, 0]
            },
            151: {
                'group': 'r_arm_group',
                'joints': [1.45, 1.26, -1.10, 2.02, 0.19, -0.04, 2.86]
            },
            155: {
                'group': 'r_arm_group',
                'joints': [0.25, 0.75, -0.02, 1.21, 0.38, 1.36, 0.9]
            },
            156: {
                'group': 'r_arm_group',
                'joints': [0.25, 0.25, -0.02, 1.21, 0.38, 1.36, 0.9]
            },
            157: {
                'group': 'r_arm_group',
                'joints': [0.25, 0.13, -0.02, 1.21, 0.13, 1.55, -0.41]
            }
        }

        self._target_pose = target_pose
        self._vel_scaling = vel_scaling
        self._ignore_collisions = ignore_collisions
        self._link_paddings = link_paddings
        self._is_cartesian = is_cartesian

        self._client = ProxyMoveitClient()

        self._failed = False
class MoveitPredefinedPoseState(EventState):
    """
	Uses moveit to go to one of the pre-defined poses.

	-- target_pose 			int 		Identifier of the pre-defined pose to be used.
	-- vel_scaling  		float 		Scales the velocity of the motion.
										Lower values for slower trajectories.
	-- ignore_collisions  	boolean 	Should collisions be ignored? Only pass True if you are sure that it is safe.
	-- link_paddings 		dict 		link_name (str) : padding (float) pairs
	-- is_cartesian 		boolean 	Execute as cartesian motion

	># side 				string 		Arm side, turning direction, etc.
										Possible values: {left, right, same}
 
	<= done 							Successfully executed the motion.
	<= failed 							Failed to execute the motion.

	"""

    # Arms
    STAND_POSE = 0
    SINGLE_ARM_STAND = 1  # for preventing unconstrained motion of the other
    BOTH_ARMS_SIDES = 2  # for safely holding the tool at the robot's side
    SINGLE_ARM_SIDE = 3
    STAND_POSE_UP = 4
    CALIBRATE_ARMS = 10  # for checking arm calibration with the head camera
    WALK_POSE = 19

    # Torso
    TURN_TORSO_CENTER_POSE = 20
    TURN_TORSO_SLIGHTLY = 21
    TURN_TORSO_MORE = 22
    TURN_TORSO_FULL = 23

    # Head
    HEAD_CENTER = 30
    HEAD_SIDE = 31

    # Task-specific poses
    # Second digit is task number. Comment is pose in the Position widget.
    CAR_ENTRY_ARMS_POSE = 111  # CarEntry
    CAR_ENTRY_LEGS_POSE = 112  # CarEntry
    CAR_ENTRY_FORE_POSE = 113  # CarEntryFore
    CAR_PREDRIVE_LARM_POSE = 114  # pre_drive
    CAR_DRIVE_LARM_POSE = 115  # drive
    CAR_DRIVE_CAMERA_POSE = 116  # CarCamera
    DOOR_READY_POSE = 131
    DOOR_OPEN_POSE_TURNED = 132
    DOOR_OPEN_POSE_STRAIGHT = 133
    DOOR_OPEN_POSE_SIDE = 134  # push_door_3
    DOOR_PUSH_SIDE = 136
    DOOR_OPEN_TURN_MANIPULATE = 138
    DOOR_OPEN_TURN_LIDAR = 139

    POKE_READY_POSE = 151
    PREP_CUT_WALL_1 = 155
    PREP_CUT_WALL_2 = 156
    PREP_CUT_WALL_3 = 157

    LADDER_READY_POSE = 181

    def __init__(self,
                 target_pose,
                 vel_scaling=0.1,
                 ignore_collisions=False,
                 link_paddings={},
                 is_cartesian=False):
        """Constructor"""
        super(MoveitPredefinedPoseState,
              self).__init__(outcomes=['done', 'failed'], input_keys=['side'])

        if not rospy.has_param("behavior/robot_namespace"):
            Logger.logerr(
                "Need to specify parameter behavior/robot_namespace at the parameter server"
            )
            return

        self._robot = rospy.get_param("behavior/robot_namespace")

        self._poses = dict()
        self._poses['flor'] = dict()
        # Position mode widget: src/vigir_ocs_eui/vigir_rqt/vigir_rqt_position_mode/launch
        self._poses['flor']['left'] = {
            # Joint names: l_arm_shz, l_arm_shx, l_arm_ely, l_arm_elx, l_arm_wry, l_arm_wrx, l_arm_wry2
            1: {
                'group': 'l_arm_group',
                'joints': [-0.25, -1.34, 1.88, 0.49, 0.0, 0.0, 0.0]
            },
            3: {
                'group': 'l_arm_group',
                'joints': [+0.72, -0.95, 2.7, 0.95, 0.0, -0.4, -0.50]
            },
            10: {
                'group': 'l_arm_group',
                'joints': [-1.0, 0.28, 1.2, 1.6, 0.3, 0.5, 0.0]
            },
            21: {
                'group': 'torso_group',
                'joints': [+0.20, 0.00, 0.00]
            },
            22: {
                'group': 'torso_group',
                'joints': [+0.40, 0.00, 0.00]
            },
            23: {
                'group': 'torso_group',
                'joints': [+0.55, 0.00, 0.00]
            },
            112: {
                'group': 'l_leg_group',
                'joints': [+0.00, +0.00, -1.60, +1.40, -0.50, 0.00]
            },  # Safety pose
            114: {
                'group': 'l_arm_group',
                'joints': [+0.76, -0.94, 0.80, 2.00, +1.00, -0.20, -1.35]
            },  # pre_drive
            115: {
                'group': 'l_arm_group',
                'joints': [+0.11, -0.16, 1.75, 1.60, +1.00, -0.90, -1.00]
            },  # drive
            116: {
                'group': 'l_arm_group',
                'joints': []
            },  # We use the right hand for the camera!
            131: {
                'group': 'l_arm_group',
                'joints': [-0.29, -0.22, 1.87, 2.17, -0.17, 0.81, 0.12]
            },
            132: {
                'group': 'l_arm_group',
                'joints': [-0.70, -0.09, 1.93, 0.66, -0.15, 1.52, 0.12]
            },
            133: {
                'group': 'l_arm_group',
                'joints': [-1.38, -0.16, 1.82, 0.57, -0.19, 1.52, 0.12]
            },
            134: {
                'group': 'l_arm_group',
                'joints': []
            },  # Most probably will never be used
            151: {
                'group': 'l_arm_group',
                'joints': [-1.01, -0.43, +1.32, +1.67, -0.91, +1.46, +0.98]
            },
            155: {
                'group': 'l_arm_group',
                'joints': [0.0, -0.37, 2.65, 1.4, -0.2, 0.9, -1.54]
            },
            156: {
                'group': 'l_arm_group',
                'joints': [-0.32, -0.9, 2.2, 1.3, 0.5, 1.0, -1.8]
            },
            157: {
                'group': 'l_arm_group',
                'joints': [-0.45, -1.0, 2.1, 1.3, 0.5, 0.8, -0.8]
            }
        }
        self._poses['flor']['same'] =  {
         0:   {'group': 'both_arms_group', 'joints': [-0.25, -1.34, +1.88, +0.49, +0.00, +0.00, +0.00, \
                     +0.25, +1.34, +1.88, -0.49, +0.00, +0.00, +0.00]},
         2:   {'group': 'both_arms_group', 'joints': [+0.72, -0.95,  2.7,   0.95,  0.0,   -0.4  ,  -0.5, \
                     -0.72,  0.95,  2.7,  -0.95,  0.0,    0.4  ,  -0.5]},
         19:  {'group': 'both_arms_group', 'joints': [-1.5,  -1.5,  +0.30, +0.50,  +0.0,  +0.8,  +0.00, \
                     +1.5,  +1.5,  +0.30, -0.50,  +0.0,  -0.8,  +0.00]},
         20:  {'group': 'torso_group',    'joints': [+0.00, 0.00, 0.00]},
         111: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, +0.00, 0.0, \
                     +0.00, +1.50, +0.00, -1.72, 0.00, +0.00, 0.0]},
         113: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, -1.57, 0.0, \
                     +0.00, +1.50, +0.00, -1.72, 0.00, +1.57, 0.0]},
         181: {'group': 'both_arms_group', 'joints': [-1.53, -0.69, +0.12, +1.47, 0.00, +0.88, 0.00, \
                     +1.53, +0.69, +0.12, -1.47, 0.00, -0.88, 0.00]}
        }
        self._poses['flor']['right'] = {
            # Joint names: r_arm_shz, r_arm_shx, r_arm_ely, r_arm_elx, r_arm_wry, r_arm_wrx, r_arm_wry2
            1: {
                'group': 'r_arm_group',
                'joints': [+0.25, 1.34, 1.88, -0.49, 0.0, 0.0, 0.0]
            },
            3: {
                'group': 'r_arm_group',
                'joints': [-0.72, 0.95, 2.7, -0.95, 0.0, 0.4, -0.50]
            },
            10: {
                'group': 'r_arm_group',
                'joints': [+1.0, -0.28, 1.2, -1.6, 0.3, -0.5, 0.0]
            },
            21: {
                'group': 'torso_group',
                'joints': [-0.20, 0.00, 0.00]
            },
            22: {
                'group': 'torso_group',
                'joints': [-0.40, 0.00, 0.00]
            },
            23: {
                'group': 'torso_group',
                'joints': [-0.55, 0.00, 0.00]
            },
            112: {
                'group': 'r_leg_group',
                'joints': [+0.00, +0.00, -1.34, +1.23, 0.00, 0.00]
            },
            115: {
                'group': 'r_arm_group',
                'joints': []
            },  # Driving is done with the left arm!
            116: {
                'group': 'r_arm_group',
                'joints': [+0.90, 0.17, 0.50, -1.58, -0.70, +1.50, -0.34]
            },  # CarCamera
            131: {
                'group': 'r_arm_group',
                'joints': [+0.29, 0.22, 1.87, -2.17, -0.17, -0.81, 0.12]
            },
            132: {
                'group': 'r_arm_group',
                'joints': [+0.70, 0.09, 1.93, -0.66, -0.15, -1.52, 0.12]
            },
            133: {
                'group': 'r_arm_group',
                'joints': [+1.38, 0.16, 1.82, -0.57, -0.19, -1.52, 0.12]
            },
            134: {
                'group': 'r_arm_group',
                'joints': [+0.00, +0.54, +0.94, -1.04, 0.80, 0.5, 0.7]
            },
            151: {
                'group': 'r_arm_group',
                'joints': [+1.01, +0.43, +1.32, -1.67, -0.91, -1.46, +0.98]
            },
            155: {
                'group': 'r_arm_group',
                'joints': [+0.00, +0.37, +2.65, -1.40, -0.20, -0.90, -1.54]
            },
            156: {
                'group': 'r_arm_group',
                'joints': [+0.32, +0.90, +2.20, -1.30, +0.50, -1.00, -1.80]
            },
            157: {
                'group': 'r_arm_group',
                'joints': [0.45, 1.0, 2.1, -1.3, 0.5, -0.8, -0.8]
            }
        }

        self._poses['thor_mang'] = dict()
        self._poses['thor_mang']['left'] = {
            1: {
                'group':
                'l_arm_group',
                'joints': [
                    0.785385646194622, -0.281153767716932,
                    0.000600782658167331, -1.57080884130538, -0.25205140042963,
                    0.01563815008, 0
                ]
            },
            3: {
                'group': 'l_arm_group',
                'joints': [0.77, -0.27, 0.02, -1.21, -0.25, 0.02, 0]
            },
            4: {
                'group':
                'l_arm_group',
                'joints': [
                    0.785385646194622, -0.281153767716932,
                    0.000600782658167331, -1.97080884130538, -0.25205140042963,
                    0.01563815008, 0
                ]
            },
            21: {
                'group': 'torso_group',
                'joints': [+0.30, 0.03]
            },
            22: {
                'group': 'torso_group',
                'joints': [+0.60, 0.03]
            },
            23: {
                'group': 'torso_group',
                'joints': [+1.02, 0.03]
            },
            31: {
                'group': 'head_group',
                'joints': [1.57, 0.00]
            },
            131: {
                'group': 'l_arm_group',
                'joints': [-1.73, -0.69, 1.75, -1.86, 0.04, -0.72, 1.63]
            },
            132: {
                'group': 'l_arm_group',
                'joints': [-1.76, -1.13, 1.68, -0.55, 0.02, -1.81, 1.63]
            },
            133: {
                'group': 'l_arm_group',
                'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]
            },
            134: {
                'group': 'l_arm_group',
                'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]
            },  # placeholder
            136: {
                'group': 'l_arm_group',
                'joints': [-2.31, -2.2, 1.83, -1.22, 4.1, 0.14, -2.08]
            },
            138: {
                'group': 'torso_group',
                'joints': [0.92, 0]
            },
            139: {
                'group': 'torso_group',
                'joints': [1.3, 0]
            },
            151: {
                'group': 'l_arm_group',
                'joints': [-1.45, -1.26, 1.10, -2.02, -0.19, 0.04, -2.86]
            },
            155: {
                'group': 'l_arm_group',
                'joints': [-0.25, -0.75, 0.02, -1.21, -0.38, -1.36, -0.9]
            },
            156: {
                'group': 'l_arm_group',
                'joints': [-0.25, -0.25, 0.02, -1.21, -0.38, -1.36, -0.9]
            },
            157: {
                'group': 'l_arm_group',
                'joints': [-0.25, -0.13, 0.02, -1.21, -0.13, -1.55, 0.41]
            }
        }
        self._poses['thor_mang']['same'] = {
            0: {
                'group':
                'both_arms_group',
                'joints': [
                    0.79, -0.27, 0, -1.57, 1.55, 0, 0, -0.79, 0.27, 0, 1.57,
                    -1.55, 0, 0
                ]
            },
            2: {
                'group':
                'both_arms_group',
                'joints':
                [-0.37, 0.0, 1.57, 0, 0, 0, 0, 0.37, 0.0, 1.57, 0, 0, 0, 0]
            },
            20: {
                'group': 'torso_group',
                'joints': [0.00, 0.03]
            },
            30: {
                'group': 'head_group',
                'joints': [0.00, 0.00]
            },
            181: {
                'group':
                'both_arms_group',
                'joints': [
                    0.785385646194622, -0.281153767716932,
                    0.000600782658167331, -1.57080884130538, -0.25205140042963,
                    0.01563815008, 0, -0.785385646194622, 0.281153767716932,
                    -0.000600782658167331, 1.57080884130538, 0.25205140042963,
                    -0.01563815008, 0
                ]
            }
        }
        self._poses['thor_mang']['right'] = {
            1: {
                'group':
                'r_arm_group',
                'joints': [
                    -0.785385646194622, 0.281153767716932,
                    -0.000600782658167331, 1.57080884130538, 0.25205140042963,
                    -0.01563815008, 0
                ]
            },
            3: {
                'group': 'r_arm_group',
                'joints': [-0.77, 0.27, -0.02, 1.21, 0.25, -0.02, 0]
            },
            4: {
                'group':
                'r_arm_group',
                'joints': [
                    -0.785385646194622, 0.281153767716932,
                    -0.000600782658167331, 1.97080884130538, 0.25205140042963,
                    -0.01563815008, 0
                ]
            },
            21: {
                'group': 'torso_group',
                'joints': [-0.30, 0.03]
            },
            22: {
                'group': 'torso_group',
                'joints': [-0.60, 0.03]
            },
            23: {
                'group': 'torso_group',
                'joints': [-1.02, 0.03]
            },
            31: {
                'group': 'head_group',
                'joints': [-1.57, 0.00]
            },
            131: {
                'group': 'r_arm_group',
                'joints': [1.73, 0.69, -1.75, 1.86, -0.04, 0.72, -1.63]
            },
            132: {
                'group': 'r_arm_group',
                'joints': [1.76, 1.13, -1.68, 0.55, -0.02, 1.81, -1.63]
            },
            133: {
                'group': 'r_arm_group',
                'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]
            },
            134: {
                'group': 'r_arm_group',
                'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]
            },  # placeholder
            136: {
                'group': 'r_arm_group',
                'joints': [2.31, 2.2, -1.83, 1.22, -4.1, -0.14, 2.08]
            },
            138: {
                'group': 'torso_group',
                'joints': [-0.92, 0]
            },
            139: {
                'group': 'torso_group',
                'joints': [-1.3, 0]
            },
            151: {
                'group': 'r_arm_group',
                'joints': [1.45, 1.26, -1.10, 2.02, 0.19, -0.04, 2.86]
            },
            155: {
                'group': 'r_arm_group',
                'joints': [0.25, 0.75, -0.02, 1.21, 0.38, 1.36, 0.9]
            },
            156: {
                'group': 'r_arm_group',
                'joints': [0.25, 0.25, -0.02, 1.21, 0.38, 1.36, 0.9]
            },
            157: {
                'group': 'r_arm_group',
                'joints': [0.25, 0.13, -0.02, 1.21, 0.13, 1.55, -0.41]
            }
        }

        self._target_pose = target_pose
        self._vel_scaling = vel_scaling
        self._ignore_collisions = ignore_collisions
        self._link_paddings = link_paddings
        self._is_cartesian = is_cartesian

        self._client = ProxyMoveitClient()

        self._failed = False

    def execute(self, userdata):
        """Execute this state"""
        if self._failed:
            return 'failed'

        if self._client.finished():
            if self._client.success():
                return 'done'
            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

        planning_group = ""
        joint_values = []
        if userdata.side == 'left' and self._target_pose in self._poses[
                self._robot]['left'].keys():
            planning_group = self._poses[self._robot]['left'][
                self._target_pose]['group']
            joint_values = self._poses[self._robot]['left'][
                self._target_pose]['joints']
        elif userdata.side == 'right' and self._target_pose in self._poses[
                self._robot]['right'].keys():
            planning_group = self._poses[self._robot]['right'][
                self._target_pose]['group']
            joint_values = self._poses[self._robot]['right'][
                self._target_pose]['joints']
        elif self._target_pose in self._poses[self._robot]['same'].keys():
            planning_group = self._poses[self._robot]['same'][
                self._target_pose]['group']
            joint_values = self._poses[self._robot]['same'][
                self._target_pose]['joints']
        else:
            Logger.logwarn(
                'Specified target pose %d is not specified for %s side' %
                (self._target_pose, userdata.side))
            self._failed = True
            return

        # create the motion goal
        self._client.new_goal(planning_group)
        self._client.add_joint_values(joint_values)
        self._client.set_velocity_scaling(self._vel_scaling)
        self._client.set_collision_avoidance(self._ignore_collisions)
        self._client.add_link_padding(link_paddings=self._link_paddings)  #dict
        if self._is_cartesian:
            self._client.set_cartesian_motion()

        # execute the motion
        try:
            self._client.start_execution()
        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 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 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
Ejemplo n.º 22
0
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 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 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.")