Self collision within splines

apps/test_drgbt.cpp

@@ -75,6 +75,8 @@ int main(int argc, char **argv)
 			output_file << "Number of extensions for generating a generalized bur:  " << RGBTConnectConfig::NUM_LAYERS << std::endl;
 			output_file << "Number of iterations when computing a single spine:     " << RBTConnectConfig::NUM_ITER_SPINE << std::endl;
 			output_file << "Using expanded bubble when generating a spine:          " << (RBTConnectConfig::USE_EXPANDED_BUBBLE ? "true" : "false") << std::endl;
+			output_file << "Trajectory interpolation:                               " << DRGBTConfig::TRAJECTORY_INTERPOLATION << std::endl;
+			output_file << "Guaranteed safe motion:                                 " << (DRGBTConfig::GUARANTEED_SAFE_MOTION ? "true" : "false") << std::endl;
 			output_file << "--------------------------------------------------------------------\n";
 			output_file << "Real-time scheduling:                                   " << DRGBTConfig::REAL_TIME_SCHEDULING << std::endl;
 			output_file	<< "Maximal iteration time [s]:                             " << DRGBTConfig::MAX_ITER_TIME << std::endl;

include/state_spaces/StateSpace.h

@@ -31,7 +31,6 @@ namespace base
 		inline size_t getNumDimensions() { return num_dimensions; }
 		inline virtual base::StateSpaceType getStateSpaceType() const { return state_space_type; };
 		virtual std::shared_ptr<base::State> getRandomState(const std::shared_ptr<base::State> q_center = nullptr) = 0;
-		virtual std::shared_ptr<base::State> getNewState(const std::shared_ptr<base::State> q) = 0;
 		virtual std::shared_ptr<base::State> getNewState(const Eigen::VectorXf &coord) = 0;
 
 		virtual float getNorm(const std::shared_ptr<base::State> q1, const std::shared_ptr<base::State> q2) = 0;

include/state_spaces/real_vector_space/RealVectorSpace.h

@@ -24,7 +24,6 @@ namespace base
 		virtual ~RealVectorSpace();
 
 		std::shared_ptr<base::State> getRandomState(const std::shared_ptr<base::State> q_center) override;
-		std::shared_ptr<base::State> getNewState(const std::shared_ptr<base::State> q) override;
 		std::shared_ptr<base::State> getNewState(const Eigen::VectorXf &coord) override;
 
 		float getNorm(const std::shared_ptr<base::State> q1, const std::shared_ptr<base::State> q2) override;

include/state_spaces/real_vector_space/RealVectorSpaceState.h

@@ -14,7 +14,6 @@ namespace base
 	{
 	public:
 		RealVectorSpaceState(const Eigen::VectorXf &coord_);
-		RealVectorSpaceState(const std::shared_ptr<base::State> state);
 		~RealVectorSpaceState() {}
 	};
 }

src/planners/drbt/Splines.cpp

@@ -214,7 +214,7 @@ bool planning::drbt::Splines::computeSafe(Eigen::VectorXf &current_pos, Eigen::V
                       spline_next_new;
 
         // std::cout << "spline_next: \n" << spline_next << "\n";
-        spline_computed = !checkCollision(ss->getNewState(q_current), t_iter);
+        spline_computed = !checkCollision(q_current, t_iter);
     }
 
     return spline_computed;
