New vector field planner impelementation

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

@@ -0,0 +1,75 @@
+#ifndef AIKIDO_PLANNER_VECTORFIELD_CONFIGURATIONSPACEVECTORFIELD_HPP_
+#define AIKIDO_PLANNER_VECTORFIELD_CONFIGURATIONSPACEVECTORFIELD_HPP_
+
+#include <aikido/planner/vectorfield/VectorFieldPlannerStatus.hpp>
+#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
+
+namespace aikido {
+namespace planner {
+namespace vectorfield {
+
+/// This class defines a vector field in a configuration space
+///
+/// Any vector filed should inherit this class to implememnt functions
+/// for calculating joint velocities giving joint positions and time,
+/// and checking planning status.
+class ConfigurationSpaceVectorField
+{
+public:
+  /// Constructor
+  ///
+  /// \param[in] _stateSpace MetaSkeleton state space
+  /// \param[in] _bn Body node of end-effector
+  ConfigurationSpaceVectorField(
+      aikido::statespace::dart::MetaSkeletonStateSpacePtr _stateSpace,
+      dart::dynamics::BodyNodePtr _bodyNode)
+    : mStateSpace(_stateSpace)
+    , mMetaSkeleton(_stateSpace->getMetaSkeleton())
+    , mBodyNode(_bodyNode)
+  {
+  }
+
+  /// Vectorfield callback function
+  ///
+  /// \param[out] _qd Joint velocities
+  /// \return Whether joint velocities are successfully computed
+  virtual bool getJointVelocities(Eigen::VectorXd& _qd) = 0;
+
+  /// Vectorfield planning status callback function
+  ///
+  /// \return Status of planning
+  virtual VectorFieldPlannerStatus checkPlanningStatus() = 0;
+
+  /// Meta skeleton spate space
+  virtual aikido::statespace::dart::MetaSkeletonStateSpacePtr
+  getMetaSkeletonStateSpace()
+  {
+    return mStateSpace;
+  }
+
+  /// Meta skeleton
+  virtual dart::dynamics::MetaSkeletonPtr getMetaSkeleton()
+  {
+    return mMetaSkeleton;
+  }
+
+  /// Body node of end-effector
+  virtual dart::dynamics::BodyNodePtr getBodyNode()
+  {
+    return mBodyNode;
+  }
+
+protected:
+  aikido::statespace::dart::MetaSkeletonStateSpacePtr mStateSpace;
+  dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
+  dart::dynamics::BodyNodePtr mBodyNode;
+};
+
+using ConfigurationSpaceVectorFieldPtr
+    = std::shared_ptr<ConfigurationSpaceVectorField>;
+
+} // namespace vectorfield
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_VECTORFIELD_CONFIGURATIONSPACEVECTORFIELD_HPP_

include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp

@@ -1,7 +1,7 @@
 #ifndef AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTOROFFSETVECTORFIELD_HPP_
 #define AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTOROFFSETVECTORFIELD_HPP_
 
-#include <aikido/planner/vectorfield/VectorFieldPlanner.hpp>
+#include <aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp>
 
 namespace aikido {
 namespace planner {
@@ -12,70 +12,58 @@ namespace vectorfield {
 /// This class defines two callback functions for vectorfield planner.
 /// One for generating joint velocity in MetaSkeleton state space,
 /// and one for determining vectorfield planner status.
-class MoveEndEffectorOffsetVectorField
+class MoveEndEffectorOffsetVectorField : public ConfigurationSpaceVectorField
 {
 public:
   /// Constructor
   ///
+  /// \param[in] _stateSpace MetaSkeleton state space
   /// \param[in] _bn Body node of end-effector
-  /// \param[in] _linearVelocity Linear velocity in all the directions
-  /// \param[in] _startTime Start time of a planned trajectory
-  /// \param[in] _endTime End time of a planned trajectory
-  /// \param[in] _timestep Time step size
-  /// \param[in] _linearGain Gain of P control on linear deviation
-  /// \param[in] _linearTolerance Tolerance on linear deviation
-  /// \param[in] _angularGain Gain of P control on angular deviation
-  /// \param[in] _angularTolerance Tolerance on angular deviation
+  /// \param[in] _direction Unit vector in the direction of motion
+  /// \param[in] _distance Minimum distance in meters
+  /// \param[in] _maxDistance Maximum distance in meters
+  /// \param[in] _positionTolerance Constraint tolerance in meters
+  /// \param[in] _angularTolerance Constraint tolerance in radians
+  /// \param[in] _linearVelocityGain Linear velocity gain in workspace.
+  /// \param[in] _initialStepSize Initial step size
+  /// \param[in] _jointLimitTolerance If less then this distance to joint
+  /// limit, velocity is bounded in that direction to 0
   /// \param[in] _optimizationTolerance Tolerance on optimization
-  /// \param[in] _padding Padding to the boundary
   MoveEndEffectorOffsetVectorField(
+      aikido::statespace::dart::MetaSkeletonStateSpacePtr _stateSpace,
       dart::dynamics::BodyNodePtr _bn,
-      const Eigen::Vector3d& _linearVelocity,
-      double _startTime,
-      double _endTime,
-      double _timestep,
-      double _linearGain = 1.,
-      double _linearTolerance = 0.01,
-      double _angularGain = 1.,
-      double _angularTolerance = 0.01,
-      double _optimizationTolerance = 1e-3,
-      double _padding = 1e-5);
+      const Eigen::Vector3d& _direction,
+      double _distance,
+      double _maxDistance = std::numeric_limits<double>::max(),
+      double _positionTolerance = 0.01,
+      double _angularTolerance = 0.15,
+      double _linearVelocityGain = 1.0,
+      double _initialStepSize = 1e-1,
+      double _jointLimitTolerance = 3e-2,
+      double _optimizationTolerance = 1e-3);
 
   /// Vectorfield callback function
   ///
-  /// \param[in] _stateSpace MetaSkeleton state space
-  /// \param[in] _t Current time being planned
-  /// \param[out] _dq Joint velocities
+  /// \param[out] _qd Joint velocities
   /// \return Whether joint velocities are successfully computed
-  bool operator()(
-      const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace,
-      double _t,
-      Eigen::VectorXd& _dq);
+  virtual bool getJointVelocities(Eigen::VectorXd& _qd) override;
 
   /// Vectorfield planning status callback function
   ///
-  /// \param[in] _stateSpace MetaSkeleton state space
-  /// \param[in] _t Current time being planned
   /// \return Status of planning
-  VectorFieldPlannerStatus operator()(
-      const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace,
-      double _t);
+  virtual VectorFieldPlannerStatus checkPlanningStatus() override;
 
-private:
-  dart::dynamics::BodyNodePtr mBodynode;
-  Eigen::Vector3d mVelocity;
-  Eigen::Vector3d mLinearDirection;
-  double mStartTime;
-  double mEndTime;
-  double mTimestep;
-  double mLinearGain;
-  double mLinearTolerance;
-  double mAngularGain;
+protected:
+  Eigen::Vector3d mDirection;
+  double mDistance;
+  double mMaxDistance;
+  double mPositionTolerance;
   double mAngularTolerance;
+  double mLinearVelocityGain;
+  double mInitialStepSize;
+  double mJointLimitTolerance;
   double mOptimizationTolerance;
-  double mPadding;
   Eigen::Isometry3d mStartPose;
-  Eigen::Isometry3d mTargetPose;
 };
 
 } // namespace vectorfield

include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp

@@ -0,0 +1,74 @@
+#ifndef AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTORPOSEVECTORFIELD_HPP_
+#define AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTORPOSEVECTORFIELD_HPP_
+
+#include <aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp>
+
+namespace aikido {
+namespace planner {
+namespace vectorfield {
+
+/// Vector field for moving end-effector by a direction and distance.
+///
+/// This class defines two callback functions for vectorfield planner.
+/// One for generating joint velocity in MetaSkeleton state space,
+/// and one for determining vectorfield planner status.
+class MoveEndEffectorPoseVectorField : public ConfigurationSpaceVectorField
+{
+public:
+  /// Constructor
+  ///
+  /// \param[in] _stateSpace MetaSkeleton state space
+  /// \param[in] _bn Body node of end-effector
+  /// \param[in] _goalPose Desired en-effector pose
+  /// \param[in] _poseErrorTolerance Constraint error tolerance in meters
+  /// \param[in] _linearVelocityGain Linear velocity gain in workspace
+  /// \param[in] _angularVelocityGain Angular velocity gain in workspace
+  /// \param[in] _initialStepSize Initial step size
+  /// \param[in] _jointLimitTolerance If less then this distance to joint
+  /// limit, velocity is bounded in that direction to 0
+  /// \param[in] _optimizationTolerance Tolerance on optimization
+  MoveEndEffectorPoseVectorField(
+      aikido::statespace::dart::MetaSkeletonStateSpacePtr _stateSpace,
+      dart::dynamics::BodyNodePtr _bn,
+      const Eigen::Isometry3d& _goalPose,
+      double _linearVelocityGain = 1.0,
+      double _angularVelocityGain = 1.0,
+      double _poseErrorTolerance = 0.5,
+      double _initialStepSize = 1e-1,
+      double _jointLimitTolerance = 3e-2,
+      double _optimizationTolerance = 5e-2);
+
+  /// Vectorfield callback function
+  ///
+  /// \param[in] _q Position in configuration space
+  /// \param[in] _t Current time being planned
+  /// \param[out] _qd Joint velocities
+  /// \return Whether joint velocities are successfully computed
+  virtual bool getJointVelocities(Eigen::VectorXd& _qd) override;
+
+  /// Vectorfield planning status callback function
+  ///
+  /// \param[in] _q Position in configuration space
+  /// \param[in] _t Current time being planned
+  /// \return Status of planning
+  virtual VectorFieldPlannerStatus checkPlanningStatus() override;
+
+protected:
+  Eigen::Isometry3d mGoalPose;
+  double mPoseErrorTolerance;
+  double mLinearVelocityGain;
+  double mAngularVelocityGain;
+  double mInitialStepSize;
+  double mJointLimitTolerance;
+  double mOptimizationTolerance;
+
+  Eigen::VectorXd mVelocityLowerLimits;
+  Eigen::VectorXd mVelocityUpperLimits;
+};
+
+} // namespace vectorfield
+} // namespace planner
+} // namespace aikido
+
+#endif // ifndef
+       // AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTORPOSEVECTORFIELD_HPP_