def __init__(self, check_designator):
"""
:param check_designator: designator resolving to True or False
"""
super(CheckBool, self).__init__(outcomes=["true", "false"])
ds.check_type(check_designator, bool)
self._check_designator = check_designator
def __init__(self,
robot,
sentence=None,
language=None,
personality=None,
voice=None,
mood=None,
block=True,
look_at_standing_person=False,
**place_holders):
"""
Constructor
State exits with 'spoken'.
:param robot: robot object
:type robot: Robot
:param sentence: Sentence to be spoken, can contain place holders to be filled in at runtime
:type sentence: str, [str], designator to str or [str]
:param language: Language of speech module
:type language: str
:param personality: Personality of speech module
:type personality: str
:param voice: Voice of speech module
:type voice: str
:param mood: Mood of speech module
:type mood: str
:param block: Wait for talking to be completed before returning, if true
:type block: bool
:param look_at_standing_person: Look at standing person if true, otherwise keep current head pose
:type look_at_standing_person: bool
:param place_holders: place holders to be filled in at runtime
:type place_holders: designator to str
"""
smach.State.__init__(self, outcomes=["spoken"])
ds.check_type(sentence, [str], str)
assert (isinstance(language, str) or isinstance(language, type(None)))
assert (isinstance(personality, str)
or isinstance(personality, type(None)))
assert (isinstance(voice, str) or isinstance(voice, type(None)))
assert (isinstance(mood, str) or isinstance(mood, type(None)))
assert (isinstance(block, bool))
assert (all(
isinstance(v, ds.Designator) for v in place_holders.values()))
self.ph_designators = place_holders
if isinstance(sentence, str) or isinstance(sentence, list):
self._check_place_holders(sentence)
self.robot = robot
self.sentence = sentence
self.language = language
self.personality = personality
self.voice = voice
self.mood = mood
self.block = block
self.look_at_standing_person = look_at_standing_person
def __init__(self, robot, item, arm):
"""
Let the given robot move to an entity and grab that entity using some arm
:param robot: Robot to use
:param item: Designator that resolves to the item to grab. E.g. EntityByIdDesignator
:param arm: Designator that resolves to the arm to use for grabbing. Eg. UnoccupiedArmDesignator
:return:
"""
smach.StateMachine.__init__(self, outcomes=['done', 'failed'])
# Check types or designator resolve types
check_type(item, Entity)
check_type(arm, PublicArm)
with self:
smach.StateMachine.add('NAVIGATE_TO_GRAB', NavigateToGrasp(robot, item, arm),
transitions={'unreachable': 'RESET_FAILURE',
'goal_not_defined': 'RESET_FAILURE',
'arrived': 'PREPARE_GRASP'})
smach.StateMachine.add('PREPARE_GRASP', PrepareEdGrasp(robot, arm, item),
transitions={'succeeded': 'GRAB',
'failed': 'RESET_FAILURE'})
smach.StateMachine.add('GRAB', PickUp(robot, arm, item),
transitions={'succeeded': 'done',
'failed': 'RESET_FAILURE'})
smach.StateMachine.add("RESET_FAILURE", ResetOnFailure(robot, arm),
transitions={'done': 'failed'})
def __init__(self, robot, arm_designator, grabbed_entity_label="", grabbed_entity_designator=None, timeout=10, arm_configuration="handover_to_human"):
"""
Hold up hand to accept an object and close hand once something is inserted
:param robot: Robot with which to execute this behavior
:param arm_designator: ArmDesignator resolving to arm accept item into
:param grabbed_entity_label: What ID to give a dummy item in case no grabbed_entity_designator is supplied
:param grabbed_entity_designator: EntityDesignator resolving to the accepted item. Can be a dummy
:param timeout: How long to hold hand over before closing without anything
:param arm_configuration: Which pose to put arm in when holding hand up for the item.
"""
smach.StateMachine.__init__(self, outcomes=['succeeded','failed','timeout'])
check_type(arm_designator, PublicArm)
if not grabbed_entity_designator and grabbed_entity_label == "":
rospy.logerr("No grabbed entity label or grabbed entity designator given")
with self:
smach.StateMachine.add("POSE", ArmToJointConfig(robot, arm_designator, arm_configuration),
transitions={'succeeded':'OPEN_BEFORE_INSERT','failed':'OPEN_BEFORE_INSERT'})
smach.StateMachine.add( 'OPEN_BEFORE_INSERT', SetGripper(robot, arm_designator, gripperstate=GripperState.OPEN),
transitions={'succeeded' : 'SAY1',
'failed' : 'SAY1'})
smach.StateMachine.add("SAY1", Say(robot,'Please hand over the object by pushing it gently in my gripper'),
transitions={'spoken':'CLOSE_AFTER_INSERT'})
smach.StateMachine.add( 'CLOSE_AFTER_INSERT', CloseGripperOnHandoverToRobot(robot,
arm_designator,
grabbed_entity_label=grabbed_entity_label,
grabbed_entity_designator=grabbed_entity_designator,
timeout=timeout),
transitions={'succeeded' : 'succeeded',
'timeout' : 'timeout',
'failed' : 'failed'})
def __init__(self, robot, arm_designator, grabbed_entity_label="", grabbed_entity_designator=None, timeout=10):
smach.StateMachine.__init__(self, outcomes=['succeeded','failed'])
check_type(arm_designator, Arm)
if not grabbed_entity_designator and grabbed_entity_label == "":
rospy.logerr("No grabbed entity label or grabbed entity designator given")
with self:
smach.StateMachine.add("POSE", ArmToJointConfig(robot, arm_designator, "handover_to_human"),
transitions={'succeeded':'OPEN_BEFORE_INSERT','failed':'OPEN_BEFORE_INSERT'})
smach.StateMachine.add( 'OPEN_BEFORE_INSERT', SetGripper(robot, arm_designator, gripperstate='open'),
transitions={'succeeded' : 'SAY1',
'failed' : 'SAY1'})
smach.StateMachine.add("SAY1", Say(robot,'Please hand over the object by pushing it gently in my gripper'),
transitions={'spoken':'CLOSE_AFTER_INSERT'})
smach.StateMachine.add( 'CLOSE_AFTER_INSERT', CloseGripperOnHandoverToRobot(robot,
arm_designator,
grabbed_entity_label=grabbed_entity_label,
grabbed_entity_designator=grabbed_entity_designator,
timeout=timeout),
transitions={'succeeded' : 'succeeded',
'failed' : 'failed'})
def __init__(self, robot, arm_designator, grabbed_entity_label="", grabbed_entity_designator=None, timeout=10, arm_configuration="handover_to_human"):
"""
Hold up hand to accept an object and close hand once something is inserted
:param robot: Robot with which to execute this behavior
:param arm_designator: ArmDesignator resolving to arm accept item into
:param grabbed_entity_label: What ID to give a dummy item in case no grabbed_entity_designator is supplied
:param grabbed_entity_designator: EntityDesignator resolving to the accepted item. Can be a dummy
:param timeout: How long to hold hand over before closing without anything
:param arm_configuration: Which pose to put arm in when holding hand up for the item.
"""
smach.StateMachine.__init__(self, outcomes=['succeeded','failed','timeout'])
check_type(arm_designator, Arm)
if not grabbed_entity_designator and grabbed_entity_label == "":
rospy.logerr("No grabbed entity label or grabbed entity designator given")
with self:
smach.StateMachine.add("POSE", ArmToJointConfig(robot, arm_designator, arm_configuration),
transitions={'succeeded':'OPEN_BEFORE_INSERT','failed':'OPEN_BEFORE_INSERT'})
smach.StateMachine.add( 'OPEN_BEFORE_INSERT', SetGripper(robot, arm_designator, gripperstate=GripperState.OPEN),
transitions={'succeeded' : 'SAY1',
'failed' : 'SAY1'})
smach.StateMachine.add("SAY1", Say(robot,'Please hand over the object by pushing it gently in my gripper'),
transitions={'spoken':'CLOSE_AFTER_INSERT'})
smach.StateMachine.add( 'CLOSE_AFTER_INSERT', CloseGripperOnHandoverToRobot(robot,
arm_designator,
grabbed_entity_label=grabbed_entity_label,
grabbed_entity_designator=grabbed_entity_designator,
timeout=timeout),
transitions={'succeeded' : 'succeeded',
'timeout' : 'timeout',
'failed' : 'failed'})
def __init__(self, robot, arm_designator, point_entity_designator):
smach.State.__init__(self, outcomes=['point_succeeded','point_failed','target_lost'])
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.robot = robot
self.point_entity_designator = point_entity_designator
def __init__(self, robot, item, arm):
"""
Let the given robot move to an entity and grab that entity using some arm
:param robot: Robot to use
:param item: Designator that resolves to the item to grab. E.g. EntityByIdDesignator
:param arm: Designator that resolves to the arm to use for grabbing. Eg. UnoccupiedArmDesignator
:return:
"""
smach.StateMachine.__init__(self, outcomes=['done', 'failed'])
# Check types or designator resolve types
check_type(item, Entity)
check_type(arm, Arm)
with self:
smach.StateMachine.add('PREPARE_GRASP', PrepareEdGrasp(robot, arm, item),
transitions={'succeeded': 'NAVIGATE_TO_GRAB',
'failed': 'RESET_FAILURE'})
smach.StateMachine.add('NAVIGATE_TO_GRAB', NavigateToGrasp(robot, item, arm),
transitions={'unreachable': 'RESET_FAILURE',
'goal_not_defined': 'RESET_FAILURE',
'arrived': 'GRAB'})
smach.StateMachine.add('GRAB', PickUp(robot, arm, item),
transitions={'succeeded': 'done',
'failed': 'RESET_FAILURE'})
smach.StateMachine.add("RESET_FAILURE", ResetOnFailure(robot),
transitions={'done': 'failed'})
def __init__(self,
robot,
sentence=None,
language=None,
personality=None,
voice=None,
mood=None,
block=True,
look_at_standing_person=False):
smach.State.__init__(self, outcomes=["spoken"])
ds.check_type(sentence, str, list)
#ds.check_type(language, str)
#ds.check_type(personality, str)
#ds.check_type(voice, str)
#ds.check_type(mood, str)
ds.check_type(block, bool)
self.robot = robot
self.sentence = sentence
self.language = language
self.personality = personality
self.voice = voice
self.mood = mood
self.block = block
self.look_at_standing_person = look_at_standing_person
def __init__(self, robot, arm_designator, drop_bag_pose):
"""
:param robot: the robot with which to execute this state machine
:param arm_designator: ArmDesignator resolving to Arm holding the bag to drop
"""
smach.StateMachine.__init__(self, outcomes=['done'])
check_type(arm_designator, Arm)
with self:
smach.StateMachine.add('DROP_POSE',
states.ArmToJointConfig(
robot, arm_designator, drop_bag_pose),
transitions={
'succeeded': 'OPEN_AFTER_DROP',
'failed': 'OPEN_AFTER_DROP'
})
smach.StateMachine.add('OPEN_AFTER_DROP',
states.SetGripper(
robot,
arm_designator,
gripperstate=GripperState.OPEN),
transitions={
'succeeded': 'RESET_ARM',
'failed': 'RESET_ARM'
})
smach.StateMachine.add('RESET_ARM',
states.ResetArms(robot),
transitions={'done': 'done'})
def __init__(self, check_designator):
"""
:param check_designator: boolean designator to be toggled
"""
super(ToggleBool, self).__init__(outcomes=["done"])
ds.is_writeable(check_designator)
ds.check_type(check_designator, bool)
self._check_designator = check_designator
def __init__(self, unavailable_drink_list_designator, drink_to_add_designator):
super(UpdateUnavailableDrinkList, self).__init__(outcomes=["done", "failed"])
ds.is_writeable(unavailable_drink_list_designator)
ds.check_type(unavailable_drink_list_designator, [str])
ds.check_type(drink_to_add_designator, str)
self.unavailable_drink_list_designator = unavailable_drink_list_designator
self.drink_to_add_designator = drink_to_add_designator
def __init__(self, robot, arm_designator, grab_entity_designator):
# Similar to PrepareGrasp but has a more elaborate joint trajectory to avoid hitting the table when standing close to it
smach.State.__init__(self, outcomes=['succeeded','failed'])
self.robot = robot
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.grab_entity_designator = grab_entity_designator
def __init__(self, robot, arm_designator, point_entity_designator):
smach.State.__init__(
self, outcomes=['point_succeeded', 'point_failed', 'target_lost'])
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.robot = robot
self.point_entity_designator = point_entity_designator
def __init__(self, robot, arm_designator, grab_entity_designator):
# Similar to PrepareGrasp but has a more elaborate joint trajectory to avoid hitting the table when standing close to it
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self.robot = robot
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.grab_entity_designator = grab_entity_designator
def __init__(self, robot, item, arm=None):
"""
Let the given robot move to an entity and grab that entity using some arm.
Supervisory implementation with arm reservation and choosing.
:param robot: Robot to use
:param item: Designator that resolves to the item to grab.
E.g. EntityByIdDesignator
:param arm: Designator that resolves to the arm to use for grabbing.
Eg. UnoccupiedArmDesignator
:return:
"""
smach.StateMachine.__init__(self, outcomes=['done', 'failed'])
# Check types or designator resolve types
check_type(item, Entity)
if arm is not None:
check_type(arm, Arm)
if arm.locked:
rospy.logwarn('Prefered arm in use picking other arm')
arm = pickClosestArm(item)
else:
arm = pickClosestArm(item)
if arm is None:
rospy.logerr('No free arm for grabSU')
return 'failed'
with self:
smach.StateMachine.add(
'CHECK_IN_WORKSPACE', # better call this init
CheckObjectInWS(robot, arm, item),
transitions={
'inWS': 'PREPARE_GRASP',
'notInWs': 'MOVE_ARM',
'failed': 'RESET_FAILURE'
})
smach.StateMachine.add('MOVE_ARM_TO_OBJECT',
MoveArmToObjectSU(robot, arm, item),
transitions={'succes'})
smach.StateMachine.add('PREPARE_GRASP',
PrepareEdGraspSU(robot, arm, item),
transitions={
'succeeded': 'NAVIGATE_TO_GRAB',
'failed': 'RESET_FAILURE'
})
smach.StateMachine.add('GRAB',
PickUpSU(robot, arm, item),
transitions={
'succeeded': 'done',
'failed': 'RESET_FAILURE'
})
smach.StateMachine.add("RESET_FAILURE",
ResetOnFailure(robot),
transitions={'done': 'failed'})
def __init__(self, robot, arm_designator, gripperstate='open', drop_from_frame=None, grab_entity_designator=None, timeout=10):
smach.State.__init__(self, outcomes=['succeeded','failed'])
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.robot = robot
self.gripperstate = gripperstate
self.grab_entity_designator = grab_entity_designator
self.timeout = timeout
def __init__(self, robot, arm_designator, timeout=10):
smach.StateMachine.__init__(self, outcomes=['succeeded','failed'])
#A designator can resolve to a different item every time its resolved. We don't want that here, so lock
check_type(arm_designator, Arm)
locked_arm = LockingDesignator(arm_designator)
with self:
smach.StateMachine.add("LOCK_ARM",
LockDesignator(locked_arm),
transitions={'locked' :'SPINDLE_MEDIUM'})
smach.StateMachine.add("SPINDLE_MEDIUM",
ResetTorso(robot),
transitions={'done' :'MOVE_HUMAN_HANDOVER_JOINT_GOAL'})
smach.StateMachine.add("MOVE_HUMAN_HANDOVER_JOINT_GOAL",
ArmToJointConfig(robot, locked_arm, 'handover_to_human'),
transitions={ 'succeeded' :'SAY_OPEN_GRIPPER',
'failed' :'SAY_OPEN_GRIPPER'})
smach.StateMachine.add("SAY_OPEN_GRIPPER",
Say(robot, [ "Watch out, I will open my gripper in one second. Please take it from me."]),
transitions={ 'spoken' :'OPEN_GRIPPER_HANDOVER'})
smach.StateMachine.add('OPEN_GRIPPER_HANDOVER', SetGripper(robot, locked_arm, gripperstate=ArmState.OPEN, timeout=2.0),
transitions={'succeeded' : 'SAY_CLOSE_NOW_GRIPPER',
'failed' : 'SAY_CLOSE_NOW_GRIPPER'})
smach.StateMachine.add("SAY_CLOSE_NOW_GRIPPER",
Say(robot, [ "I will close my gripper now"]),
transitions={ 'spoken' :'CLOSE_GRIPPER_HANDOVER'})
# smach.StateMachine.add('OPEN_GRIPPER_ON_HANDOVER', OpenGripperOnHandoverToHuman(robot, locked_arm, timeout=timeout),
# transitions={'succeeded' : 'CLOSE_GRIPPER_HANDOVER',
# 'failed' : 'SAY_I_WILL_KEEP_IT'})
smach.StateMachine.add('CLOSE_GRIPPER_HANDOVER', SetGripper(robot, locked_arm, gripperstate=ArmState.CLOSE, timeout=0.0),
transitions={'succeeded' : 'RESET_ARM',
'failed' : 'RESET_ARM'})
# smach.StateMachine.add("SAY_I_WILL_KEEP_IT",
# Say(robot, [ "If you don't want it, I will keep it"]),
# transitions={ 'spoken' :'RESET_ARM'})
smach.StateMachine.add('RESET_ARM',
ArmToJointConfig(robot, locked_arm, 'reset'),
transitions={'succeeded' :'RESET_TORSO',
'failed' :'RESET_TORSO' })
smach.StateMachine.add('RESET_TORSO',
ResetTorso(robot),
transitions={'done':'UNLOCK_ARM'})
smach.StateMachine.add("UNLOCK_ARM",
UnlockDesignator(locked_arm),
transitions={'unlocked' :'succeeded'})
def __init__(self, robot, arm_designator, timeout=10):
smach.StateMachine.__init__(self, outcomes=['succeeded','failed'])
#A designator can resolve to a different item every time its resolved. We don't want that here, so lock
check_type(arm_designator, Arm)
locked_arm = LockingDesignator(arm_designator)
with self:
smach.StateMachine.add("LOCK_ARM",
LockDesignator(locked_arm),
transitions={'locked' :'SPINDLE_MEDIUM'})
smach.StateMachine.add("SPINDLE_MEDIUM",
ResetPart(robot, robot.torso),
transitions={'done' :'MOVE_HUMAN_HANDOVER_JOINT_GOAL'})
smach.StateMachine.add("MOVE_HUMAN_HANDOVER_JOINT_GOAL",
ArmToJointConfig(robot, locked_arm, 'handover_to_human'),
transitions={ 'succeeded' :'SAY_OPEN_GRIPPER',
'failed' :'SAY_OPEN_GRIPPER'})
smach.StateMachine.add("SAY_OPEN_GRIPPER",
Say(robot, [ "Watch out, I will open my gripper in one second. Please take it from me."]),
transitions={ 'spoken' :'OPEN_GRIPPER_HANDOVER'})
smach.StateMachine.add('OPEN_GRIPPER_HANDOVER', SetGripper(robot, locked_arm, gripperstate=GripperState.OPEN, timeout=2.0),
transitions={'succeeded' : 'SAY_CLOSE_NOW_GRIPPER',
'failed' : 'SAY_CLOSE_NOW_GRIPPER'})
smach.StateMachine.add("SAY_CLOSE_NOW_GRIPPER",
Say(robot, [ "I will close my gripper now"]),
transitions={ 'spoken' :'CLOSE_GRIPPER_HANDOVER'})
# smach.StateMachine.add('OPEN_GRIPPER_ON_HANDOVER', OpenGripperOnHandoverToHuman(robot, locked_arm, timeout=timeout),
# transitions={'succeeded' : 'CLOSE_GRIPPER_HANDOVER',
# 'failed' : 'SAY_I_WILL_KEEP_IT'})
smach.StateMachine.add('CLOSE_GRIPPER_HANDOVER', SetGripper(robot, locked_arm, gripperstate=GripperState.CLOSE, timeout=0.0),
transitions={'succeeded' : 'RESET_ARM',
'failed' : 'RESET_ARM'})
# smach.StateMachine.add("SAY_I_WILL_KEEP_IT",
# Say(robot, [ "If you don't want it, I will keep it"]),
# transitions={ 'spoken' :'RESET_ARM'})
smach.StateMachine.add('RESET_ARM',
ArmToJointConfig(robot, locked_arm, 'reset'),
transitions={'succeeded' :'RESET_TORSO',
'failed' :'RESET_TORSO' })
smach.StateMachine.add('RESET_TORSO',
ResetPart(robot, robot.torso),
transitions={'done':'UNLOCK_ARM'})
smach.StateMachine.add("UNLOCK_ARM",
UnlockDesignator(locked_arm),
transitions={'unlocked' :'succeeded'})
def __init__(self, robot, arm_designator, gripperstate=GripperState.OPEN, drop_from_frame=None, grab_entity_designator=None, timeout=10):
smach.State.__init__(self, outcomes=['succeeded','failed'])
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.robot = robot
self.gripperstate = gripperstate
self.grab_entity_designator = grab_entity_designator
self.timeout = timeout
def __init__(self, message_type, drink_request_des, operator_name_des, name=None):
super(DescriptionStrDesignator, self).__init__(resolve_type=str, name=name)
ds.check_type(operator_name_des, str)
ds.check_type(drink_request_des, str)
self.message_type = message_type
self.operator_name_des = operator_name_des
self.drink_request_des = drink_request_des
def __init__(self, robot, seat_ids, room, name=None):
super(SeatsInRoomDesignator, self).__init__(resolve_type=[Entity],
name=name)
self.robot = robot
ds.check_type(seat_ids, [str])
ds.check_type(room, Entity)
self.room = room
self.seat_ids = seat_ids
def __init__(self, robot, entity, area, waittime=2.0):
""" Constructor
:param robot: robot object
:param entity: EdEntityDesignator with the area to look at
:param area: string with the area to look at
:param waittime: (optional) waittime (in seconds) between giving a head target and returning
:return:
"""
ds.check_type(entity, EntityInfo)
State.__init__(self, locals(), outcomes=['succeeded', 'failed'])
def __init__(self, robot, entity, area, waittime=2.0):
""" Constructor
:param robot: robot object
:param entity: EdEntityDesignator with the area to look at
:param area: string with the area to look at
:param waittime: (optional) waittime (in seconds) between giving a head target and returning
:return:
"""
ds.check_type(entity, EntityInfo)
State.__init__(self, locals(), outcomes=['succeeded', 'failed'])
def __init__(self, objects, classification_list_designator, unavailable_drink_designator, max_unavailable_drinks):
# TODO: Change unavailable_drink_designator resolve type to list to make the implementation more generalized as
# this state becomes a bug when max_unavailable_drinks > 1
super(IdentifyUnavailableDrinkFromRecognitions, self).__init__(outcomes=["done", "failed"])
ds.is_writeable(unavailable_drink_designator)
ds.check_type(unavailable_drink_designator, str)
ds.check_type(classification_list_designator, [ClassificationResult])
self._drinks_list = [obj["name"] for obj in objects if obj["category"] == "drink"]
self._classification_list_designator = classification_list_designator
self._unavailable_drink_designator = unavailable_drink_designator
self._max_unavailable_drinks = max_unavailable_drinks
def __init__(self, robot, arm_designator, configuration):
"""
Put arm of robot in some joint configuration
:param robot: robot to execute state with
:param arm_designator: designator that resolves to arm to put in given configuration
:param configuration: joint configuration to put arm in
"""
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self.robot = robot
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.configuration = configuration
def __init__(self, robot, arm_designator, configuration):
"""
Put arm of robot in some joint configuration
:param robot: robot to execute state with
:param arm_designator: designator that resolves to arm to put in given configuration
:param configuration: joint configuration to put arm in
"""
smach.State.__init__(self, outcomes=['succeeded','failed'])
self.robot = robot
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.configuration = configuration
def __init__(self, robot, arm_designator, trajectory):
"""
Make arm of robot follow a joint trajectory
:param robot: robot to execute state with
:param arm_designator: designator that resolves to arm that must follow the given joint trajectory
:param trajectory: joint trajectory for the arm
"""
smach.State.__init__(self, outcomes=['succeeded','failed'])
self.robot = robot
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.trajectory = trajectory
def __init__(self, robot, arm_designator, trajectory):
"""
Make arm of robot follow a joint trajectory
:param robot: robot to execute state with
:param arm_designator: designator that resolves to arm that must follow the given joint trajectory
:param trajectory: joint trajectory for the arm
"""
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self.robot = robot
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.trajectory = trajectory
def __init__(self, robot, arm_designator, timeout=10):
"""
Handover the object in arm to a human.
:param robot: robot to execute state with
:param arm_designator: designator that resolves to arm holding an object
:param timeout: float time the operation may take
"""
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self.robot = robot
check_type(arm_designator, PublicArm)
self.arm_designator = arm_designator
self.timeout = timeout
def __init__(self, robot, room, found_people_designator,
look_range=(-np.pi/2, np.pi/2),
look_steps=8):
"""
Constructor
:param robot: robot object
:param area: (str) if a waypoint "<area>_waypoint" is present in the world model, the robot will navigate
to this waypoint. Else, it will navigate to the room called "<area>"
:param name: (str) Name of the person to look for
:param discard_other_labels: (bool) Whether or not to discard faces based on label
:param found_person_designator: (Designator) A designator that will resolve to the found object
"""
smach.StateMachine.__init__(self, outcomes=["found", "not_found"])
waypoint_designator = ds.EntityByIdDesignator(robot=robot, id=room + "_waypoint")
room_designator = ds.EntityByIdDesignator(robot=robot, id=room)
ds.check_type(found_people_designator, [Entity])
ds.is_writeable(found_people_designator)
with self:
smach.StateMachine.add("DECIDE_NAVIGATE_STATE",
_DecideNavigateState(robot=robot, waypoint_designator=waypoint_designator,
room_designator=room_designator),
transitions={"waypoint": "NAVIGATE_TO_WAYPOINT",
"room": "NAVIGATE_TO_ROOM",
"none": "not_found"})
smach.StateMachine.add("NAVIGATE_TO_WAYPOINT",
states.NavigateToWaypoint(robot=robot,
waypoint_designator=waypoint_designator, radius=0.15),
transitions={"arrived": "FIND_PEOPLE",
"unreachable": "not_found",
"goal_not_defined": "not_found"})
smach.StateMachine.add("NAVIGATE_TO_ROOM", states.NavigateToRoom(robot=robot,
entity_designator_room=room_designator),
transitions={"arrived": "FIND_PEOPLE",
"unreachable": "not_found",
"goal_not_defined": "not_found"})
# Wait for the operator to appear and detect what he's pointing at
smach.StateMachine.add("FIND_PEOPLE",
FindPeople(robot=robot,
query_entity_designator=room_designator,
found_people_designator=found_people_designator,
speak=True,
look_range=look_range,
look_steps=look_steps),
transitions={"found": "found",
"failed": "not_found"})
def __init__(self,
robot,
arm_designator,
grabbed_entity_label="",
grabbed_entity_designator=None,
timeout=10):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
check_type(arm_designator, Arm)
if not grabbed_entity_designator and grabbed_entity_label == "":
rospy.logerr(
"No grabbed entity label or grabbed entity designator given")
with self:
smach.StateMachine.add("POSE",
ArmToJointConfig(robot, arm_designator,
"handover_to_human"),
transitions={
'succeeded': 'OPEN_BEFORE_INSERT',
'failed': 'OPEN_BEFORE_INSERT'
})
smach.StateMachine.add('OPEN_BEFORE_INSERT',
SetGripper(robot,
arm_designator,
gripperstate='open'),
transitions={
'succeeded': 'SAY1',
'failed': 'SAY1'
})
smach.StateMachine.add(
"SAY1",
Say(
robot,
'Please hand over the object by pushing it gently in my gripper'
),
transitions={'spoken': 'CLOSE_AFTER_INSERT'})
smach.StateMachine.add(
'CLOSE_AFTER_INSERT',
CloseGripperOnHandoverToRobot(
robot,
arm_designator,
grabbed_entity_label=grabbed_entity_label,
grabbed_entity_designator=grabbed_entity_designator,
timeout=timeout),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
def __init__(self, robot, arm_designator, grabbed_entity_label="", grabbed_entity_designator=None, timeout=15,
arm_configuration="handover_to_human"):
"""
Hold up hand to accept an object and close hand once something is inserted
:param robot: Robot with which to execute this behavior
:param arm_designator: ArmDesignator resolving to arm accept item into
:param grabbed_entity_label: What ID to give a dummy item in case no grabbed_entity_designator is supplied
:param grabbed_entity_designator: EntityDesignator resolving to the accepted item. Can be a dummy
:param timeout: How long to hold hand over before closing without anything
:param arm_configuration: Which pose to put arm in when holding hand up for the item.
"""
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed', 'timeout'])
ds.check_type(arm_designator, PublicArm)
if not grabbed_entity_designator and grabbed_entity_label == "":
rospy.logerr("No grabbed entity label or grabbed entity designator given")
with self:
smach.StateMachine.add("POSE", manipulation.ArmToJointConfig(robot, arm_designator, arm_configuration),
transitions={'succeeded': 'OPEN_BEFORE_INSERT', 'failed': 'OPEN_BEFORE_INSERT'})
smach.StateMachine.add('OPEN_BEFORE_INSERT', manipulation.SetGripper(robot=robot,
arm_designator=arm_designator,
gripperstate=manipulation.GripperState.
OPEN),
transitions={'succeeded': 'SAY1', 'failed': 'SAY1'})
smach.StateMachine.add("SAY1", states.Say(robot, 'Please hand over the trash by putting the top of the bag'
' between my grippers and push firmly into my camera as'
' will be shown on my screen.'),
transitions={'spoken': 'SHOW_IMAGE'})
smach.StateMachine.add("SHOW_IMAGE",
states.ShowImageState(
robot=robot,
image_filename="~/ros/kinetic/system/src/challenge_take_out_the_garbage/src"
"/challenge_take_out_the_garbage/beun_picture.png",
seconds=5),
transitions={'succeeded': 'CLOSE_AFTER_INSERT'})
smach.StateMachine.add('CLOSE_AFTER_INSERT', manipulation.CloseGripperOnHandoverToRobot(
robot,
arm_designator,
grabbed_entity_label=grabbed_entity_label,
grabbed_entity_designator=grabbed_entity_designator,
timeout=timeout),
transitions={'succeeded': 'succeeded',
'timeout': 'failed',
'failed': 'failed'})
def __init__(self, robot, arm):
"""
Drive the robot back a little and move the designated arm to place the designated item at the designated pose
:param robot: Robot to execute state with
:param arm: Designator -> arm to place with, so Arm that holds entity_to_place, e.g. via
ArmHoldingEntityDesignator
"""
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
# Check types or designator resolve types
check_type(arm, PublicArm)
# Assign member variables
self._robot = robot
self._arm_designator = arm
def __init__(self, robot, arm, grab_entity):
"""
Set the arm in the appropriate position before actually grabbing
:param robot: robot to execute state with
:param arm: Designator that resolves to arm to grab with. E.g. UnoccupiedArmDesignator
:param grab_entity: Designator that resolves to the entity to grab. e.g EntityByIdDesignator
"""
smach.State.__init__(self, outcomes=['succeeded', 'failed'], output_keys=['arm'])
# Assign member variables
self.robot = robot
self.arm_designator = arm
self.grab_entity_designator = grab_entity
check_type(grab_entity, Entity)
def __init__(self, robot, arm_designator, grab_point_designator):
"""
Grab an entity using visual servoing.
:param robot: Robot to use for grabbing
:param arm_designator: Designator that resolves to arm to grab with. E.g. UnoccupiedArmDesignator
:param grab_point_designator: Designator that resolves to a PointStamped
:return:
"""
smach.State.__init__(self, outcomes=['succeeded','failed','target_lost'])
self.robot = robot
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.grab_point_designator = grab_point_designator
def __init__(self, robot, arm, grab_entity):
"""
Pick up an item given an arm and an entity to be picked up
:param robot: robot to execute this state with
:param arm: Designator that resolves to the arm to grab the grab_entity with. E.g. UnoccupiedArmDesignator
:param grab_entity: Designator that resolves to the entity to grab. e.g EntityByIdDesignator
"""
smach.State.__init__(self, outcomes=['succeeded', 'failed'], output_keys=['arm'])
# Assign member variables
self.robot = robot
self.arm_designator = arm
check_type(grab_entity, Entity)
self.grab_entity_designator = grab_entity
self._gpd = GraspPointDeterminant(robot)
def __init__(self, robot, arm, point_entity_designator):
"""
Points at an item, similar to picking it up.
:param robot: robot to execute this state with
:param arm: Designator that resolves to the arm to point at the entity with. E.g. UnoccupiedArmDesignator
:param point_entity_designator: Designator that resolves to the entity to point at. e.g EntityByIdDesignator
"""
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
# Assign member variables
self.robot = robot
self.arm_designator = arm
ds.check_type(point_entity_designator, Entity)
self.point_entity_designator = point_entity_designator
def __init__(self, robot, arm, grab_entity):
# type: (Robot, ArmDesignator, Designator) -> None
"""
Set the arm in the appropriate position before actually grabbing
:param robot: robot to execute state with
:param arm: Designator that resolves to arm to grab with. E.g. UnoccupiedArmDesignator
:param grab_entity: Designator that resolves to the entity to grab. e.g EntityByIdDesignator
"""
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
# Assign member variables
self.robot = robot
self.arm_designator = arm
self.grab_entity_designator = grab_entity
check_type(grab_entity, Entity)
def __init__(self, knowledge, room_des, cleanup_locations):
"""
Determine cleaning locations on runtime
:param knowledge: challenge knowledge
:param room_des: Designator resolving to room
:type room_des: Designator(str)
:param cleanup_locations: Writable designator, which will be filled with a list of cleaning locations
"""
smach.State.__init__(self, outcomes=["done"])
ds.check_type(room_des, str)
ds.is_writeable(cleanup_locations)
assert (hasattr(knowledge, "cleaning_locations"))
self._knowledge = knowledge
self._room_des = room_des
self._cleanup_locations = cleanup_locations
def __init__(self, robot, arm_designator, grab_point_designator):
"""
Grab an entity using visual servoing.
:param robot: Robot to use for grabbing
:param arm_designator: Designator that resolves to arm to grab with. E.g. UnoccupiedArmDesignator
:param grab_point_designator: Designator that resolves to a PointStamped
:return:
"""
smach.State.__init__(self,
outcomes=['succeeded', 'failed', 'target_lost'])
self.robot = robot
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.grab_point_designator = grab_point_designator
def __init__(self, robot, arm, grab_entity, check_occupancy=False):
"""
Pick up an item given an arm and an entity to be picked up
:param robot: robot to execute this state with
:param arm: Designator that resolves to the arm to grab the grab_entity with. E.g. UnoccupiedArmDesignator
:param grab_entity: Designator that resolves to the entity to grab. e.g EntityByIdDesignator
"""
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
# Assign member variables
self.robot = robot
self.arm_designator = arm
check_type(grab_entity, Entity)
self.grab_entity_designator = grab_entity
self._gpd = GraspPointDeterminant(robot)
self._check_occupancy = check_occupancy
def __init__(self, robot, sentence=None, language=None, personality=None, voice=None, mood=None, block=True, look_at_standing_person=False):
smach.State.__init__(self, outcomes=["spoken"])
ds.check_type(sentence, str, list)
#ds.check_type(language, str)
#ds.check_type(personality, str)
#ds.check_type(voice, str)
#ds.check_type(mood, str)
ds.check_type(block, bool)
self.robot = robot
self.sentence = sentence
self.language = language
self.personality = personality
self.voice = voice
self.mood = mood
self.block = block
self.look_at_standing_person = look_at_standing_person
def __init__(self, robot, placement_pose, arm):
"""
Drive the robot back a little and move the designated arm to place the designated item at the designated pose
:param robot: Robot to execute state with
:param placement_pose: Designator that resolves to the pose to place at. E.g. an EmptySpotDesignator
:param arm: Designator -> arm to place with, so Arm that holds entity_to_place, e.g. via ArmHoldingEntityDesignator
"""
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
# Check types or designator resolve types
check_type(placement_pose, PoseStamped)
check_type(arm, Arm)
# Assign member variables
self._robot = robot
self._placement_pose_designator = placement_pose
self._arm_designator = arm
def __init__(self, robot, placement_pose, arm):
"""
Drive the robot back a little and move the designated arm to place the designated item at the designated pose
:param robot: Robot to execute state with
:param placement_pose: Designator that resolves to the pose to place at. E.g. an EmptySpotDesignator
:param arm: Designator -> arm to place with, so Arm that holds entity_to_place, e.g. via ArmHoldingEntityDesignator
"""
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
# Check types or designator resolve types
check_type(placement_pose, PoseStamped)
check_type(arm, Arm)
# Assign member variables
self._robot = robot
self._placement_pose_designator = placement_pose
self._arm_designator = arm
def __init__(self, robot, arm_designator, grab_entity_designator):
"""@param grab_entity_designator resolves to an entity to grab.
Some child states require a (designator of) the PointStamped of that entity. PointStampedOfEntityDesignator does this conversion"""
smach.StateMachine.__init__(self, outcomes=['succeeded','failed'])
check_type(arm_designator, Arm)
# check check input and output keys
with self:
smach.StateMachine.add('PREPARE_GRAB', ArmToJointConfig(robot, arm_designator, "prepare_grasp"),
transitions={'succeeded' : 'OPEN_GRIPPER',
'failed' : 'failed'})
smach.StateMachine.add('OPEN_GRIPPER', SetGripper(robot, arm_designator, gripperstate=ArmState.OPEN, timeout=0.0),
transitions={'succeeded' : 'PREPARE_ORIENTATION',
'failed' : 'PREPARE_ORIENTATION'})
smach.StateMachine.add('PRE_GRASP', ArmToQueryPoint(robot, arm_designator,
PointStampedOfEntityDesignator(grab_entity_designator),
time_out=20, pre_grasp=True, first_joint_pos_only=True),
transitions={'succeeded' : 'GRAB',
'failed' : 'CLOSE_GRIPPER_UPON_FAIL'})
smach.StateMachine.add('GRAB', GrabWithVisualServoing(robot, arm_designator, grab_entity_designator),
transitions={'grab_succeeded': 'CLOSE_GRIPPER',
'grab_failed' : 'CLOSE_GRIPPER',
'target_lost' : 'CLOSE_GRIPPER_UPON_FAIL'})
smach.StateMachine.add('CLOSE_GRIPPER', SetGripper(robot, arm_designator, gripperstate=ArmState.CLOSE, grab_entity_designator=grab_entity_designator),
transitions={'succeeded' : 'LIFT',
'failed' : 'LIFT'})
smach.StateMachine.add('LIFT', ArmToUserPose(arm_designator, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, time_out=20, pre_grasp=False, frame_id="/amigo/base_link", delta=True),
transitions={'succeeded':'RETRACT','failed':'RETRACT'})
smach.StateMachine.add('RETRACT', ArmToUserPose(arm_designator, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, time_out=20, pre_grasp=False, frame_id="/amigo/base_link", delta=True),
transitions={'succeeded':'CARR_POS','failed':'CARR_POS'})
smach.StateMachine.add('CARR_POS', ArmToJointConfig(robot, arm_designator, "carrying_pose"),
transitions={'succeeded':'succeeded','failed':'CLOSE_GRIPPER_UPON_FAIL'})
smach.StateMachine.add('CLOSE_GRIPPER_UPON_FAIL', SetGripper(robot, arm_designator, gripperstate=ArmState.CLOSE, timeout=0.0),
transitions={'succeeded' : 'failed',
'failed' : 'failed'})
def __init__(self, robot, arm_designator, drop_bag_pose):
"""
:param robot: the robot with which to execute this state machine
:param arm_designator: ArmDesignator resolving to Arm holding the bag to drop
"""
smach.StateMachine.__init__(self, outcomes=['done'])
check_type(arm_designator, Arm)
with self:
smach.StateMachine.add('DROP_POSE', states.ArmToJointConfig(robot, arm_designator, drop_bag_pose),
transitions={'succeeded': 'OPEN_AFTER_DROP',
'failed': 'OPEN_AFTER_DROP'})
smach.StateMachine.add('OPEN_AFTER_DROP',
states.SetGripper(robot, arm_designator, gripperstate=GripperState.OPEN),
transitions={'succeeded': 'RESET_ARM',
'failed': 'RESET_ARM'})
smach.StateMachine.add('RESET_ARM', states.ResetArms(robot),
transitions={'done': 'done'})
def __init__(self, robot, arm_designator, grab_entity_designator):
smach.StateMachine.__init__(self, outcomes=['succeeded','failed'])
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.robot = robot
self.grab_entity_designator = grab_entity_designator
'''check check input and output keys'''
with self:
smach.StateMachine.add('CLOSE_GRIPPER', SetGripper(robot, arm_designator, gripperstate=ArmState.CLOSE),
transitions={'succeeded' : 'PRE_POINT',
'failed' : 'PRE_POINT'})
smach.StateMachine.add('PRE_POINT', ArmToQueryPoint(robot, arm_designator, grab_entity_designator, time_out=20, pre_grasp=True, first_joint_pos_only=True),
transitions={'succeeded' : 'POINT',
'failed' : 'RESET_ARM_FAILED'})
smach.StateMachine.add('POINT', Point_at_object(arm_designator, robot, grab_entity_designator),
transitions={'point_succeeded': 'SAY_POINTED',
'point_failed' : 'RETRACT',
'target_lost' : 'RESET_ARM_FAILED'})
smach.StateMachine.add('SAY_POINTED',
Say(robot, "I hope this is the object you were looking for."),
transitions={ 'spoken':'RETRACT' })
smach.StateMachine.add('RETRACT',
ArmToJointConfig(robot, arm_designator, 'retract'),
transitions={'succeeded':'RESET_ARM_SUCCEEDED','failed':'RESET_ARM_SUCCEEDED'})
smach.StateMachine.add('RESET_ARM_SUCCEEDED',
ArmToJointConfig(robot, arm_designator, 'reset'),
transitions={ 'done':'succeeded',
'failed':'succeeded'})
smach.StateMachine.add('RESET_ARM_FAILED',
ArmToJointConfig(robot, arm_designator, 'reset'),
transitions={ 'done':'failed',
'failed':'failed'})
def __init__(self, robot, entity, keep_following=False, waittime=0.0):
ds.check_type(entity, EntityInfo)
State.__init__(self, locals(), outcomes=['succeeded', 'failed'])
