Fix bug in collision checking of VPF (#303)

personalrobotics · Feb 6, 2018 · 1c53ebb · 1c53ebb
1 parent 8f50e38
commit 1c53ebb
Show file tree

Hide file tree

Showing 6 changed files with 9 additions and 8 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -19,6 +19,7 @@
 * Planner
 
   * Added parabolic timing for linear spline [#302](https://github.com/personalrobotics/aikido/pull/302)
+  * Fixed step sequence iteration in VPF: [#303](https://github.com/personalrobotics/aikido/pull/303)
 
 ### 0.2.0 (2018-01-09)
 
@@ -39,7 +40,7 @@
 * Planner
 
   * Added World class: [#243](https://github.com/personalrobotics/aikido/pull/243), [#252](https://github.com/personalrobotics/aikido/pull/252), [#265](https://github.com/personalrobotics/aikido/pull/265)
-  * Added vector field planner [#246](https://github.com/personalrobotics/aikido/pull/246), [#262](https://github.com/personalrobotics/aikido/pull/262), [#268](https://github.com/personalrobotics/aikido/pull/268)
+  * Added vector field planner: [#246](https://github.com/personalrobotics/aikido/pull/246), [#262](https://github.com/personalrobotics/aikido/pull/262), [#268](https://github.com/personalrobotics/aikido/pull/268)
 
 * RViz
 

diff --git a/include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp b/include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp
@@ -73,7 +73,7 @@ class BodyNodePoseVectorField : public VectorField
       const aikido::constraint::Testable* constraint,
       double evalStepSize,
       double& evalTimePivot,
-      bool excludeEndTime) const override;
+      bool includeEndTime) const override;
 
   /// Return meta skeleton state space.
   aikido::statespace::dart::MetaSkeletonStateSpacePtr

diff --git a/include/aikido/planner/vectorfield/VectorField.hpp b/include/aikido/planner/vectorfield/VectorField.hpp
@@ -48,15 +48,15 @@ class VectorField
   /// \param[in/out] evalTimePivot Input provides the start time of the
   /// trajectory
   /// to evaluate; output returns the end time of the trajectory evaluate.
-  /// \param[in] excludeEndTime Whether end time is excluded in evaluation.
+  /// \param[in] includeEndTime Whether end time is included in evaluation.
   /// evaluate.
   /// satisfaction.
   virtual bool evaluateTrajectory(
       const aikido::trajectory::Trajectory& trajectory,
       const aikido::constraint::Testable* constraint,
       double evalStepSize,
       double& evalTimePivot,
-      bool excludeEndTime) const = 0;
+      bool includeEndTime) const = 0;
 
   /// Returns state space.
   aikido::statespace::StateSpacePtr getStateSpace();

diff --git a/src/planner/vectorfield/BodyNodePoseVectorField.cpp b/src/planner/vectorfield/BodyNodePoseVectorField.cpp
@@ -97,7 +97,7 @@ bool BodyNodePoseVectorField::evaluateTrajectory(
     const aikido::constraint::Testable* constraint,
     double evalStepSize,
     double& evalTimePivot,
-    bool excludeEndTime) const
+    bool includeEndTime) const
 {
   if (constraint == nullptr)
   {
@@ -108,7 +108,7 @@ bool BodyNodePoseVectorField::evaluateTrajectory(
   aikido::common::StepSequence seq(
       evalStepSize,
       true,
-      excludeEndTime,
+      includeEndTime,
       evalTimePivot,
       trajectory.getEndTime());
 

diff --git a/src/planner/vectorfield/VectorFieldPlanner.cpp b/src/planner/vectorfield/VectorFieldPlanner.cpp
@@ -115,7 +115,7 @@ std::unique_ptr<aikido::trajectory::Spline> followVectorField(
             &constraint,
             checkConstraintResolution,
             lastEvaluationTime,
-            false))
+            true))
     {
       planningResult->message = "Constraint violated.";
       return nullptr;

diff --git a/src/planner/vectorfield/detail/VectorFieldIntegrator.cpp b/src/planner/vectorfield/detail/VectorFieldIntegrator.cpp
@@ -131,7 +131,7 @@ void VectorFieldIntegrator::check(const Eigen::VectorXd& q, double t)
             mConstraint,
             mConstraintCheckResolution,
             mLastEvaluationTime,
-            true))
+            false))
     {
       throw ConstraintViolatedError();
     }