The goal of the task is to implement a function to steer the simulated robot toward a given goal.

---

In class HexapodController.py implement the goto function. The purpose of the function is to produce the steering command that will drive the simulated hexapod robot toward the goal position based on the current odometry of the robot and the current collision state of the robot. The input parameters of the function are:

1. goal - Pose - only position.x and position.y parameters are used as the goal position.
2. odometry - Odometry - pose.position.x, pose.position.y and pose.orientation.quaternion encode the current robot absolute position in the environment.
3. collision - boolean - True if the robot is colliding with some obstacle, False otherwise.

The function returns:

1. Zero command cmd_msg = Twist() when any of the input data are invalid.
2. None when the robot collides with any obstacle.
3. None when the robot has reached the given goal (note that in the real world, the exact position is impossible to reach by the robot; hence the goal is considered reached when the robot is in the DELTA_DISTANCE vicinity of the goal position).
4. Otherwise, the function returns the steering command in the form of a velocity command (Twist message) consisting of linear and angular parts that steer the robot towards the goal position.

The goto function has the following prescription

    def goto(self, goal, odometry, collision):
        """Method to steer the robot towards the goal position given its current 
           odometry and collision status
        Args:
            goal: Pose of the robot goal.
            odometry: Perceived odometry of the robot.
            collision: bool of the robot collision status.
        Returns:
            cmd: Twist steering command.
        """

---

The open-loop locomotion toward a given goal can be approached using either a discrete controller or a continuous control function.

The discrete controller operates as follows (pseudocode).

while not goal_reached:
    If the difference between the current heading and the heading to the target is higher than ORIENTATION_THRESHOLD:
        Full-speed turn toward the targets
    Else:
        go straight        

The operation of the discrete controller with the ORIENTATION_THRESHOLD = PI/16 can be seen in the following video (4x speed up).

On the other hand, the continuous navigation function is much more elegant and can follow the pseudocode.

while not goal_reached:
    dphi = the difference between the current heading and the heading toward the target
    linear speed = distance toward the target
    angular speed = dphi*C_TURNING_SPEED

Where C_TURNING_SPEED is a constant that defines the aggression with which the robot turns towards the desired heading. The linear speed can also be set as a constant value. However, since it is desirable to slow down gradually toward the target, the herein-presented pseudocode uses a simple distance towards target heuristic. Note that the continuous navigation function is inspired by the Braitenberg vehicle model, which is discussed in Exteroceptive sensing, Mapping, and Reactive-based Obstacle Avoidance.

The operation of the continuous controller can be seen in the following videos (4x speed up) that differ only in the magnitude of the C_TURNING_SPEED constant. It can be seen that the locomotion is overall smoother in comparison to the discrete controller.

The command goto should return None when the robot is within sufficient distance to the desired goal pose such as

if odometry.pose.dist(goal) < DELTA_DISTANCE
    return None

where DELTA_DISTANCE can be a suitable value such as 0.12.

---

The evaluation focuses on the ability of the robot to reach the given goal locations. The core functionality in all the t1 tasks is built upon this ability.

The code can be evaluated using the following script (also attached as t1a-ctrl.py).

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
 
import matplotlib.pyplot as plt
 
import sys
import os
import math
import numpy as np
 
sys.path.append('../')
sys.path.append('../redcp/hexapod_robot')
 
from redcp import * # import data types
import HexapodRobot as hexapod #import hexapod robot
 
from HexapodController import Controller
 
def plot_robot_path(path):
    fig, ax = plt.subplots()
    path.plot(ax, 30)
    plt.xlabel('x[m]')
    plt.ylabel('y[m]')
    plt.axis('equal')
    plt.show()
 
if __name__=="__main__":
    robot = hexapod.HexapodRobot(Controller())
    robot.turn_on() #turn on the robot 
    robot.start_navigation() #start navigation thread
 
    goals = [ #assign goal for navigation
        Pose(Vector3(1, -1, 0), Quaternion(1, 0, 0, 0)), 
        Pose(Vector3(1, 1, 0), Quaternion(1, 0, 0, 0)), 
        Pose(Vector3(-1, 0, 0), Quaternion(1, 0, 0, 0)), 
        Pose(Vector3(-3, 0, 0), Quaternion(1, 0, 0, 0)), 
    ]
 
    path = Path()
    for goal in goals: #go from goal to goal
        robot.goto(goal)
        while robot.navigation_goal is not None:
            if robot.odometry_ is not None: #sample the current odometry
                path.poses.append(robot.odometry_.pose)
                odom = robot.odometry_.pose #check the robot distance to goal
                odom.position.z = 0 #compensate for the height of the robot as we are interested only in achieved planar distance 
                dist = goal.dist(odom) #calculate the distance
                sys.stdout.write("\r[t1a-ctrl] actual distance to goal: %.2f" % dist)
            time.sleep(0.1) #wait for a while
        print("\n[t1a-ctrl] distance to goal: %.2f" % dist)
    robot.stop_navigation()
    robot.turn_off()
    plot_robot_path(path)

The expected output is the print of the distance readings below or equal to the DELTA_DISTANCE threshold and the plot of the robot path between the individual goals similar to the following figure.