def __init__(self, hand_type, configuration): '''Constructor''' super(FingerConfigurationState, self).__init__(outcomes=['done', 'failed'], input_keys=['hand_side']) self._joint_names = dict() self._joint_names['left'] = dict() self._joint_names['left']['robotiq'] = ['left_f0_j1', 'left_f1_j0', 'left_f1_j1', 'left_f2_j1'] self._joint_names['left']['vt_hand'] = ['l_f0_j0', 'l_f1_j0'] self._joint_names['right'] = dict() self._joint_names['right']['robotiq'] = ['right_f0_j1', 'right_f1_j0', 'right_f1_j1', 'right_f2_j1'] self._joint_names['right']['vt_hand'] = ['r_f0_j0', 'r_f1_j0'] self._joint_values = dict() self._joint_values['robotiq'] = map(lambda x: x * configuration, [1.22, 0.08, 1.13, 1.13]) # 2nd value is the scissor joint self._joint_values['vt_hand'] = map(lambda x: x * configuration, [2.685, 2.685]) self._execution_state = HandTrajectoryState(hand_type) self._internal_userdata = smach.UserData() if not hand_type in self._joint_names['left'].keys(): raise Exception("Unspecified hand type %s, only have %s" % (hand_type, str(self._joint_names['left'].keys()))) self._hand_type = hand_type self._configuration = self._joint_values[hand_type] self._print = True self._failed = False
def create(self): pull_affordance = "pull" affordance_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == "left" else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM pull_displacement = 0.3 # meters # x:383 y:840, x:483 y:490 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.none = None _state_machine.userdata.step_back_distance = 1.0 # meters _state_machine.userdata.grasp_preference = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] self._pull_displacement = pull_displacement # [/MANUAL_CREATE] # x:1033 y:40, x:333 y:90, x:1033 y:190 _sm_go_to_grasp_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'again'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference']) with _sm_go_to_grasp_0: # x:33 y:49 OperatableStateMachine.add('Get_Grasp_Info', GetTemplateGraspState(), transitions={'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'Inform_Grasp_Failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low, 'not_available': Autonomy.Low}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'grasp': 'grasp_pose'}) # x:40 y:293 OperatableStateMachine.add('Convert_Waypoints', CalculationState(calculation=lambda msg: [msg.pose]), transitions={'done': 'Plan_To_Grasp'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_pose', 'output_value': 'grasp_waypoints'}) # x:242 y:292 OperatableStateMachine.add('Plan_To_Grasp', PlanEndeffectorCartesianWaypointsState(ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_To_Grasp_Pose', 'incomplete': 'Move_To_Grasp_Pose', 'failed': 'Decide_Which_Grasp'}, autonomy={'planned': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.High}, remapping={'waypoints': 'grasp_waypoints', 'hand': 'hand_side', 'frame_id': 'grasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:494 y:175 OperatableStateMachine.add('Move_To_Grasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Optional_Template_Adjustment', 'failed': 'Decide_Which_Grasp'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:226 y:177 OperatableStateMachine.add('Inform_Grasp_Failed', LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:970 y:294 OperatableStateMachine.add('Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'again'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:41 y:178 OperatableStateMachine.add('Extract_Frame_Id', CalculationState(calculation=lambda pose: pose.header.frame_id), transitions={'done': 'Convert_Waypoints'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_pose', 'output_value': 'grasp_frame_id'}) # x:712 y:78 OperatableStateMachine.add('Optional_Template_Adjustment', OperatorDecisionState(outcomes=["grasp", "pregrasp", "skip"], hint="Consider adjusting the template's pose", suggestion="skip"), transitions={'grasp': 'Get_Grasp_Info', 'pregrasp': 'again', 'skip': 'finished'}, autonomy={'grasp': Autonomy.Full, 'pregrasp': Autonomy.Full, 'skip': Autonomy.High}) # x:754 y:294 OperatableStateMachine.add('Decide_Which_Grasp', OperatorDecisionState(outcomes=["same", "next"], hint='Try the same grasp or the next one?', suggestion='same'), transitions={'same': 'Optional_Template_Adjustment', 'next': 'Increase_Preference_Index'}, autonomy={'same': Autonomy.High, 'next': Autonomy.High}) # x:133 y:390, x:433 y:190, x:983 y:140 _sm_perform_grasp_1 = OperatableStateMachine(outcomes=['finished', 'failed', 'next'], input_keys=['hand_side', 'grasp_preference', 'template_id', 'pregrasp_pose'], output_keys=['grasp_preference']) with _sm_perform_grasp_1: # x:68 y:76 OperatableStateMachine.add('Get_Finger_Configuration', GetTemplateFingerConfigState(), transitions={'done': 'Close_Fingers', 'failed': 'failed', 'not_available': 'Inform_Closing_Failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High, 'not_available': Autonomy.High}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'finger_config': 'finger_config'}) # x:293 y:328 OperatableStateMachine.add('Convert_Waypoints', CalculationState(calculation=lambda msg: [msg.pose]), transitions={'done': 'Plan_Back_To_Pregrasp'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_waypoints'}) # x:496 y:328 OperatableStateMachine.add('Plan_Back_To_Pregrasp', PlanEndeffectorCartesianWaypointsState(ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_Back_To_Pregrasp_Pose', 'incomplete': 'Move_Back_To_Pregrasp_Pose', 'failed': 'failed'}, autonomy={'planned': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Low}, remapping={'waypoints': 'pregrasp_waypoints', 'hand': 'hand_side', 'frame_id': 'pregrasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:662 y:228 OperatableStateMachine.add('Move_Back_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Increase_Preference_Index', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:296 y:228 OperatableStateMachine.add('Extract_Frame_Id', CalculationState(calculation=lambda pose: pose.header.frame_id), transitions={'done': 'Convert_Waypoints'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_frame_id'}) # x:673 y:128 OperatableStateMachine.add('Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'next'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:81 y:228 OperatableStateMachine.add('Close_Fingers', HandTrajectoryState(hand_type=self.hand_type), transitions={'done': 'finished', 'failed': 'Extract_Frame_Id'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'finger_trajectory': 'finger_config', 'hand_side': 'hand_side'}) # x:490 y:75 OperatableStateMachine.add('Inform_Closing_Failed', LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:733 y:190, x:383 y:40 _sm_go_to_pregrasp_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference', 'pregrasp_pose']) with _sm_go_to_pregrasp_2: # x:27 y:68 OperatableStateMachine.add('Get_Pregrasp_Info', GetTemplatePregraspState(), transitions={'done': 'Plan_To_Pregrasp_Pose', 'failed': 'failed', 'not_available': 'Inform_Pregrasp_Failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High, 'not_available': Autonomy.High}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'pre_grasp': 'pregrasp_pose'}) # x:269 y:153 OperatableStateMachine.add('Inform_Pregrasp_Failed', LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Low}) # x:537 y:228 OperatableStateMachine.add('Move_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'Decide_Which_Pregrasp'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:25 y:328 OperatableStateMachine.add('Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Get_Pregrasp_Info'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:266 y:228 OperatableStateMachine.add('Plan_To_Pregrasp_Pose', PlanEndeffectorPoseState(ignore_collisions=False, include_torso=False, allowed_collisions=[], planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_To_Pregrasp_Pose', 'failed': 'Decide_Which_Pregrasp'}, autonomy={'planned': Autonomy.Low, 'failed': Autonomy.High}, remapping={'target_pose': 'pregrasp_pose', 'hand': 'hand_side', 'joint_trajectory': 'joint_trajectory'}) # x:266 y:327 OperatableStateMachine.add('Decide_Which_Pregrasp', OperatorDecisionState(outcomes=["same", "next"], hint='Try the same pregrasp or the next one?', suggestion='same'), transitions={'same': 'Get_Pregrasp_Info', 'next': 'Increase_Preference_Index'}, autonomy={'same': Autonomy.High, 'next': Autonomy.High}) # x:30 y:444, x:162 y:478, x:230 y:478 _sm_planning_pipeline_3 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['stand_pose'], output_keys=['plan_header']) with _sm_planning_pipeline_3: # x:34 y:57 OperatableStateMachine.add('Create_Step_Goal', CreateStepGoalState(pose_is_pelvis=True), transitions={'done': 'Plan_To_Waypoint', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'target_pose': 'stand_pose', 'step_goal': 'step_goal'}) # x:553 y:481 OperatableStateMachine.add('Modify_Plan', InputState(request=InputState.FOOTSTEP_PLAN_HEADER, message='Modify plan, VALIDATE, and confirm.'), transitions={'received': 'finished', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'plan_header'}) # x:34 y:484 OperatableStateMachine.add('Plan_To_Waypoint', PlanFootstepsState(mode=self.parameter_set), transitions={'planned': 'Modify_Plan', 'failed': 'Decide_Replan_without_Collision'}, autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'}) # x:139 y:314 OperatableStateMachine.add('Decide_Replan_without_Collision', OperatorDecisionState(outcomes=['replan', 'fail'], hint='Try replanning without collision avoidance.', suggestion='replan'), transitions={'replan': 'Replan_without_Collision', 'fail': 'failed'}, autonomy={'replan': Autonomy.Low, 'fail': Autonomy.Full}) # x:319 y:406 OperatableStateMachine.add('Replan_without_Collision', PlanFootstepsState(mode='drc_step_no_collision'), transitions={'planned': 'Modify_Plan', 'failed': 'failed'}, autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'}) # x:1103 y:424, x:130 y:478 _sm_grasp_trigger_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'grasp_preference']) with _sm_grasp_trigger_4: # x:86 y:72 OperatableStateMachine.add('Go_to_Pregrasp', _sm_go_to_pregrasp_2, transitions={'finished': 'Open_Fingers', 'failed': 'Grasp_Manually'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id', 'pregrasp_pose': 'pregrasp_pose'}) # x:789 y:172 OperatableStateMachine.add('Perform_Grasp', _sm_perform_grasp_1, transitions={'finished': 'finished', 'failed': 'Grasp_Manually', 'next': 'Close_Fingers'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'next': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id', 'pregrasp_pose': 'pregrasp_pose'}) # x:332 y:178 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState(hand_type=self.hand_type, configuration=0.0), transitions={'done': 'Go_to_Grasp', 'failed': 'Grasp_Manually'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.High}, remapping={'hand_side': 'hand_side'}) # x:332 y:28 OperatableStateMachine.add('Close_Fingers', FingerConfigurationState(hand_type=self.hand_type, configuration=1.0), transitions={'done': 'Go_to_Pregrasp', 'failed': 'Grasp_Manually'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.High}, remapping={'hand_side': 'hand_side'}) # x:324 y:428 OperatableStateMachine.add('Grasp_Manually', OperatorDecisionState(outcomes=["fingers_closed", "abort"], hint="Grasp the object manually, continue when fingers are closed.", suggestion=None), transitions={'fingers_closed': 'finished', 'abort': 'failed'}, autonomy={'fingers_closed': Autonomy.Full, 'abort': Autonomy.Full}) # x:543 y:172 OperatableStateMachine.add('Go_to_Grasp', _sm_go_to_grasp_0, transitions={'finished': 'Perform_Grasp', 'failed': 'Grasp_Manually', 'again': 'Close_Fingers'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'again': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id'}) # x:30 y:478, x:130 y:478, x:230 y:478 _sm_walk_to_template_5 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['template_id', 'grasp_preference', 'hand_side']) with _sm_walk_to_template_5: # x:265 y:28 OperatableStateMachine.add('Decide_Request_Template', DecisionState(outcomes=['request', 'continue'], conditions=lambda x: 'continue' if x is not None else 'request'), transitions={'request': 'Request_Template', 'continue': 'Get_Stand_Pose'}, autonomy={'request': Autonomy.Low, 'continue': Autonomy.Off}, remapping={'input_value': 'template_id'}) # x:1033 y:106 OperatableStateMachine.add('Increment_Stand_Pose', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Inform_About_Retry'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:1162 y:29 OperatableStateMachine.add('Inform_About_Retry', LogState(text="Stand pose choice failed. Trying again.", severity=Logger.REPORT_INFO), transitions={'done': 'Get_Stand_Pose'}, autonomy={'done': Autonomy.Off}) # x:567 y:118 OperatableStateMachine.add('Inform_About_Fail', LogState(text="Unable to find a suitable stand pose for the template.", severity=Logger.REPORT_WARN), transitions={'done': 'Decide_Repeat_Request'}, autonomy={'done': Autonomy.Off}) # x:554 y:274 OperatableStateMachine.add('Get_Goal_from_Operator', InputState(request=InputState.WAYPOINT_GOAL_POSE, message="Provide a waypoint in front of the template."), transitions={'received': 'Walk_To_Waypoint', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'plan_header'}) # x:279 y:110 OperatableStateMachine.add('Request_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Specify target template"), transitions={'received': 'Get_Stand_Pose', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Off, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'template_id'}) # x:825 y:461 OperatableStateMachine.add('Wait_For_Stand', CheckCurrentControlModeState(target_mode=CheckCurrentControlModeState.STAND, wait=True), transitions={'correct': 'finished', 'incorrect': 'failed'}, autonomy={'correct': Autonomy.Low, 'incorrect': Autonomy.Full}, remapping={'control_mode': 'control_mode'}) # x:1143 y:277 OperatableStateMachine.add('Decide_Stand_Preference', OperatorDecisionState(outcomes=["same", "next", "abort"], hint="Same or next stand pose?", suggestion="next"), transitions={'same': 'Inform_About_Retry', 'next': 'Increment_Stand_Pose', 'abort': 'aborted'}, autonomy={'same': Autonomy.Full, 'next': Autonomy.Full, 'abort': Autonomy.Full}) # x:842 y:152 OperatableStateMachine.add('Planning_Pipeline', _sm_planning_pipeline_3, transitions={'finished': 'Walk_To_Waypoint', 'failed': 'Decide_Stand_Preference', 'aborted': 'Decide_Stand_Preference'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'stand_pose': 'stand_pose', 'plan_header': 'plan_header'}) # x:833 y:276 OperatableStateMachine.add('Walk_To_Waypoint', ExecuteStepPlanActionState(), transitions={'finished': 'Wait_For_Stand', 'failed': 'Decide_Stand_Preference'}, autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'plan_header': 'plan_header'}) # x:554 y:195 OperatableStateMachine.add('Decide_Repeat_Request', OperatorDecisionState(outcomes=['repeat_id', 'request_goal'], hint=None, suggestion=None), transitions={'repeat_id': 'Request_Template', 'request_goal': 'Get_Goal_from_Operator'}, autonomy={'repeat_id': Autonomy.Low, 'request_goal': Autonomy.High}) # x:547 y:27 OperatableStateMachine.add('Get_Stand_Pose', GetTemplateStandPoseState(), transitions={'done': 'Planning_Pipeline', 'failed': 'Inform_About_Fail', 'not_available': 'Inform_About_Fail'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low, 'not_available': Autonomy.High}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'stand_pose': 'stand_pose'}) # x:133 y:340, x:383 y:140 _sm_perform_step_back_6 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['step_back_distance']) with _sm_perform_step_back_6: # x:78 y:78 OperatableStateMachine.add('Plan_Steps_Back', FootstepPlanRelativeState(direction=FootstepPlanRelativeState.DIRECTION_BACKWARD), transitions={'planned': 'Do_Steps_Back', 'failed': 'failed'}, autonomy={'planned': Autonomy.High, 'failed': Autonomy.Full}, remapping={'distance': 'step_back_distance', 'plan_header': 'plan_header'}) # x:74 y:228 OperatableStateMachine.add('Do_Steps_Back', ExecuteStepPlanActionState(), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'plan_header': 'plan_header'}) # x:133 y:340, x:333 y:90 _sm_release_trigger_7 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'none']) with _sm_release_trigger_7: # x:82 y:78 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState(hand_type=self.hand_type, configuration=0), transitions={'done': 'Take_Hand_Back', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'hand_side': 'hand_side'}) # x:96 y:178 OperatableStateMachine.add('Take_Hand_Back', LogState(text="Take hand slightly back", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Full}) # x:733 y:240, x:33 y:289 _sm_pull_trigger_8 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none']) with _sm_pull_trigger_8: # x:202 y:28 OperatableStateMachine.add('Ready_To_Pull', LogState(text="Ready to pull the trigger down", severity=Logger.REPORT_INFO), transitions={'done': 'Get_Pull_Affordance'}, autonomy={'done': Autonomy.High}) # x:192 y:328 OperatableStateMachine.add('Plan_Pull', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Pull', 'incomplete': 'Execute_Pull', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full}, remapping={'affordance': 'affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:176 y:428 OperatableStateMachine.add('Execute_Pull', ExecuteTrajectoryMsgState(controller=affordance_controller), transitions={'done': 'Decide_Repeat_Pull', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:183 y:228 OperatableStateMachine.add('Scale_Pull_Affordance', CalculationState(calculation=lambda x: x), transitions={'done': 'Plan_Pull'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'affordance', 'output_value': 'affordance'}) # x:173 y:128 OperatableStateMachine.add('Get_Pull_Affordance', GetTemplateAffordanceState(identifier=pull_affordance), transitions={'done': 'Scale_Pull_Affordance', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'affordance'}) # x:437 y:228 OperatableStateMachine.add('Decide_Repeat_Pull', OperatorDecisionState(outcomes=['done', 'repeat'], hint="Pull further?", suggestion='done'), transitions={'done': 'finished', 'repeat': 'Get_Pull_Affordance'}, autonomy={'done': Autonomy.High, 'repeat': Autonomy.Full}) with _state_machine: # x:73 y:78 OperatableStateMachine.add('Request_Trigger_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place trigger template"), transitions={'received': 'Decide_Walking', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'template_id'}) # x:337 y:78 OperatableStateMachine.add('Decide_Walking', OperatorDecisionState(outcomes=["walk", "stand"], hint="Walk to template?", suggestion="walk"), transitions={'walk': 'Walk_To_Template', 'stand': 'Set_Manipulate'}, autonomy={'walk': Autonomy.High, 'stand': Autonomy.Full}) # x:844 y:322 OperatableStateMachine.add('Pull_Trigger', _sm_pull_trigger_8, transitions={'finished': 'Release_Trigger', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none'}) # x:836 y:422 OperatableStateMachine.add('Release_Trigger', _sm_release_trigger_7, transitions={'finished': 'Warn_Stand', 'failed': 'Warn_Stand'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'none': 'none'}) # x:826 y:678 OperatableStateMachine.add('Go_To_Stand_Pose', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.1, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={'done': 'Set_Stand', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'side': 'none'}) # x:566 y:78 OperatableStateMachine.add('Set_Manipulate', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE), transitions={'changed': 'Set_Template_Frame', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:858 y:578 OperatableStateMachine.add('Warn_Stand', LogState(text="Will go to stand now", severity=Logger.REPORT_INFO), transitions={'done': 'Go_To_Stand_Pose'}, autonomy={'done': Autonomy.High}) # x:566 y:678 OperatableStateMachine.add('Set_Stand', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND), transitions={'changed': 'Decide_Step_Back', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:337 y:678 OperatableStateMachine.add('Decide_Step_Back', OperatorDecisionState(outcomes=["walk", "stand"], hint="Step back?", suggestion="walk"), transitions={'walk': 'Perform_Step_Back', 'stand': 'finished'}, autonomy={'walk': Autonomy.High, 'stand': Autonomy.Full}) # x:77 y:672 OperatableStateMachine.add('Perform_Step_Back', _sm_perform_step_back_6, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'step_back_distance': 'step_back_distance'}) # x:330 y:172 OperatableStateMachine.add('Walk_To_Template', _sm_walk_to_template_5, transitions={'finished': 'Set_Manipulate', 'failed': 'failed', 'aborted': 'Set_Manipulate'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side'}) # x:841 y:222 OperatableStateMachine.add('Grasp_Trigger', _sm_grasp_trigger_4, transitions={'finished': 'Pull_Trigger', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'grasp_preference': 'grasp_preference'}) # x:846 y:128 OperatableStateMachine.add('Look_At_Trigger', LookAtTargetState(), transitions={'done': 'Grasp_Trigger'}, autonomy={'done': Autonomy.Off}, remapping={'frame': 'template_frame'}) # x:837 y:28 OperatableStateMachine.add('Set_Template_Frame', CalculationState(calculation=lambda x: "template_tf_%d" % x), transitions={'done': 'Look_At_Trigger'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'template_id', 'output_value': 'template_frame'}) return _state_machine
class FingerConfigurationState(EventState): ''' Sends the fingers to a certain configuration such as opened or closed. -- hand_type string Type of hand (e.g. 'robotiq') -- configuration float Fraction of which the fingers should be closed. 1 corresponds to completely closed, 0 to opened. ># hand_side string Which hand side the trajectory refers to (e.g. 'left') <= done The trajectory was successfully executed. <= failed Failed to execute trajectory. ''' def __init__(self, hand_type, configuration): '''Constructor''' super(FingerConfigurationState, self).__init__(outcomes=['done', 'failed'], input_keys=['hand_side']) self._joint_names = dict() self._joint_names['left'] = dict() self._joint_names['left']['robotiq'] = ['left_f0_j1', 'left_f1_j0', 'left_f1_j1', 'left_f2_j1'] self._joint_names['left']['vt_hand'] = ['l_f0_j0', 'l_f1_j0'] self._joint_names['right'] = dict() self._joint_names['right']['robotiq'] = ['right_f0_j1', 'right_f1_j0', 'right_f1_j1', 'right_f2_j1'] self._joint_names['right']['vt_hand'] = ['r_f0_j0', 'r_f1_j0'] self._joint_values = dict() self._joint_values['robotiq'] = map(lambda x: x * configuration, [1.22, 0.08, 1.13, 1.13]) # 2nd value is the scissor joint self._joint_values['vt_hand'] = map(lambda x: x * configuration, [2.685, 2.685]) self._execution_state = HandTrajectoryState(hand_type) self._internal_userdata = smach.UserData() if not hand_type in self._joint_names['left'].keys(): raise Exception("Unspecified hand type %s, only have %s" % (hand_type, str(self._joint_names['left'].keys()))) self._hand_type = hand_type self._configuration = self._joint_values[hand_type] self._print = True self._failed = False def execute(self, userdata): '''During execution of the state, keep checking for action servers response''' if self._failed: return 'failed' self._execution_state._entering = False return self._execution_state.execute(self._internal_userdata) def on_enter(self, userdata): self._failed = False try: trajectory = JointTrajectory(joint_names = self._joint_names[userdata.hand_side][self._hand_type]) trajectory.points.append(JointTrajectoryPoint(time_from_start = rospy.Duration.from_sec(0.1), positions = self._configuration)) self._internal_userdata.finger_trajectory = trajectory self._internal_userdata.merge(userdata, userdata.keys(), dict()) self._execution_state.on_enter(self._internal_userdata) except Exception as e: Logger.logwarn('Unable to execute finger configuration') rospy.logwarn(str(e)) self._failed = True