search

ros-industrial-consortium / scan_n_plan_workshop

48 stars 25 forks source link

Fully support min distance to contact during planning #88

Closed marip8 closed 3 months ago

marip8 commented 3 months ago

Currently the contact check task checks if a trajectory is a specified minimum distance from contact, but none of the planners create trajectories considering this constraint. This PR adds this minimum distance to contact value to the collision margin config of the custom Descartes, OMPL, and Trajopt profiles

Builds on #87

marip8 commented 3 months ago

This seems to fail Descartes often for non-zero values of min_contact_dist for our applications because the TCP link is generally very close to the scan link, but not generally added to the list of touch links. We may need to consider adding an allowed collision between a tool link and the scan link specifically in the Descartes (and possibly TrajOpt) collision checking config to account for this

marrts commented 3 months ago

Instead of adding an allowed collision you could add a unique collision pair between the tcp link and the scan that is 0. Would be a nice addition to this demo code as well.