Given a [moveit::core::RobotModel](https://github.com/ros-planning/moveit/blob/kinetic-devel/moveit_core/robot_model/include/moveit/robot_model/robot_model.h#L72) that is constructed from a URDF and an SRDF, I init a [planning_scene::PlanningScene](https://github.com/ros-planning/moveit/blob/kinetic-devel/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h). 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](https://github.com/ros-planning/moveit/tree/kinetic-devel/moveit_planners/chomp), 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.
↧