Lab 6: Trajectory Generation

Overview

In this lab, we will focus on how to generate smooth trajectories while following a path with specified waypoints. We will implement two different approaches:

1) Gradient Descent Path Smoothing. The task is to apply gradient descent method to minimize a cost function that balances two cost terms:

Path Following: Keeping the new (smoothed) path close to the original waypoints.
Path Smoothing: Ensuring that the path changes gradually between neighboring points.

2) Third-Order Polynomial Time Scaling. The task is to implement the 3rd order polynomial time scaling and apply it for each segment of the trajectory and for both x and y coordinates. Waypoints will be provided in the sample script, and should be included in the boundary/continuity constraints when generating trajectories.

Submission

Submission: group submission via Gradescope. Important: If any of the two team members have ROS Noetic installed, it is highly recommended to setup the code on this computer and not on the M1/M2 computer.
Demo: not required
Due time: 11:59pm, March 3, Monday
Files to submit:
- lab6_report.pdf (Include the plots of the trajectories and screenshots of the robot at 4 different positions for each path)
- gradient_descent.py
- polynomial_timescaling.py
Grading rubric:
- + 30% Clearly describe your approach and explain your code in the lab report and video.
- + 20% Plot the trajectories and discuss the results under different parameters.
- +25% Pass the performance test on the Gradient Descent Path Smoothing Task.
- + 25% Pass all waypoints using 3rd order polynomial trajectories.
- - 10% Penalty applies for each late day. No submission will be accepted beyond four (2) days from the due date.

Autograder

All code submissions will be graded automatically by an autograder uploaded to Gradescope. The scripts will be tested on a Ubuntu cloud server using a similar ROS + Gazebo environment. The grading results will be available in a couple of minutes after submission.

Testing parameters are as follows.

Gradient Descent: The autograder will evaluate how closely the smoothed path adheres to the original waypoints and measure the overall smoothness using 3 test case trajectories along with one hidden test case.
Polynomial Timescaling:

The tolerance for distance error is set to 0.1m.
- For example, passing point [3.96, 3.94] is approximately equivalent to passing point [4, 4].
The autograder will check the waypoints for both paths and check for the tolerance.
The time limit is set to 400 seconds.

Sample Code for Gradient Descent

Open a new terminal and go to your ee144w25 package. We will start from a new python script.

roscd ee144w25/src

touch gradient_descent.py

gedit gradient_descent.py

Please copy and paste the following code.

#!/usr/bin/env python3

from math import pi, sqrt, atan2, cos, sin

import numpy as np

from copy import deepcopy

import rospy

import tf

from std_msgs.msg import Empty

from nav_msgs.msg import Odometry

from geometry_msgs.msg import Twist, Pose2D

from std_srvs.srv import Empty as srvEmpty

class Turtlebot():

def __init__(self, path, num_path, wf, ws):

# rospy.init_node(node_name)

# rospy.loginfo("Press Ctrl + C to terminate")

self.vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=10)

self.rate = rospy.Rate(10)

# reset odometry to zero

self.reset_pub = rospy.Publisher("mobile_base/commands/reset_odometry", Empty, queue_size=10)

for i in range(10):

self.reset_pub.publish(Empty())

self.rate.sleep()

# set the path

self.path = path

self.wf = wf

self.ws = ws

# subscribe to odometry

self.pose = Pose2D()

self.logging_counter = 0

self.trajectory = list()

self.odom_sub = rospy.Subscriber("odom", Odometry, self.odom_callback)

#initialize your terms

try:

self.run()

except rospy.ROSInterruptException:

rospy.loginfo("Action terminated.")

finally:

# save trajectory into csv file

np.savetxt('trajectory'+str(num_path)+'.csv', np.array(self.trajectory), fmt='%f', delimiter=',')

self.reset_gazebo()

def run(self):

waypoints = self.path

while True:

if len(waypoints) == 1:

break

waypoints[0] = [self.pose.x, self.pose.y]

# use gradient descent for the optimal trajectory

# call self.wf and self.ws to get weight_follow and weight_smooth

waypoints =

self.move_to_point(waypoints[0], waypoints[1])

# update the path to optimize the rest of the trajectory

def move_to_point(self, current_waypoint, next_waypoint):

# generate gradient descent trajectory and move to current_waypoint

# next_waypoint is to help determine the velocity to pass current_waypoint

pass

def smooth(self, path, alpha=1, weight_follow = 0.5, weight_smooth = 0.1, tolerance = 0.000001):

newpath = deepcopy(path) # Make a copy of the original path

change = float('inf') # Large value

while change > tolerance: # Iterative update (repeat until convergence)

change = 0

#############Fill in your code here######################

##############Return smoothed path#######################

return newpath

def odom_callback(self, msg):

# get pose = (x, y, theta) from odometry topic

quarternion = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,\

msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]

