11 files changed, 538 insertions, 0 deletions
diff --git a/controllers/final/arm_controller.py b/controllers/final/arm_controller.py
new file mode 100644
index 0000000..bcaf5b7
--- /dev/null
+++ b/controllers/final/arm_controller.py
@@ -0,0 +1,35 @@
+from py_trees.behaviour import Behaviour
+from py_trees.common import Status
+from setup import robot, motors, sensors
+from constants import JOINT_TOLERANCE, TIMEOUT
+
+# Move the provided joints towards the target values.
+# Mainly used to control the arm.
+class MoveArm(Behaviour):
+    def __init__(self, name, targets):
+        super().__init__(name)
+        self.targets = targets
+
+    def initialise(self):
+        self.initial_time = robot.getTime()
+        for joint, target in self.targets.items():
+            motors[joint].setPosition(target)
+        print("%s.initialise()" % (self.name))
+
+    def update(self):
+        if robot.getTime() - self.initial_time > TIMEOUT:
+            print(
+                "%s.update()[%s->%s], Timed out"
+                % (self.name, self.status, Status.FAILURE)
+            )
+            return Status.FAILURE
+
+        # All joints are within JOINT_TOLERANCE from their respective targets.
+        if all([abs(sensors[joint].getValue() - target) < JOINT_TOLERANCE
+                for joint, target in self.targets.items()]):
+            print(
+                "%s.update()[%s->%s]"
+                % (self.name, self.status, Status.SUCCESS)
+            )
+            return Status.SUCCESS
+        return Status.RUNNING
diff --git a/controllers/final/constants.py b/controllers/final/constants.py
new file mode 100644
index 0000000..0c559b6
--- /dev/null
+++ b/controllers/final/constants.py
@@ -0,0 +1,18 @@
+from math import pi
+
+P1 = 4. # Cooeffecient for correcting the angle.
+P2 = 4. # Cooeffecient for correcting the distance.
+
+JAR_OFFSETS = [0.16, 0.03, 0.07] # An offset for each jar when calculating the position to pick it up.
+PG_DIST = [.2, .44, .37] # Distance to allow from the counter after grabbing.
+
+MAX_SPEED = 2. * pi
+WALK_SPEED = 1. # The speed to use when going at a constant pace.
+ROTATION_SPEED = 1. # The speed to use when only rotating the robot.
+TIMEOUT = 30. # Typical timeout limit for each behaviour.
+GRIPPER_TIMEOUT = 3. # Timeout for the gripper behaviours.
+GRASP_THRESHOLD = -5. # Force with which to grab the jars.
+APPROACH_STOP = .16 # Point at which to stop when approaching the jar.
+JOINT_TOLERANCE = .01 # Tolerance for joint movement.
+ANGLE_TOLERANCE = .05 # Tolerance for when rotating the robot.
+ARM_OFFSET = .6 # Must take into account the length of the arm when grabbing the jars.
diff --git a/controllers/final/detector.py b/controllers/final/detector.py
new file mode 100644
index 0000000..d874e73
--- /dev/null
+++ b/controllers/final/detector.py
@@ -0,0 +1,43 @@
+from py_trees.behaviour import Behaviour
+from py_trees.common import Status
+from constants import TIMEOUT, JAR_OFFSETS
+from setup import blackboard, robot, gps, compass, camera
+from utilities import robot_to_world_coords, grasp_point
+from numpy import arctan2
+
+# Detect the jar using the camera and store its position in the blackboard.
+class JarFinder(Behaviour):
+    def __init__(self, name, target_jar, jar_offset):
+        super().__init__(name)
+        self.target_jar = target_jar
+        self.offset = jar_offset
+
+    def initialise(self):
+        self.initial_time = robot.getTime()
+        print("%s.initialise(), targeting %s" % (self.name, self.target_jar))
+
+    def update(self):
+        if robot.getTime() - self.initial_time > TIMEOUT:
+            print(
+                "%s.update()[%s->%s], Timed out"
+                % (self.name, self.status, Status.FAILURE)
+            )
+            return Status.FAILURE
+
+        gps_pos = gps.getValues()
+        theta = arctan2(*compass.getValues()[:2])
+        for jar in camera.getRecognitionObjects():
+            print("%s.update(), %s detected." % (self.name, jar.getModel()))
+            if jar.getModel() == self.target_jar:
+                jar_pos = robot_to_world_coords(jar.getPosition(), gps_pos, theta)
+                blackboard["target_pos"] = grasp_point(jar_pos, gps_pos, self.offset)
+                print(
+                    "%s.update()[%s->%s], jar at [%.3f, %.3f]"
+                    % (self.name, self.status, Status.SUCCESS, *jar_pos)
+                )
+                return Status.SUCCESS
+        print(
+            "%s.update()[%s->%s], %s not found yet."
+            % (self.name, self.status, self.target_jar, Status.RUNNING)
+        )
+        return Status.RUNNING
diff --git a/controllers/final/final.py b/controllers/final/final.py
new file mode 100644
index 0000000..676d162
--- /dev/null
+++ b/controllers/final/final.py
@@ -0,0 +1,65 @@
+from py_trees.composites import Sequence, Parallel
+from py_trees.trees import BehaviourTree
+from py_trees.common import ParallelPolicy, Status
+from setup import robot, timestep
+from arm_controller import MoveArm
+from detector import JarFinder
+from go_back import GoBack
+from gripper import OpenGripper, CloseGripper
+from navigator import ApproachJar, RotateRobot
+from positions import grasp_pos, lift_pos, retract_pos, place_pos, raise_torso, rotate_pos
+from sleep import Sleep
+from constants import JAR_OFFSETS, PG_DIST
+
+# This is the sequence of picking up the jar, placing it, then turning back to
+# the counter. It was placed in a function given its repetitiveness.
+def generate_repeat_sequence(i):
+    id = (i+1, ) # The number to show in the names.
+    seq = Sequence("Moving jar %d"%id, memory=True)
+    seq.add_child(JarFinder("Detect jar %d"%id, "honey jar" if i==1 else "jam jar", JAR_OFFSETS[i]))
+    if i: # Move back to get some space, but only for the 2nd and 3rd jars.
+        seq.add_child(GoBack("Move back for pick up %d"%id, distance=.45))
+    # Pick up sequence.
+    seq.add_child(MoveArm("Grasping pose %d"%id, grasp_pos))
+    seq.add_child(ApproachJar("Approach jar %d"%id))
+    seq.add_child(CloseGripper("Grasp jar %d"%id))
+    seq.add_child(GoBack("Move back after pick up %d"%id, distance=PG_DIST[i]))
+    seq.add_child(MoveArm("Lift arm %d"%id, lift_pos))
+    seq.add_child(MoveArm("Retract the arm after pick up %d"%id, retract_pos))
+    seq.add_child(Sleep(name="Pause to kill the inertia"))
+    # Face the table to place the jars on the table.
+    seq.add_child(RotateRobot("Rotate towards table %d"%id, -2.5))
+    seq.add_child(Sleep(name="Pause to kill the inertia"))
+    # Place the jar on the table.
+    seq.add_child(MoveArm("Releasing pose %d"%id, place_pos))
+    seq.add_child(Sleep(name="Pause to kill the inertia"))
+    seq.add_child(OpenGripper("Release the jar %d"%id))
+    seq.add_child(MoveArm("Raise the torso %d"%id, raise_torso))
+    if i == 2: # There is no need to continue after this for the 3rd jar.
+        return seq # There is no need to continue after this for the 3rd jar.
+    # Return to the kitchen counter to get the next jar.
+    seq.add_child(Sleep(name="Pause to kill the inertia"))
+    seq.add_child(GoBack("Create some space from the table %d"%id, distance=.4 if i else .0))
+    seq.add_child(MoveArm("Retract the arm after placement %d"%id, rotate_pos))
+    seq.add_child(Sleep(name="Pause to kill the inertia"))
+    seq.add_child(RotateRobot("Rotate towards the counter %d"%id, .3))
+    seq.add_child(Sleep(name="Pause to kill the inertia"))
+    return seq
+
+tree = Sequence("Root", memory=True) # The main sequence.
+tree.add_child(
+    Parallel("Initialising", policy=ParallelPolicy.SuccessOnAll(), children=[
+        Sleep(name="Wait for world to stabilise", steps=120),
+        MoveArm("Initialise to the grasp position", grasp_pos),
+        MoveArm("Correct head angle", {"head_2_joint": -.3}), # Better camera angle.
+    ]))
+for i in range(3):
+    tree.add_child(generate_repeat_sequence(i)) # Move the jars one by one.
+tree = BehaviourTree(tree) # BehaviourTree allows better control.
+tree.setup()
+
+while robot.step(timestep) != -1:
+    tree.tick()
+    if tree.root.status in (Status.SUCCESS, Status.FAILURE):
+        print("Tree finished executing with status %s" % tree.root.status)
+        break
diff --git a/controllers/final/go_back.py b/controllers/final/go_back.py
new file mode 100644
index 0000000..c4c792b
--- /dev/null
+++ b/controllers/final/go_back.py
@@ -0,0 +1,52 @@
+from py_trees.behaviour import Behaviour
+from py_trees.common import Status
+from constants import WALK_SPEED, TIMEOUT
+from setup import robot, gps, compass, left_motor, right_motor
+from utilities import euclidean, bound_by, deg, fix_arctan
+from numpy import arctan2
+
+# Move back a little to give some space for arm movement, while trying to
+# maintain the original heading.
+class GoBack(Behaviour):
+    def __init__(self, name, distance):
+        super().__init__(name)
+        self.target = distance
+
+    def initialise(self):
+        self.initial_time = robot.getTime()
+        self.initial_pos = gps.getValues()[:2]
+        self.initial_theta = arctan2(*compass.getValues()[:2])
+        print(
+            "%s.initialise(), from (%.3f, %.3f), at %.1f°"
+            % (self.name, *self.initial_pos, deg(self.initial_theta))
+        )
+
+    def update(self):
+        if robot.getTime() - self.initial_time > TIMEOUT:
+            print(
+                "%s.update()[%s->%s], Timed out"
+                % (self.name, self.status, Status.FAILURE)
+            )
+            return Status.FAILURE
+
+        theta = arctan2(*compass.getValues()[:2])
+        alpha = fix_arctan(theta - self.initial_theta)
+
+        if euclidean(self.initial_pos, gps.getValues()) >= self.target:
+            print(
+                "%s.update()[%s->%s], angle error = %.1f°"
+                % (self.name, self.status, Status.SUCCESS, deg(alpha))
+            )
+            return Status.SUCCESS
+
+        left_motor.setVelocity(-WALK_SPEED + alpha)
+        right_motor.setVelocity(-WALK_SPEED - alpha)
+        return Status.RUNNING
+
+    def terminate(self, new_status):
+        left_motor.setVelocity(0.0)
+        right_motor.setVelocity(0.0)
+        print(
+            "%s.terminate()[%s->%s], new position: (%.3f, %.3f)"
+            % (self.name, self.status, new_status, *gps.getValues()[:2])
+        )
diff --git a/controllers/final/gripper.py b/controllers/final/gripper.py
new file mode 100644
index 0000000..e9e073e
--- /dev/null
+++ b/controllers/final/gripper.py
@@ -0,0 +1,87 @@
+from py_trees.behaviour import Behaviour
+from py_trees.common import Status
+from constants import GRIPPER_TIMEOUT, GRASP_THRESHOLD, JOINT_TOLERANCE
+from setup import robot, motors, sensors
+
+joints = ["gripper_left_finger_joint", "gripper_right_finger_joint", ]
+# For this module, ensure motors and sensors only contain the relevant entries.
+motors = [motors[joint] for joint in joints]
+sensors = [sensors[joint] for joint in joints]
+
+class CloseGripper(Behaviour):
+    def __init__(self, name):
+        super().__init__(name)
+        self.position = .045
+
+    def initialise(self):
+        for motor in motors:
+            motor.setPosition(self.position)
+        self.initial_time = robot.getTime()
+        print("%s.initialise()" % (self.name, ))
+
+    def update(self):
+        if robot.getTime() - self.initial_time > GRIPPER_TIMEOUT:
+            print(
+                "%s.update()[%s->%s], Timed out"
+                % (self.name, self.status, Status.FAILURE)
+            )
+            return Status.FAILURE
+
+        # Close the grip little by little, as setting the position to 0 all at
+        # once creates strain on the gripper.
+        self.position -= .003
+        for motor in motors:
+            motor.setPosition(max(0, self.position))
+
+        # Terminate with SUCCESS when both grippers are exerting enough force.
+        if all([motor.getForceFeedback() < GRASP_THRESHOLD for motor in motors]):
+            print(
+                "%s.update()[%s->%s], both ends grasped with force > %.2f"
+                % (self.name, self.status, Status.SUCCESS, abs(GRASP_THRESHOLD))
+            )
+            return Status.SUCCESS
+
+        # Both grippers reached 0 without enough force being exerted, meaning
+        # the jar was not grabbed.
+        if all([sensor.getValue() < JOINT_TOLERANCE for sensor in sensors]):
+            print(
+                "%s.update()[%s->%s], gripper completely closed (empty)"
+                % (self.name, self.status, Status.FAILURE)
+            )
+            return Status.FAILURE
+
+        return Status.RUNNING
+
+    def terminate(self, new_status):
+        print(
+            "%s.terminate()[%s->%s], gripper positions: %.2f, %.2f"
+            % (self.name, self.status, new_status, *[s.getValue() for s in sensors])
+        )
+
+class OpenGripper(Behaviour):
+    def __init__(self, name):
+        super().__init__(name)
+
+    def initialise(self):
+        print("%s.initialise()" % (self.name, ))
+        for motor in motors:
+            motor.setPosition(0.045)
+        self.initial_time = robot.getTime()
+
+    def update(self):
+        if robot.getTime() - self.initial_time > GRIPPER_TIMEOUT:
+            print(
+                "%s.update()[%s->%s], Timed out"
+                % (self.name, self.status, Status.FAILURE)
+            )
+            return Status.FAILURE
+
+        # Both joints are within JOINT_TOLERANCE from their respective targets.
+        if all([abs(sensor.getValue() - .045) < JOINT_TOLERANCE for sensor in sensors]):
+            print(
+                "%s.update()[%s->%s], gripper completely opened"
+                % (self.name, self.status, Status.SUCCESS)
+            )
+            return Status.SUCCESS
+
+        return Status.RUNNING
diff --git a/controllers/final/navigator.py b/controllers/final/navigator.py
new file mode 100644
index 0000000..c9cb876
--- /dev/null
+++ b/controllers/final/navigator.py
@@ -0,0 +1,97 @@
+from py_trees.behaviour import Behaviour
+from py_trees.common import Status
+from constants import MAX_SPEED, ROTATION_SPEED, P1, P2, APPROACH_STOP, ANGLE_TOLERANCE, TIMEOUT
+from setup import blackboard, robot, gps, compass, left_motor, right_motor
+from utilities import deg, bound_by, euclidean, fix_arctan
+from numpy import arctan2, sign
+
+# Move to the target_pos set by JarFinder.
+class ApproachJar(Behaviour):
+    def __init__(self, name):
+        super().__init__(name)
+
+    def initialise(self):
+        self.initial_time = robot.getTime()
+        print(
+            "%s.initialise(), going towards (%.3f, %.3f)"
+            % (self.name, *blackboard["target_pos"])
+        )
+
+    def update(self):
+        if robot.getTime() - self.initial_time > TIMEOUT:
+            print(
+                "%s.update()[%s->%s], Timed out"
+                % (self.name, self.status, Status.FAILURE)
+            )
+            return Status.FAILURE
+
+        robot_pos = gps.getValues()[:2]
+        theta = arctan2(*compass.getValues()[:2])
+        d_dist = blackboard["target_pos"] - robot_pos
+
+        rho = euclidean(robot_pos, blackboard["target_pos"])
+        alpha = fix_arctan(arctan2(d_dist[1], d_dist[0]) - theta)
+
+        left_speed = bound_by(-P1 * alpha + P2 * rho , MAX_SPEED)
+        right_speed = bound_by(P1 * alpha + P2 * rho , MAX_SPEED)
+        left_motor.setVelocity(left_speed)
+        right_motor.setVelocity(right_speed)
+
+        if rho < APPROACH_STOP: # The robot is now close enough.
+            print(
+                "%s.update()[%s->%s], angle error = %.1f°"
+                % (self.name, self.status, Status.SUCCESS, deg(alpha))
+            )
+            return Status.SUCCESS
+        return Status.RUNNING
+
+    def terminate(self, new_status):
+        left_motor.setVelocity(0.)
+        right_motor.setVelocity(0.)
+        print(
+            "%s.terminate()[%s->%s], new position: (%.3f, %.3f)"
+            % (self.name, self.status, new_status, *gps.getValues()[:2])
+        )
+
+class RotateRobot(Behaviour):
+    def __init__(self, name, target_angle):
+        super().__init__(name)
+        self.target_angle = target_angle
+
+    def initialise(self):
+        self.initial_time = robot.getTime()
+        print(
+            "%s.initialise(), rotating towards %.1f°"
+            % (self.name, deg(self.target_angle))
+        )
+
+    def update(self):
+        if robot.getTime() - self.initial_time > TIMEOUT:
+            print(
+                "%s.update()[%s->%s], Timed out"
+                % (self.name, self.status, Status.FAILURE)
+            )
+            return Status.FAILURE
+
+        theta = arctan2(*compass.getValues()[:2])
+        alpha = fix_arctan(self.target_angle - theta)
+
+        if abs(alpha) < ANGLE_TOLERANCE: # Close enough.
+            print(
+                "%s.update()[%s->%s], angle error = %.1f°"
+                % (self.name, self.status, Status.SUCCESS, deg(alpha))
+            )
+            return Status.SUCCESS
+
+        direction = sign(alpha)
+        left_motor.setVelocity(-ROTATION_SPEED * direction)
+        right_motor.setVelocity(ROTATION_SPEED * direction)
+        return Status.RUNNING
+
+    def terminate(self, new_status):
+        left_motor.setVelocity(0.)
+        right_motor.setVelocity(0.)
+        print(
+            "%s.terminate()[%s->%s], new position: (%.3f, %.3f)"
+            % (self.name, self.status, new_status, *gps.getValues()[:2])
+        )
diff --git a/controllers/final/positions.py b/controllers/final/positions.py
new file mode 100644
index 0000000..e47d841
--- /dev/null
+++ b/controllers/final/positions.py
@@ -0,0 +1,30 @@
+# Predefined positions of different joints.
+lift_pos = {
+    "torso_lift_joint": .25,
+    "arm_1_joint": 1.6,
+    "arm_2_joint": .5,
+    "arm_4_joint": .5,
+    "arm_7_joint": 1.55,
+}
+retract_pos = {
+    "torso_lift_joint": .25,
+    "arm_1_joint": 1.6,
+    "arm_2_joint": .85,
+    "arm_4_joint": 1.,
+    "arm_7_joint": 1.55,
+}
+raise_torso = {"torso_lift_joint": .35}
+place_pos = {
+    "torso_lift_joint": .05,
+    "arm_1_joint": 1.6,
+    "arm_2_joint": .5,
+    "arm_4_joint": .5,
+    "arm_7_joint": 1.55,
+}
+# | merges two dictionaries.
+grasp_pos = place_pos | {
+    "torso_lift_joint": .15,
+    "gripper_left_finger_joint": .045,
+    "gripper_right_finger_joint": .045,
+}
+rotate_pos = retract_pos | { "arm_4_joint": 2.25, "arm_2_joint": 1., }
diff --git a/controllers/final/setup.py b/controllers/final/setup.py
new file mode 100644
index 0000000..dc3a7b3
--- /dev/null
+++ b/controllers/final/setup.py
@@ -0,0 +1,57 @@
+"""
+All the robot devices as well as the blackboard are defined and initialised
+here for an important reason - imported modules in python are run only once.
+This means that there will be no redundant re-execution and all the variables
+will be accessible from the other files as necessary. Modifications to the
+blackboard will be visible from all files, which is the intended effect.
+"""
+from controller import Robot
+
+robot = Robot()
+timestep = int(robot.getBasicTimeStep())
+blackboard = {} # Defined as a dictionary as it is much simpler.
+
+# Setup the motors and sensors as dictionaries, with the relevant joint names
+# as keys.
+joints = [
+    "gripper_left_finger_joint", "gripper_right_finger_joint", "head_2_joint",
+    "arm_1_joint", "arm_2_joint", "arm_4_joint", "arm_7_joint",
+    "torso_lift_joint",
+]
+finger_sensor_names = {
+    "gripper_left_finger_joint": "gripper_left_sensor_finger_joint",
+    "gripper_right_finger_joint": "gripper_right_sensor_finger_joint",
+}
+sensors = {}
+motors = {}
+for joint in joints:
+    motors[joint] = robot.getDevice(joint)
+    # If the name is in finger_sensor_names, use it. Fallback to %s_sensor.
+    sensor = finger_sensor_names.get(joint, "%s_sensor" % joint)
+    sensor = robot.getDevice(sensor)
+    sensor.enable(timestep)
+    sensors[joint] = sensor
+
+# Enable force feedback for the grippers.
+for finger in finger_sensor_names:
+    motors[finger].enableForceFeedback(timestep)
+
+# Initialise the wheel motors.
+left_motor = robot.getDevice("wheel_left_joint")
+right_motor = robot.getDevice("wheel_right_joint")
+left_motor.setPosition(float("inf"))
+right_motor.setPosition(float("inf"))
+left_motor.setVelocity(.0)
+right_motor.setVelocity(.0)
+
+# Enable the gps and compass to gain better situational awareness.
+gps = robot.getDevice("gps")
+gps.enable(timestep)
+compass = robot.getDevice("compass")
+compass.enable(timestep)
+
+# Setup and enable the camera, including the recognition functionality to
+# obtain the jar locations.
+camera = robot.getDevice("Astra rgb")
+camera.enable(timestep)
+camera.recognitionEnable(timestep)
diff --git a/controllers/final/sleep.py b/controllers/final/sleep.py
new file mode 100644
index 0000000..34d8fa4
--- /dev/null
+++ b/controllers/final/sleep.py
@@ -0,0 +1,16 @@
+from py_trees.behaviour import Behaviour
+from py_trees.common import Status
+
+# This provides the same functionality as py_trees's internal Timer class, but
+# uses the number of steps instead of time, ensuring the same result regardless
+# of the simulation speed.
+class Sleep(Behaviour):
+    def __init__(self, name, steps=20):
+        super().__init__(name)
+        self.steps = steps
+
+    def update(self):
+        while self.steps > 0:
+            self.steps -= 1
+            return Status.RUNNING
+        return Status.SUCCESS
diff --git a/controllers/final/utilities.py b/controllers/final/utilities.py
new file mode 100644
index 0000000..9b3abdd
--- /dev/null
+++ b/controllers/final/utilities.py
@@ -0,0 +1,38 @@
+import numpy as np
+from constants import ARM_OFFSET
+
+# Convert radian measurements to degrees.
+def deg(radians):
+    return radians * 180 / np.pi
+
+# Make sure a variable is between -bound and +bound.
+def bound_by(variable, bound):
+    return max(-bound, min(bound, variable))
+
+# Calculate the euclidean distance between two points.
+# Only uses the x and y coordinates.
+def euclidean(u, v):
+    return np.sqrt( (u[0]-v[0])**2 + (u[1]-v[1])**2 )
+
+# Calculate the position to which the robot should move to grab the jar, given
+# the position of the jar provided by the camera and the robot's current
+# position.
+def grasp_point(jar_pos, robot_pos, jar_offset=0.):
+    d_dist = jar_pos - robot_pos[:2] # dx,dy between the jar and the robot.
+    distance = np.linalg.norm(d_dist) # Euclidean distance.
+    d_dist *= (distance - ARM_OFFSET) / distance # Account for ARM_OFFSET.
+    d_dist[0] += jar_offset # Add the jar_offset to the x-axis.
+    return robot_pos[:2] + d_dist
+
+# Make sure angles are between -pi and +pi.
+def fix_arctan(radians):
+    radians += np.pi
+    radians %= 2 * np.pi
+    return radians - np.pi
+
+# Convert from robot coordinates to world coordinates.
+def robot_to_world_coords(robot_coor, gps_pos, theta):
+    c, s = np.cos(theta), np.sin(theta)
+    matrix = np.array([[c, -s], [s, c]]) # Rotation matrix for theta.
+    # Adjust for angle and add the robot's position to convert to world coords.
+    return matrix @ robot_coor[:2] + gps_pos[:2]