diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h
index 3912b8a3a3..faf8d71928 100644
--- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h
+++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h
@@ -207,6 +207,8 @@ class PlanningContextManager
 
 protected:
   typedef std::function<const ModelBasedStateSpaceFactoryPtr&(const std::string&)> StateSpaceFactoryTypeSelector;
+  // ros::M_string::const_iterator constrained_state_space_type_iterator_ ;
+  // std::string constrained_state_space_type_;
 
   ConfiguredPlannerAllocator plannerSelector(const std::string& planner) const;
 
diff --git a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp
index 17552d6d1b..d66dbc56da 100644
--- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp
+++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp
@@ -543,7 +543,7 @@ void LinearSystemPositionConstraint::function(const Eigen::Ref<const Eigen::Vect
   out[1] = (end_position_.z() - start_position_.z())*(cartesianPosition.x() - start_position_.x())-(end_position_.x() - start_position_.x())*(cartesianPosition.z() - start_position_.z());
   // (xb-xa)(y-ya)-(yb-ya)(x-xa)=0
   // residual[2] = (end_position_.x() - start_position_.x())*(cartesianPosition.y() - start_position_.y())-(end_position_.y() - start_position_.y())*(cartesianPosition.x() - start_position_.x());
-  out[2]=0.0;
+  // out[2]=0.0;
   // for (std::size_t dim{ 0 }; dim < 3; ++dim)
   // {
   //   if (is_dim_constrained_[dim])
@@ -587,7 +587,7 @@ void LinearSystemPositionConstraint::function(const Eigen::Ref<const Eigen::Vect
   // // ROS_INFO_STREAM_NAMED(LOGNAME, "Residual of the start position " << residual);
 
   // center_position_ =center_position;
-  out[2] = 0.0;  // unbounded dimension
+  // out[2] = 0.0;  // unbounded dimension
   }
   else if(knot_number_>3) {
   ROS_ERROR_STREAM_NAMED(LOGNAME, "Splines are currently not supported. Number of knots given: " << knot_number_);
diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
index f8edb660f0..678a58d6f4 100644
--- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
+++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
@@ -49,11 +49,15 @@
 #include <moveit/profiler/profiler.h>
 #include <moveit/utils/lexical_casts.h>
 
+#include <moveit_msgs/Constraints.h>
+
 #include <ompl/config.h>
 #include <ompl/base/samplers/UniformValidStateSampler.h>
 #include <ompl/base/goals/GoalLazySamples.h>
 #include <ompl/tools/config/SelfConfig.h>
 #include <ompl/base/spaces/SE3StateSpace.h>
+#include <ompl/base/spaces/constraint/AtlasStateSpace.h>
+#include <ompl/base/spaces/constraint/TangentBundleStateSpace.h>
 #include <ompl/datastructures/PDF.h>
 // TODO: remove when ROS Melodic and older are no longer supported
 #if OMPL_VERSION_VALUE < 1005000
@@ -120,11 +124,49 @@ void ompl_interface::ModelBasedPlanningContext::configure(const ros::NodeHandle&
   if (spec_.constrained_state_space_)
   {
     // convert the input state to the corresponding OMPL state
-    ompl::base::ScopedState<> ompl_start_state(spec_.constrained_state_space_);
+    ob::ScopedState<> ompl_start_state(spec_.constrained_state_space_);
     spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState());
     ompl_simple_setup_->setStartState(ompl_start_state);
     ompl_simple_setup_->setStateValidityChecker(
         ob::StateValidityCheckerPtr(std::make_shared<ConstrainedPlanningStateValidityChecker>(this)));
+    ROS_INFO_STREAM("start state from getCompleteInitialRobotState: " << getCompleteInitialRobotState());
+
+    std::map<std::string, std::string> cfg = getSpecificationConfig();
+    auto it = cfg.find("constrained_state_space");
+    if (it != cfg.end())
+    {
+      // extract joint state goal from planning request
+      // auto constraints = request_.goal_constraints[0];
+      // ROS_INFO_STREAM("goal constraints from planning request: " << constraints);
+
+      // ob::ScopedState<> goal(spec_.constrained_state_space_);
+      // goal->as<ob::ConstrainedStateSpace::StateType>()->copy(goal_joint_positions);
+      // ROS_INFO_STREAM("goal reconstructed from planning request: " << goal);
+
+      // for Atlas and TangentBundle, the start and goal states have to be anchored.
+      if (it->second == "TangentBundleStateSpace")
+      {
+        ROS_INFO_NAMED("model_based_planning_context",
+                       "trying to anchor start states for constrained tangent bundle state space.");
+        spec_.constrained_state_space_->as<ob::TangentBundleStateSpace>()->anchorChart(ompl_start_state.get());
+        ROS_INFO_NAMED("model_based_planning_context",
+                       "trying to anchor goal states for constrained tangent bundle state space.");
+        // spec_.constrained_state_space_->as<ob::TangentBundleStateSpace>()->anchorChart(goal.get());
+        ROS_INFO_NAMED("model_based_planning_context",
+                       "start and goal states anchored for constrained tangent bundle state space.");
+      }
+      else if (it->second == "AtlasStateSpace")
+      {
+        ROS_INFO_NAMED("model_based_planning_context",
+                       "trying to anchor start states for constrained atlas state space.");
+        spec_.constrained_state_space_->as<ob::AtlasStateSpace>()->anchorChart(ompl_start_state.get());
+        ROS_INFO_NAMED("model_based_planning_context",
+                       "trying to anchor goal states for constrained atlas state space.");
+        // spec_.constrained_state_space_->as<ob::AtlasStateSpace>()->anchorChart(goal.get());
+        ROS_INFO_NAMED("model_based_planning_context",
+                       "start and goal states anchored for constrained atlas state space.");
+      }
+    }
   }
   else
   {
@@ -133,6 +175,7 @@ void ompl_interface::ModelBasedPlanningContext::configure(const ros::NodeHandle&
     spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState());
     ompl_simple_setup_->setStartState(ompl_start_state);
     ompl_simple_setup_->setStateValidityChecker(std::make_shared<StateValidityChecker>(this));
+    ROS_INFO_NAMED("model_based_planning_context", "start and goal states not anchored.");
   }
 
   if (path_constraints_ && constraints_library_)
diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
index e69cb52d5e..726e3bec26 100644
--- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
+++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
@@ -179,7 +179,8 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations()
     // the set of planning parameters that can be specific for the group (inherited by configurations of that group)
     static const std::string KNOWN_GROUP_PARAMS[] = { "projection_evaluator", "longest_valid_segment_fraction",
                                                       "enforce_joint_model_state_space",
-                                                      "enforce_constrained_state_space" };
+                                                      "enforce_constrained_state_space",
+                                                      "constrained_state_space" };
 
     // get parameters specific for the robot planning group
     std::map<std::string, std::string> specific_group_params;
diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp
index 49a36a55f8..50d1651f1a 100644
--- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp
+++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp
@@ -69,6 +69,8 @@
 
 #include <ompl/base/ConstrainedSpaceInformation.h>
 #include <ompl/base/spaces/constraint/ProjectedStateSpace.h>
+#include <ompl/base/spaces/constraint/TangentBundleStateSpace.h>
+#include <ompl/base/spaces/constraint/AtlasStateSpace.h>
 
 #include <moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h>
 #include <moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h>
@@ -358,11 +360,34 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana
       ompl::base::ConstraintPtr ompl_constraint =
           ompl_interface::createOMPLConstraint(robot_model_, config.group, req.path_constraints);
 
-      // Create a constrained state space of type "projected state space".
       // Other types are available, so we probably should add another setting to ompl_planning.yaml
       // to choose between them.
+  //     const std::map<std::string, std::string>& config = config.config;
+  // if (config.empty())
+  //   return;
+  std::map<std::string, std::string> cfg = config.config;
+  auto it = cfg.find("constrained_state_space");
+    if (it != cfg.end())
+  {    
+if (it->second == "TangentBundleStateSpace")
+{
+      context_spec.constrained_state_space_ =
+          std::make_shared<ob::TangentBundleStateSpace>(context_spec.state_space_, ompl_constraint);
+  ROS_INFO_NAMED("planning_context_manager", "Created a constrained state space of type: tangent bundle state space.");
+}
+else if (it->second == "AtlasStateSpace")
+{
+      context_spec.constrained_state_space_ =
+          std::make_shared<ob::AtlasStateSpace>(context_spec.state_space_, ompl_constraint);
+  ROS_INFO_NAMED("planning_context_manager", "Created a constrained state space of type: atlas state space.");
+}}
+else // ProjectedStateSpace
+{
+      // Create a constrained state space of type "projected state space".
       context_spec.constrained_state_space_ =
           std::make_shared<ob::ProjectedStateSpace>(context_spec.state_space_, ompl_constraint);
+  ROS_INFO_NAMED("planning_context_manager", "Created a constrained state space of type: projected state space.");
+}
 
       // Pass the constrained state space to ompl simple setup through the creation of a
       // ConstrainedSpaceInformation object. This makes sure the state space is properly initialized.
@@ -524,6 +549,8 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana
   // in JointModelStateSpace.
   StateSpaceFactoryTypeSelector factory_selector;
   auto constrained_planning_iterator = pc->second.config.find("enforce_constrained_state_space");
+  // constrained_state_space_type_iterator_ = pc->second.config.find("constrained_state_space");
+// constrained_state_space_type_ = constrained_state_space_type_iterator->second;
   auto joint_space_planning_iterator = pc->second.config.find("enforce_joint_model_state_space");
 
   if (constrained_planning_iterator != pc->second.config.end() &&