def gen_traj_pre_limits(self, limits_side): """Create trajectories for going to 90 percent of joint limits (either upper or lower limits)""" joint_config = {'left_arm': self._joint_calib['left_arm'][limits_side], 'right_arm': self._joint_calib['right_arm'][limits_side] } return AtlasFunctions.gen_arm_trajectory_from_joint_configuration(joint_config)
def gen_traj_pre_limits(self, limits_side): """Create trajectories for going to 90 percent of joint limits (either upper or lower limits)""" joint_config = { 'left_arm': self._joint_calib['left_arm'][limits_side], 'right_arm': self._joint_calib['right_arm'][limits_side] } return AtlasFunctions.gen_arm_trajectory_from_joint_configuration( joint_config)
def create(self): initial_mode = "stand" motion_mode = "manipulate" mantis_mode = "manipulate_limits" percent_past_limits = 0.10 # before: 0.075 # x:788 y:72, x:474 y:133 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.target_limits = 'upper' _state_machine.userdata.cycle_counter = 1 _state_machine.userdata.stand_posture = None # calculated _state_machine.userdata.offsets = {'left_arm': dict(), 'right_arm': dict()} # Additional creation code can be added inside the following tags # [MANUAL_CREATE] self._percent_past_limits = percent_past_limits # Create STAND posture trajectory _state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory() # [/MANUAL_CREATE] # x:222 y:281, x:349 y:167 _sm_determine_offsets_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'offsets'], output_keys=['offsets']) with _sm_determine_offsets_0: # x:61 y:53 OperatableStateMachine.add('Get_Left_Joint_Positions', CurrentJointPositionsState(planning_group="l_arm_group"), transitions={'retrieved': 'Determine_Closest_Limits_Left', 'failed': 'failed'}, autonomy={'retrieved': Autonomy.Off, 'failed': Autonomy.Low}, remapping={'joint_positions': 'joint_positions'}) # x:319 y:54 OperatableStateMachine.add('Determine_Closest_Limits_Left', CalculationState(calculation=self.get_closest_limits_left), transitions={'done': 'Store_Offsets_Left'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'joint_positions', 'output_value': 'joint_limits'}) # x:598 y:162 OperatableStateMachine.add('Get_Right_Joint_Positions', CurrentJointPositionsState(planning_group="r_arm_group"), transitions={'retrieved': 'Determine_Closest_Limits_Right', 'failed': 'failed'}, autonomy={'retrieved': Autonomy.Off, 'failed': Autonomy.Low}, remapping={'joint_positions': 'joint_positions'}) # x:584 y:275 OperatableStateMachine.add('Determine_Closest_Limits_Right', CalculationState(calculation=self.get_closest_limits_right), transitions={'done': 'Store_Offsets_Right'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'joint_positions', 'output_value': 'joint_limits'}) # x:608 y:54 OperatableStateMachine.add('Store_Offsets_Left', FlexibleCalculationState(calculation=self.store_offsets_left, input_keys=['limits', 'value', 'offsets', 'counter']), transitions={'done': 'Get_Right_Joint_Positions'}, autonomy={'done': Autonomy.Off}, remapping={'limits': 'joint_limits', 'value': 'joint_positions', 'offsets': 'offsets', 'counter': 'cycle_counter', 'output_value': 'offsets'}) # x:340 y:274 OperatableStateMachine.add('Store_Offsets_Right', FlexibleCalculationState(calculation=self.store_offsets_right, input_keys=['limits', 'value', 'offsets', 'counter']), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'limits': 'joint_limits', 'value': 'joint_positions', 'offsets': 'offsets', 'counter': 'cycle_counter', 'output_value': 'offsets'}) # x:528 y:401, x:707 y:282 _sm_manipulate_limits_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'offsets'], output_keys=['offsets', 'traj_past_limits']) with _sm_manipulate_limits_1: # x:100 y:156 OperatableStateMachine.add('Prevent_Runtime_Failure', CalculationState(calculation=lambda x: dict()), transitions={'done': 'Go_to_MANIPULATE_LIMITS'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'cycle_counter', 'output_value': 'traj_past_limits'}) # x:387 y:55 OperatableStateMachine.add('Wait_for_Control_Mode_change', WaitState(wait_time=1.0), transitions={'done': 'Get_Left_Joint_Positions'}, autonomy={'done': Autonomy.Low}) # x:895 y:279 OperatableStateMachine.add('Gen_Traj_from_90%_to_110%', CalculationState(calculation=self.gen_traj_past_limits), transitions={'done': 'Go_to_110%_Joint_Limits'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'current_joint_values', 'output_value': 'traj_past_limits'}) # x:893 y:391 OperatableStateMachine.add('Go_to_110%_Joint_Limits', ExecuteTrajectoryBothArmsState(controllers=['left_arm_traj_controller', 'right_arm_traj_controller']), transitions={'done': 'Determine_Offsets', 'failed': 'Determine_Offsets'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'trajectories': 'traj_past_limits'}) # x:651 y:385 OperatableStateMachine.add('Determine_Offsets', _sm_determine_offsets_0, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'cycle_counter': 'cycle_counter', 'offsets': 'offsets'}) # x:648 y:54 OperatableStateMachine.add('Get_Left_Joint_Positions', CurrentJointPositionsState(planning_group="l_arm_group"), transitions={'retrieved': 'Get_Right_Joint_Positions', 'failed': 'failed'}, autonomy={'retrieved': Autonomy.Off, 'failed': Autonomy.High}, remapping={'joint_positions': 'joint_positions_left'}) # x:904 y:53 OperatableStateMachine.add('Get_Right_Joint_Positions', CurrentJointPositionsState(planning_group="r_arm_group"), transitions={'retrieved': 'Generate_Joint_Positions_Struct', 'failed': 'failed'}, autonomy={'retrieved': Autonomy.Off, 'failed': Autonomy.High}, remapping={'joint_positions': 'joint_positions_right'}) # x:886 y:168 OperatableStateMachine.add('Generate_Joint_Positions_Struct', FlexibleCalculationState(calculation=lambda ik: {'left_arm': ik[0], 'right_arm': ik[1]}, input_keys=['left', 'right']), transitions={'done': 'Gen_Traj_from_90%_to_110%'}, autonomy={'done': Autonomy.Off}, remapping={'left': 'joint_positions_left', 'right': 'joint_positions_right', 'output_value': 'current_joint_values'}) # x:92 y:55 OperatableStateMachine.add('Go_to_MANIPULATE_LIMITS', ChangeControlModeActionState(target_mode=mantis_mode), transitions={'changed': 'Wait_for_Control_Mode_change', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High}) # x:574 y:247, x:276 y:549 _sm_update_calibration_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['offsets']) with _sm_update_calibration_2: # x:46 y:44 OperatableStateMachine.add('Process_Offsets', CalculationState(calculation=self.process_offsets), transitions={'done': 'Print_Offset_Info'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'offsets', 'output_value': 'offsets'}) # x:227 y:45 OperatableStateMachine.add('Print_Offset_Info', CalculationState(calculation=self.print_offset_info), transitions={'done': 'Publish_Offsets'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'offsets', 'output_value': 'none'}) # x:390 y:158 OperatableStateMachine.add('Ask_Perform_Update', OperatorDecisionState(outcomes=['update', 'no_update'], hint="Do you want to apply the calculated offsets for calibration?", suggestion=None), transitions={'update': 'Convert_Offset_Data', 'no_update': 'finished'}, autonomy={'update': Autonomy.Full, 'no_update': Autonomy.Full}) # x:232 y:337 OperatableStateMachine.add('Update_Calibration', UpdateJointCalibrationState(joint_names=self._joint_names['left_arm'][0:4] + self._joint_names['right_arm'][0:4]), transitions={'updated': 'Calibration_Successful', 'failed': 'Calibration_Failed'}, autonomy={'updated': Autonomy.Low, 'failed': Autonomy.High}, remapping={'joint_offsets': 'offset_list'}) # x:241 y:242 OperatableStateMachine.add('Convert_Offset_Data', CalculationState(calculation=lambda o: o['left_arm']['avg'] + o['right_arm']['avg']), transitions={'done': 'Update_Calibration'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'offsets', 'output_value': 'offset_list'}) # x:522 y:337 OperatableStateMachine.add('Calibration_Successful', LogState(text="Successfully updated calibration offsets.", severity=Logger.REPORT_INFO), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:246 y:445 OperatableStateMachine.add('Calibration_Failed', LogState(text="Failed to apply calibration offsets!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:399 y:44 OperatableStateMachine.add('Publish_Offsets', CalculationState(calculation=self.publish_offsets), transitions={'done': 'Ask_Perform_Update'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'offsets', 'output_value': 'none'}) # x:978 y:197, x:394 y:80 _sm_perform_checks_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'target_limits', 'offsets'], output_keys=['cycle_counter', 'offsets']) with _sm_perform_checks_3: # x:105 y:74 OperatableStateMachine.add('Go_to_Intermediate_Mode', ChangeControlModeActionState(target_mode=motion_mode), transitions={'changed': 'Gen_Traj_to_90%_Limits', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High}) # x:653 y:274 OperatableStateMachine.add('Manipulate_Limits', _sm_manipulate_limits_1, transitions={'finished': 'Gen_Traj_back_to_90%_Limits', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'cycle_counter': 'cycle_counter', 'offsets': 'offsets', 'traj_past_limits': 'traj_past_limits'}) # x:903 y:78 OperatableStateMachine.add('Increment_Cycle_counter', CalculationState(calculation=lambda counter: counter + 1), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'cycle_counter', 'output_value': 'cycle_counter'}) # x:344 y:277 OperatableStateMachine.add('Move_to_90%_Joint_Limits', MoveitStartingPointState(vel_scaling=0.3), transitions={'reached': 'Manipulate_Limits', 'failed': 'Move_to_90%_Joint_Limits'}, autonomy={'reached': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'trajectories': 'trajectories_90'}) # x:114 y:276 OperatableStateMachine.add('Gen_Traj_to_90%_Limits', CalculationState(calculation=self.gen_traj_pre_limits), transitions={'done': 'Move_to_90%_Joint_Limits'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'target_limits', 'output_value': 'trajectories_90'}) # x:636 y:78 OperatableStateMachine.add('Go_back_to_90%_Joint_Limits', ExecuteTrajectoryBothArmsState(controllers=['left_arm_traj_controller', 'right_arm_traj_controller']), transitions={'done': 'Increment_Cycle_counter', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'trajectories': 'traj_back_to_90'}) # x:636 y:172 OperatableStateMachine.add('Gen_Traj_back_to_90%_Limits', FlexibleCalculationState(calculation=self.gen_traj_back_from_limits, input_keys=['trajectories_90', 'traj_past_limits']), transitions={'done': 'Go_back_to_90%_Joint_Limits'}, autonomy={'done': Autonomy.Off}, remapping={'trajectories_90': 'trajectories_90', 'traj_past_limits': 'traj_past_limits', 'output_value': 'traj_back_to_90'}) with _state_machine: # x:110 y:52 OperatableStateMachine.add('Initial_Control_Mode', ChangeControlModeActionState(target_mode=initial_mode), transitions={'changed': 'Perform_Checks', 'failed': 'failed'}, autonomy={'changed': Autonomy.High, 'failed': Autonomy.High}) # x:712 y:317 OperatableStateMachine.add('Initial_Mode_before_exit', ChangeControlModeActionState(target_mode=initial_mode), transitions={'changed': 'Update_Calibration', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High}) # x:122 y:302 OperatableStateMachine.add('Perform_Checks', _sm_perform_checks_3, transitions={'finished': 'Are_We_Done_Yet?', 'failed': 'Intermediate_Mode_before_exit'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'cycle_counter': 'cycle_counter', 'target_limits': 'target_limits', 'offsets': 'offsets'}) # x:126 y:505 OperatableStateMachine.add('Are_We_Done_Yet?', DecisionState(outcomes=["done", "more"], conditions=lambda counter: "done" if counter >= 2 else "more"), transitions={'done': 'Intermediate_Mode_before_exit', 'more': 'Setup_next_Cycle'}, autonomy={'done': Autonomy.Low, 'more': Autonomy.High}, remapping={'input_value': 'cycle_counter'}) # x:15 y:404 OperatableStateMachine.add('Setup_next_Cycle', CalculationState(calculation=lambda lim: 'lower' if lim == 'upper' else 'upper'), transitions={'done': 'Perform_Checks'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'target_limits', 'output_value': 'target_limits'}) # x:725 y:186 OperatableStateMachine.add('Update_Calibration', _sm_update_calibration_2, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'offsets': 'offsets'}) # x:726 y:427 OperatableStateMachine.add('Move_to_Stand_Posture', MoveitStartingPointState(vel_scaling=0.3), transitions={'reached': 'Initial_Mode_before_exit', 'failed': 'Move_to_Stand_Posture'}, autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'trajectories': 'stand_posture'}) # x:412 y:427 OperatableStateMachine.add('Intermediate_Mode_before_exit', ChangeControlModeActionState(target_mode=motion_mode), transitions={'changed': 'Move_to_Stand_Posture', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High}) return _state_machine
def create(self): bag_folder_in = "~/sysid_tests" bag_folder_out = "" # optional (if not provided, an /out folder will be created in the bag_folder_in directory prefix = "" # optional (applies to bag and video files) input_bagfiles = [] # calculated (leave empty) output_bagfiles = [] # calculated (leave empty) desired_num_tests = 1 # num of tests per trajectory wait_time = 3.0 # before execution (for rosbag record) settling_time = 2.0 # after execution mode_for_tests = "dance" # "manipulate", "system_id", etc. desired_controllers = ["left_arm_traj_controller","right_arm_traj_controller", "left_leg_traj_controller","right_leg_traj_controller","torso_traj_controller"] # x:880 y:104, x:660 y:14 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.traj_index = 0 _state_machine.userdata.tests_counter = 0 _state_machine.userdata.none = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # Get folder containing input bagfiles and create folder for output bag_folder_in = os.path.expanduser(bag_folder_in) if not os.path.exists(bag_folder_in): Logger.logwarn('Path to input bag folder does not exist (%s)' % bag_folder_in) if not bag_folder_out: bag_folder_out = os.path.join(bag_folder_in, 'out') if not os.path.exists(bag_folder_out): os.makedirs(bag_folder_out) else: # The user has provided a directory for output bagfiles bag_folder_out = os.path.expanduser(bag_folder_out) if not os.path.exists(bag_folder_out): os.makedirs(bag_folder_out) # Initialize lists again, in case the user did alter them input_bagfiles = [] output_bagfiles = [] # Get all input bagfile names from bag folder and also name the corresponding output bagfiles os.chdir(bag_folder_in) for bagfile in sorted(glob.glob("*.bag")): input_bagfiles.append(os.path.join(bag_folder_in, bagfile)) bare_name = bagfile.split(".")[0] output_name = bare_name + "_" + time.strftime("%Y-%m-%d-%H_%M") + "_" # file name will be completed by flexible calculation state output_bagfiles.append(output_name) Logger.loginfo('Found %d input bag files in %s' % (len(input_bagfiles), bag_folder_in)) # Create STAND posture trajectories _state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory() # [/MANUAL_CREATE] # x:836 y:51, x:858 y:296 _sm_starting_point_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['experiment_name', 'trajectories']) with _sm_starting_point_0: # x:49 y:42 OperatableStateMachine.add('Gen_Starting_Name', CalculationState(calculation=lambda en: en + "_starting"), transitions={'done': 'Gen_Starting_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'experiment_name', 'output_value': 'starting_name'}) # x:42 y:231 OperatableStateMachine.add('Record_Starting_Point', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_for_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={'bagfile_name': 'output_bagfile_starting', 'rosbag_process': 'rosbag_process_starting'}) # x:38 y:330 OperatableStateMachine.add('Wait_for_Rosbag_Record', WaitState(wait_time=wait_time), transitions={'done': 'Extract_Left_Arm_Part'}, autonomy={'done': Autonomy.Off}) # x:29 y:133 OperatableStateMachine.add('Gen_Starting_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join(bag_folder_out, en) + ".bag"), transitions={'done': 'Record_Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'starting_name', 'output_value': 'output_bagfile_starting'}) # x:536 y:47 OperatableStateMachine.add('Stop_Recording_Starting_Point', StopRecordLogsState(), transitions={'stopped': 'finished'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:228 y:757 OperatableStateMachine.add('Plan_to_Starting_Point_Left_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Right_Arm', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_left_arm', 'plan_to_goal': 'plan_to_goal_left_arm'}) # x:236 y:655 OperatableStateMachine.add('Plan_to_Starting_Point_Right_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Left_Leg', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_right_arm', 'plan_to_goal': 'plan_to_goal_right_arm'}) # x:272 y:47 OperatableStateMachine.add('Go_to_Starting_Point', ExecuteTrajectoryWholeBodyState(controllers=desired_controllers), transitions={'done': 'Stop_Recording_Starting_Point', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'trajectories': 'trajectories_all'}) # x:536 y:169 OperatableStateMachine.add('Report_Starting_Point_Failure', LogState(text="Failed to plan or go to starting point!", severity=Logger.REPORT_WARN), transitions={'done': 'Stop_Recording_When_Failed'}, autonomy={'done': Autonomy.Full}) # x:34 y:424 OperatableStateMachine.add('Extract_Left_Arm_Part', CalculationState(calculation=lambda t: {'left_arm': t['left_arm']} if 'left_arm' in t else None), transitions={'done': 'Extract_Right_Arm_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_left_arm'}) # x:21 y:507 OperatableStateMachine.add('Extract_Right_Arm_Part', CalculationState(calculation=lambda t: {'right_arm': t['right_arm']} if 'right_arm' in t else None), transitions={'done': 'Extract_Left_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_right_arm'}) # x:293 y:146 OperatableStateMachine.add('Combine_Plans', FlexibleCalculationState(calculation=self.combine_plans, input_keys=['left_arm', 'right_arm', 'left_leg', 'right_leg', 'torso']), transitions={'done': 'Go_to_Starting_Point'}, autonomy={'done': Autonomy.Low}, remapping={'left_arm': 'plan_to_goal_left_arm', 'right_arm': 'plan_to_goal_right_arm', 'left_leg': 'plan_to_goal_left_leg', 'right_leg': 'plan_to_goal_right_leg', 'torso': 'plan_to_goal_torso', 'output_value': 'trajectories_all'}) # x:789 y:167 OperatableStateMachine.add('Stop_Recording_When_Failed', StopRecordLogsState(), transitions={'stopped': 'failed'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:24 y:601 OperatableStateMachine.add('Extract_Left_Leg_Part', CalculationState(calculation=lambda t: {'left_leg': t['left_leg']} if 'left_leg' in t else None), transitions={'done': 'Extract_Right_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_left_leg'}) # x:22 y:665 OperatableStateMachine.add('Extract_Right_Leg_Part', CalculationState(calculation=lambda t: {'right_leg': t['right_leg']} if 'right_leg' in t else None), transitions={'done': 'Extract_Torso_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_right_leg'}) # x:33 y:765 OperatableStateMachine.add('Extract_Torso_Part', CalculationState(calculation=lambda t: {'torso': t['torso']} if 'torso' in t else None), transitions={'done': 'Plan_to_Starting_Point_Left_Arm'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_torso'}) # x:227 y:410 OperatableStateMachine.add('Plan_to_Starting_Point_Right_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Torso', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_right_leg', 'plan_to_goal': 'plan_to_goal_right_leg'}) # x:237 y:531 OperatableStateMachine.add('Plan_to_Starting_Point_Left_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Right_Leg', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_left_leg', 'plan_to_goal': 'plan_to_goal_left_leg'}) # x:241 y:296 OperatableStateMachine.add('Plan_to_Starting_Point_Torso', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Combine_Plans', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_torso', 'plan_to_goal': 'plan_to_goal_torso'}) # x:1118 y:103, x:348 y:32 _sm_execute_individual_trajectory_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['traj_index', 'tests_counter']) with _sm_execute_individual_trajectory_1: # x:79 y:28 OperatableStateMachine.add('Get_Input_Bagfile', CalculationState(calculation=lambda idx: input_bagfiles[idx]), transitions={'done': 'Gen_Experiment_Name'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'traj_index', 'output_value': 'input_bagfile'}) # x:904 y:187 OperatableStateMachine.add('Stop_Recording', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging'}, autonomy={'stopped': Autonomy.Low}, remapping={'rosbag_process': 'rosbag_process'}) # x:494 y:292 OperatableStateMachine.add('Wait_For_Rosbag_Record', WaitState(wait_time=wait_time), transitions={'done': 'Execute_Trajs_from_Bagfile'}, autonomy={'done': Autonomy.Low}) # x:507 y:384 OperatableStateMachine.add('Record_SysID_Test', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_For_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={'bagfile_name': 'output_bagfile', 'rosbag_process': 'rosbag_process'}) # x:484 y:106 OperatableStateMachine.add('Stop_Recording_After_Failure', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging_After_Failure'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process'}) # x:727 y:188 OperatableStateMachine.add('Wait_for_Settling', WaitState(wait_time=settling_time), transitions={'done': 'Stop_Recording'}, autonomy={'done': Autonomy.Off}) # x:482 y:188 OperatableStateMachine.add('Execute_Trajs_from_Bagfile', ExecuteTrajectoryWholeBodyState(controllers=desired_controllers), transitions={'done': 'Wait_for_Settling', 'failed': 'Stop_Recording_After_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low}, remapping={'trajectories': 'trajectories'}) # x:48 y:283 OperatableStateMachine.add('Load_Trajs_from_Bagfile', LoadTrajectoryFromBagfileState(), transitions={'done': 'Start_Video_Logging', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'bagfile_name': 'input_bagfile', 'trajectories': 'trajectories'}) # x:65 y:113 OperatableStateMachine.add('Gen_Experiment_Name', FlexibleCalculationState(calculation=lambda i: prefix + output_bagfiles[i[0]] + str(i[1] + 1), input_keys=["idx", "counter"]), transitions={'done': 'Gen_Output_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={'idx': 'traj_index', 'counter': 'tests_counter', 'output_value': 'experiment_name'}) # x:56 y:192 OperatableStateMachine.add('Gen_Output_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join(bag_folder_out, en) + ".bag"), transitions={'done': 'Load_Trajs_from_Bagfile'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'experiment_name', 'output_value': 'output_bagfile'}) # x:903 y:97 OperatableStateMachine.add('Stop_Video_Logging', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'}) # x:472 y:26 OperatableStateMachine.add('Stop_Video_Logging_After_Failure', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'}) # x:285 y:378 OperatableStateMachine.add('Starting_Point', _sm_starting_point_0, transitions={'finished': 'Record_SysID_Test', 'failed': 'Stop_Video_Logging_After_Failure'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'experiment_name': 'experiment_name', 'trajectories': 'trajectories'}) # x:71 y:384 OperatableStateMachine.add('Start_Video_Logging', VideoLoggingState(command=VideoLoggingState.START, no_video=False, no_bags=True), transitions={'done': 'Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'}) # x:475 y:405 _sm_execute_sysid_trajectories_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['traj_index', 'tests_counter'], output_keys=['traj_index']) with _sm_execute_sysid_trajectories_2: # x:367 y:24 OperatableStateMachine.add('Execute_Individual_Trajectory', _sm_execute_individual_trajectory_1, transitions={'finished': 'Increment_Tests_per_Traj_counter', 'failed': 'Wait_before_Fail'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'traj_index': 'traj_index', 'tests_counter': 'tests_counter'}) # x:714 y:27 OperatableStateMachine.add('Increment_Tests_per_Traj_counter', CalculationState(calculation=lambda counter: counter + 1), transitions={'done': 'More_Tests_for_this_Traj?'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'tests_counter', 'output_value': 'tests_counter'}) # x:738 y:207 OperatableStateMachine.add('More_Tests_for_this_Traj?', DecisionState(outcomes=['yes', 'no'], conditions=lambda counter: 'no' if counter >= desired_num_tests else 'yes'), transitions={'yes': 'Execute_Individual_Trajectory', 'no': 'Reset_Tests_counter'}, autonomy={'yes': Autonomy.Low, 'no': Autonomy.Low}, remapping={'input_value': 'tests_counter'}) # x:752 y:298 OperatableStateMachine.add('Reset_Tests_counter', CalculationState(calculation=lambda counter: 0), transitions={'done': 'Increment_Traj_Index'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'tests_counter', 'output_value': 'tests_counter'}) # x:752 y:399 OperatableStateMachine.add('Increment_Traj_Index', CalculationState(calculation=lambda idx: idx + 1), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'traj_index', 'output_value': 'traj_index'}) # x:405 y:141 OperatableStateMachine.add('Wait_before_Fail', WaitState(wait_time=2.0), transitions={'done': 'Increment_Tests_per_Traj_counter'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:122 y:26 OperatableStateMachine.add('Starting_Execution', LogState(text="Execution is starting. Confirm first transition!", severity=Logger.REPORT_HINT), transitions={'done': 'Go_to_Desired_Mode'}, autonomy={'done': Autonomy.High}) # x:328 y:298 OperatableStateMachine.add('Execute SysID Trajectories', _sm_execute_sysid_trajectories_2, transitions={'finished': 'More_Trajectories?'}, autonomy={'finished': Autonomy.Inherit}, remapping={'traj_index': 'traj_index', 'tests_counter': 'tests_counter'}) # x:129 y:215 OperatableStateMachine.add('More_Trajectories?', DecisionState(outcomes=['yes', 'no'], conditions=lambda idx: 'no' if idx >= len(input_bagfiles) else 'yes'), transitions={'yes': 'Notify_Next_Trajectory', 'no': 'Move_to_Stand_Posture'}, autonomy={'yes': Autonomy.Low, 'no': Autonomy.High}, remapping={'input_value': 'traj_index'}) # x:592 y:103 OperatableStateMachine.add('Go_to_FREEZE_before_Exit', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.FREEZE), transitions={'changed': 'finished', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Low}) # x:584 y:214 OperatableStateMachine.add('Failed_To_Go_To_Stand_Posture', LogState(text="Failed to go to stand posture", severity=Logger.REPORT_WARN), transitions={'done': 'Go_to_FREEZE_before_Exit'}, autonomy={'done': Autonomy.Off}) # x:121 y:393 OperatableStateMachine.add('Notify_Next_Trajectory', LogState(text="Continuing with next trajectory.", severity=Logger.REPORT_INFO), transitions={'done': 'Execute SysID Trajectories'}, autonomy={'done': Autonomy.Off}) # x:112 y:102 OperatableStateMachine.add('Go_to_Desired_Mode', ChangeControlModeActionState(target_mode=mode_for_tests), transitions={'changed': 'More_Trajectories?', 'failed': 'Go_to_FREEZE_before_Exit'}, autonomy={'changed': Autonomy.High, 'failed': Autonomy.High}) # x:329 y:158 OperatableStateMachine.add('Move_to_Stand_Posture', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}), transitions={'done': 'Go_to_FREEZE_before_Exit', 'failed': 'Failed_To_Go_To_Stand_Posture'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.High}, remapping={'side': 'none'}) return _state_machine
def create(self): warn_threshold = 0.1 # for joint offsets bag_folder_out = "~/ft_calib/ft_logs" initial_mode = "stand" motion_mode = "manipulate" transitiontime = 0.5 settlingtime = 0.5 txtfile_name_left_arm = "~/ft_calib/input/SI_E047_FT_Calib_Arms_l_arm.txt" txtfile_name_right_arm = "~/ft_calib/input/SI_E047_FT_Calib_Arms_r_arm.txt" calibration_chain = ["right_arm"] static_calibration_data = {} # x:783 y:13, x:649 y:73 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.parameter_keys_dict = None _state_machine.userdata.calibration_chain_user = calibration_chain _state_machine.userdata.txtfile_name_left_arm_user = txtfile_name_left_arm _state_machine.userdata.txtfile_name_right_arm_user = txtfile_name_right_arm # Additional creation code can be added inside the following tags # [MANUAL_CREATE] bag_folder_out = os.path.expanduser(bag_folder_out) if not os.path.exists(bag_folder_out): os.makedirs(bag_folder_out) # Create STAND posture trajectory _state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory( ) # [/MANUAL_CREATE] # x:861 y:31, x:1047 y:103 _sm_starting_point_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['experiment_name', 'trajectories']) with _sm_starting_point_0: # x:49 y:42 OperatableStateMachine.add( 'Gen_Starting_Name', CalculationState(calculation=lambda en: en + "_starting"), transitions={'done': 'Gen_Starting_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'experiment_name', 'output_value': 'starting_name' }) # x:42 y:231 OperatableStateMachine.add( 'Record_Starting_Point', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_for_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={ 'bagfile_name': 'output_bagfile_starting', 'rosbag_process': 'rosbag_process_starting' }) # x:38 y:330 OperatableStateMachine.add( 'Wait_for_Rosbag_Record', WaitState(wait_time=1.0), transitions={'done': 'Extract_Left_Arm_Part'}, autonomy={'done': Autonomy.Off}) # x:29 y:133 OperatableStateMachine.add( 'Gen_Starting_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join( bag_folder_out, en) + ".bag"), transitions={'done': 'Record_Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'starting_name', 'output_value': 'output_bagfile_starting' }) # x:536 y:47 OperatableStateMachine.add( 'Stop_Recording_Starting_Point', StopRecordLogsState(), transitions={'stopped': 'finished'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:228 y:757 OperatableStateMachine.add( 'Plan_to_Starting_Point_Left_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Plan_to_Starting_Point_Right_Arm', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_left_arm', 'plan_to_goal': 'plan_to_goal_left_arm' }) # x:236 y:655 OperatableStateMachine.add( 'Plan_to_Starting_Point_Right_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Plan_to_Starting_Point_Left_Leg', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_right_arm', 'plan_to_goal': 'plan_to_goal_right_arm' }) # x:272 y:47 OperatableStateMachine.add( 'Go_to_Starting_Point', ExecuteTrajectoryWholeBodyState(controllers=[]), transitions={ 'done': 'Stop_Recording_Starting_Point', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'trajectories': 'trajectories_all'}) # x:536 y:169 OperatableStateMachine.add( 'Report_Starting_Point_Failure', LogState(text="Failed to plan or go to starting point!", severity=Logger.REPORT_WARN), transitions={'done': 'Stop_Recording_When_Failed'}, autonomy={'done': Autonomy.Full}) # x:34 y:424 OperatableStateMachine.add( 'Extract_Left_Arm_Part', CalculationState( calculation=lambda t: {'left_arm': t['left_arm']} if 'left_arm' in t else None), transitions={'done': 'Extract_Right_Arm_Part'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_left_arm' }) # x:21 y:507 OperatableStateMachine.add( 'Extract_Right_Arm_Part', CalculationState( calculation=lambda t: {'right_arm': t['right_arm']} if 'right_arm' in t else None), transitions={'done': 'Extract_Left_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_right_arm' }) # x:293 y:146 OperatableStateMachine.add( 'Combine_Plans', FlexibleCalculationState(calculation=self.combine_plans, input_keys=[ 'left_arm', 'right_arm', 'left_leg', 'right_leg', 'torso' ]), transitions={'done': 'Go_to_Starting_Point'}, autonomy={'done': Autonomy.Low}, remapping={ 'left_arm': 'plan_to_goal_left_arm', 'right_arm': 'plan_to_goal_right_arm', 'left_leg': 'plan_to_goal_left_leg', 'right_leg': 'plan_to_goal_right_leg', 'torso': 'plan_to_goal_torso', 'output_value': 'trajectories_all' }) # x:789 y:167 OperatableStateMachine.add( 'Stop_Recording_When_Failed', StopRecordLogsState(), transitions={'stopped': 'failed'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:24 y:601 OperatableStateMachine.add( 'Extract_Left_Leg_Part', CalculationState( calculation=lambda t: {'left_leg': t['left_leg']} if 'left_leg' in t else None), transitions={'done': 'Extract_Right_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_left_leg' }) # x:22 y:665 OperatableStateMachine.add( 'Extract_Right_Leg_Part', CalculationState( calculation=lambda t: {'right_leg': t['right_leg']} if 'right_leg' in t else None), transitions={'done': 'Extract_Torso_Part'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_right_leg' }) # x:33 y:765 OperatableStateMachine.add( 'Extract_Torso_Part', CalculationState(calculation=lambda t: {'torso': t['torso']} if 'torso' in t else None), transitions={'done': 'Plan_to_Starting_Point_Left_Arm'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_torso' }) # x:227 y:410 OperatableStateMachine.add( 'Plan_to_Starting_Point_Right_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Plan_to_Starting_Point_Torso', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_right_leg', 'plan_to_goal': 'plan_to_goal_right_leg' }) # x:237 y:531 OperatableStateMachine.add( 'Plan_to_Starting_Point_Left_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Plan_to_Starting_Point_Right_Leg', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_left_leg', 'plan_to_goal': 'plan_to_goal_left_leg' }) # x:241 y:296 OperatableStateMachine.add( 'Plan_to_Starting_Point_Torso', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Combine_Plans', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_torso', 'plan_to_goal': 'plan_to_goal_torso' }) # x:1090 y:55, x:340 y:59 _sm_execute_individual_trajectory_1 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['output_bagfile', 'trajectories', 'experiment_name']) with _sm_execute_individual_trajectory_1: # x:124 y:257 OperatableStateMachine.add( 'Start_Video_Logging', VideoLoggingState(command=VideoLoggingState.START, no_video=False, no_bags=True), transitions={'done': 'Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={ 'experiment_name': 'experiment_name', 'description': 'experiment_name' }) # x:484 y:106 OperatableStateMachine.add( 'Stop_Recording_After_Failure', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging_After_Failure'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process'}) # x:727 y:188 OperatableStateMachine.add('Wait_for_Settling', WaitState(wait_time=1.0), transitions={'done': 'Stop_Recording'}, autonomy={'done': Autonomy.Off}) # x:482 y:188 OperatableStateMachine.add( 'Execute_Trajs_from_Bagfile', ExecuteTrajectoryWholeBodyState(controllers=[]), transitions={ 'done': 'Wait_for_Settling', 'failed': 'Stop_Recording_After_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Low }, remapping={'trajectories': 'trajectories'}) # x:903 y:97 OperatableStateMachine.add( 'Stop_Video_Logging', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'experiment_name': 'experiment_name', 'description': 'experiment_name' }) # x:472 y:26 OperatableStateMachine.add( 'Stop_Video_Logging_After_Failure', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={ 'experiment_name': 'experiment_name', 'description': 'experiment_name' }) # x:285 y:378 OperatableStateMachine.add('Starting_Point', _sm_starting_point_0, transitions={ 'finished': 'Record_SysID_Test', 'failed': 'Stop_Video_Logging_After_Failure' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'experiment_name': 'experiment_name', 'trajectories': 'trajectories' }) # x:904 y:187 OperatableStateMachine.add( 'Stop_Recording', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging'}, autonomy={'stopped': Autonomy.Low}, remapping={'rosbag_process': 'rosbag_process'}) # x:494 y:292 OperatableStateMachine.add( 'Wait_For_Rosbag_Record', WaitState(wait_time=1.0), transitions={'done': 'Execute_Trajs_from_Bagfile'}, autonomy={'done': Autonomy.Low}) # x:507 y:384 OperatableStateMachine.add( 'Record_SysID_Test', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_For_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={ 'bagfile_name': 'output_bagfile', 'rosbag_process': 'rosbag_process' }) # x:1267 y:273, x:406 y:121 _sm_perform_calibration_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['output_bagfile', 'experiment_name', 'trajectories'], output_keys=['trajectories_command']) with _sm_perform_calibration_2: # x:136 y:171 OperatableStateMachine.add( 'Go_to_Intermediate_Mode', ChangeControlModeActionState(target_mode=motion_mode), transitions={ 'changed': 'Execute_Individual_Trajectory', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Low }) # x:514 y:314 OperatableStateMachine.add( 'Intermediate_Mode_before_exit', ChangeControlModeActionState(target_mode=motion_mode), transitions={ 'changed': 'Initial_Mode_before_exit', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.High }) # x:995 y:274 OperatableStateMachine.add( 'Initial_Mode_before_exit', ChangeControlModeActionState(target_mode=initial_mode), transitions={ 'changed': 'finished', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.High }) # x:222 y:296 OperatableStateMachine.add('Execute_Individual_Trajectory', _sm_execute_individual_trajectory_1, transitions={ 'finished': 'Intermediate_Mode_before_exit', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'output_bagfile': 'output_bagfile', 'trajectories': 'trajectories', 'experiment_name': 'experiment_name' }) # x:221 y:562, x:709 y:85 _sm_update_calibration_3 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'output_bagfile', 'trajectories', 'calibration_chain_key_local' ]) with _sm_update_calibration_3: # x:111 y:94 OperatableStateMachine.add( 'Calculate_Calibration', CalculateForceTorqueCalibration( calibration_chain=calibration_chain, settlingtime=settlingtime, static_calibration_data=static_calibration_data), transitions={ 'done': 'Ask_Perform_Update', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'bag_filename': 'output_bagfile', 'trajectories_command': 'trajectories', 'ft_calib_data': 'ft_calib_data' }) # x:420 y:515 OperatableStateMachine.add( 'Calibration_Successful', LogState(text="Successfully updated calibration offsets.", severity=Logger.REPORT_INFO), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:774 y:277 OperatableStateMachine.add( 'Calibration_Failed', LogState(text="Failed to apply calibration offsets!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:122 y:202 OperatableStateMachine.add( 'Ask_Perform_Update', OperatorDecisionState( outcomes=['update', 'no_update'], hint= "Do you want to apply the calculated offsets for calibration?", suggestion=None), transitions={ 'update': 'Generate_Keys_Dict', 'no_update': 'finished' }, autonomy={ 'update': Autonomy.Full, 'no_update': Autonomy.Full }) # x:207 y:308 OperatableStateMachine.add( 'Generate_Keys_Dict', FlexibleCalculationState( calculation=self.create_parameter_keys_dict, input_keys=["input"]), transitions={'done': 'Update_Dynamic_Reconfigure'}, autonomy={'done': Autonomy.Off}, remapping={ 'input': 'calibration_chain_key_local', 'output_value': 'parameter_keys_dict' }) # x:408 y:303 OperatableStateMachine.add( 'Update_Dynamic_Reconfigure', UpdateDynamicParameterImpedanceControllerState( controller_chain=calibration_chain), transitions={ 'updated': 'Calibration_Successful', 'failed': 'Calibration_Failed' }, autonomy={ 'updated': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'parameter_keys': 'parameter_keys_dict', 'parameter_values': 'ft_calib_data' }) with _state_machine: # x:160 y:20 OperatableStateMachine.add( 'Execution_Starting', LogState( text= "Execution has started. Please confirm transition to first state.", severity=Logger.REPORT_HINT), transitions={'done': 'Gen_Experiment_Name'}, autonomy={'done': Autonomy.Full}) # x:685 y:431 OperatableStateMachine.add('Update_Calibration', _sm_update_calibration_3, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'output_bagfile': 'output_bagfile', 'trajectories': 'trajectories_command', 'calibration_chain_key_local': 'calibration_chain_user' }) # x:99 y:533 OperatableStateMachine.add( 'Decision_Perform_Experiment', OperatorDecisionState( outcomes=["record_data", "use_existing"], hint= "Use existing measurement data bag file or record a new one?", suggestion=None), transitions={ 'record_data': 'Perform_Calibration', 'use_existing': 'Update_Calibration' }, autonomy={ 'record_data': Autonomy.Low, 'use_existing': Autonomy.Low }) # x:130 y:185 OperatableStateMachine.add( 'Gen_Experiment_Name', FlexibleCalculationState(calculation=lambda i: 'FTCalib', input_keys=[]), transitions={'done': 'Gen_Output_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={'output_value': 'experiment_name'}) # x:134 y:267 OperatableStateMachine.add( 'Gen_Output_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join( bag_folder_out, en) + ".bag"), transitions={'done': 'Generate_Textfiles_Dict'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'experiment_name', 'output_value': 'output_bagfile' }) # x:443 y:252 OperatableStateMachine.add('Perform_Calibration', _sm_perform_calibration_2, transitions={ 'finished': 'Update_Calibration', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'output_bagfile': 'output_bagfile', 'experiment_name': 'experiment_name', 'trajectories': 'trajectories_command', 'trajectories_command': 'trajectories_command' }) # x:70 y:421 OperatableStateMachine.add('Load_Traj_From_Txt', GenerateTrajectoryFromTxtfileState( chains=calibration_chain, transitiontime=transitiontime, settlingtime=settlingtime), transitions={ 'done': 'Decision_Perform_Experiment', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'txtfilepaths': 'txtfiles_dict', 'trajectories': 'trajectories_command' }) # x:92 y:350 OperatableStateMachine.add( 'Generate_Textfiles_Dict', FlexibleCalculationState(calculation=self.create_txtfiles_dict, input_keys=[ "calibration_chain", "txtfile_left_arm", "txtfile_right_arm" ]), transitions={'done': 'Load_Traj_From_Txt'}, autonomy={'done': Autonomy.Off}, remapping={ 'calibration_chain': 'calibration_chain_user', 'txtfile_left_arm': 'txtfile_name_left_arm_user', 'txtfile_right_arm': 'txtfile_name_right_arm_user', 'output_value': 'txtfiles_dict' }) return _state_machine
def create(self): warn_threshold = 0.1 # for joint offsets bag_folder_out = "~/ft_calib/ft_logs" initial_mode = "stand" motion_mode = "manipulate" transitiontime = 0.5 settlingtime = 0.5 txtfile_name_left_arm = "~/ft_calib/input/SI_E047_FT_Calib_Arms_l_arm.txt" txtfile_name_right_arm = "~/ft_calib/input/SI_E047_FT_Calib_Arms_r_arm.txt" calibration_chain = ["right_arm"] static_calibration_data = {} # x:783 y:13, x:649 y:73 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.parameter_keys_dict = None _state_machine.userdata.calibration_chain_user = calibration_chain _state_machine.userdata.txtfile_name_left_arm_user = txtfile_name_left_arm _state_machine.userdata.txtfile_name_right_arm_user = txtfile_name_right_arm # Additional creation code can be added inside the following tags # [MANUAL_CREATE] bag_folder_out = os.path.expanduser(bag_folder_out) if not os.path.exists(bag_folder_out): os.makedirs(bag_folder_out) # Create STAND posture trajectory _state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory() # [/MANUAL_CREATE] # x:861 y:31, x:1047 y:103 _sm_starting_point_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['experiment_name', 'trajectories']) with _sm_starting_point_0: # x:49 y:42 OperatableStateMachine.add('Gen_Starting_Name', CalculationState(calculation=lambda en: en + "_starting"), transitions={'done': 'Gen_Starting_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'experiment_name', 'output_value': 'starting_name'}) # x:42 y:231 OperatableStateMachine.add('Record_Starting_Point', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_for_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={'bagfile_name': 'output_bagfile_starting', 'rosbag_process': 'rosbag_process_starting'}) # x:38 y:330 OperatableStateMachine.add('Wait_for_Rosbag_Record', WaitState(wait_time=1.0), transitions={'done': 'Extract_Left_Arm_Part'}, autonomy={'done': Autonomy.Off}) # x:29 y:133 OperatableStateMachine.add('Gen_Starting_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join(bag_folder_out, en) + ".bag"), transitions={'done': 'Record_Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'starting_name', 'output_value': 'output_bagfile_starting'}) # x:536 y:47 OperatableStateMachine.add('Stop_Recording_Starting_Point', StopRecordLogsState(), transitions={'stopped': 'finished'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:228 y:757 OperatableStateMachine.add('Plan_to_Starting_Point_Left_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Right_Arm', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_left_arm', 'plan_to_goal': 'plan_to_goal_left_arm'}) # x:236 y:655 OperatableStateMachine.add('Plan_to_Starting_Point_Right_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Left_Leg', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_right_arm', 'plan_to_goal': 'plan_to_goal_right_arm'}) # x:272 y:47 OperatableStateMachine.add('Go_to_Starting_Point', ExecuteTrajectoryWholeBodyState(controllers=[]), transitions={'done': 'Stop_Recording_Starting_Point', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'trajectories': 'trajectories_all'}) # x:536 y:169 OperatableStateMachine.add('Report_Starting_Point_Failure', LogState(text="Failed to plan or go to starting point!", severity=Logger.REPORT_WARN), transitions={'done': 'Stop_Recording_When_Failed'}, autonomy={'done': Autonomy.Full}) # x:34 y:424 OperatableStateMachine.add('Extract_Left_Arm_Part', CalculationState(calculation=lambda t: {'left_arm': t['left_arm']} if 'left_arm' in t else None), transitions={'done': 'Extract_Right_Arm_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_left_arm'}) # x:21 y:507 OperatableStateMachine.add('Extract_Right_Arm_Part', CalculationState(calculation=lambda t: {'right_arm': t['right_arm']} if 'right_arm' in t else None), transitions={'done': 'Extract_Left_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_right_arm'}) # x:293 y:146 OperatableStateMachine.add('Combine_Plans', FlexibleCalculationState(calculation=self.combine_plans, input_keys=['left_arm', 'right_arm', 'left_leg', 'right_leg', 'torso']), transitions={'done': 'Go_to_Starting_Point'}, autonomy={'done': Autonomy.Low}, remapping={'left_arm': 'plan_to_goal_left_arm', 'right_arm': 'plan_to_goal_right_arm', 'left_leg': 'plan_to_goal_left_leg', 'right_leg': 'plan_to_goal_right_leg', 'torso': 'plan_to_goal_torso', 'output_value': 'trajectories_all'}) # x:789 y:167 OperatableStateMachine.add('Stop_Recording_When_Failed', StopRecordLogsState(), transitions={'stopped': 'failed'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:24 y:601 OperatableStateMachine.add('Extract_Left_Leg_Part', CalculationState(calculation=lambda t: {'left_leg': t['left_leg']} if 'left_leg' in t else None), transitions={'done': 'Extract_Right_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_left_leg'}) # x:22 y:665 OperatableStateMachine.add('Extract_Right_Leg_Part', CalculationState(calculation=lambda t: {'right_leg': t['right_leg']} if 'right_leg' in t else None), transitions={'done': 'Extract_Torso_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_right_leg'}) # x:33 y:765 OperatableStateMachine.add('Extract_Torso_Part', CalculationState(calculation=lambda t: {'torso': t['torso']} if 'torso' in t else None), transitions={'done': 'Plan_to_Starting_Point_Left_Arm'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_torso'}) # x:227 y:410 OperatableStateMachine.add('Plan_to_Starting_Point_Right_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Torso', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_right_leg', 'plan_to_goal': 'plan_to_goal_right_leg'}) # x:237 y:531 OperatableStateMachine.add('Plan_to_Starting_Point_Left_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Right_Leg', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_left_leg', 'plan_to_goal': 'plan_to_goal_left_leg'}) # x:241 y:296 OperatableStateMachine.add('Plan_to_Starting_Point_Torso', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Combine_Plans', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_torso', 'plan_to_goal': 'plan_to_goal_torso'}) # x:1090 y:55, x:340 y:59 _sm_execute_individual_trajectory_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['output_bagfile', 'trajectories', 'experiment_name']) with _sm_execute_individual_trajectory_1: # x:124 y:257 OperatableStateMachine.add('Start_Video_Logging', VideoLoggingState(command=VideoLoggingState.START, no_video=False, no_bags=True), transitions={'done': 'Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'}) # x:484 y:106 OperatableStateMachine.add('Stop_Recording_After_Failure', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging_After_Failure'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process'}) # x:727 y:188 OperatableStateMachine.add('Wait_for_Settling', WaitState(wait_time=1.0), transitions={'done': 'Stop_Recording'}, autonomy={'done': Autonomy.Off}) # x:482 y:188 OperatableStateMachine.add('Execute_Trajs_from_Bagfile', ExecuteTrajectoryWholeBodyState(controllers=[]), transitions={'done': 'Wait_for_Settling', 'failed': 'Stop_Recording_After_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low}, remapping={'trajectories': 'trajectories'}) # x:903 y:97 OperatableStateMachine.add('Stop_Video_Logging', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'}) # x:472 y:26 OperatableStateMachine.add('Stop_Video_Logging_After_Failure', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'}) # x:285 y:378 OperatableStateMachine.add('Starting_Point', _sm_starting_point_0, transitions={'finished': 'Record_SysID_Test', 'failed': 'Stop_Video_Logging_After_Failure'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'experiment_name': 'experiment_name', 'trajectories': 'trajectories'}) # x:904 y:187 OperatableStateMachine.add('Stop_Recording', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging'}, autonomy={'stopped': Autonomy.Low}, remapping={'rosbag_process': 'rosbag_process'}) # x:494 y:292 OperatableStateMachine.add('Wait_For_Rosbag_Record', WaitState(wait_time=1.0), transitions={'done': 'Execute_Trajs_from_Bagfile'}, autonomy={'done': Autonomy.Low}) # x:507 y:384 OperatableStateMachine.add('Record_SysID_Test', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_For_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={'bagfile_name': 'output_bagfile', 'rosbag_process': 'rosbag_process'}) # x:1267 y:273, x:406 y:121 _sm_perform_calibration_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['output_bagfile', 'experiment_name', 'trajectories'], output_keys=['trajectories_command']) with _sm_perform_calibration_2: # x:136 y:171 OperatableStateMachine.add('Go_to_Intermediate_Mode', ChangeControlModeActionState(target_mode=motion_mode), transitions={'changed': 'Execute_Individual_Trajectory', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Low}) # x:514 y:314 OperatableStateMachine.add('Intermediate_Mode_before_exit', ChangeControlModeActionState(target_mode=motion_mode), transitions={'changed': 'Initial_Mode_before_exit', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.High}) # x:995 y:274 OperatableStateMachine.add('Initial_Mode_before_exit', ChangeControlModeActionState(target_mode=initial_mode), transitions={'changed': 'finished', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.High}) # x:222 y:296 OperatableStateMachine.add('Execute_Individual_Trajectory', _sm_execute_individual_trajectory_1, transitions={'finished': 'Intermediate_Mode_before_exit', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'output_bagfile': 'output_bagfile', 'trajectories': 'trajectories', 'experiment_name': 'experiment_name'}) # x:221 y:562, x:709 y:85 _sm_update_calibration_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['output_bagfile', 'trajectories', 'calibration_chain_key_local']) with _sm_update_calibration_3: # x:111 y:94 OperatableStateMachine.add('Calculate_Calibration', CalculateForceTorqueCalibration(calibration_chain=calibration_chain, settlingtime=settlingtime, static_calibration_data=static_calibration_data), transitions={'done': 'Ask_Perform_Update', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'bag_filename': 'output_bagfile', 'trajectories_command': 'trajectories', 'ft_calib_data': 'ft_calib_data'}) # x:420 y:515 OperatableStateMachine.add('Calibration_Successful', LogState(text="Successfully updated calibration offsets.", severity=Logger.REPORT_INFO), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:774 y:277 OperatableStateMachine.add('Calibration_Failed', LogState(text="Failed to apply calibration offsets!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:122 y:202 OperatableStateMachine.add('Ask_Perform_Update', OperatorDecisionState(outcomes=['update', 'no_update'], hint="Do you want to apply the calculated offsets for calibration?", suggestion=None), transitions={'update': 'Generate_Keys_Dict', 'no_update': 'finished'}, autonomy={'update': Autonomy.Full, 'no_update': Autonomy.Full}) # x:207 y:308 OperatableStateMachine.add('Generate_Keys_Dict', FlexibleCalculationState(calculation=self.create_parameter_keys_dict, input_keys=["input"]), transitions={'done': 'Update_Dynamic_Reconfigure'}, autonomy={'done': Autonomy.Off}, remapping={'input': 'calibration_chain_key_local', 'output_value': 'parameter_keys_dict'}) # x:408 y:303 OperatableStateMachine.add('Update_Dynamic_Reconfigure', UpdateDynamicParameterImpedanceControllerState(controller_chain=calibration_chain), transitions={'updated': 'Calibration_Successful', 'failed': 'Calibration_Failed'}, autonomy={'updated': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'parameter_keys': 'parameter_keys_dict', 'parameter_values': 'ft_calib_data'}) with _state_machine: # x:160 y:20 OperatableStateMachine.add('Execution_Starting', LogState(text="Execution has started. Please confirm transition to first state.", severity=Logger.REPORT_HINT), transitions={'done': 'Gen_Experiment_Name'}, autonomy={'done': Autonomy.Full}) # x:685 y:431 OperatableStateMachine.add('Update_Calibration', _sm_update_calibration_3, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'output_bagfile': 'output_bagfile', 'trajectories': 'trajectories_command', 'calibration_chain_key_local': 'calibration_chain_user'}) # x:99 y:533 OperatableStateMachine.add('Decision_Perform_Experiment', OperatorDecisionState(outcomes=["record_data", "use_existing"], hint="Use existing measurement data bag file or record a new one?", suggestion=None), transitions={'record_data': 'Perform_Calibration', 'use_existing': 'Update_Calibration'}, autonomy={'record_data': Autonomy.Low, 'use_existing': Autonomy.Low}) # x:130 y:185 OperatableStateMachine.add('Gen_Experiment_Name', FlexibleCalculationState(calculation=lambda i: 'FTCalib', input_keys=[]), transitions={'done': 'Gen_Output_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={'output_value': 'experiment_name'}) # x:134 y:267 OperatableStateMachine.add('Gen_Output_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join(bag_folder_out, en) + ".bag"), transitions={'done': 'Generate_Textfiles_Dict'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'experiment_name', 'output_value': 'output_bagfile'}) # x:443 y:252 OperatableStateMachine.add('Perform_Calibration', _sm_perform_calibration_2, transitions={'finished': 'Update_Calibration', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'output_bagfile': 'output_bagfile', 'experiment_name': 'experiment_name', 'trajectories': 'trajectories_command', 'trajectories_command': 'trajectories_command'}) # x:70 y:421 OperatableStateMachine.add('Load_Traj_From_Txt', GenerateTrajectoryFromTxtfileState(chains=calibration_chain, transitiontime=transitiontime, settlingtime=settlingtime), transitions={'done': 'Decision_Perform_Experiment', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'txtfilepaths': 'txtfiles_dict', 'trajectories': 'trajectories_command'}) # x:92 y:350 OperatableStateMachine.add('Generate_Textfiles_Dict', FlexibleCalculationState(calculation=self.create_txtfiles_dict, input_keys=["calibration_chain", "txtfile_left_arm", "txtfile_right_arm"]), transitions={'done': 'Load_Traj_From_Txt'}, autonomy={'done': Autonomy.Off}, remapping={'calibration_chain': 'calibration_chain_user', 'txtfile_left_arm': 'txtfile_name_left_arm_user', 'txtfile_right_arm': 'txtfile_name_right_arm_user', 'output_value': 'txtfiles_dict'}) return _state_machine
def create(self): initial_mode = "stand" motion_mode = "manipulate" mantis_mode = "manipulate_limits" percent_past_limits = 0.10 # before: 0.075 # x:788 y:72, x:474 y:133 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.target_limits = 'upper' _state_machine.userdata.cycle_counter = 1 _state_machine.userdata.stand_posture = None # calculated _state_machine.userdata.offsets = { 'left_arm': dict(), 'right_arm': dict() } # Additional creation code can be added inside the following tags # [MANUAL_CREATE] self._percent_past_limits = percent_past_limits # Create STAND posture trajectory _state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory( ) # [/MANUAL_CREATE] # x:222 y:281, x:349 y:167 _sm_determine_offsets_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'offsets'], output_keys=['offsets']) with _sm_determine_offsets_0: # x:61 y:53 OperatableStateMachine.add( 'Get_Left_Joint_Positions', CurrentJointPositionsState(planning_group="l_arm_group"), transitions={ 'retrieved': 'Determine_Closest_Limits_Left', 'failed': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'failed': Autonomy.Low }, remapping={'joint_positions': 'joint_positions'}) # x:319 y:54 OperatableStateMachine.add( 'Determine_Closest_Limits_Left', CalculationState(calculation=self.get_closest_limits_left), transitions={'done': 'Store_Offsets_Left'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'joint_positions', 'output_value': 'joint_limits' }) # x:598 y:162 OperatableStateMachine.add( 'Get_Right_Joint_Positions', CurrentJointPositionsState(planning_group="r_arm_group"), transitions={ 'retrieved': 'Determine_Closest_Limits_Right', 'failed': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'failed': Autonomy.Low }, remapping={'joint_positions': 'joint_positions'}) # x:584 y:275 OperatableStateMachine.add( 'Determine_Closest_Limits_Right', CalculationState(calculation=self.get_closest_limits_right), transitions={'done': 'Store_Offsets_Right'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'joint_positions', 'output_value': 'joint_limits' }) # x:608 y:54 OperatableStateMachine.add( 'Store_Offsets_Left', FlexibleCalculationState( calculation=self.store_offsets_left, input_keys=['limits', 'value', 'offsets', 'counter']), transitions={'done': 'Get_Right_Joint_Positions'}, autonomy={'done': Autonomy.Off}, remapping={ 'limits': 'joint_limits', 'value': 'joint_positions', 'offsets': 'offsets', 'counter': 'cycle_counter', 'output_value': 'offsets' }) # x:340 y:274 OperatableStateMachine.add( 'Store_Offsets_Right', FlexibleCalculationState( calculation=self.store_offsets_right, input_keys=['limits', 'value', 'offsets', 'counter']), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'limits': 'joint_limits', 'value': 'joint_positions', 'offsets': 'offsets', 'counter': 'cycle_counter', 'output_value': 'offsets' }) # x:528 y:401, x:707 y:282 _sm_manipulate_limits_1 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'offsets'], output_keys=['offsets', 'traj_past_limits']) with _sm_manipulate_limits_1: # x:100 y:156 OperatableStateMachine.add( 'Prevent_Runtime_Failure', CalculationState(calculation=lambda x: dict()), transitions={'done': 'Go_to_MANIPULATE_LIMITS'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'cycle_counter', 'output_value': 'traj_past_limits' }) # x:387 y:55 OperatableStateMachine.add( 'Wait_for_Control_Mode_change', WaitState(wait_time=1.0), transitions={'done': 'Get_Left_Joint_Positions'}, autonomy={'done': Autonomy.Low}) # x:895 y:279 OperatableStateMachine.add( 'Gen_Traj_from_90%_to_110%', CalculationState(calculation=self.gen_traj_past_limits), transitions={'done': 'Go_to_110%_Joint_Limits'}, autonomy={'done': Autonomy.Low}, remapping={ 'input_value': 'current_joint_values', 'output_value': 'traj_past_limits' }) # x:893 y:391 OperatableStateMachine.add( 'Go_to_110%_Joint_Limits', ExecuteTrajectoryBothArmsState(controllers=[ 'left_arm_traj_controller', 'right_arm_traj_controller' ]), transitions={ 'done': 'Determine_Offsets', 'failed': 'Determine_Offsets' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={'trajectories': 'traj_past_limits'}) # x:651 y:385 OperatableStateMachine.add('Determine_Offsets', _sm_determine_offsets_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'cycle_counter': 'cycle_counter', 'offsets': 'offsets' }) # x:648 y:54 OperatableStateMachine.add( 'Get_Left_Joint_Positions', CurrentJointPositionsState(planning_group="l_arm_group"), transitions={ 'retrieved': 'Get_Right_Joint_Positions', 'failed': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'failed': Autonomy.High }, remapping={'joint_positions': 'joint_positions_left'}) # x:904 y:53 OperatableStateMachine.add( 'Get_Right_Joint_Positions', CurrentJointPositionsState(planning_group="r_arm_group"), transitions={ 'retrieved': 'Generate_Joint_Positions_Struct', 'failed': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'failed': Autonomy.High }, remapping={'joint_positions': 'joint_positions_right'}) # x:886 y:168 OperatableStateMachine.add( 'Generate_Joint_Positions_Struct', FlexibleCalculationState(calculation=lambda ik: { 'left_arm': ik[0], 'right_arm': ik[1] }, input_keys=['left', 'right']), transitions={'done': 'Gen_Traj_from_90%_to_110%'}, autonomy={'done': Autonomy.Off}, remapping={ 'left': 'joint_positions_left', 'right': 'joint_positions_right', 'output_value': 'current_joint_values' }) # x:92 y:55 OperatableStateMachine.add( 'Go_to_MANIPULATE_LIMITS', ChangeControlModeActionState(target_mode=mantis_mode), transitions={ 'changed': 'Wait_for_Control_Mode_change', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:574 y:247, x:276 y:549 _sm_update_calibration_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['offsets']) with _sm_update_calibration_2: # x:46 y:44 OperatableStateMachine.add( 'Process_Offsets', CalculationState(calculation=self.process_offsets), transitions={'done': 'Print_Offset_Info'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'offsets', 'output_value': 'offsets' }) # x:227 y:45 OperatableStateMachine.add( 'Print_Offset_Info', CalculationState(calculation=self.print_offset_info), transitions={'done': 'Publish_Offsets'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'offsets', 'output_value': 'none' }) # x:390 y:158 OperatableStateMachine.add( 'Ask_Perform_Update', OperatorDecisionState( outcomes=['update', 'no_update'], hint= "Do you want to apply the calculated offsets for calibration?", suggestion=None), transitions={ 'update': 'Convert_Offset_Data', 'no_update': 'finished' }, autonomy={ 'update': Autonomy.Full, 'no_update': Autonomy.Full }) # x:232 y:337 OperatableStateMachine.add( 'Update_Calibration', UpdateJointCalibrationState( joint_names=self._joint_names['left_arm'][0:4] + self._joint_names['right_arm'][0:4]), transitions={ 'updated': 'Calibration_Successful', 'failed': 'Calibration_Failed' }, autonomy={ 'updated': Autonomy.Low, 'failed': Autonomy.High }, remapping={'joint_offsets': 'offset_list'}) # x:241 y:242 OperatableStateMachine.add( 'Convert_Offset_Data', CalculationState(calculation=lambda o: o['left_arm']['avg'] + o['right_arm']['avg']), transitions={'done': 'Update_Calibration'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'offsets', 'output_value': 'offset_list' }) # x:522 y:337 OperatableStateMachine.add( 'Calibration_Successful', LogState(text="Successfully updated calibration offsets.", severity=Logger.REPORT_INFO), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:246 y:445 OperatableStateMachine.add( 'Calibration_Failed', LogState(text="Failed to apply calibration offsets!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:399 y:44 OperatableStateMachine.add( 'Publish_Offsets', CalculationState(calculation=self.publish_offsets), transitions={'done': 'Ask_Perform_Update'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'offsets', 'output_value': 'none' }) # x:978 y:197, x:394 y:80 _sm_perform_checks_3 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'target_limits', 'offsets'], output_keys=['cycle_counter', 'offsets']) with _sm_perform_checks_3: # x:105 y:74 OperatableStateMachine.add( 'Go_to_Intermediate_Mode', ChangeControlModeActionState(target_mode=motion_mode), transitions={ 'changed': 'Gen_Traj_to_90%_Limits', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:653 y:274 OperatableStateMachine.add('Manipulate_Limits', _sm_manipulate_limits_1, transitions={ 'finished': 'Gen_Traj_back_to_90%_Limits', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'cycle_counter': 'cycle_counter', 'offsets': 'offsets', 'traj_past_limits': 'traj_past_limits' }) # x:903 y:78 OperatableStateMachine.add( 'Increment_Cycle_counter', CalculationState(calculation=lambda counter: counter + 1), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'cycle_counter', 'output_value': 'cycle_counter' }) # x:344 y:277 OperatableStateMachine.add( 'Move_to_90%_Joint_Limits', MoveitStartingPointState(vel_scaling=0.3), transitions={ 'reached': 'Manipulate_Limits', 'failed': 'Move_to_90%_Joint_Limits' }, autonomy={ 'reached': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'trajectories': 'trajectories_90'}) # x:114 y:276 OperatableStateMachine.add( 'Gen_Traj_to_90%_Limits', CalculationState(calculation=self.gen_traj_pre_limits), transitions={'done': 'Move_to_90%_Joint_Limits'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'target_limits', 'output_value': 'trajectories_90' }) # x:636 y:78 OperatableStateMachine.add( 'Go_back_to_90%_Joint_Limits', ExecuteTrajectoryBothArmsState(controllers=[ 'left_arm_traj_controller', 'right_arm_traj_controller' ]), transitions={ 'done': 'Increment_Cycle_counter', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={'trajectories': 'traj_back_to_90'}) # x:636 y:172 OperatableStateMachine.add( 'Gen_Traj_back_to_90%_Limits', FlexibleCalculationState( calculation=self.gen_traj_back_from_limits, input_keys=['trajectories_90', 'traj_past_limits']), transitions={'done': 'Go_back_to_90%_Joint_Limits'}, autonomy={'done': Autonomy.Off}, remapping={ 'trajectories_90': 'trajectories_90', 'traj_past_limits': 'traj_past_limits', 'output_value': 'traj_back_to_90' }) with _state_machine: # x:110 y:52 OperatableStateMachine.add( 'Initial_Control_Mode', ChangeControlModeActionState(target_mode=initial_mode), transitions={ 'changed': 'Perform_Checks', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.High, 'failed': Autonomy.High }) # x:712 y:317 OperatableStateMachine.add( 'Initial_Mode_before_exit', ChangeControlModeActionState(target_mode=initial_mode), transitions={ 'changed': 'Update_Calibration', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:122 y:302 OperatableStateMachine.add('Perform_Checks', _sm_perform_checks_3, transitions={ 'finished': 'Are_We_Done_Yet?', 'failed': 'Intermediate_Mode_before_exit' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'cycle_counter': 'cycle_counter', 'target_limits': 'target_limits', 'offsets': 'offsets' }) # x:126 y:505 OperatableStateMachine.add( 'Are_We_Done_Yet?', DecisionState(outcomes=["done", "more"], conditions=lambda counter: "done" if counter >= 2 else "more"), transitions={ 'done': 'Intermediate_Mode_before_exit', 'more': 'Setup_next_Cycle' }, autonomy={ 'done': Autonomy.Low, 'more': Autonomy.High }, remapping={'input_value': 'cycle_counter'}) # x:15 y:404 OperatableStateMachine.add( 'Setup_next_Cycle', CalculationState(calculation=lambda lim: 'lower' if lim == 'upper' else 'upper'), transitions={'done': 'Perform_Checks'}, autonomy={'done': Autonomy.Low}, remapping={ 'input_value': 'target_limits', 'output_value': 'target_limits' }) # x:725 y:186 OperatableStateMachine.add('Update_Calibration', _sm_update_calibration_2, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'offsets': 'offsets'}) # x:726 y:427 OperatableStateMachine.add( 'Move_to_Stand_Posture', MoveitStartingPointState(vel_scaling=0.3), transitions={ 'reached': 'Initial_Mode_before_exit', 'failed': 'Move_to_Stand_Posture' }, autonomy={ 'reached': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'trajectories': 'stand_posture'}) # x:412 y:427 OperatableStateMachine.add( 'Intermediate_Mode_before_exit', ChangeControlModeActionState(target_mode=motion_mode), transitions={ 'changed': 'Move_to_Stand_Posture', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) return _state_machine