MoveIt planning: How can I get a collision distance field from a PlanningScene?
Given a moveit::core::RobotModel that is constructed from a URDF and an SRDF, I init a planning_scene::PlanningScene. I would like to use these two objects to compute the distance to the nearest obstacle for each robot link. I've been trying to follow the CHOMP implementation, because I know their trajectory optimization uses a signed distance field to compute collision costs, but it's difficult to see where, and how it integrates with PlanningScene.
add a comment
