bool TrajectoryChecker::isTrajectoryValid(const std::vector<PlanarPose> &poses) const { for (auto p : poses) { if (!isPoseValid(p)) return false; } return true; }
bool RRT::sampleFree(KDL::Frame &sample_free) const { KDL::Frame x; for (int i=0; i < 100; i++) { sampleSpace(x); if (isPoseValid(x)) { sample_free = x; return true; } } return false; }