Extend Robot.cpp to allow setting/getting of Default VF Params (#630)

* Remove bug where Kunz hangs on empty trajectory * Update CHANGELOG * Increase timeout and add fuzzy zero to robot trajectory execution * Shorten timeout again, that wasn't an issue * Add VFParams getter/setter to Robot class
personalrobotics · Feb 11, 2023 · 549bca6 · 549bca6
1 parent bd2e675
commit 549bca6
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 17 deletions.
diff --git a/include/aikido/robot/Robot.hpp b/include/aikido/robot/Robot.hpp
@@ -413,6 +413,12 @@ class Robot
   // Utility function to get root skeleton
   dart::dynamics::SkeletonPtr getRootSkeleton();
 
+  // Utility function to get Default VF Params
+  util::VectorFieldPlannerParameters getVFParams() const;
+
+  // Utility function to set Default VF Params
+  void setVFParams(util::VectorFieldPlannerParameters params);
+
 protected:
   std::string mName;
   dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
@@ -447,6 +453,9 @@ class Robot
   using ConfigurationMap
       = std::unordered_map<std::string, const Eigen::VectorXd>;
   ConfigurationMap mNamedConfigurations;
+
+  // Parameters for Default VF Planners
+  util::VectorFieldPlannerParameters mVFParams;
 };
 
 } // namespace robot

diff --git a/src/robot/Robot.cpp b/src/robot/Robot.cpp
@@ -566,18 +566,17 @@ trajectory::TrajectoryPtr Robot::planToOffset(
   {
     using planner::vectorfield::
         VectorFieldConfigurationToEndEffectorOffsetPlanner;
-    auto vfParams = util::VectorFieldPlannerParameters();
     dartPlanner
         = std::make_shared<VectorFieldConfigurationToEndEffectorOffsetPlanner>(
             mStateSpace,
             getMetaSkeleton(),
-            vfParams.distanceTolerance,
-            vfParams.positionTolerance,
-            vfParams.angularTolerance,
-            vfParams.initialStepSize,
-            vfParams.jointLimitTolerance,
-            vfParams.constraintCheckResolution,
-            vfParams.timeout);
+            mVFParams.distanceTolerance,
+            mVFParams.positionTolerance,
+            mVFParams.angularTolerance,
+            mVFParams.initialStepSize,
+            mVFParams.jointLimitTolerance,
+            mVFParams.constraintCheckResolution,
+            mVFParams.timeout);
   }
 
   // Solve the problem with the DART planner.
@@ -672,19 +671,18 @@ trajectory::TrajectoryPtr Robot::planToPoseOffset(
   {
     using planner::vectorfield::
         VectorFieldConfigurationToEndEffectorPosePlanner;
-    auto vfParams = util::VectorFieldPlannerParameters();
     dartPlanner
         = std::make_shared<VectorFieldConfigurationToEndEffectorPosePlanner>(
             mStateSpace,
             getMetaSkeleton(),
-            vfParams.goalTolerance,
-            vfParams.angleDistanceRatio,
-            vfParams.positionTolerance,
-            vfParams.angularTolerance,
-            vfParams.initialStepSize,
-            vfParams.jointLimitTolerance,
-            vfParams.constraintCheckResolution,
-            vfParams.timeout);
+            mVFParams.goalTolerance,
+            mVFParams.angleDistanceRatio,
+            mVFParams.positionTolerance,
+            mVFParams.angularTolerance,
+            mVFParams.initialStepSize,
+            mVFParams.jointLimitTolerance,
+            mVFParams.constraintCheckResolution,
+            mVFParams.timeout);
     result = std::make_shared<
         VectorFieldConfigurationToEndEffectorPosePlanner::Result>();
   }
@@ -976,6 +974,20 @@ dart::dynamics::SkeletonPtr Robot::getRootSkeleton()
   return mMetaSkeleton->getBodyNode(0)->getSkeleton();
 }
 
+//=============================================================================
+// Utility function to get Default VF Params
+util::VectorFieldPlannerParameters Robot::getVFParams() const
+{
+  return mVFParams;
+}
+
+//=============================================================================
+// Utility function to set Default VF Params
+void Robot::setVFParams(util::VectorFieldPlannerParameters params)
+{
+  mVFParams = params;
+}
+
 } // namespace robot
 } // namespace aikido