(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(quarternion)

self.pose.theta = yaw

self.pose.x = msg.pose.pose.position.x

self.pose.y = msg.pose.pose.position.y

# logging once every 100 times (Gazebo runs at 1000Hz; we save it at 10Hz)

self.logging_counter += 1

if self.logging_counter == 100:

self.logging_counter = 0

self.trajectory.append([self.pose.x, self.pose.y]) # save trajectory

rospy.loginfo("odom: x=" + str(self.pose.x) +\

"; y=" + str(self.pose.y) + "; theta=" + str(yaw))

def reset_gazebo(self):

rospy.wait_for_service('/gazebo/reset_world')

try:

reset_world = rospy.ServiceProxy('/gazebo/reset_world', srvEmpty)

reset_world()

print('resetting')

except rospy.ServiceException as e:

print('failed' % e)

def printpaths(self, path,newpath): # This will show the initial path before smoothing

for old,new in zip(path,newpath):

print ('['+ ', '.join('%.3f'%x for x in old) + \

'] -> ['+ ', '.join('%.3f'%x for x in new) +']')

def plot(self, path, newpath):

x = [x for [x, y] in path]

y = [y for [ax, y] in path]

# This plots the smooth path overlayed on the original

plt.plot(x, y, color='r', marker='o')

x2 = [x for [x, y] in newpath]

y2 = [y for [ax, y] in newpath]

plt.plot(x2, y2, color='b', marker='o')

plt.xlim(-1, 3)

plt.ylim(-1, 3)

plt.show()

if __name__ == '__main__':

#path1

path1 = [[0,0], [0,1], [0,2], [0,3], [0,4], [1,4], [2,4], [2,3], [2,2], [3,2], [4,2], [4,1], [4,0]]

#path2

path2 = [[0.5, 0], [0.5, -0.5], [1, -0.5], [1, 0], [1, 0.5],\

[1.5, 0.5], [1.5, 0], [1.5, -0.5], [1, -0.5], [1, 0],\

[1, 0.5], [0.5, 0.5], [0.5, 0], [0, 0], [0, 0]]

rospy.init_node('turtlebotmove')

rospy.loginfo("Press Ctrl + C to terminate")

whatever1 = Turtlebot(path1, 1, wf=0.1, ws=0.1)

# whatever2 = Turtlebot(path2, 2, wf=0.1, ws=0.1)

Gradient Descent Path Smoothing

Let P denote a set of original points (returned by discrete planning algorithm, like A*). Let T be the set of points belonging to the optimized trajectory.

For each optimized tᵢ point, define the cost function as:

Path Following (error between original and optimized point)
Smoothness term (distance between optimized point and the previous and next point)

Sample Code for Polynomial Time Scaling

Open a new terminal and go to your ee144w25 package. We will start from a new python script.

roscd ee144w25/src

touch polynomial_timescaling.py

gedit polynomial_timescaling.py

Please copy and paste the following code. If both of the team members have an M1/M2 computer, please follow the corresponding approach by replacing the Python version with ‘’python3’’ and the ROS Topic with “cmd_vel”.

#!/usr/bin/env python3

from math import pi, sqrt, atan2, cos, sin

import numpy as np

from copy import deepcopy

import rospy

import tf

from std_msgs.msg import Empty

from nav_msgs.msg import Odometry

from geometry_msgs.msg import Twist, Pose2D

from std_srvs.srv import Empty as srvEmpty

class Turtlebot():

def __init__(self, path, num_path):

# rospy.init_node(node_name)

# rospy.loginfo("Press Ctrl + C to terminate")

self.vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=10)

self.rate = rospy.Rate(10)

# reset odometry to zero

self.reset_pub = rospy.Publisher("mobile_base/commands/reset_odometry", Empty, queue_size=10)

for i in range(10):

self.reset_pub.publish(Empty())

self.rate.sleep()

# set the path

self.path = path

# subscribe to odometry

self.pose = Pose2D()

self.logging_counter = 0

self.trajectory = list()

self.odom_sub = rospy.Subscriber("odom", Odometry, self.odom_callback)

#initial conditions

try:

self.run()

except rospy.ROSInterruptException:

rospy.loginfo("Action terminated.")

finally:

# save trajectory into csv file

np.savetxt('trajectory'+str(num_path)+'.csv', np.array(self.trajectory), fmt='%f', delimiter=',')

self.reset_gazebo()

def run(self):

waypoints = self.path

for i in range(len(waypoints)-1):

self.move_to_point(waypoints[i], waypoints[i+1])

def move_to_point(self, current_waypoint, next_waypoint):

# generate polynomial trajectory and move to current_waypoint

# next_waypoint is to help determine the velocity to pass current_waypoint

pass

def polynomial_time_scaling_3rd_order(self, p_start, v_start, p_end, v_end, T):

# input: p,v: position and velocity of start/end point

# T: the desired time to complete this segment of trajectory (in second)

# output: the coefficients of this polynomial

pass

def odom_callback(self, msg):

# get pose = (x, y, theta) from odometry topic

quarternion = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,\

msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]

