Update for robot/util planning clean-up. (#47)

personalrobotics · Oct 23, 2020 · 7c3cb88 · 7c3cb88
1 parent d878d3f
commit 7c3cb88
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 65 deletions.
diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp
@@ -72,7 +72,7 @@ class Ada final : public aikido::robot::Robot
 
   const aikido::robot::util::VectorFieldPlannerParameters vfParams
       = aikido::robot::util::VectorFieldPlannerParameters(
-          0.2, 0.03, 0.03, 0.01, 1e-3, 1e-3, 1.0, 0.2, 0.01);
+          0.2, 0.03, 0.01, 1e-3, 1e-3, 1.0, 0.2, 0.01);
 
   /// Construct Ada metaskeleton using a URI.
   /// \param[in] env World (either for planning, post-processing, or executing).
@@ -219,16 +219,6 @@ class Ada final : public aikido::robot::Robot
       size_t maxNumTrials,
       const aikido::distance::ConfigurationRankerPtr& ranker = nullptr);
 
-  /// \copydoc ConcreteRobot::planToTSRwithTrajctoryConstraint
-  aikido::trajectory::TrajectoryPtr planToTSRwithTrajectoryConstraint(
-      const aikido::statespace::dart::MetaSkeletonStateSpacePtr& space,
-      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
-      const dart::dynamics::BodyNodePtr& bodyNode,
-      const aikido::constraint::dart::TSRPtr& goalTsr,
-      const aikido::constraint::dart::TSRPtr& constraintTsr,
-      const aikido::constraint::dart::CollisionFreePtr& collisionFree,
-      double timelimit);
-
   /// Plans to a named configuration.
   /// \param[in] startState Starting state
   /// \param[in] name Name of the configuration to plan to
@@ -250,10 +240,6 @@ class Ada final : public aikido::robot::Robot
   /// \return true if all controllers have been successfully switched
   bool stopTrajectoryExecutor();
 
-  /// \copydoc ConcreteRobot::setCRRTPlannerParameters
-  void setCRRTPlannerParameters(
-      const aikido::robot::util::CRRTPlannerParameters& crrtParameters);
-
   /// Sets VectorFieldPlanner parameters.
   /// TODO: To be removed with Planner API.
   /// \param[in] vfParameters VectorField Parameters

diff --git a/src/Ada.cpp b/src/Ada.cpp
@@ -14,7 +14,6 @@
 #include <aikido/distance/defaults.hpp>
 #include <aikido/io/yaml.hpp>
 #include <aikido/planner/kunzretimer/KunzRetimer.hpp>
-#include <aikido/planner/ompl/CRRTConnect.hpp>
 #include <aikido/planner/ompl/Planner.hpp>
 #include <aikido/robot/ConcreteManipulator.hpp>
 #include <aikido/robot/ConcreteRobot.hpp>
@@ -48,7 +47,6 @@ using aikido::robot::ConstConcreteManipulatorPtr;
 using aikido::robot::Hand;
 using aikido::robot::HandPtr;
 using aikido::robot::Robot;
-using aikido::robot::util::CRRTPlannerParameters;
 using aikido::robot::util::parseYAMLToNamedConfigurations;
 using aikido::robot::util::VectorFieldPlannerParameters;
 using aikido::statespace::StateSpace;
@@ -211,21 +209,6 @@ Ada::Ada(
 
   mTrajectoryExecutor = createTrajectoryExecutor();
 
-  // TODO: (GL) Ideally this should be set in the header as const but since
-  // it requires mRng I am keeping it here.
-  // Create default parameters (otherwise, they are undefined by default in
-  // aikido). These parameters are taken mainly from the default constructors of
-  // the structs (aikido/robot/util.hpp) and one of the herb demos.
-  CRRTPlannerParameters crrtParams(
-      &mRng,
-      5,
-      std::numeric_limits<double>::infinity(),
-      0.1,
-      0.05,
-      0.1,
-      20,
-      1e-4);
-
   // Setting arm base and end names
   mArmBaseName = "j2n6s200_link_base";
   mArmEndName = "j2n6s200_link_6";
@@ -238,7 +221,6 @@ Ada::Ada(
       mTrajectoryExecutor,
       collisionDetector,
       selfCollisionFilter);
-  mArm->setCRRTPlannerParameters(crrtParams);
   mArm->setVectorFieldPlannerParameters(vfParams);
 
   mArmSpace = mArm->getStateSpace();
@@ -252,7 +234,6 @@ Ada::Ada(
       mTrajectoryExecutor,
       collisionDetector,
       selfCollisionFilter);
-  mRobot->setCRRTPlannerParameters(crrtParams);
 
   // TODO: When the named configurations are set in resources.
   // Load the named configurations
@@ -451,26 +432,6 @@ TrajectoryPtr Ada::planToTSR(
       ranker);
 }
 
