Search
The goal of the task is to implement a function to steer the simulated robot toward a given goal.
HexapodController.py
t1a-ctrl.py
plain.ttt
Controller.goto()
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:
goto
goal
position.x
position.y
odometry
pose.position.x
pose.position.y
pose.orientation.quaternion
collision
True
False
The function returns:
cmd_msg = Twist()
None
DELTA_DISTANCE
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
ORIENTATION_THRESHOLD = PI/16
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
C_TURNING_SPEED
distance towards target
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
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.
t1
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)