def __handle_move_head(self, command: sml.Identifier):
"""
Handle a Soar move-head action.
The Soar output should look like:
(I3 ^move-head Vx)
(Vx ^angle [ang])
where [ang] is a real number in the range [-0.44, 0.78]. This command moves the head to the
the given angle, where 0 is looking straight ahead and the angle is degrees from that
position.
:param command: Soar command object
:return: True if successful, False otherwise
"""
try:
angle = float(command.GetParameterValue("angle"))
except ValueError as e:
print("Invalid angle format {}".format(
command.GetParameterValue("angle")))
return False
print("Moving head {}".format(angle))
set_head_angle_action = self.robot.set_head_angle(degrees(angle),
in_parallel=True)
status_wme = psl.SoarWME("status", "running")
status_wme.add_to_wm(command)
status_wme.update_wm()
return set_head_angle_action, status_wme
def __handle_move_lift(self, command: sml.Identifier):
"""
Handle a Soar move-lift action.
The Soar output should look like:
(I3 ^move-lift Vx)
(Vx ^height [hgt])
where [hgt] is a real number in the range [0, 1]. This command moves the lift to the
the given height, where 0 is the lowest possible position and 1 is the highest.
:param command: Soar command object
:return: True if successful, False otherwise
"""
try:
height = float(command.GetParameterValue("height"))
except ValueError as e:
print("Invalid height format {}".format(
command.GetParameterValue("height")))
return False
print("Moving lift {}".format(height))
set_lift_height_action = self.robot.set_lift_height(height,
in_parallel=True)
status_wme = psl.SoarWME("status", "running")
status_wme.add_to_wm(command)
status_wme.update_wm()
return set_lift_height_action, status_wme
def __handle_turn_to_face(self, command: sml.Identifier):
"""
Handle a Soar turn-to-face action.
The Soar output should look like:
(I3 ^turn-to-face Vx)
(Vx ^face-id [fid])
where [fid] is the integer ID associated with the face to turn towards.
:param command: Soar command object
:return: True if successful, False otherwise
"""
try:
fid = int(command.GetParameterValue("face-id"))
except ValueError as e:
print("Invalid face id format {}".format(
command.GetParameterValue("face-id")))
return False
if fid not in self.faces.keys():
print("Face {} not recognized".format(fid))
return False
print("Turning to face {}".format(fid))
target_face = self.faces[fid]
turn_towards_face_action = self.r.turn_towards_face(target_face,
in_parallel=True)
status_wme = psl.SoarWME("status", "running")
status_wme.add_to_wm(command)
status_wme.update_wm()
return turn_towards_face_action, status_wme
def __handle_pick_up_object(self, command: sml.Identifier):
"""
Handle a Soar pick-up-object action.
The Sour output should look like:
(I3 ^pick-up-object Vx)
(Vx ^object-id [id])
where [id] is the object id of the object to pick up. Cozmo will approach the object
autonomously and try to grasp it with its lift, then lift the lift up. This action is
partiularly prone to failing.
:param command: Soar command object
:return: True if successful, False otherwise
"""
try:
target_id = int(command.GetParameterValue("object-id"))
except ValueError as e:
print("Invalid object-id format {}".format(
command.GetParameterValue("object-id")))
return False
obj_designation = "obj{}".format(target_id)
if not self.objects.get(obj_designation):
print("Couldn't find target object")
return False
print("Picking up object {}".format(obj_designation))
target_obj = self.objects[obj_designation]
pick_up_object_action = self.robot.pickup_object(target_obj,
in_parallel=True)
status_wme = psl.SoarWME("status", "running")
status_wme.add_to_wm(command)
status_wme.update_wm()
return pick_up_object_action, status_wme
def __handle_dock_with_cube(self, command: sml.Identifier):
"""
Handle a Soar dock-with-cube action.
The Sour output should look like:
(I3 ^dock-with-cube Vx)
(Vx ^object-id [id])
where [id] is the object id of the cube to dock with. Cozmo will approach the cube until
its lift hooks are under the grip holes.
:param command: Soar command object
:return: True if successful, False otherwise
"""
try:
target_id = int(command.GetParameterValue("object-id"))
target_id = "obj{}".format(target_id)
except ValueError as e:
print("Invalid target-object-id format {}".format(
command.GetParameterValue("object-id")))
return False
if target_id not in self.objects.keys():
print("Couldn't find target object")
return False
print("Docking with cube with object id {}".format(target_id))
target_obj = self.objects[target_id]
dock_with_cube_action = self.robot.dock_with_cube(target_obj,
in_parallel=True)
status_wme = psl.SoarWME("status", "running")
status_wme.add_to_wm(command)
status_wme.update_wm()
return dock_with_cube_action, status_wme
def __handle_place_on_object(self, command: sml.Identifier):
"""
Handle a Soar place-on-object action.
The Sour output should look like:
(I3 ^place-on-object Vx)
(Vx ^object-id [id])
where [id] is the object id of the object that Cozmo should place to object its holding
on top of.
:param command: Soar command object
:return: True if successful, False otherwise
"""
try:
target_id = int(command.GetParameterValue("object-id"))
except ValueError as e:
print("Invalid object-id format {}".format(
command.GetParameterValue("object-id")))
return False
target_dsg = "obj{}".format(target_id)
if target_dsg not in self.objects.keys():
print("Couldn't find target object")
print(self.objects)
return False
print("Placing held object on top of {}".format(target_dsg))
target_obj = self.objects[target_dsg]
place_on_object_action = self.robot.place_on_object(target_obj,
in_parallel=True)
status_wme = psl.SoarWME("status", "running")
status_wme.add_to_wm(command)
status_wme.update_wm()
return place_on_object_action, status_wme
def __handle_set_backpack_lights(self, command: sml.Identifier):
"""
Handle a Soar set-backpack-lights action.
The Sour output should look like:
(I3 ^set-backpack-lights Vx)
(Vx ^color [color])
where [color] is a string indicating which color the lights should be set to. The colors
are "red", "blue", "green", "white", and "off".
:param command: Soar command object
:return: True if successful, False otherwise
"""
color_str = command.GetParameterValue("color")
if color_str not in COLORS:
print("Invalid backpack lights color {}".format(color_str))
return False
elif color_str == "red":
light = cozmo.lights.red_light
elif color_str == "green":
light = cozmo.lights.green_light
elif color_str == "blue":
light = cozmo.lights.blue_light
elif color_str == "white":
light = cozmo.lights.white_light
else:
light = cozmo.lights.off_light
self.r.set_all_backpack_lights(light=light)
command.AddStatusComplete()
return (None, None)
def __handle_change_block_color(self, command: sml.Identifier):
"""
Handle a Soar change-block-color command.
The Soar output should look like:
(I3 ^change-block-color Vx)
(Vx ^color [str]
^object-id [id])
where color is the color name to change to from the valid colors and id
is the object-id of the cube which should have its color changed.
:param command: Soar command object
:return: True if successful, False otherwise
"""
try:
target_id = int(command.GetParameterValue("object-id"))
except ValueError as e:
#TODO: Update action WME to have failure codes
print("Invalid object-id format, must be int")
return False
if f"obj{target_id}" not in self.objects.keys():
#TODO: Update action WME to have failure codes
print(f"Invalid object-id {target_id}, can't find it")
return False
color = command.GetParameterValue("color").lower()
if color not in COLORS:
print(f"Invalid color choice: {color}")
status_wme = psl.SoarWME("status", "failed")
fail_code_wme = psl.SoarWME("failure-code", "invalid-color")
fail_reason_wme = psl.SoarWME("failure-reason",
"invalid-color: {}".format(color))
status_wme.add_to_wm(command)
fail_code_wme.add_to_wm(command)
fail_reason_wme.add_to_wm(command)
status_wme.update_wm()
return False
print(f"Changing object {target_id} to color {color}")
target_block = self.objects[f"obj{target_id}"]
print("Target object: {}".format(target_block.cube_id))
target_block.set_lights_off()
target_block.set_lights(LIGHTS_DICT[color])
status_wme = psl.SoarWME("status", "complete")
status_wme.add_to_wm(command)
status_wme.update_wm()
return (None, None)
def __handle_go_to_object(self, command: sml.Identifier):
"""
Handle a Soar go-to-object action.
The Sour output should look like:
(I3 ^go-to-object Vx)
(Vx ^object-id [id]
^distance [dist])
where [id] is the object id of the object to go to and [dist] indicates
how far to stop from the object in mm. Only works on LightCubes.
:param command: Soar command object
:return: True if successful, False otherwise
"""
try:
target_id = int(command.GetParameterValue("object-id"))
target_id = f"obj{target_id}"
except ValueError as e:
print("Invalid target-object-id format {}".format(
command.GetParameterValue("object-id")))
return False
if target_id not in self.objects.keys():
print("Couldn't find target object")
return False
try:
distance = distance_mm(float(
command.GetParameterValue("distance")))
except ValueError as e:
print("Invalid distance format {}".format(
command.GetParameterValue("distance")))
return False
print("Going to object {}".format(target_id))
target_obj = self.objects[target_id]
go_to_object_action = self.robot.go_to_object(target_obj,
distance,
in_parallel=True)
status_wme = psl.SoarWME("status", "running")
status_wme.add_to_wm(command)
status_wme.update_wm()
return go_to_object_action, status_wme
def __handle_drive_forward(self, command: sml.Identifier):
"""
Handle a Soar drive-forward action.
The Sour output should look like:
(I3 ^drive-forward Vx)
(Vx ^distance [dist]
^speed [spd])
where [dist] is a real number indicating how far Cozmo should travel (negatives go
backwards) and speed is how fast Cozmo should travel. Units are mm and mm/s, respectively.
:param command: Soar command object
:return: True if successful, False otherwise
"""
try:
distance = distance_mm(float(
command.GetParameterValue("distance")))
except ValueError as e:
print("Invalid distance format {}".format(
command.GetParameterValue("distance")))
return False
try:
speed = speed_mmps(float(command.GetParameterValue("speed")))
except ValueError as e:
print("Invalid speed format {}".format(
command.GetParameterValue("speed")))
return False
print("Driving forward {}mm at {}mm/s".format(distance.distance_mm,
speed.speed_mmps))
drive_forward_action = self.r.drive_straight(distance,
speed,
in_parallel=True)
status_wme = psl.SoarWME("status", "running")
status_wme.add_to_wm(command)
status_wme.update_wm()
return drive_forward_action, status_wme
def __handle_turn_in_place(self, command: sml.Identifier):
"""
Handle a Soar turn-in-place action.
The Sour output should look like:
(I3 ^turn-in-place Vx)
(Vx ^angle [ang]
^speed [spd])
where [ang] is the amount Cozmo should rotate in degrees and speed is the speed at which
Cozmo should rotate in deg/s.
:param command: Soar command object
:return: True if successful, False otherwise
"""
try:
angle = degrees(float(command.GetParameterValue("angle")))
except ValueError as e:
print("Invalid angle format {}".format(
command.GetParameterValue("angle")))
return False
try:
speed = degrees(float(command.GetParameterValue("speed")))
except ValueError as e:
print("Invalid speed format {}".format(
command.GetParameterValue("speed")))
return False
print("Rotating in place {} degrees at {}deg/s".format(
angle.degrees, speed.degrees))
turn_in_place_action = self.r.turn_in_place(angle=angle,
speed=speed,
in_parallel=True)
status_wme = psl.SoarWME("status", "running")
status_wme.add_to_wm(command)
status_wme.update_wm()
return turn_in_place_action, status_wme
