Changes the contact test types in the motion planner profiles to:
FIRST for Descartes and OMPL, since they just need to know whether or not a state is in collision
CLOSEST for TrajOpt, which allows it to converge more quickly for large collision objects (because fewer constraint equations get added to the optimization) while generally converging successfully
Allow users to select how the scan mesh is represented as a collision object for planning
convex_mesh: current behavior, converts the scan mesh into a convex hull. This is generally results in the fastest motion plans, especially with TrajOpt, but can be too conservative and causing motion planning failures if the scan object is not actually convex or nearly convex
mesh: represents the collision object as the exact "detailed" mesh represented by the mesh file. With the contact test type CLOSEST for TrajOpt, this representation results in a somewhat slower planning time than convex_mesh, but not significantly longer
octree: represents the collision object as an octree comprised of spheres with a diameter specified by the octree_resolution parameter. With the contact test type CLOSEST for TrajOpt, this representation results in slightly faster planning times than mesh but slower than convex_mesh
I also changed the planning server to add and remove this link to the nominal planning environment, such that a representation of its collision geometry would show up in Rviz using the Tesseract widgets
marip8
commented
9 months ago
This PR makes two changes for the motion planner:
FIRSTfor Descartes and OMPL, since they just need to know whether or not a state is in collisionCLOSESTfor TrajOpt, which allows it to converge more quickly for large collision objects (because fewer constraint equations get added to the optimization) while generally converging successfullyconvex_mesh: current behavior, converts the scan mesh into a convex hull. This is generally results in the fastest motion plans, especially with TrajOpt, but can be too conservative and causing motion planning failures if the scan object is not actually convex or nearly convexmesh: represents the collision object as the exact "detailed" mesh represented by the mesh file. With the contact test typeCLOSESTfor TrajOpt, this representation results in a somewhat slower planning time thanconvex_mesh, but not significantly longeroctree: represents the collision object as an octree comprised of spheres with a diameter specified by theoctree_resolutionparameter. With the contact test typeCLOSESTfor TrajOpt, this representation results in slightly faster planning times thanmeshbut slower thanconvex_meshI also changed the planning server to add and remove this link to the nominal planning environment, such that a representation of its collision geometry would show up in Rviz using the Tesseract widgets
Convex mesh
Mesh
Octree