===== T1a-ctrl - Open-loop locomotion control ======
The main task is to implement a function that will steer the simulated robot towards a given goal.
|**Deadline** | 4. October 2020, 23:59 PST |
|**Points** | 3 |
|**Label in BRUTE** | t1a-ctrl |
|**Files to submit** | archive with ''HexapodController.py'' file |
|**Resources** | {{ :courses:b4m36uir:hw:b4m36uir_t1_resource_pack.zip | B4M36UIR_t1_resource_pack}} |
----
===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 towards 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:b4m36uir:hw:support:messages|Pose message]] - only ''position.x'' and ''position.y'' parameters are used as the goal position
- ''odometry'' - [[courses:b4m36uir:hw:support:messages|Odometry message]] - ''pose.position.x'', ''pose.position.y'' and ''pose.orientation.quaternion'' encodes the current robot absolute position in the environment
- ''collision'' - boolean - ''True'' if the robot collides 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 currently collides with any obstacle.
- ''None'' when the robot has reached the given goal (note that in 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 form of a velocity command ([[courses:b4m36uir:hw:support:messages|Twist message]]) consisting of linear and angular parts that steers 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
"""
In class ''HexapodController.py'' you can change whatever you want. In evaluation, the given interfaces are fixed and the evaluation script is fixed.
----
=== Approach ===
The open-loop locomotion towards a given goal can be approached either using a discrete controller, or using 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 towards 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):
{{:courses:b4m36uir:hw:t1a_ctrl_disc.gif?400|}}
On the other hand, the continuous navigation function is much more elegant and can look like e.g. (pseudocode):
while not goal_reached:
dphi = the difference between the current heading and the heading towards the target
linear speed = distance towards target
angular speed = dphi*C_TURNING_SPEED
Where ''C_TURNING_SPEED'' is a constant that defines the aggression with which the robot will turn towards the desired heading. The linear speed can be set as a constant value, but it make more sense to slow down gradually towards the target, hence, the herein presented pseudocode uses a very simple ''distance towards target'' as a heuristics.
Note, the continuous navigation function is inspired by the Braitenberg vehicle model which will be discussed during [[courses:b4m36uir:labs:lab02|Lab02 - 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:b4m36uir:hw:t1a_ctrl_cont1.gif?400|}}
{{:courses:b4m36uir:hw:t1a_ctrl_cont2.gif?400|}}
----
=== Evaluation ===
The evaluation focus on the ability of the robot to reach the given goal locations. It is the core functionality in all the ''t1'' tasks which are build 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('hexapod_robot')
#import hexapod robot
import HexapodRobot as hexapod
#import communication messages
from messages import *
if __name__=="__main__":
robot = hexapod.HexapodRobot(0)
#turn on the robot
robot.turn_on()
#start navigation thread
robot.start_navigation()
#assign goal for navigation
goals = [
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()
#go from goal to goal
for goal in goals:
robot.goto(goal)
while robot.navigation_goal is not None:
#sample the current odometry
if robot.odometry_ is not None:
path.poses.append(robot.odometry_.pose)
#wait
time.sleep(0.1)
#check the robot distance to goal
odom = robot.odometry_.pose
#compensate for the height of the robot as we are interested only in achieved planar distance
odom.pose.position.z = 0
#calculate the distance
dist = goal.dist(odom)
print("[t1c_eval] distance to goal: %.2f" % dist)
robot.stop_navigation()
robot.turn_off()
#plot the robot path
fig, ax = plt.subplots()
path.plot(ax, 30)
plt.xlabel('x[m]')
plt.ylabel('y[m]')
plt.axis('equal')
plt.show()
The expected output is the print of the distance readings below or equal the ''DELTA_DISTANCE'' threshold and the plot of the robot path between the individual goals similar to the following figure.
{{:courses:b4m36uir:hw:path.jpg?600|}}