In this work we present CppFlow - a novel and performant planner for the Cartesian Path Planning problem, which finds valid trajectories up to 129x faster than current methods, while also succeeding on more difficult problems where others fail. At the core of the proposed algorithm is the use of a learned, generative Inverse Kinematics solver, which is able to efficiently produce promising entire candidate solution trajectories on the GPU. Precise, valid solutions are then found through classical approaches such as differentiable programming, global search, and optimization. In combining approaches from these two paradigms we get the best of both worlds - efficient approximate solutions from generative AI which are made exact using the guarantees of traditional planning and optimization. We evaluate our system against other state of the art methods on a set of established baselines as well as new ones introduced in this work and find that our method significantly outperforms others in terms of the time to find a valid solution and planning success rate, and performs comparably in terms of trajectory length over time. The work is made open source and will be made available soon.
The Cartesian Path Planning problem, otherwise known as Pathwise-Inverse Kinematics (Pathwise-IK) problem is defined whereby the robot must generate smooth, collision free trajectories (including robot-robot and robot-environment collisions) that result in the end effector tracking a specified cartesian space path. In this paper, we consider this task for redundant robots - those with 7 or greater degrees of freedom (DoFs) - which may have an infinite number of IK solutions for a given pose. It is this redundancy which makes the problem difficult, as IK solutions no longer have a discrete family they can be checked against - such as elbow up or down.
To address the CPP problem, we present CppFlow, a novel planner that utilizes recent advances in learned, generative IK to generate smooth, collision free paths faster than existing SOTA methods while also succeeding on more difficult problems. Using a generative IK model addresses a key issue with trajectory optimization methods - that they work well but require a good initial solution. In this work we use the Levenberg-Marquardt algorithm for trajectory optimization, a powerful quasi-Newton optimization procedure that quickly converges to precise and constraint satisfying solutions. Additionally, a search module finds the optimally smooth config-space path by interweaving the trajectories returned by the generative IK model, which dramatically improves the quality of the optimization seed.
Trajectory length (rad) convergence results for CppFlow and Torm. A lower value is better, as this indicates a trajectory which can be executed in shorter time. Plots are generated by averaging the trajectory length convergence curves of the designated planner from 10 planning runs. The mean time to a valid solution time is the first time instant plotted. The convergence behavior for CppFlow is strictly better for 'Fetch.Full - square' and 'Panda - 1cube', as measured by initial valid solution time and asymptotic limit. CppFlow has a 1.169x larger final trajectory length for 'Fetch.Arm - circle' problem as compared to Torm, however it takes Torm 1.83x as long for Torm to find an initial valid trajectory for this problem
Results from CppFlow, Torm, and Stampede on the 13 test problems. 'Time to Valid Solution' - median time to find a trajectory with less than 1mm/.1deg pose error and all other constraints satisfied. 'Initial Solution Time' - median time to find a trajectory, regardless of its validity. Failed runs count for inf planning time for both 'Time to Valid Solution' and 'Initial Solution Time'. 'Planning Success Rate, x seconds' - the percentage of runs which have found a valid trajectory before x seconds have elapsed. 'Trajectory length (rad/m), x seconds': The mean cumulative change in configuration-space of the revolute, prismatic joints of the latest valid trajectory found before x seconds have elapsed. A lower trajectory is better, as generally this means the robot can execute the trajectory faster. The astricks on the stampede* rows indicates that the planning success rate and trajectory length is for the final returned trajectory, which may be returned after 50s. Stampede failed to run at all with the Fetch.Full kinematic chain so was excluded from the results. CppFlow returns an initial solution faster than either of the other methods on all problems. It will take Torm and Stampede 19.84x and 132.8x as long on average respectively to find a valid solution
@INPROCEEDINGS{10611724,
author={Morgan, Jeremy and Millard, David and Sukhatme, Gaurav S.},
booktitle={2024 IEEE International Conference on Robotics and Automation (ICRA)},
title={CppFlow: Generative Inverse Kinematics for Efficient and Robust Cartesian Path Planning},
year={2024},
volume={},
number={},
pages={12279-12785},
keywords={Adaptation models;Generative AI;Graphics processing units;Kinematics;Programming;Trajectory;Planning},
doi={10.1109/ICRA57147.2024.10611724}
}