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