(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(quarternion)

self.pose.theta = yaw

self.pose.x = msg.pose.pose.position.x

self.pose.y = msg.pose.pose.position.y

# logging once every 100 times (Gazebo runs at 1000Hz; we save it at 10Hz)

self.logging_counter += 1

if self.logging_counter == 100:

self.logging_counter = 0

self.trajectory.append([self.pose.x, self.pose.y]) # save trajectory

rospy.loginfo("odom: x=" + str(self.pose.x) +\

"; y=" + str(self.pose.y) + "; theta=" + str(yaw))

def reset_gazebo(self):

rospy.wait_for_service('/gazebo/reset_world')

try:

reset_world = rospy.ServiceProxy('/gazebo/reset_world', srvEmpty)

reset_world()

print('resetting')

except rospy.ServiceException as e:

print('failed' % e)

if __name__ == '__main__':

#path1

path1 = [[0,0], [0,1], [0,2], [0,3], [0,4], [1,4], [2,4], [2,3], [2,2], [3,2], [4,2], [4,1], [4,0]]

#path2

path2 = [[0.5, 0], [0.5, -0.5], [1, -0.5], [1, 0], [1, 0.5],\

[1.5, 0.5], [1.5, 0], [1.5, -0.5], [1, -0.5], [1, 0],\

[1, 0.5], [0.5, 0.5], [0.5, 0], [0, 0], [0, 0]]

rospy.init_node('turtlebotmove')

rospy.loginfo("Press Ctrl + C to terminate")

whatever1 = Turtlebot(path1, 1)

# whatever2 = Turtlebot(path2, 2)

This script is following the same structure as the one used in Lab 3, except for the changes under run function.
You need to complete the move_to_point function in this code, and make sure the robot can pass all the waypoints. The polynomial_time_scaling_3rd_order function is provided for your information only. You may or may not use it.

Visualization

You can reuse the visualization python script provided in Lab 3 to plot the trajectory. Remember to adjust the limit on x and y axes and include the plot in the lab report.
An example of the trajectory is provided as follows. It is a bit overshooting. You can do better :)

Polynomial Time Scaling

Suppose that we have figured out a path from point A to point B. The question is how fast the robot should follow the path (i.e. where it should be at each moment). This process of generating intermediate points (each associated with a time stamp) is called time scaling, or trajectory generation at large.

Recall that sending constant velocity commands from the very beginning would assume the robot can have infinite large acceleration at the first moment, which is impractical. Therefore, we would like to have a smooth trajectory that can best fit robot kinematics.

On the other hand, polynomial functions are smooth (infinitely differentiable) functions that can provide us smooth trajectories at all orders (position, velocity, acceleration, the derivative of acceleration, and so on). To this end, polynomial time scaling becomes a good choice to generate trajectories.

Suppose that we plan to move from point A to point B in T seconds by following a straight line. Then where the robot should be at each moment can be described by the following function. It provides the position of the robot x(t) at any given time t.

Accordingly, the expected velocity at any time t can be described by the derivative of this polynomial function.

The key is to figure out the coefficients of this function for each and every trajectory segment and for both x and y coordinates. In other words, coefficients can vary from segment to segment, as each segment should adopt different initial and terminal conditions to best fit its needs. Fortunately, we can formulate this problem into a linear system of equation as follows, where:

are initial/terminal position/velocity respectively.

To solve this equation of the form x=Tα, we can simply take the advantage of the inverse matrix and have the solution α=T⁻¹ x. Once the coefficients are known, the position and the velocity at each moment can be obtained by evaluating the function x(t) and ẋ(t) at t=0,0.1,0.2,...,T. (This is an example of running at 10Hz where the time interval is 0.1s.)

Finally, a PID controller (introduced in Lab 3) can be applied to track the desired position and velocity at each moment. To closely track the trajectory, the parameter Kp can take a larger value. As before, it is possible to only track the orientation by the PID controller and simply set the linear velocity to be the magnitude. Note that the orientation setpoint in this lab is changing all the time as the robot follows the trajectory (as opposed to a fixed setpoint in Lab 3).

So far we have introduced the basic steps to solve for a polynomial time scaling problem. The following are three final remarks regarding the selection of parameters.

Two ways to pick the time interval T (this is one of the drawbacks of this approach; you have to specify T ahead of time)
- Fixed time interval for all segments.
- Pick a preferred average speed, then determine T based on the distance to travel.
Notes on boundary conditions and continuity constraints
- It is better to look at not only the current waypoint, but also the next one. Because normally the next waypoint can provide useful information to help determine how to pass the current waypoint.
- For example, when moving from point A to point B by following a straight line, knowing that point C is to the right of point B is a good indicator to curve the current trajectory a bit more, such that this turning behavior can be evenly distributed in the trajectory and avoid a sharp turn at point B.
- In practice, the magnitude of the velocity can be set to a preferred speed, and the direction of the velocity of passing point B can be set to the direction from point A to point C.
Discussion on the numerical stability of polynomial functions. It is possible to use a continuous timeline for all trajectories (i.e. [0, T₁ ] for the first segment, [T₁, T₂ ] for the second, and so on). However, this approach is not numerically stable, especially when the order of the polynomial is higher. For example, in a 7th order polynomial function, as T grows larger, to make the term α₇ t⁷ reasonably small, the parameter α₇ will have to be at the level of 10⁻¹⁰ or even smaller. Conclusion: we recommend using relative time scale [0, T] for all segments (i.e. reset timing every time).