-//==============================================================================
-TrajectoryPtr Ada::planToTSRwithTrajectoryConstraint(
-    const MetaSkeletonStateSpacePtr& space,
-    const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
-    const dart::dynamics::BodyNodePtr& bodyNode,
-    const TSRPtr& goalTsr,
-    const TSRPtr& constraintTsr,
-    const CollisionFreePtr& collisionFree,
-    double timelimit)
-{
-  return mRobot->planToTSRwithTrajectoryConstraint(
-      space,
-      metaSkeleton,
-      bodyNode,
-      goalTsr,
-      constraintTsr,
-      collisionFree,
-      timelimit);
-}
-
 //==============================================================================
 TrajectoryPtr Ada::planToNamedConfiguration(
     const std::string& name,
@@ -498,12 +459,6 @@ bool Ada::stopTrajectoryExecutor()
                                mHandTrajectoryExecutorName});
 }
 
-//=============================================================================
-void Ada::setCRRTPlannerParameters(const CRRTPlannerParameters& crrtParameters)
-{
-  mRobot->setCRRTPlannerParameters(crrtParameters);
-}
-
 //=============================================================================
 void Ada::setVectorFieldPlannerParameters(
     const VectorFieldPlannerParameters& vfParameters)

diff --git a/src/AdaHand.cpp b/src/AdaHand.cpp
@@ -5,8 +5,12 @@
 #include <aikido/constraint/Satisfied.hpp>
 #include <aikido/control/KinematicSimulationTrajectoryExecutor.hpp>
 #include <aikido/control/ros/RosTrajectoryExecutor.hpp>
+#include <aikido/planner/SnapConfigurationToConfigurationPlanner.hpp>
 #include <aikido/planner/World.hpp>
+#include <aikido/planner/dart/ConfigurationToConfiguration.hpp>
+#include <aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToConfiguration.hpp>
 #include <aikido/robot/util.hpp>
+#include <aikido/statespace/GeodesicInterpolator.hpp>
 
 #include "libada/AdaHandKinematicSimulationPositionCommandExecutor.hpp"
 
@@ -18,6 +22,11 @@
 
 namespace ada {
 
+using aikido::planner::SnapConfigurationToConfigurationPlanner;
+using aikido::planner::dart::ConfigurationToConfiguration;
+using aikido::planner::dart::
+    ConfigurationToConfiguration_to_ConfigurationToConfiguration;
+using aikido::statespace::GeodesicInterpolator;
 using aikido::statespace::dart::MetaSkeletonStateSpace;
 using aikido::statespace::dart::MetaSkeletonStateSpacePtr;
 using dart::dynamics::BodyNode;
@@ -272,11 +281,21 @@ std::future<void> AdaHand::executePreshape(const std::string& preshapeName)
 
   auto satisfied = std::make_shared<Satisfied>(mSpace);
 
-  // TODO(Gilwoo): passing nullptr as random seed because
-  // this shoud just use snap planner. It should be changed to
-  // explicitly call SnapPlanner
-  auto trajectory = aikido::robot::util::planToConfiguration(
-      mSpace, mHand, goalState, satisfied, nullptr, 1.0);
+  // Snap planner is the only planner that makes sense for this.
+  auto snapConfigToConfigPlanner
+      = std::make_shared<SnapConfigurationToConfigurationPlanner>(
+          mSpace, std::make_shared<GeodesicInterpolator>(mSpace));
+
+  // Convert to DART planner.
+  auto dartSnapConfigToConfigPlanner = std::make_shared<
+      ConfigurationToConfiguration_to_ConfigurationToConfiguration>(
+      snapConfigToConfigPlanner, mHand);
+
+  // Create planning problem and plan.
+  auto problem
+      = ConfigurationToConfiguration(mSpace, mHand, goalState, satisfied);
+  auto trajectory
+      = dartSnapConfigToConfigPlanner->plan(problem, /*result*/ nullptr);
 
   if (!trajectory)
   {