The PHSpline trajectory planner is a modified version of a global trajectory planner that uses 2 dimensional PH Splines to interpolate between consecutive goals. It includes an implementation of the Pythagorean Hodograph spline in Matlab. It has a modified version of the goal_passer Global Planner, that plugs into
nav_core::BaseGlobal planner interface. In addition, it uses the pose_follower as a Local planner that plugs into
nav_core::BaseLocalPlanner . It introduces a new package called phspline_generator that generates control points for the PH Spline and interfaces with Matlab.
hg clone ssh://firstname.lastname@example.org/pkganeshpk/phspline_planner
What are PH Splines?
As given in the linked document above, a PH Spline is a special kind of interpolating spline used to smooth the path between a set of waypoints. It is a variation of a BSpline, with the advantage that its arclength, curvature and other desired parameters can be found using purely analytical expressions. PH splines are computationally tractable and provide good enough solutions to nontrivial optimization problems. Also, their equations can be determined with relative ease by a nonlinear solver, and their shape can be tweaked to tradeoff arclength against curvature.
- phspline_generator (new)
- phspline_goal_passer (modified goal_passer)
- phspline_pose_follower (modified pose_follower)
- + Matlab code
The nodes , component APIs, services, etc. are exactly the same as that for a standard invocation of move_base . However, PH Spline Trajectory Planner introduces the following configuration options for phspline_goal_passer, which in turn get passed on to phspline_generator:
- ~cpg_number : The number of control points to generate between a start and end position, default 5
- ~cpg_horiz_tol : Horizontal tolerance for the control points, default 0.5. These get generated uniformly between the start and end points; we need to “perturb” them so that a BSpline approximation can be got. This parameter specifies the extent to which we can perturb the x coordinate.
- ~cpg_vert_tol: The same as above, except that y-coordinates are perturbed.
- ~ph_control_points: The list of control points for the PH Spline, no default.
- ~ph_use_control_as_waypoints: Retrofit control points as waypoints, default: false. Not yet implemented for the true case.
- ~ph_use_tsp: Use TSP to order intermediate control points, default:false.
- ~ph_output_discretization: The interval at which points on the output PH Spline need to be sampled, default: 0.5.
- ~ph_limit_spline: The last waypoint at which to give output
Consider a standard invocation of move_base with goals being sent on /move_base/goal. With the unmodified version of goal_passer, the goal gets passed on to pose_follower. The pose_follower follows a straight line from current pose to target pose. This combination of goal_passer/pose_follower closely mimics the behaviour of NavFnROS/base_local_planner on a blank costmap with no obstacles.
In our modified version, the following is the code flow:
- The goal pose gets sent to pshpline_goal_passer, which uses a ControlPointGenerator class (within the phspline_generator package) to generate intermediate control points between the start and the end poses. The ~cpg params are used in doing so.
- The control point vector thus generated gets passed on to a PHSpline generator class , together with the ~ph params.
- This latter class interfaces with Matlab (using a YAML format as input to Matlab) and causes Matlab to generate an output file of sampled PH spline poses (positions + orientations).
- These points get read by phspline_goal_passer and passed on to pose_follower as a trajectory.
Note that the PH spline trajectory always passes through the start and the goal points.
To reinforce the points made above, the following is a summary of the involved classes:
- GoalPasser: A modified version of GoalPasser, using PHSplines ; package: phspline_goal_passer
- ControlPointGenerator: generates control points with GoalPasser input; package : phspline_generator
- PHSplineGenerator: generates a PH Spline in the way detailed above; package: phspline_generator
- PoseFollower: A modified version of existing PoseFollower; disables collision checks and backwards movement; package:phspline_pose_follower
Default goal_passer outperforms phspline_goal_passer
Consider a set of goals the goals (0,0) (2.5,0), (5.0,0) and (5.0,5.0). The goal_passer gets these goals in serial order, while phspline_goal_passer is configured with the same goals . We record and analyze the cmd_vel and odom of the rosbag outputs in each case.
- A modified configuration file for phspline_goal_passer : With a better choice of control points, phspline_goal_passer does as follows with cmd_vel:
How to get this output : Use the configuration files here
from the command line in Terminal Window 1. Next, open the simple navigation goals package in Terminal Window 2 and run
./bin/simple_navigation_goals input.txt . In Terminal window 3, collect rosbag output.
The odom plot shows the 4 waypoints marked with black dots. The path in red is the one taken by erratic.
Above is the cmd_vel plot of erratic during the same tour. There are 2 spikes in velocity marked S1 and S2 where the robot turned abruptly. Our goal with PH splines is to minimize the number of spikes.
How to run this code : Use the configuration files here
demo_2dnav_empty.launch and in Window 2, run simple navigation goals as given above. Note that arbitrary goals may be sent here . They have no effect but to trigger goal_passer to execute code that passes through the PH Spline defined by the control points. When the matlab window opens up, type “main_wrapper” in the command prompt. You will see Matlab quickly execute commands and then quit. Next, erratic begins to move along the spline waypoints.
Clearly the angular velocity profile is much more jagged than the one with goal_passer, which has clearly done better in this case.
How to run this code : Same as above, except use the control points in here . Use the procedure given in Point 2 above. Observe that we changed the control points as well as used the limit_spline parameter to sidestep the loop in the PH Spline above.
- The above simulation indicates that we can achieve a smooth path , suitable for the vehicle to take, through well-chosen control points. We do not know any auotmated way to do this. We advocate the judicious use of PH Splines in trajectory planning.
- Currently, the package uses files to communicate between ROS and Matlab. We have been unable to get
IPC bridge or other similar packages working on our systems. We plan to follow this more standard approach sometime in future.