{{indexmenu_n>1}}

===== t1a-ctrl - Open-loop locomotion control ======
The goal of the task is to implement a function to steer the simulated robot toward a given goal.

|**Files**  |  ''HexapodController.py'' - File to be implemented \\ ''t1a-ctrl.py'' - Evaluation script |
|**Resources**  | [[courses:crl-courses:redcp:tutorials:simloc|Introduction to CoppeliaSim/V-REP and Open-Loop Robot Locomotion Control]] \\ [[courses:crl-courses:redcp:tutorials:reactive|Exteroceptive sensing and Reactive-based Obstacle Avoidance]] \\ {{ :courses:crl-courses:redcp:tasks:redcp.zip | redcp.zip}}, {{ :courses:crl-courses:redcp:tasks:scenes.zip |}} \\  {{ :courses:crl-courses:redcp:tasks:t1a-ctrl.zip |}} - **<fc #ff0000>Initial files and evaluation script</fc>** |

<note tip>
  * Become familiar with [[courses:crl-courses:redcp:tutorials:simloc|Introduction to CoppeliaSim]], use the ''plain.ttt'' scene.
  * Implement ''Controller.goto()'' in ''HexapodController.py''.
</note>
----

===Assignment===
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:
  - ''goal'' - [[courses:crl-courses:redcp:tasks:data_types|Pose]] - only ''position.x'' and ''position.y'' parameters are used as the goal position.
  - ''odometry'' - [[courses:crl-courses:redcp:tasks:data_types|Odometry]] - ''pose.position.x'', ''pose.position.y'' and ''pose.orientation.quaternion'' encode the current robot absolute position in the environment.
  - ''collision'' - boolean - ''True'' if the robot is colliding with some obstacle, ''False'' otherwise.

The function returns:
  - Zero command ''cmd_msg = Twist()'' when any of the input data are invalid.
  - ''None'' when the robot collides with any obstacle.
  - ''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).
  - Otherwise, the function returns the steering command in the form of a velocity command ([[:courses:crl-courses:redcp:tasks:data_types|Twist message]]) consisting of linear and angular parts that steer the robot towards the goal position. 

The ''goto'' function has the following prescription
<code python>
    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.
        """
</code>
<note tip>In the class ''HexapodController.py'', you can change whatever you want. In the evaluation, the given interfaces are fixed, and the evaluation script is fixed.</note>

----

=== Approach ===
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).
<code python>
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        
</code>
The operation of the discrete controller with the ''ORIENTATION_THRESHOLD = PI/16'' can be seen in the following video (4x speed up).

{{:courses:crl-courses:redcp:tasks:t1-ctrl_disc.gif?400|}}
 
On the other hand, the continuous navigation function is much more elegant and can follow the pseudocode.
<code python>
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
</code>
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 [[:courses:crl-courses:redcp:tutorials:reactive|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.

{{:courses:crl-courses:redcp:tasks:t1-ctrl_cont1.gif?400|}}
{{:courses:crl-courses:redcp:tasks:t1-ctrl_cont2.gif?400|}}

The command ''goto'' should return ''None'' when the robot is within sufficient distance to the desired goal pose such as 
<code python>
if odometry.pose.dist(goal) < DELTA_DISTANCE
    return None
</code>
where ''DELTA_DISTANCE'' can be a suitable value such as ''0.12''.

----

=== Evaluation ===
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'').
<code python>
#!/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)
</code>
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.

{{::courses:crl-courses:redcp:tasks:path.jpg?600|}}