{{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 |}} - **Initial files and evaluation script** |
* Become familiar with [[courses:crl-courses:redcp:tutorials:simloc|Introduction to CoppeliaSim]], use the ''plain.ttt'' scene.
* Implement ''Controller.goto()'' in ''HexapodController.py''.
----
===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
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 the class ''HexapodController.py'', you can change whatever you want. In the evaluation, the given interfaces are fixed, and the evaluation script is fixed.
----
=== 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).
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).
{{: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.
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 [[: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
if odometry.pose.dist(goal) < DELTA_DISTANCE
return None
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'').
#!/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.
{{::courses:crl-courses:redcp:tasks:path.jpg?600|}}