Fix segment fault on 32-bit machine (#459)

* fix memory leak

* increase timelimit for success rate

* update CHANGELOG

* change to use scopedState

* Update CHANGELOG.md

* Minor wording fix.

Author: dqyi11

CHANGELOG.md

@@ -50,6 +50,7 @@
   * Changed interface for TrajectoryPostProcessor: [#341](https://github.com/personalrobotics/aikido/pull/341)
   * Planning calls with InverseKinematicsSampleable constraints explicitly set MetaSkeleton to solve IK with: [#379](https://github.com/personalrobotics/aikido/pull/379)
   * Added a kinodynamic timer that generates a time-optimal smooth trajectory without completely stopping at each waypoint: [#443](https://github.com/personalrobotics/aikido/pull/443)
+  * Fixed segmentation fault on 32-bit machines in vector-field planner: [#459](https://github.com/personalrobotics/aikido/pull/459)
 
 * Robot
 

src/planner/vectorfield/detail/VectorFieldIntegrator.cpp

@@ -54,13 +54,14 @@ VectorFieldIntegrator::VectorFieldIntegrator(
     double checkConstraintResolution)
   : mVectorField(vectorField)
   , mConstraint(collisionFreeConstraint)
+  , mCacheIndex(-1)
+  , mDimension(mVectorField->getStateSpace()->getDimension())
   , mTimelimit(timelimit)
   , mConstraintCheckResolution(checkConstraintResolution)
+  , mState(mVectorField->getStateSpace()->createState())
+  , mLastEvaluationTime(0.0)
 {
-  mCacheIndex = -1;
-  mLastEvaluationTime = 0.0;
-  mDimension = mVectorField->getStateSpace()->getDimension();
-  mState = mVectorField->getStateSpace()->createState();
+  // Do nothing
 }
 
 //==============================================================================

src/planner/vectorfield/detail/VectorFieldIntegrator.hpp

@@ -115,7 +115,7 @@ class VectorFieldIntegrator
   double mConstraintCheckResolution;
 
   /// Current state in integration
-  aikido::statespace::StateSpace::State* mState;
+  aikido::statespace::StateSpace::ScopedState mState;
 
   /// Last evaluation time in checking trajectory
   double mLastEvaluationTime;

tests/planner/vectorfield/test_VectorFieldPlanner.cpp

@@ -331,10 +331,10 @@ TEST_F(VectorFieldPlannerTest, PlanToEndEffectorOffsetTest)
 
   double positionTolerance = 0.01;
   double angularTolerance = 0.15;
-  double initialStepSize = 0.001;
+  double initialStepSize = 0.01;
   double jointLimitTolerance = 1e-3;
-  double constraintCheckResolution = 1e-3;
-  std::chrono::duration<double> timelimit(10.0);
+  double constraintCheckResolution = 1e-2;
+  std::chrono::duration<double> timelimit(60.0);
 
   // Create problem.
   auto offsetProblem = ConfigurationToEndEffectorOffset(
@@ -453,9 +453,9 @@ TEST_F(VectorFieldPlannerTest, PlanToEndEffectorPoseTest)
   double poseErrorTolerance = 0.01;
   double initialStepSize = 0.05;
   double r = 1.0;
-  double jointLimitTolerance = 1e-3;
-  double constraintCheckResolution = 1e-3;
-  std::chrono::duration<double> timelimit(5.);
+  double jointLimitTolerance = 1e-2;
+  double constraintCheckResolution = 1e-2;
+  std::chrono::duration<double> timelimit(300.);
 
   mSkel->setPositions(mStartConfig);