New vector field planner impelementation

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

@@ -0,0 +1,70 @@
+#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 field 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);
+
+  /// Vectorfield callback function
+  ///
+  /// \param[out] _qd Joint velocities
+  /// \return Whether joint velocities are successfully computed
+  virtual bool getJointVelocities(Eigen::VectorXd& _qd) const = 0;
+
+  /// Vectorfield planning status callback function
+  ///
+  /// \return Status of planning
+  virtual VectorFieldPlannerStatus checkPlanningStatus() const = 0;
+
+  /// Meta skeleton state space
+  aikido::statespace::dart::MetaSkeletonStateSpacePtr
+  getMetaSkeletonStateSpace() const;
+
+  /// Returns meta skeleton
+  dart::dynamics::MetaSkeletonPtr getMetaSkeleton();
+
+  /// Returns const meta skeleton
+  dart::dynamics::ConstMetaSkeletonPtr getMetaSkeleton() const;
+
+  /// Returns body node of end-effector
+  dart::dynamics::BodyNodePtr getBodyNode();
+
+  /// Returns const body node of end-effector
+  dart::dynamics::ConstBodyNodePtr getBodyNode() const;
+
+protected:
+  /// State space of vector field
+  aikido::statespace::dart::MetaSkeletonStateSpacePtr mStateSpace;
+  /// Meta Skeleton of state space
+  dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
+  /// BodyNode of end-effector
+  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,68 @@ 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] _jointLimitPadding 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 _jointLimitPadding = 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);
+  bool getJointVelocities(Eigen::VectorXd& _qd) const 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);
+  VectorFieldPlannerStatus checkPlanningStatus() const 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:
+  /// Movement direction.
+  Eigen::Vector3d mDirection;
+  /// Movement distance.
+  double mDistance;
+  /// Maximum distance allowed to move.
+  double mMaxDistance;
+  /// Tolerance of linear deviation error.
+  double mPositionTolerance;
+  /// Tolerance of angular error.
   double mAngularTolerance;
+  /// Linear velocity gain.
+  double mLinearVelocityGain;
+  /// Initial step size in adaptive integration.
+  double mInitialStepSize;
+  /// Padding for joint limits.
+  double mJointLimitPadding;
+  /// Tolerance of optimization solver.
   double mOptimizationTolerance;
-  double mPadding;
+  /// Start pose of the end-effector.
   Eigen::Isometry3d mStartPose;
-  Eigen::Isometry3d mTargetPose;
 };
 
 } // namespace vectorfield

include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp

@@ -0,0 +1,80 @@
+#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 end-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] _jointLimitPadding 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 _jointLimitPadding = 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
+  bool getJointVelocities(Eigen::VectorXd& _qd) const override;
+
+  /// Vectorfield planning status callback function
+  ///
+  /// \param[in] _q Position in configuration space
+  /// \param[in] _t Current time being planned
+  /// \return Status of planning
+  VectorFieldPlannerStatus checkPlanningStatus() const override;
+
+protected:
+  /// Goal pose.
+  Eigen::Isometry3d mGoalPose;
+  /// Tolerance of pose error.
+  double mPoseErrorTolerance;
+  /// Linear velocity gain.
+  double mLinearVelocityGain;
+  /// Angular velocit gain.
+  double mAngularVelocityGain;
+  /// Initial step size of adaptive integration.
+  double mInitialStepSize;
+  /// Padding of joint limits
+  double mJointLimitPadding;
+  /// Tolerance of optimization solver.
+  double mOptimizationTolerance;
+  Eigen::VectorXd mVelocityLowerLimits;
+  Eigen::VectorXd mVelocityUpperLimits;
+};
+
+} // namespace vectorfield
+} // namespace planner
+} // namespace aikido
+
+#endif // ifndef
+       // AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTORPOSEVECTORFIELD_HPP_