def createSM():
sm_take_coll_map_snapshot = smach.StateMachine(outcomes=['done',
'preempted',
'aborted'],
input_keys=['pan_angle',
'tilt_angle'],
output_keys=['error_message',
'error_code'])
with sm_take_coll_map_snapshot:
sm_take_coll_map_snapshot.userdata.head_goal_trajectory = JointTrajectory()
# wait
sm_take_coll_map_snapshot.userdata.wait_0sec = 0.0
sm_take_coll_map_snapshot.userdata.wait_1sec = 1.0
sm_take_coll_map_snapshot.userdata.wait_5sec = 5.0
sm_take_coll_map_snapshot.userdata.wait_6sec = 6.0
smach.StateMachine.add('PrepareHeadGoal',
PrepareGoal(),
remapping={'pan_angle':'pan_angle',
'tilt_angle':'tilt_angle',
'head_goal_trajectory':'head_goal_trajectory'},
transitions={'prepared':'MoveHead'})
smach.StateMachine.add('MoveHead',
SimpleActionState('/korus/head_controller',
control_msgs.msg.FollowJointTrajectoryAction,
goal_slots=['trajectory'],
result_cb=trajectory_control.generalResponseCb,
output_keys=['error_code']),
remapping={'trajectory':'head_goal_trajectory',
'error_code':'error_code'},
transitions={'succeeded':'ParseMoveHeadErrorCode',
'aborted':'ParseMoveHeadErrorCode',
'preempted':'preempted'})
smach.StateMachine.add('ParseMoveHeadErrorCode',
trajectory_control.FollowJointTrajectoryErrorCodesParser(),
transitions={'success':'WaitForPointcloudAlignment',
'parsed':'aborted'},
remapping={'error_code':'error_code',
'error_message':'error_message'})
smach.StateMachine.add('WaitForPointcloudAlignment', misc_tools.Wait(),
remapping={'duration':'wait_0sec'},
transitions={'done':'TakeSnapshot'})
smach.StateMachine.add('TakeSnapshot',
ServiceState('/korus/pointcloud_throttle_service/pass_on_pointcloud',
std_srvs.srv.Empty()),
transitions={'succeeded':'done',
'preempted':'preempted',
'aborted':'aborted'})
return sm_take_coll_map_snapshot
def createSM():
sm = smach.StateMachine(outcomes=['picked', 'pick_failed', 'preempted'],
input_keys=[
'object_name', 'object_pose',
'collision_object', 'pre_grasp_dist',
'pre_grasp_height', 'grasp_dist',
'grasp_height', 'post_grasp_dist',
'post_grasp_height', 'angle_gripper_opened',
'angle_gripper_closed', 'pose_arm_default'
],
output_keys=['object_pose'])
with sm:
sm.userdata.pre_grasp_pose = geometry_msgs.PoseStamped()
sm.userdata.grasp_pose = geometry_msgs.PoseStamped()
sm.userdata.post_grasp_pose = geometry_msgs.PoseStamped()
sm.userdata.true = True
sm.userdata.false = False
sm.userdata.wait_5sec = 5.0
sm.userdata.move_arm_up_distance = 0.03
smach.StateMachine.add('Prepare',
Prepare(),
remapping={
'object_pose': 'object_pose',
'pre_grasp_pose': 'pre_grasp_pose',
'grasp_pose': 'grasp_pose',
'post_grasp_pose': 'post_grasp_pose',
'pre_grasp_dist': 'pre_grasp_dist',
'pre_grasp_height': 'pre_grasp_height',
'grasp_dist': 'grasp_dist',
'grasp_height': 'grasp_height',
'post_grasp_dist': 'post_grasp_dist',
'post_grasp_height': 'post_grasp_height'
},
transitions={'prepared': 'MoveArmPreGrasp'})
smach.StateMachine.add('MoveArmPreGrasp',
SimpleActionState(
'move_arm_planner',
pick_and_place_msgs.MoveArmAction,
goal_slots=['goal_pose']),
remapping={'goal_pose': 'pre_grasp_pose'},
transitions={
'succeeded': 'OpenGripper',
'aborted': 'MoveArmDefaultFailed',
'preempted': 'preempted'
})
smach.StateMachine.add(
'OpenGripper',
SimpleActionState(
'gripper_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.gripperControlGoalCb,
result_cb=trajectory_control.gripperControlResultCb),
remapping={
'angle': 'angle_gripper_opened',
'open_gripper': 'true',
'close_gripper': 'false'
},
transitions={
'succeeded': 'MoveArmIKGrasp',
'aborted': 'MoveArmDefaultFailed',
'preempted': 'preempted'
})
smach.StateMachine.add('MoveArmIKGrasp',
SimpleActionState(
'move_arm_ik',
pick_and_place_msgs.MoveArmAction,
goal_slots=['goal_pose']),
remapping={'goal_pose': 'grasp_pose'},
transitions={
'succeeded': 'CloseGripper',
'aborted': 'MoveArmDefaultFailed',
'preempted': 'preempted'
})
smach.StateMachine.add(
'CloseGripper',
SimpleActionState(
'gripper_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.gripperControlGoalCb,
result_cb=trajectory_control.gripperControlResultCb),
remapping={
'angle': 'angle_gripper_closed',
'open_gripper': 'false',
'close_gripper': 'true'
},
transitions={
'succeeded': 'AttachObject',
'aborted': 'MoveArmDefaultFailed',
'preempted': 'preempted'
})
smach.StateMachine.add('AttachObject',
misc_tools.AttachObjectToRobot(),
remapping={
'collision_object': 'collision_object',
'attach': 'true'
},
transitions={'done': 'WaitForObjectAdded'})
smach.StateMachine.add('WaitForObjectAdded',
misc_tools.Wait(),
remapping={'duration': 'wait_5sec'},
transitions={'done': 'RetrieveJointStates'})
smach.StateMachine.add('RetrieveJointStates',
misc_tools.RetrieveJointStates(),
remapping={'joint_states': 'joint_states'},
transitions={
'success': 'MoveArmUp',
'error': 'MoveArmIKPostGrasp'
})
smach.StateMachine.add(
'MoveArmUp',
SimpleActionState('arm_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.moveArmUpGoalCb,
result_cb=trajectory_control.generalResponseCb),
remapping={
'joint_states': 'joint_states',
'distance': 'move_arm_up_distance'
},
transitions={
'succeeded': 'MoveArmIKPostGrasp',
'aborted': 'MoveArmIKPostGrasp',
'preempted': 'preempted'
})
smach.StateMachine.add('MoveArmIKPostGrasp',
SimpleActionState(
'move_arm_ik',
pick_and_place_msgs.MoveArmAction,
goal_slots=['goal_pose']),
remapping={'goal_pose': 'post_grasp_pose'},
transitions={
'succeeded': 'MoveArmDefault',
'aborted': 'MoveArmDefaultFailed',
'preempted': 'preempted'
})
smach.StateMachine.add('MoveArmDefault',
SimpleActionState(
'move_arm_planner',
pick_and_place_msgs.MoveArmAction,
goal_slots=['goal_pose']),
remapping={'goal_pose': 'pose_arm_default'},
transitions={
'succeeded': 'picked',
'aborted': 'pick_failed',
'preempted': 'preempted'
})
smach.StateMachine.add('MoveArmDefaultFailed',
SimpleActionState(
'move_arm_planner',
pick_and_place_msgs.MoveArmAction,
goal_slots=['goal_pose']),
remapping={'goal_pose': 'pose_arm_default'},
transitions={
'succeeded': 'pick_failed',
'aborted': 'pick_failed',
'preempted': 'preempted'
})
return sm
def createSM():
sm_find_object = smach.StateMachine(
outcomes=['object_found', 'no_objects_found', 'preempted', 'aborted'],
input_keys=[
'table_pose', 'look_around', 'min_confidence', 'object_names',
'tf_listener'
],
output_keys=[
'recognised_objects', 'object_names', 'objects_info',
'tabletop_centre_pose', 'tabletop_front_place_pose',
'error_message', 'error_code', 'tf_listener'
])
with sm_find_object:
# move head
sm_find_object.userdata.motors = ['head_pan', 'head_tilt']
sm_find_object.userdata.tabletop_centre_pose = geometry_msgs.PoseStamped(
)
sm_find_object.userdata.tabletop_front_place_pose = geometry_msgs.PoseStamped(
)
sm_find_object.userdata.pose_bottom_left = geometry_msgs.PoseStamped()
sm_find_object.userdata.pose_bottom_left.header.stamp = rospy.Time.now(
)
sm_find_object.userdata.pose_bottom_left.header.frame_id = "base_footprint"
sm_find_object.userdata.pose_bottom_left.pose.position.x = 0.2
sm_find_object.userdata.pose_bottom_left.pose.position.y = -0.2
sm_find_object.userdata.pose_bottom_left.pose.position.z = 0.0
sm_find_object.userdata.pose_bottom_left.pose.orientation.w = 1.0
sm_find_object.userdata.pose_bottom_centre = geometry_msgs.PoseStamped(
)
sm_find_object.userdata.pose_bottom_centre.header = sm_find_object.userdata.pose_bottom_left.header
sm_find_object.userdata.pose_bottom_centre.pose.position.x = 0.2
sm_find_object.userdata.pose_bottom_centre.pose.position.y = 0.0
sm_find_object.userdata.pose_bottom_centre.pose.position.z = 0.0
sm_find_object.userdata.pose_bottom_centre.pose.orientation.w = 1.0
sm_find_object.userdata.pose_bottom_right = geometry_msgs.PoseStamped()
sm_find_object.userdata.pose_bottom_right.header = sm_find_object.userdata.pose_bottom_left.header
sm_find_object.userdata.pose_bottom_right.pose.position.x = 0.2
sm_find_object.userdata.pose_bottom_right.pose.position.y = 0.2
sm_find_object.userdata.pose_bottom_right.pose.position.z = 0.0
sm_find_object.userdata.pose_bottom_right.pose.orientation.w = 1.0
sm_find_object.userdata.pose_top_left = geometry_msgs.PoseStamped()
sm_find_object.userdata.pose_top_left.header = sm_find_object.userdata.pose_bottom_left.header
sm_find_object.userdata.pose_top_left.pose.position.x = 0.5
sm_find_object.userdata.pose_top_left.pose.position.y = -0.5
sm_find_object.userdata.pose_top_left.pose.position.z = 0.5
sm_find_object.userdata.pose_top_left.pose.orientation.w = 1.0
sm_find_object.userdata.pose_top_centre = geometry_msgs.PoseStamped()
sm_find_object.userdata.pose_top_centre.header = sm_find_object.userdata.pose_bottom_left.header
sm_find_object.userdata.pose_top_centre.pose.position.x = 0.5
sm_find_object.userdata.pose_top_centre.pose.position.y = 0.0
sm_find_object.userdata.pose_top_centre.pose.position.z = 0.5
sm_find_object.userdata.pose_top_centre.pose.orientation.w = 1.0
sm_find_object.userdata.pose_top_right = geometry_msgs.PoseStamped()
sm_find_object.userdata.pose_top_right.header = sm_find_object.userdata.pose_bottom_left.header
sm_find_object.userdata.pose_top_right.pose.position.x = 0.5
sm_find_object.userdata.pose_top_right.pose.position.y = 0.5
sm_find_object.userdata.pose_top_right.pose.position.z = 0.5
sm_find_object.userdata.pose_top_right.pose.orientation.w = 1.0
sm_find_object.userdata.pose_centre = geometry_msgs.PoseStamped()
sm_find_object.userdata.pose_centre.header = sm_find_object.userdata.pose_bottom_left.header
sm_find_object.userdata.pose_centre.pose.position.x = 0.5
sm_find_object.userdata.pose_centre.pose.position.y = 0.0
sm_find_object.userdata.pose_centre.pose.position.z = 0.0
sm_find_object.userdata.pose_centre.pose.orientation.w = 1.0
# object recognition
sm_find_object.userdata.error_message = str()
sm_find_object.userdata.error_code = int()
# wait
sm_find_object.userdata.wait_0sec = 0.0
sm_find_object.userdata.wait_1sec = 1.0
sm_find_object.userdata.wait_2sec = 2.0
sm_find_object.userdata.wait_3sec = 3.0
sm_find_object.userdata.wait_5sec = 5.0
sm_find_object.userdata.wait_10sec = 10.0
# reset TryAgain
sm_find_object.userdata.reset_true = True
sm_find_object.userdata.reset_false = False
# reset TryAgain
sm_find_object.userdata.reset_true = True
sm_find_object.userdata.reset_false = False
sm_find_object.userdata.objects_info = []
# 3D sensor modes
sm_find_object.userdata.color_mode_high_res = 5
sm_find_object.userdata.depth_mode_high_res = sm_find_object.userdata.color_mode_high_res
sm_find_object.userdata.ir_mode_high_res = sm_find_object.userdata.color_mode_high_res
sm_find_object.userdata.color_mode_low_res = 8
sm_find_object.userdata.depth_mode_low_res = sm_find_object.userdata.color_mode_low_res
sm_find_object.userdata.ir_mode_low_res = sm_find_object.userdata.color_mode_low_res
smach.StateMachine.add('EnableHighResPointCloud',
misc_tools.change3DSensorDriverMode(),
transitions={'done': 'EnableMotors'},
remapping={
'color_mode': 'color_mode_high_res',
'depth_mode': 'depth_mode_high_res',
'ir_mode': 'ir_mode_high_res'
})
smach.StateMachine.add('EnableMotors',
misc_tools.EnableMotors(),
transitions={'success': 'ResetTryAgain'},
remapping={'motors': 'motors'})
try_again_sm = TryAgain()
smach.StateMachine.add('ResetTryAgain',
try_again_sm,
remapping={
'reset': 'reset_true',
'look_around': 'look_around'
},
transitions={
'bottom_centre': 'aborted',
'bottom_right': 'aborted',
'top_right': 'aborted',
'top_centre': 'aborted',
'top_left': 'aborted',
'done': 'CheckTablePose'
})
smach.StateMachine.add('CheckTablePose',
CheckTablePose(),
transitions={
'look_around': 'MoveHeadBottomLeft',
'is_zero': 'RecognizeObjects',
'is_not_zero': 'MoveHead'
},
remapping={
'look_around': 'look_around',
'table_pose': 'table_pose'
})
smach.StateMachine.add(
'MoveHead',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['tf_listener', 'error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'table_pose',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'MoveHeadBottomLeft',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['tf_listener', 'error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'pose_bottom_left',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'MoveHeadBottomCentre',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['tf_listener', 'error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'pose_bottom_centre',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'MoveHeadBottomRight',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['tf_listener', 'error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'pose_bottom_right',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'MoveHeadTopRight',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'pose_top_right',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'MoveHeadTopCentre',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'pose_top_centre',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'MoveHeadTopLeft',
SimpleActionState('head_controller',
control_msgs.FollowJointTrajectoryAction,
goal_cb=trajectory_control.headControlRequestCb,
result_cb=trajectory_control.generalResponseCb,
input_keys=['tf_listener', 'goal_pose'],
output_keys=['error_code']),
remapping={
'tf_listener': 'tf_listener',
'goal_pose': 'pose_top_left',
'error_code': 'error_code'
},
transitions={
'succeeded': 'ParseMoveHeadErrorCode',
'aborted': 'ParseMoveHeadErrorCode',
'preempted': 'preempted'
})
smach.StateMachine.add(
'ParseMoveHeadErrorCode',
trajectory_control.FollowJointTrajectoryErrorCodesParser(),
transitions={
'success': 'WaitForPointcloudAlignment',
'parsed': 'aborted'
},
remapping={
'error_code': 'error_code',
'error_message': 'error_message'
})
smach.StateMachine.add('WaitForPointcloudAlignment',
misc_tools.Wait(),
remapping={'duration': 'wait_1sec'},
transitions={'done': 'RecognizeObjects'})
smach.StateMachine.add(
'RecognizeObjects',
SimpleActionState(
'recognize_objects',
object_recognition_msgs.ObjectRecognitionAction,
goal_cb=object_recognition.objectRecognitionGoalCb,
result_cb=object_recognition.objectRecognitionResultCb,
input_keys=[
'min_confidence', 'error_code', 'error_message',
'tf_listener'
],
output_keys=[
'recognised_objects', 'object_names', 'error_code',
'error_message', 'tf_listener'
]),
remapping={
'min_confidence': 'min_confidence',
'recognised_objects': 'recognised_objects',
'object_names': 'object_names',
'error_code': 'error_code',
'error_message': 'error_message',
'tf_listener': 'tf_listener'
},
transitions={
'succeeded': 'DisableHighResPointCloudSuccess',
'no_objects_found': 'TryAgain',
'preempted': 'preempted',
'aborted': 'aborted'
})
smach.StateMachine.add('TryAgain',
try_again_sm,
remapping={
'reset': 'reset_false',
'look_around': 'look_around'
},
transitions={
'bottom_centre': 'MoveHeadBottomCentre',
'bottom_right': 'MoveHeadBottomRight',
'top_right': 'MoveHeadTopRight',
'top_centre': 'MoveHeadTopCentre',
'top_left': 'MoveHeadTopLeft',
'done': 'DisableHighResPointCloudNoSuccess'
})
smach.StateMachine.add('DisableHighResPointCloudNoSuccess',
misc_tools.change3DSensorDriverMode(),
transitions={'done': 'no_objects_found'},
remapping={
'color_mode': 'color_mode_low_res',
'depth_mode': 'depth_mode_low_res',
'ir_mode': 'ir_mode_low_res'
})
smach.StateMachine.add('DisableHighResPointCloudSuccess',
misc_tools.change3DSensorDriverMode(),
transitions={'done': 'GetObjectMeshes'},
remapping={
'color_mode': 'color_mode_low_res',
'depth_mode': 'depth_mode_low_res',
'ir_mode': 'ir_mode_low_res'
})
smach.StateMachine.add(
'GetObjectMeshes',
object_recognition.GetObjectInformation(),
remapping={
'recognised_objects': 'recognised_objects',
'objects_info': 'objects_info',
'error_message': 'error_message',
'error_code': 'error_code'
},
transitions={'done': 'AddObjectsToPlanningScene'})
smach.StateMachine.add('AddObjectsToPlanningScene',
misc_tools.AddObjectsToPlanningScene(),
remapping={
'recognised_objects': 'recognised_objects',
'object_names': 'object_names',
'objects_info': 'objects_info',
'error_message': 'error_message',
'error_code': 'error_code'
},
transitions={'done': 'AddTableToPlanningScene'})
smach.StateMachine.add('AddTableToPlanningScene',
object_recognition.GetTable(),
remapping={
'tabletop_centre_pose':
'tabletop_centre_pose',
'tabletop_front_place_pose':
'tabletop_front_place_pose',
'tf_listener': 'tf_listener'
},
transitions={
'table_added': 'object_found',
'no_table_added': 'aborted'
})
return sm_find_object
def createSM():
sm_arm_calibration = smach.StateMachine(
outcomes=['calibrated', 'calibration_failed'])
with sm_arm_calibration:
sm_arm_calibration.userdata.wait_2sec = 2.0
'''
Torso calibration
'''
# sm_torso = smach.StateMachine(outcomes=['torso_calibrated',
# 'torso_calibration_failed'])
#
# with sm_torso:
#
# sm_torso.userdata.motor_torso_turn = ["torso_turn"]
# sm_torso.userdata.forward_direction = 1
# sm_torso.userdata.backward_direction = -1
# sm_torso.userdata.get_one_limit_only_true = True
# sm_torso.userdata.get_one_limit_only_false = False
# sm_torso.userdata.complete_calibration_true = True
# sm_torso.userdata.complete_calibration_false = False
# sm_torso.userdata.encoder_goal_true = True
# sm_torso.userdata.encoder_goal_false = False
# sm_torso.userdata.move_to_hard_limit_true = True
# sm_torso.userdata.move_to_hard_limit_false = False
# sm_torso.userdata.move_to_zero_true = True
# sm_torso.userdata.move_to_zero_false = False
# sm_torso.userdata.actuator_torso_turn_name = "torso_turn"
# sm_torso.userdata.actuator_torso_turn_array_pos = 6
# sm_torso.userdata.actuator_torso_turn_max_current = 80
# sm_torso.userdata.actuator_torso_turn_angle_ratio = 2.1625 # = (max - min) / max with min = -186, max = 160
# sm_torso.userdata.actuator_torso_turn_speed = 0.015
# sm_torso.userdata.actuator_torso_turn_encoder_ticks = 276000
# sm_torso.userdata.actuator_torso_turn_limit_min = 0
# sm_torso.userdata.actuator_torso_turn_limit_max = 0
# sm_torso.userdata.actuator_torso_turn_encoder_centre = 0
#
# smach.StateMachine.add('CalibrateTorsoTurn',
# actuator.CalibrateActuator(),
# remapping={'actuator_name':'actuator_torso_turn_name',
# 'actuator_array_pos':'actuator_torso_turn_array_pos',
# 'actuator_max_current':'actuator_torso_turn_max_current',
# 'actuator_angle_ratio':'actuator_torso_turn_angle_ratio',
# 'actuator_speed':'actuator_torso_turn_speed',
# 'actuator_direction':'forward_direction',
# 'encoder_ticks':'actuator_torso_turn_encoder_ticks',
# 'actuator_limit_min':'actuator_torso_turn_limit_min',
# 'actuator_limit_max':'actuator_torso_turn_limit_max',
# 'actuator_encoder_centre':'actuator_torso_turn_encoder_centre',
# 'move_to_zero':'move_to_zero_true',
# 'get_one_limit_only':'get_one_limit_only_false',
# 'complete_calibration':'complete_calibration_true'},
# transitions={'success':'torso_calibrated',
# 'error':'torso_calibration_failed'}),
# smach.StateMachine.add('TorsoCalibration',
# sm_torso,
# transitions={'torso_calibrated':'ArmCalibration',
# 'torso_calibration_failed':'calibration_failed'})
sm_arm = smach.StateMachine(
outcomes=['arm_calibrated', 'arm_calibration_failed'])
with sm_arm:
sm_arm.userdata.forward_direction = 1
sm_arm.userdata.backward_direction = -1
sm_arm.userdata.get_one_limit_only_true = True
sm_arm.userdata.get_one_limit_only_false = False
sm_arm.userdata.complete_calibration_true = True
sm_arm.userdata.complete_calibration_false = False
sm_arm.userdata.encoder_goal_true = True
sm_arm.userdata.encoder_goal_false = False
sm_arm.userdata.move_to_hard_limit_true = True
sm_arm.userdata.move_to_hard_limit_false = False
sm_arm.userdata.move_to_zero_true = True
sm_arm.userdata.move_to_zero_false = False
sm_arm.userdata.actuator_torso_lift_name = "torso_lift"
sm_arm.userdata.actuator_torso_lift_array_pos = 5
sm_arm.userdata.actuator_torso_lift_max_current = 80
sm_arm.userdata.actuator_torso_lift_speed = 0.15
sm_arm.userdata.actuator_torso_lift_encoder_ticks = 28800
sm_arm.userdata.actuator_shoulder_name = "shoulder"
sm_arm.userdata.actuator_shoulder_array_pos = 4
sm_arm.userdata.actuator_shoulder_max_current = 80
sm_arm.userdata.actuator_shoulder_angle_ratio = 1.76086956522 # '(max - min) / max' with min = -70, max = 92
sm_arm.userdata.actuator_shoulder_speed = 0.005
sm_arm.userdata.actuator_shoulder_encoder_ticks = 336000
sm_arm.userdata.actuator_shoulder_limit_min = 0
sm_arm.userdata.actuator_shoulder_limit_max = 0
sm_arm.userdata.actuator_shoulder_encoder_centre = 0
sm_arm.userdata.actuator_shoulder_goal_pos = 0.0 # 90 / 360 * 336000 = 84,000
sm_arm.userdata.actuator_elbow_name = "elbow"
sm_arm.userdata.actuator_elbow_array_pos = 0
sm_arm.userdata.actuator_elbow_max_current = 140
sm_arm.userdata.actuator_elbow_angle_ratio = 3.23913043478 # '(max - min) / max' with min = -206 (-200), max = 92
sm_arm.userdata.actuator_elbow_speed = 0.004
sm_arm.userdata.actuator_elbow_low_speed = 0.001
sm_arm.userdata.actuator_elbow_encoder_ticks = 268800
sm_arm.userdata.actuator_elbow_limit_min = 0
sm_arm.userdata.actuator_elbow_limit_max = 0
sm_arm.userdata.actuator_elbow_encoder_centre = 0
sm_arm.userdata.actuator_elbow_goal_pos = 1.90
sm_arm.userdata.actuator_wrist_name = "wrist"
sm_arm.userdata.actuator_wrist_array_pos = 9
sm_arm.userdata.actuator_wrist_max_current = 200
sm_arm.userdata.actuator_wrist_angle_ratio = 2 # min/max = -/+ 100
sm_arm.userdata.actuator_wrist_speed = 0.01
sm_arm.userdata.actuator_wrist_encoder_ticks = 417000
sm_arm.userdata.actuator_wrist_limit_min = 0
sm_arm.userdata.actuator_wrist_limit_max = 0
sm_arm.userdata.actuator_wrist_encoder_centre = 0
sm_arm.userdata.actuator_gripper_name = "gripper"
sm_arm.userdata.actuator_gripper_array_pos = 1
sm_arm.userdata.actuator_gripper_max_current = 140
sm_arm.userdata.actuator_gripper_angle_ratio = 1.025641026 # min = -2 (changed from -2), max = 78
sm_arm.userdata.actuator_gripper_speed = 0.01
sm_arm.userdata.actuator_gripper_encoder_ticks = 873600
sm_arm.userdata.actuator_gripper_limit_min = 0
sm_arm.userdata.actuator_gripper_limit_max = 0
sm_arm.userdata.actuator_gripper_encoder_centre = 0
sm_arm.userdata.wait_0sec = 0
sm_arm.userdata.wait_1sec = 1.0
'''
Get shoulder max and elbow min limits
'''
sm_shoulder_max_elbow_min = smach.Concurrence(
outcomes=['success', 'error'],
default_outcome='error',
outcome_map={
'success': {
'GetShoulderMaxLimit': 'success',
'GetElbowMinLimit': 'success'
},
'error': {
'GetShoulderMaxLimit': 'error',
'GetElbowMinLimit': 'error'
}
},
input_keys=[
'forward_direction', 'backward_direction',
'get_one_limit_only_true', 'complete_calibration_false',
'move_to_zero_false', 'actuator_shoulder_name',
'actuator_shoulder_array_pos',
'actuator_shoulder_max_current',
'actuator_shoulder_angle_ratio', 'actuator_shoulder_speed',
'actuator_shoulder_encoder_ticks',
'actuator_shoulder_limit_min',
'actuator_shoulder_limit_max', 'actuator_elbow_name',
'actuator_elbow_array_pos', 'actuator_elbow_max_current',
'actuator_elbow_angle_ratio', 'actuator_elbow_speed',
'actuator_elbow_encoder_ticks', 'actuator_elbow_limit_min',
'actuator_elbow_limit_max', 'actuator_elbow_encoder_centre'
],
output_keys=[
'actuator_elbow_limit_min', 'actuator_elbow_limit_max',
'actuator_shoulder_limit_min',
'actuator_shoulder_limit_max'
])
with sm_shoulder_max_elbow_min:
smach.Concurrence.add('GetShoulderMaxLimit',
actuator.CalibrateActuator(),
remapping={
'actuator_name':
'actuator_shoulder_name',
'actuator_array_pos':
'actuator_shoulder_array_pos',
'actuator_max_current':
'actuator_shoulder_max_current',
'actuator_angle_ratio':
'actuator_shoulder_angle_ratio',
'actuator_speed':
'actuator_shoulder_speed',
'actuator_direction':
'forward_direction',
'encoder_ticks':
'actuator_shoulder_encoder_ticks',
'actuator_limit_min':
'actuator_shoulder_limit_min',
'actuator_limit_max':
'actuator_shoulder_limit_max',
'actuator_encoder_centre':
'actuator_shoulder_encoder_centre',
'move_to_zero':
'move_to_zero_false',
'get_one_limit_only':
'get_one_limit_only_true',
'complete_calibration':
'complete_calibration_false'
})
smach.Concurrence.add('GetElbowMinLimit',
actuator.CalibrateActuator(),
remapping={
'actuator_name':
'actuator_elbow_name',
'actuator_array_pos':
'actuator_elbow_array_pos',
'actuator_max_current':
'actuator_elbow_max_current',
'actuator_angle_ratio':
'actuator_elbow_angle_ratio',
'actuator_speed':
'actuator_elbow_speed',
'actuator_direction':
'backward_direction',
'encoder_ticks':
'actuator_elbow_encoder_ticks',
'actuator_limit_min':
'actuator_elbow_limit_min',
'actuator_limit_max':
'actuator_elbow_limit_max',
'actuator_encoder_centre':
'actuator_elbow_encoder_centre',
'move_to_zero':
'move_to_zero_false',
'get_one_limit_only':
'get_one_limit_only_true',
'complete_calibration':
'complete_calibration_false'
})
smach.StateMachine.add(
'GetShoulderMaxAndElbowMin',
sm_shoulder_max_elbow_min,
transitions={
'success':
'MoveTorsoAndElbowUpAndCalibrateWristAndGripper',
'error': 'arm_calibration_failed'
})
'''
Move torso and elbow up and calibrate wrist and gripper
'''
sm_to_el_up_cal_wr_gr = smach.Concurrence(
outcomes=['done', 'error'],
default_outcome='error',
outcome_map={
'done': {
'MoveTorsoAndElbowUp': 'moved',
'WristAndGripperCalibration': 'success'
},
'error': {
'MoveTorsoAndElbowUp': 'error',
'WristAndGripperCalibration': 'error'
}
},
input_keys=[
'forward_direction', 'encoder_goal_false',
'move_to_hard_limit_false', 'actuator_torso_lift_name',
'actuator_torso_lift_array_pos',
'actuator_torso_lift_max_current',
'actuator_torso_lift_speed',
'actuator_torso_lift_encoder_ticks', 'actuator_elbow_name',
'actuator_elbow_array_pos', 'actuator_elbow_max_current',
'actuator_elbow_speed', 'actuator_elbow_encoder_ticks',
'move_to_zero_true', 'get_one_limit_only_false',
'complete_calibration_true', 'actuator_wrist_name',
'actuator_wrist_array_pos', 'actuator_wrist_max_current',
'actuator_wrist_angle_ratio', 'actuator_wrist_speed',
'actuator_wrist_encoder_ticks', 'actuator_wrist_limit_min',
'actuator_wrist_limit_max',
'actuator_wrist_encoder_centre', 'actuator_gripper_name',
'actuator_gripper_array_pos',
'actuator_gripper_max_current',
'actuator_gripper_angle_ratio', 'actuator_gripper_speed',
'actuator_gripper_encoder_ticks',
'actuator_gripper_limit_min', 'actuator_gripper_limit_max',
'actuator_gripper_encoder_centre', 'wait_1sec'
])
sm_to_el_up_cal_wr_gr.userdata.actuator_torso_lift_goal_pos = 65
sm_to_el_up_cal_wr_gr.userdata.actuator_elbow_goal_pos = 1.57
with sm_to_el_up_cal_wr_gr:
'''
Move torso and elbow up
'''
sm_torso_elbow_up = smach.Concurrence(
outcomes=['moved', 'error'],
default_outcome='error',
outcome_map={
'moved': {
'MoveTorsoLiftUp': 'success',
'MoveElbowUp': 'success'
},
'error': {
'MoveTorsoLiftUp': 'error',
'MoveElbowUp': 'error'
}
},
input_keys=[
'forward_direction', 'encoder_goal_false',
'move_to_hard_limit_false', 'actuator_torso_lift_name',
'actuator_torso_lift_array_pos',
'actuator_torso_lift_max_current',
'actuator_torso_lift_speed',
'actuator_torso_lift_encoder_ticks',
'actuator_elbow_name', 'actuator_elbow_array_pos',
'actuator_elbow_max_current', 'actuator_elbow_speed',
'actuator_elbow_encoder_ticks'
])
sm_torso_elbow_up.userdata.actuator_torso_lift_goal_pos = 65
sm_torso_elbow_up.userdata.actuator_elbow_goal_pos = 1.57
with sm_torso_elbow_up:
smach.Concurrence.add(
'MoveTorsoLiftUp',
actuator.MoveActuator(),
remapping={
'actuator_name': 'actuator_torso_lift_name',
'actuator_array_pos':
'actuator_torso_lift_array_pos',
'actuator_max_current':
'actuator_torso_lift_max_current',
'actuator_speed': 'actuator_torso_lift_speed',
'actuator_direction': 'forward_direction',
'encoder_ticks':
'actuator_torso_lift_encoder_ticks',
'actuator_goal_pos':
'actuator_torso_lift_goal_pos',
'encoder_goal': 'encoder_goal_false',
'move_to_hard_limit': 'move_to_hard_limit_false'
})
smach.Concurrence.add('MoveElbowUp',
actuator.MoveActuator(),
remapping={
'actuator_name':
'actuator_elbow_name',
'actuator_array_pos':
'actuator_elbow_array_pos',
'actuator_max_current':
'actuator_elbow_max_current',
'actuator_speed':
'actuator_elbow_speed',
'actuator_direction':
'forward_direction',
'encoder_ticks':
'actuator_elbow_encoder_ticks',
'actuator_goal_pos':
'actuator_elbow_goal_pos',
'encoder_goal':
'encoder_goal_false',
'move_to_hard_limit':
'move_to_hard_limit_false'
})
smach.Concurrence.add('MoveTorsoAndElbowUp', sm_torso_elbow_up)
'''
Wrist and gripper calibration
'''
sm_wrist_gripper_calibration = smach.StateMachine(
outcomes=['success', 'error'],
input_keys=[
'forward_direction', 'move_to_zero_true',
'get_one_limit_only_false',
'complete_calibration_true', 'actuator_wrist_name',
'actuator_wrist_array_pos',
'actuator_wrist_max_current',
'actuator_wrist_angle_ratio', 'actuator_wrist_speed',
'actuator_wrist_encoder_ticks',
'actuator_wrist_limit_min', 'actuator_wrist_limit_max',
'actuator_wrist_encoder_centre',
'actuator_gripper_name', 'actuator_gripper_array_pos',
'actuator_gripper_max_current',
'actuator_gripper_angle_ratio',
'actuator_gripper_speed',
'actuator_gripper_encoder_ticks',
'actuator_gripper_limit_min',
'actuator_gripper_limit_max',
'actuator_gripper_encoder_centre', 'wait_1sec'
])
with sm_wrist_gripper_calibration:
smach.StateMachine.add(
'WaitBeforeWristAndGripperCalibration',
misc_tools.Wait(),
remapping={'duration': 'wait_1sec'},
transitions={'done': 'Calibration'})
sm_calibration = smach.Concurrence(
outcomes=['calibrated', 'calibration_failed'],
default_outcome='calibration_failed',
outcome_map={
'calibrated': {
'CalibrateWrist': 'success',
'CalibrateGripper': 'success'
},
'calibration_failed': {
'CalibrateWrist': 'error',
'CalibrateGripper': 'error'
}
},
input_keys=[
'forward_direction', 'move_to_zero_true',
'get_one_limit_only_false',
'complete_calibration_true', 'actuator_wrist_name',
'actuator_wrist_array_pos',
'actuator_wrist_max_current',
'actuator_wrist_angle_ratio',
'actuator_wrist_speed',
'actuator_wrist_encoder_ticks',
'actuator_wrist_limit_min',
'actuator_wrist_limit_max',
'actuator_wrist_encoder_centre',
'actuator_gripper_name',
'actuator_gripper_array_pos',
'actuator_gripper_max_current',
'actuator_gripper_angle_ratio',
'actuator_gripper_speed',
'actuator_gripper_encoder_ticks',
'actuator_gripper_limit_min',
'actuator_gripper_limit_max',
'actuator_gripper_encoder_centre'
])
with sm_calibration:
smach.Concurrence.add(
'CalibrateWrist',
actuator.CalibrateActuator(),
remapping={
'actuator_name': 'actuator_wrist_name',
'actuator_array_pos':
'actuator_wrist_array_pos',
'actuator_max_current':
'actuator_wrist_max_current',
'actuator_angle_ratio':
'actuator_wrist_angle_ratio',
'actuator_speed': 'actuator_wrist_speed',
'actuator_direction': 'forward_direction',
'encoder_ticks':
'actuator_wrist_encoder_ticks',
'actuator_limit_min':
'actuator_wrist_limit_min',
'actuator_limit_max':
'actuator_wrist_limit_max',
'actuator_encoder_centre':
'actuator_wrist_encoder_centre',
'move_to_zero': 'move_to_zero_true',
'get_one_limit_only':
'get_one_limit_only_false',
'complete_calibration':
'complete_calibration_true'
})
smach.Concurrence.add(
'CalibrateGripper',
actuator.CalibrateActuator(),
remapping={
'actuator_name': 'actuator_gripper_name',
'actuator_array_pos':
'actuator_gripper_array_pos',
'actuator_max_current':
'actuator_gripper_max_current',
'actuator_angle_ratio':
'actuator_gripper_angle_ratio',
'actuator_speed': 'actuator_gripper_speed',
'actuator_direction': 'forward_direction',
'encoder_ticks':
'actuator_gripper_encoder_ticks',
'actuator_limit_min':
'actuator_gripper_limit_min',
'actuator_limit_max':
'actuator_gripper_limit_max',
'actuator_encoder_centre':
'actuator_gripper_encoder_centre',
'move_to_zero': 'move_to_zero_true',
'get_one_limit_only':
'get_one_limit_only_false',
'complete_calibration':
'complete_calibration_true'
})
smach.StateMachine.add(
'Calibration',
sm_calibration,
# remapping={'forward_direction':'forward_direction',
# 'move_to_zero_true':'move_to_zero_true',
# 'get_one_limit_only_false':'get_one_limit_only_false',
# 'complete_calibration_true':'complete_calibration_true',
# 'actuator_wrist_name':'actuator_wrist_name',
# 'actuator_wrist_array_pos':'actuator_wrist_array_pos',
# 'actuator_wrist_max_current':'actuator_wrist_max_current',
# 'actuator_wrist_angle_ratio':'actuator_wrist_angle_ratio',
# 'actuator_wrist_speed':'actuator_wrist_speed',
# 'actuator_wrist_encoder_ticks':'actuator_wrist_encoder_ticks',
# 'actuator_wrist_limit_min':'actuator_wrist_limit_min',
# 'actuator_wrist_limit_max':'actuator_wrist_limit_max',
# 'actuator_wrist_encoder_centre':'actuator_wrist_encoder_centre',
# 'actuator_gripper_name':'actuator_gripper_name',
# 'actuator_gripper_array_pos':'actuator_gripper_array_pos',
# 'actuator_gripper_max_current':'actuator_gripper_max_current',
# 'actuator_gripper_angle_ratio':'actuator_gripper_angle_ratio',
# 'actuator_gripper_speed':'actuator_gripper_speed',
# 'actuator_gripper_encoder_ticks':'actuator_gripper_encoder_ticks',
# 'actuator_gripper_limit_min':'actuator_gripper_limit_min',
# 'actuator_gripper_limit_max':'actuator_gripper_limit_max',
# 'actuator_gripper_encoder_centre':'actuator_gripper_encoder_centre'},
transitions={
'calibrated': 'success',
'calibration_failed': 'error'
})
smach.Concurrence.add('WristAndGripperCalibration',
sm_wrist_gripper_calibration)
smach.StateMachine.add(
'MoveTorsoAndElbowUpAndCalibrateWristAndGripper',
sm_to_el_up_cal_wr_gr,
transitions={
'done': 'MoveShoulderHardMin',
'error': 'arm_calibration_failed'
})
'''
TODO:
try to add stay at hard limit to CalibrateActuator
then combine the below with FinishShoulderCalibration
'''
smach.StateMachine.add('MoveShoulderHardMin',
actuator.MoveActuator(),
transitions={
'success': 'FinishElbowCalibration',
'error': 'arm_calibration_failed'
},
remapping={
'actuator_name':
'actuator_shoulder_name',
'actuator_array_pos':
'actuator_shoulder_array_pos',
'actuator_max_current':
'actuator_shoulder_max_current',
'actuator_speed':
'actuator_shoulder_speed',
'actuator_direction':
'backward_direction',
'encoder_ticks':
'actuator_shoulder_encoder_ticks',
'actuator_goal_pos':
'actuator_shoulder_goal_pos',
'encoder_goal':
'encoder_goal_true',
'move_to_hard_limit':
'move_to_hard_limit_true'
})
smach.StateMachine.add('FinishElbowCalibration',
actuator.CalibrateActuator(),
transitions={
'success': 'MoveElbowAgain',
'error': 'arm_calibration_failed'
},
remapping={
'actuator_name':
'actuator_elbow_name',
'actuator_array_pos':
'actuator_elbow_array_pos',
'actuator_max_current':
'actuator_elbow_max_current',
'actuator_angle_ratio':
'actuator_elbow_angle_ratio',
'actuator_speed':
'actuator_elbow_speed',
'actuator_direction':
'forward_direction',
'encoder_ticks':
'actuator_elbow_encoder_ticks',
'actuator_limit_min':
'actuator_elbow_limit_min',
'actuator_limit_max':
'actuator_elbow_limit_max',
'actuator_encoder_centre':
'actuator_elbow_encoder_centre',
'move_to_zero':
'move_to_zero_false',
'get_one_limit_only':
'get_one_limit_only_true',
'complete_calibration':
'complete_calibration_true'
})
smach.StateMachine.add('MoveElbowAgain',
actuator.MoveActuator(),
transitions={
'success': 'FinishShoulderCalibration',
'error': 'arm_calibration_failed'
},
remapping={
'actuator_name':
'actuator_elbow_name',
'actuator_array_pos':
'actuator_elbow_array_pos',
'actuator_max_current':
'actuator_elbow_max_current',
'actuator_speed':
'actuator_elbow_speed',
'actuator_direction':
'backward_direction',
'encoder_ticks':
'actuator_elbow_encoder_ticks',
'actuator_goal_pos':
'actuator_elbow_goal_pos',
'encoder_goal':
'encoder_goal_false',
'move_to_hard_limit':
'move_to_hard_limit_false'
})
smach.StateMachine.add('FinishShoulderCalibration',
actuator.CalibrateActuator(),
transitions={
'success':
'MoveTorsoDownAndElbowToZero',
'error': 'arm_calibration_failed'
},
remapping={
'actuator_name':
'actuator_shoulder_name',
'actuator_array_pos':
'actuator_shoulder_array_pos',
'actuator_max_current':
'actuator_shoulder_max_current',
'actuator_angle_ratio':
'actuator_shoulder_angle_ratio',
'actuator_speed':
'actuator_shoulder_speed',
'actuator_direction':
'backward_direction',
'encoder_ticks':
'actuator_shoulder_encoder_ticks',
'actuator_limit_min':
'actuator_shoulder_limit_min',
'actuator_limit_max':
'actuator_shoulder_limit_max',
'actuator_encoder_centre':
'actuator_shoulder_encoder_centre',
'move_to_zero':
'move_to_zero_true',
'get_one_limit_only':
'get_one_limit_only_true',
'complete_calibration':
'complete_calibration_true'
})
'''
Move torso down and elbow to zero
'''
sm_torso_down_elbow_zero = smach.Concurrence(
outcomes=['moved', 'error'],
default_outcome='error',
outcome_map={
'moved': {
'MoveElbowToZero': 'success',
'MoveTorsoLiftDown': 'success'
},
'error': {
'MoveElbowToZero': 'error',
'MoveTorsoLiftDown': 'error'
}
},
input_keys=[
'forward_direction',
'backward_direction',
'encoder_goal_true',
'encoder_goal_false',
'move_to_hard_limit_false',
'actuator_elbow_name',
'actuator_elbow_array_pos',
'actuator_elbow_max_current',
'actuator_elbow_speed',
'actuator_elbow_encoder_ticks',
'actuator_elbow_encoder_centre',
'actuator_torso_lift_name',
'actuator_torso_lift_array_pos',
'actuator_torso_lift_max_current',
'actuator_torso_lift_speed',
'actuator_torso_lift_encoder_ticks',
])
sm_torso_down_elbow_zero.userdata.actuator_torso_lift_goal_pos = 0.0
with sm_torso_down_elbow_zero:
smach.Concurrence.add('MoveElbowToZero',
actuator.MoveActuator(),
remapping={
'actuator_name':
'actuator_elbow_name',
'actuator_array_pos':
'actuator_elbow_array_pos',
'actuator_max_current':
'actuator_elbow_max_current',
'actuator_speed':
'actuator_elbow_speed',
'actuator_direction':
'backward_direction',
'actuator_goal_pos':
'actuator_elbow_encoder_centre',
'encoder_ticks':
'actuator_elbow_encoder_ticks',
'encoder_goal':
'encoder_goal_true',
'move_to_hard_limit':
'move_to_hard_limit_false'
})
smach.Concurrence.add('MoveTorsoLiftDown',
actuator.MoveActuator(),
remapping={
'actuator_name':
'actuator_torso_lift_name',
'actuator_array_pos':
'actuator_torso_lift_array_pos',
'actuator_max_current':
'actuator_torso_lift_max_current',
'actuator_speed':
'actuator_torso_lift_speed',
'actuator_direction':
'backward_direction',
'actuator_goal_pos':
'actuator_torso_lift_goal_pos',
'encoder_ticks':
'actuator_torso_lift_encoder_ticks',
'encoder_goal':
'encoder_goal_false',
'move_to_hard_limit':
'move_to_hard_limit_false'
})
smach.StateMachine.add('MoveTorsoDownAndElbowToZero',
sm_torso_down_elbow_zero,
transitions={
'moved': 'ResetEncoderTorsoTurn',
'error': 'arm_calibration_failed'
})
smach.StateMachine.add(
'ResetEncoderTorsoTurn',
ServiceState(
'goo/reset_encoders',
goo_srvs.ResetEncoders,
request=goo_srvs.ResetEncodersRequest("torso_turn")),
transitions={
'succeeded': 'ResetEncoderShoulder',
'preempted': 'arm_calibration_failed',
'aborted': 'arm_calibration_failed',
})
smach.StateMachine.add(
'ResetEncoderShoulder',
ServiceState(
'goo/reset_encoders',
goo_srvs.ResetEncoders,
request=goo_srvs.ResetEncodersRequest("shoulder")),
transitions={
'succeeded': 'ResetEncoderElbow',
'preempted': 'arm_calibration_failed',
'aborted': 'arm_calibration_failed',
})
smach.StateMachine.add(
'ResetEncoderElbow',
ServiceState('goo/reset_encoders',
goo_srvs.ResetEncoders,
request=goo_srvs.ResetEncodersRequest("elbow")),
transitions={
'succeeded': 'ResetEncoderWrist',
'preempted': 'arm_calibration_failed',
'aborted': 'arm_calibration_failed',
})
smach.StateMachine.add(
'ResetEncoderWrist',
ServiceState('goo/reset_encoders',
goo_srvs.ResetEncoders,
request=goo_srvs.ResetEncodersRequest("wrist")),
transitions={
'succeeded': 'ResetEncoderGripper',
'preempted': 'arm_calibration_failed',
'aborted': 'arm_calibration_failed',
})
smach.StateMachine.add(
'ResetEncoderGripper',
ServiceState('goo/reset_encoders',
goo_srvs.ResetEncoders,
request=goo_srvs.ResetEncodersRequest("gripper")),
transitions={
'succeeded': 'arm_calibrated',
'preempted': 'arm_calibration_failed',
'aborted': 'arm_calibration_failed',
})
smach.StateMachine.add('ArmCalibration',
sm_arm,
transitions={
'arm_calibrated':
'WaitBeforeMoveArmToDefaultPose',
'arm_calibration_failed':
'calibration_failed'
})
smach.StateMachine.add('WaitBeforeMoveArmToDefaultPose',
misc_tools.Wait(),
remapping={'duration': 'wait_2sec'},
transitions={'done': 'MoveArmToDefaultPose'})
trajectory = trajectory_msgs.JointTrajectory()
trajectory.joint_names = [
"torso_turn", "torso_lift", "shoulder", "elbow", "wrist"
]
waypoint = trajectory_msgs.JointTrajectoryPoint()
# waypoint.positions = [0.0, 0.0, 1.57, -3.14, 1.57] when well calibrated
waypoint.positions = [0.0, 0.0, 1.56, -3.13,
1.57] # workaround after PID tuning
waypoint.velocities = [0.3] * len(trajectory.joint_names)
waypoint.accelerations = [0.0] * len(trajectory.joint_names)
trajectory.points.append(waypoint)
sm_arm_calibration.userdata.arm_default_pose_trajectory = trajectory
smach.StateMachine.add(
'MoveArmToDefaultPose',
SimpleActionState('arm_controller',
control_msgs.FollowJointTrajectoryAction,
goal_slots=['trajectory']),
remapping={'trajectory': 'arm_default_pose_trajectory'},
transitions={
'succeeded': 'calibrated',
'aborted': 'calibration_failed',
'preempted': 'calibration_failed'
})
return sm_arm_calibration