@@ -234,24 +234,27 @@ bool planning::drbt::Splines::checkCollision(std::shared_ptr<base::State> q_init
     float rho_robot {};
     float rho_obs {};
     float t_init { 0 };
-    Eigen::VectorXf q_final {};
+    std::shared_ptr<base::State> q_final { nullptr };
 	Eigen::VectorXf delta_q {};
 
     for (float t = delta_t; t <= spline_next->getTimeFinal() + RealVectorSpaceConfig::EQUALITY_THRESHOLD; t += delta_t)
     {
         // std::cout << "Considering t: " << t << "\n";
+        q_final = ss->getNewState(spline_next->getPosition(t));
+        if (ss->robot->checkSelfCollision(q_final))
+            return true;
+
         t_iter += delta_t;
-        q_final = spline_next->getPosition(t);
         rho_obs = max_obs_vel * (t_iter - t_init);
         rho_robot = 0;
-        delta_q = (q_final - q_init->getCoord()).cwiseAbs();
+        delta_q = (q_final->getCoord() - q_init->getCoord()).cwiseAbs();
         for (size_t i = 0; i < ss->robot->getNumDOFs(); i++)
             rho_robot += q_init->getEnclosingRadii()->col(i+1).dot(delta_q);
 
         if (rho_robot + rho_obs >= q_init->getDistance())    // Possible collision
         {
             // std::cout << "********** Possible collision ********** \n";
-            q_init = ss->getNewState(q_final);
+            q_init = q_final;
             q_init->setDistance(computeDistanceUnderestimation(q_init, q_current->getNearestPoints(), t_iter));
             ss->robot->computeEnclosingRadii(q_init);
             t_init = t_iter;

src/planners/drbt/replanning.cpp

@@ -25,23 +25,25 @@ bool planning::drbt::DRGBT::whetherToReplan()
 std::unique_ptr<planning::AbstractPlanner> planning::drbt::DRGBT::initStaticPlanner(float max_planning_time)
 {
     // std::cout << "Static planner (for replanning): " << DRGBTConfig::STATIC_PLANNER_TYPE << "\n";
+    std::shared_ptr<base::State> q_goal_ { ss->getNewState(q_goal->getCoord()) };
+
     switch (DRGBTConfig::STATIC_PLANNER_TYPE)
     {
     case planning::PlannerType::RGBMTStar:
         RGBMTStarConfig::MAX_PLANNING_TIME = max_planning_time;
-        return std::make_unique<planning::rbt_star::RGBMTStar>(ss, q_current, q_goal);
+        return std::make_unique<planning::rbt_star::RGBMTStar>(ss, q_current, q_goal_);
 
     case planning::PlannerType::RGBTConnect:
         RGBTConnectConfig::MAX_PLANNING_TIME = max_planning_time;
-        return std::make_unique<planning::rbt::RGBTConnect>(ss, q_current, q_goal);
+        return std::make_unique<planning::rbt::RGBTConnect>(ss, q_current, q_goal_);
 
     case planning::PlannerType::RBTConnect:
         RBTConnectConfig::MAX_PLANNING_TIME = max_planning_time;
-        return std::make_unique<planning::rbt::RBTConnect>(ss, q_current, q_goal);
+        return std::make_unique<planning::rbt::RBTConnect>(ss, q_current, q_goal_);
 
     case planning::PlannerType::RRTConnect:
         RRTConnectConfig::MAX_PLANNING_TIME = max_planning_time;
-        return std::make_unique<planning::rrt::RRTConnect>(ss, q_current, q_goal);
+        return std::make_unique<planning::rrt::RRTConnect>(ss, q_current, q_goal_);
 
     default:
         throw std::domain_error("The requested static planner is not found! ");

src/planners/rbt/RBTConnect.cpp

@@ -157,7 +157,7 @@ base::State::Status planning::rbt::RBTConnect::connectSpine
 
 	while (status == base::State::Status::Advanced && num_ext++ < RRTConnectConfig::MAX_EXTENSION_STEPS)
 	{
-		q_temp = ss->getNewState(q_new);
+		q_temp = q_new;
 		if (d_c > RBTConnectConfig::D_CRIT)
 		{
 			tie(status, q_new) = extendSpine(q_temp, q_e);

src/planners/rbt/RGBTConnect.cpp

@@ -101,7 +101,7 @@ std::tuple<base::State::Status, std::shared_ptr<std::vector<std::shared_ptr<base
 
     for (size_t i = 0; i < RGBTConnectConfig::NUM_LAYERS; i++)
     {
-        q_temp = ss->getNewState(q_new);
+        q_temp = q_new;
         tie(status, q_new) = extendSpine(q_temp, q_e);
 		q_new_list.emplace_back(q_new);
         d_c = ss->computeDistanceUnderestimation(q_new, q->getNearestPoints());
@@ -124,7 +124,7 @@ base::State::Status planning::rbt::RGBTConnect::connectGenSpine
 
 	while (status == base::State::Status::Advanced && num_ext++ < RRTConnectConfig::MAX_EXTENSION_STEPS)
 	{
-		q_temp = ss->getNewState(q_new);
+		q_temp = q_new;
 		if (d_c > RBTConnectConfig::D_CRIT)
 		{
 			tie(status, q_new_list) = extendGenSpine2(q_temp, q_e);

src/planners/rbt_star/RGBMTStar.cpp

@@ -219,7 +219,7 @@ std::tuple<base::State::Status, std::shared_ptr<base::State>> planning::rbt_star
 
 	while (status == base::State::Status::Advanced)
 	{
-		q_temp = ss->getNewState(q_new);
+		q_temp = q_new;
 		if (d_c > RBTConnectConfig::D_CRIT)
 		{
 			tie(status, q_new) = extendGenSpine(q_temp, q_e);
@@ -266,27 +266,27 @@ std::shared_ptr<base::State> planning::rbt_star::RGBMTStar::optimize
     std::shared_ptr<base::State> q_opt { nullptr }; 
     if (q_reached->getParent() != nullptr)
     {
-        Eigen::VectorXf q_opt_ { q_reached->getCoord() };  // It is surely collision-free. It will become an optimal state later
-        Eigen::VectorXf q_parent_ { q_reached->getParent()->getCoord() };   // Needs to be collision-checked
+        Eigen::VectorXf q_opt_coord { q_reached->getCoord() };  // It is surely collision-free. It will become an optimal state later
+        Eigen::VectorXf q_parent_coord { q_reached->getParent()->getCoord() };   // Needs to be collision-checked
         std::shared_ptr<base::State> q_middle { ss->getRandomState() };
         size_t max_iter = std::ceil(std::log2(ss->getNorm(q_reached->getParent(), q_reached) / RRTConnectConfig::EPS_STEP));
         bool update { false };
 
         for (size_t i = 0; i < max_iter; i++)
         {
-            q_middle->setCoord((q_opt_ + q_parent_) / 2);
+            q_middle->setCoord((q_opt_coord + q_parent_coord) / 2);
             if (std::get<0>(connectGenSpine(q, q_middle)) == base::State::Status::Reached)
             {
-                q_opt_ = q_middle->getCoord();
+                q_opt_coord = q_middle->getCoord();
                 update = true;
             }
             else
-                q_parent_ = q_middle->getCoord();
+                q_parent_coord = q_middle->getCoord();
         }
 
         if (update)
         {
-            q_opt = ss->getNewState(q_opt_);
+            q_opt = ss->getNewState(q_opt_coord);
             q_opt->setCost(q_reached->getParent()->getCost() + computeCostToCome(q_reached->getParent(), q_opt));
             q_opt->addChild(q_reached);
             tree->upgradeTree(q_opt, q_reached->getParent());
@@ -321,8 +321,8 @@ void planning::rbt_star::RGBMTStar::unifyTrees(const std::shared_ptr<base::Tree>
                                                const std::shared_ptr<base::State> q0_con)
 {
     std::shared_ptr<base::State> q_considered { nullptr };
-    std::shared_ptr<base::State> q_con_new { ss->getNewState(q_con) };
-    std::shared_ptr<base::State> q0_con_new { ss->getNewState(q0_con) };
+    std::shared_ptr<base::State> q_con_new { q_con };
+    std::shared_ptr<base::State> q0_con_new { q0_con };
 
     while (true)
     {

src/planners/rrt/RRTConnect.cpp

@@ -109,12 +109,13 @@ base::State::Status planning::rrt::RRTConnect::connect
 {
 	// std::cout << "Inside connect. \n";
 	std::shared_ptr<base::State> q_new { q };
+	std::shared_ptr<base::State> q_temp { nullptr };
 	base::State::Status status { base::State::Status::Advanced };
 	size_t num_ext { 0 };
 
 	while (status == base::State::Status::Advanced && num_ext++ < RRTConnectConfig::MAX_EXTENSION_STEPS)
 	{
-		std::shared_ptr<base::State> q_temp { ss->getNewState(q_new) };
+		q_temp = q_new;
 		tie(status, q_new) = extend(q_temp, q_e);
 		if (status != base::State::Status::Trapped)
 			tree->upgradeTree(q_new, q_temp);

src/robots/xArm6.cpp

@@ -287,7 +287,7 @@ bool robots::xArm6::checkSelfCollision(const std::shared_ptr<base::State> q1, st
 		return false;
 
 	// auto time_start { std::chrono::steady_clock::now() };
-	std::shared_ptr<base::State> q1_temp { std::make_shared<base::RealVectorSpaceState>(q1) };
+	std::shared_ptr<base::State> q1_temp { q1 };
 	size_t num_iter { 0 };
 	size_t max_num_iter { 5 };
 	float phi2 {}, phi4 {}, phi5 {}, phi6 {};

src/state_spaces/real_vector_space/RealVectorSpace.cpp

@@ -26,23 +26,17 @@ namespace base
 // If 'q_center' is passed, it is added to the random state 
 std::shared_ptr<base::State> base::RealVectorSpace::getRandomState(const std::shared_ptr<base::State> q_center)
 {
-	Eigen::VectorXf q_rand { Eigen::VectorXf::Random(num_dimensions) };
+	Eigen::VectorXf q_rand_coord { Eigen::VectorXf::Random(num_dimensions) };
 	std::vector<std::pair<float, float>> limits { robot->getLimits() };
 
 	for (size_t i = 0; i < num_dimensions; i++)
-		q_rand(i) = ((limits[i].second - limits[i].first) * q_rand(i) + limits[i].first + limits[i].second) / 2;
+		q_rand_coord(i) = ((limits[i].second - limits[i].first) * q_rand_coord(i) + limits[i].first + limits[i].second) / 2;
 
 	if (q_center != nullptr)
-		q_rand += q_center->getCoord();
+		q_rand_coord += q_center->getCoord();
 
-	// std::cout << "Random state coord: " << q_rand.transpose();
-	return getNewState(q_rand);
-}
-
-// Get a copy of 'state'
-std::shared_ptr<base::State> base::RealVectorSpace::getNewState(const std::shared_ptr<base::State> state)
-{
-	return std::make_shared<base::RealVectorSpaceState>(state);
+	// std::cout << "Random state coord: " << q_rand_coord.transpose();
+	return getNewState(q_rand_coord);
 }
 
 // Get completely a new state with the same coordinates as 'state'
@@ -112,7 +106,7 @@ std::tuple<base::State::Status, std::shared_ptr<base::State>> base::RealVectorSp
 	}
 	else
 	{
-		q_new = getNewState(q2);
+		q_new = getNewState(q2->getCoord());
 		status = base::State::Status::Reached;
 	}
 

src/state_spaces/real_vector_space/RealVectorSpaceState.cpp

@@ -4,23 +4,3 @@ base::RealVectorSpaceState::RealVectorSpaceState(const Eigen::VectorXf &coord_)
 {
 	state_space_type = base::StateSpaceType::RealVectorSpace;
 }
-
-// Make a copy of 'state'
-base::RealVectorSpaceState::RealVectorSpaceState(const std::shared_ptr<base::State> state) : State()
-{
-	state_space_type = base::StateSpaceType::RealVectorSpace;
-	coord = state->getCoord();
-	num_dimensions = state->getNumDimensions();
-	tree_idx = state->getTreeIdx();
-	idx = state->getIdx();
-	d_c = state->getDistance();
-	d_c_profile = state->getDistanceProfile();
-	is_real_d_c = state->getIsRealDistance();
-	cost = state->getCost();
-	nearest_points = state->getNearestPoints();
-	parent = state->getParent();
-	children = state->getChildren();
-	frames = state->getFrames();
-	skeleton = state->getSkeleton();
-	enclosing_radii = state->getEnclosingRadii();
-}