New vector field planner impelementation #268

dqyi11 · 2017-11-26T20:01:58Z

This PR address the enhancement #267 .
The changes include

A new ConfigurationSpaceVectorField is created.
- MoveEndEffectorOffsetVectorField is added
- MoveEndEffectorPoseVectorField is added
A new VectorFieldPlanner class is created
- Call boost::numeric::odeint::integrate_adaptive to integrate by adaptive step size
- Add planToEndEffectorPose
Added test cases for planToEndEffectorPose

TODO
These will be added in another PR.

Collision checking of a trajectory (or segment) will be added (using sampling based method in prpy)
MoveEndEffectorAlongWorkspacePathVectorField will be added
planWorkspacePath will be added
Document new methods and classes
Format code with make format

Before merging a pull request

Set version target by selecting a milestone on the right side
Summarize this change in CHANGELOG.md
Add unit test(s) for this change

codecov · 2017-11-26T20:36:34Z

Codecov Report

Merging #268 into master will increase coverage by 0.32%.
The diff coverage is 70.6%.

@@            Coverage Diff             @@
##           master     #268      +/-   ##
==========================================
+ Coverage   70.56%   70.88%   +0.32%     
==========================================
  Files         188      195       +7     
  Lines        5537     5605      +68     
  Branches      869      875       +6     
==========================================
+ Hits         3907     3973      +66     
- Misses       1097     1099       +2     
  Partials      533      533

Impacted Files	Coverage Δ
include/aikido/statespace/StateSpace.hpp	`100% <ø> (ø)`	⬆️
.../aikido/statespace/dart/MetaSkeletonStateSpace.hpp	`100% <ø> (ø)`	⬆️
include/aikido/planner/vectorfield/VectorField.hpp	`100% <100%> (ø)`
...r/vectorfield/MoveEndEffectorOffsetVectorField.hpp	`100% <100%> (ø)`	⬆️
...do/planner/vectorfield/BodyNodePoseVectorField.hpp	`100% <100%> (ø)`
...anner/vectorfield/detail/VectorFieldIntegrator.hpp	`100% <100%> (ø)`
...ner/vectorfield/MoveEndEffectorPoseVectorField.hpp	`100% <100%> (ø)`
...ectorfield/detail/VectorFieldPlannerExceptions.cpp	`18.18% <18.18%> (ø)`
...ectorfield/detail/VectorFieldPlannerExceptions.hpp	`20% <20%> (ø)`
...r/vectorfield/MoveEndEffectorOffsetVectorField.cpp	`48.71% <52.94%> (-7.29%)`	⬇️
... and 17 more

jslee02 · 2017-11-27T20:04:32Z

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

+
+/// This class defines a vector field in a configuration space
+///
+/// Any vector filed should inherit this class to implememnt functions


typo: filed --> field

jslee02 · 2017-11-27T20:05:25Z

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

+    , mMetaSkeleton(_stateSpace->getMetaSkeleton())
+    , mBodyNode(_bodyNode)
+  {
+  }


Please move the function definitions to the .cpp file.

jslee02 · 2017-11-27T20:06:17Z

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

+  ///
+  /// \param[out] _qd Joint velocities
+  /// \return Whether joint velocities are successfully computed
+  virtual bool getJointVelocities(Eigen::VectorXd& _qd) = 0;


I think this function can be a const function.

jslee02 · 2017-11-27T20:06:41Z

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

+  /// Vectorfield planning status callback function
+  ///
+  /// \return Status of planning
+  virtual VectorFieldPlannerStatus checkPlanningStatus() = 0;


I think this function can be a const function.

jslee02 · 2017-11-27T20:25:47Z

include/aikido/planner/vectorfield/VectorFieldPlanner.hpp

@@ -2,58 +2,88 @@
 #define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNER_HPP_

 #include <functional>
+#include <dart/common/Timer.hpp>


I guess you used this for debugging. We could remove this.

Never mind, dart::common::Timer is actually used in this class.

jslee02 · 2017-11-28T00:41:58Z

src/planner/vectorfield/VectorFieldUtil.cpp

@@ -52,6 +53,7 @@ std::unique_ptr<aikido::trajectory::Spline> convertToSpline(
 }

 //==============================================================================
+


Nit: Adding empty line here is not our convention.

jslee02 · 2017-11-28T00:42:24Z

src/planner/vectorfield/VectorFieldUtil.cpp

@@ -62,26 +64,32 @@ DesiredTwistFunction::DesiredTwistFunction(
 }

 //==============================================================================
+


Nit: Adding empty line here is not our convention.

jslee02 · 2017-11-28T00:42:28Z

src/planner/vectorfield/VectorFieldUtil.cpp

 double DesiredTwistFunction::eval(const Eigen::VectorXd& _qd)
 {
  return 0.5 * (mJacobian * _qd - mTwist).squaredNorm();
 }

 //==============================================================================
+


Nit: Adding empty line here is not our convention.

jslee02 · 2017-11-28T00:42:31Z

src/planner/vectorfield/VectorFieldUtil.cpp

 void DesiredTwistFunction::evalGradient(
    const Eigen::VectorXd& _qd, Eigen::Map<Eigen::VectorXd> _grad)
 {
  _grad = mJacobian.transpose() * (mJacobian * _qd - mTwist);
 }

 //==============================================================================
+


Nit: Adding empty line here is not our convention.

…com/personalrobotics/aikido into enhancement/dqyi/vectorFieldPlanner

jslee02

I like this set of changes in terms of creating a class for planning algorithm and adding more useful vector field classes.

I've made some comments.

jslee02 · 2017-11-28T03:15:48Z

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

+  /// \return Status of planning
+  virtual VectorFieldPlannerStatus checkPlanningStatus() = 0;
+
+  /// Meta skeleton spate space


Typo: spate --> state

jslee02 · 2017-11-28T03:33:35Z

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

+  virtual VectorFieldPlannerStatus checkPlanningStatus() = 0;
+
+  /// Meta skeleton spate space
+  virtual aikido::statespace::dart::MetaSkeletonStateSpacePtr


Why are these functions (and below getters) virtual? Any possibility that a subclass overrides this function?

No. I don't know why I wrote that way. Good catch! Thanks!

jslee02 · 2017-11-28T03:34:31Z

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

+  getMetaSkeletonStateSpace()
+  {
+    return mStateSpace;
+  }


Please add const functions for

getMetaSkeletonStateSpace()

getMetaSkeleton()

getBodyNode()

that return pointers to const objects accordingly like aikido::statespace::dart::MetaSkeletonStateSpaceConstPtr getMetaSkeletonStateSpace() const where MetaSkeletonStateSpaceConstPtr is defined as using MetaSkeletonStateSpaceConstPtr = std::shared_ptr<const MetaSkeletonStateSpace>.

This is necessary for const-correctness. Without const functions of this getters, a const instance of ConfigurationSpaceVectorField might be able to modify the MetaSkeletonStateSpace through this getter for example, which is not an expected behavior from a const instance.

Edit: MetaSkeletonStateSpaceConstPtr --> ConstMetaSkeletonStateSpacePtr

Should ConstPtr be defined in original header file? Fox example using MetaSkeletonStateSpaceConstPtr = std::shared_ptr<const MetaSkeletonStateSpace> in MetaSkeletonStateSpace.hpp?

jslee02 · 2017-11-28T03:40:11Z

include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp

-      double _optimizationTolerance = 1e-3,
-      double _padding = 1e-5);
+      const Eigen::Vector3d& _direction,
+      double _distance,


Nit: _minDistance to be consistent with _maxDistance?

This is following the naming in prpy. This is usually the expected distance to move. The planner will terminate once this distance is reached. It will fail it _maxDistance is reached.

That is not what the planner currently does it PrPy. It integrates as far as possible until it either reaches _maxDistance or is unable to continue. Then, it returns success if the resulting trajectory has length greater than _distance. I.e. the planner will return a trajectory of length [_distance, _maxDistance] upon success.

I agree with @jslee02's naming suggestion - I think _minDistance is more descriptive.

jslee02 · 2017-11-28T03:42:38Z

include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp

+  ///
+  /// \param[in] _stateSpace MetaSkeleton state space
+  /// \param[in] _bn Body node of end-effector
+  /// \param[in] _goalPose Desired en-effector pose


Typo: en-effector --> end-effector

jslee02 · 2017-11-29T02:06:22Z

include/aikido/planner/vectorfield/VectorFieldPlanner.hpp

+  ///
+  /// \param[in] _q Position in configuration space
+  /// \param[in] _t Current time being planned
+  void check(const Eigen::VectorXd& _q, double _t);


I think step and check functions should be protected because they're just used internally in this class.

jslee02 · 2017-11-29T02:10:36Z

include/aikido/planner/vectorfield/VectorFieldPlanner.hpp

-    double _t,
-    Eigen::VectorXd& _dq)>;
+  VectorFieldPlanner(
+      const aikido::planner::vectorfield::ConfigurationSpaceVectorFieldPtr


Nit: We don't need to specify the whole namespaces. This can be simplified as

const ConfigurationSpaceVectorFieldPtr

Please remove aikido::[planner::[vectorfield]] in this header.

jslee02 · 2017-11-29T02:19:35Z

include/aikido/planner/vectorfield/VectorFieldPlanner.hpp

+  int mIndex;
+
+  bool mEnableCollisionCheck;
+  bool mEnableDofLimitCheck;


It seems mEnableCollisionCheck and mEnableDofLimitCheck are not actually used because they are always overwritten in followVectorField().

I think we should either (1) remove these member variables and always pass them as parameters of followVectorField() or (2) add getters and setters for them and remove those parameters from followVectorField(). I'm inclined to (2) because those parameters should represent state of the planner but not temporary variables.

If we choose (2) then we might want to set those parameters in the constrcutor.

These two member variables are actually used in check() and step(). These two functions are only internally called inside followVectorField(). These two parameters are about how you shall follow a vector field (like integral time), so I put them as input arguments of followVectorField(). They are set to member variables so that check() and step() could access. Any suggestion?

jslee02 · 2017-11-29T02:36:40Z

include/aikido/planner/vectorfield/VectorFieldPlanner.hpp

@@ -63,31 +93,81 @@ std::unique_ptr<aikido::trajectory::Spline> planPathByVectorField(
 /// \param[in] _constraint Trajectory-wide constraint that must be satisfied
 /// \param[in] _direction Direction of moving the end-effector
 /// \param[in] _distance  Distance of moving the end-effector
-/// \param[in] _linearVelocity Linear velocity of moving the end-effector
-/// \param[in] _linearTolerance How a planned trajectory is allowed to
+/// \param[in] _maxDistance Max distance of moving the end-effector


I'm confused with what this function wants to do. Does this function plan a trajectory moving the EE for a certain distance (> distance & > maxDistance) along the given direction, which is my interpretation according to the logic of checkPlanningStatus().

Once the moved distance is larger than distance, it will terminate and return a trajectory.
But if the moved distance is larger than maxDistance, it violates and will fail.

It's different from my understanding of your implementation. According to this logic, the integration keeps going even when movedDistance is greater than mDistance (that is set by _maxDistance), and it only terminates when mDistance is greater than mMaxDistance. Am I missing something here?

Yes. You are right. I will update the doc strings accordingly :)

No, the logic that @jslee02 described is correct. See my other comment about what the behavior currently is in PrPy, which is what this function mimics.

I am happy to revisit that behavior 🙃 But I think it's quite useful in practice.

jslee02 · 2017-11-29T02:38:18Z

src/planner/vectorfield/VectorFieldPlanner.cpp

+      || status == VectorFieldPlannerStatus::CACHE_AND_TERMINATE)
+  {
+    throw VectorFieldTerminated(
+        "Planning was terminated by the StatusCallback.");


Why do we treat the termination of planning (which I believe it's successfully terminated) as an exception?

You are right. This is the way of stopping integration before it is done. Usually we set 10 sec to integrate but can reach an expected distance before that. Any other suggested way?

jslee02 · 2017-11-29T08:44:35Z

include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp

      double _optimizationTolerance = 1e-3);

  /// Vectorfield callback function
  ///
  /// \param[out] _qd Joint velocities
  /// \return Whether joint velocities are successfully computed
-  virtual bool getJointVelocities(Eigen::VectorXd& _qd) override;
+  virtual bool getJointVelocities(Eigen::VectorXd& _qd) const override;


Nit: Please remove virtual. virtual is redundant for an overridden function.

jslee02 · 2017-11-29T08:44:39Z

include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp


  /// Vectorfield planning status callback function
  ///
  /// \return Status of planning
-  virtual VectorFieldPlannerStatus checkPlanningStatus() override;
+  virtual VectorFieldPlannerStatus checkPlanningStatus() const override;


Nit: Please remove virtual. virtual is redundant for an overridden function.

jslee02 · 2017-11-29T08:44:50Z

include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp

@@ -44,22 +44,22 @@ class MoveEndEffectorPoseVectorField : public ConfigurationSpaceVectorField
  /// \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;
+  virtual bool getJointVelocities(Eigen::VectorXd& _qd) const override;


Nit: Please remove virtual. virtual is redundant for an overridden function.

jslee02 · 2017-11-29T08:44:54Z

include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp


  /// 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;
+  virtual VectorFieldPlannerStatus checkPlanningStatus() const override;


Nit: Please remove virtual. virtual is redundant for an overridden function.

jslee02

Looks good to me! 👍

mkoval

Thanks for tackling this refactor @dqyi11!

Some high-level comments:

Integration Time. The time parameter used by the integrator is not meaningful because the planner does not consider dynamics. Therefore, integration time should be treated as an implementation detail of the integrator, i.e. not used as a termination condition or argument to the vector field. This may require calling the Stepper directly, rather than using the integrate_adaptive() helper method. Thankfully, that method does not actually do very much.

Non-Rn State Space Support. To be consistent with the rest of Aikido's planners, a vector field should be evaluated on a State - not an Eigen::VectorXd. This means that the integrator needs to use the operations defined on StateSpace (e.g. compose(), expMap(), logMap()) to interact with the states rather than vector operations. This is important to support configuration spaces with other topologies.

The correct way to implement this using Boost.Numeric.OdeInt is likely to configure the integrator to define custom StateType, Algebra, Operations types that internally call functions on the base Aikido StateSpace API. This should be straightforward because there is a relatively direct mapping between the Aikido API and the API desired by Boost.

Non-MetaSkeletonStateSpace Support. The base vector field planner should work on an arbitrary StateSpace using state space operations (see above). A MetaSkeletonStateSpace is only required when mapping from workspace to configuration space. See below for one idea about how to implement this.

Joint Limits and Collision Checking. These should be implemented using the Aikido constraint API. The easiest way of implementing this is likely to accept a Testable constraint. The user can then pass in the appropriate NonColliding and joint limit constraints for their robot.

Constraint Validation. The resolution at which constraints are checked should not be related to the integration step size. Those are independent quantities: constraints should be checked at a user defined resolution (e.g. chosen with knowledge of the robot's kinematics and the amount of padding on objects in the environment) and the latter is an internal parameter chosen by the integrator (e.g. larger when the vector field is flat and smaller when it's derivatives are large).

Conceptually, you can think about these as two separate operations: the planner first integrates a trajectory, then tests the entire trajectory for constraint satisfaction at a user-defined resolution. In practice, to be more efficient, we can interleave the two operations.

To do this, the planner must maintain two pieces of state: (1) the trajectory integrated so far and (2) the last point in the trajectory that was checked for constraint satisfaction. Then, after taking a step, the planner can check the new portion of the trajectory for validity. I can sketch this out in more detail if my explanation is not clear.

Here is a potential sketch of an API that makes the base VectorField class for for an arbitrary StateSpace:

A base VectorField class that operates on an arbitrary StateSpace.

class VectorField {
public:
  bool evaluateVelocity(const StateSpace::State* _state,
                        Eigen::VectorXd* _velocity) = 0;

  // Preferable: Status is a function of the entire trajectory so far. 
  VectorFieldPlannerStatus evaluateStatus(
    const aikido::trajectory::Trajectory& _trajectory) = 0;

  // Alternative: Status is only a function of the current state.
  VectorFieldPlannerStatus evaluateStatus(
    const StateSpace::State* _state) = 0;
};

A BodyNodePoseVectorField defined for a MetaSkeletonStateSpace that converts a vector field for a BodyNode's pose (i.e. defined over SE(3)) to the robot's configuration space.

class BodyNodePoseVectorField : public VectorField {
public:
  BodyNodePoseVectorField(MetaSkeletonStateSpacePtr _stateSpace,
                          const BodyNode* _bodyNode);

  bool evaluatePoseVelocity(const SE3::State* _state,
                            Eigen::VectorXd* _velocity) = 0;

protected:
  // Defined in terms of evaluatePoseVelocity().
  bool evaluateVelocity(const StateSpace::State* _state,
                        Eigen::VectorXd* _velocity);
};

MoveEndEffectorOffsetVectorField and MoveEndEffectorPoseVectorField are defined using BodyNodePoseVectorField.

mkoval · 2017-11-29T23:50:41Z

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

+  /// \param[in] _bn Body node of end-effector
+  ConfigurationSpaceVectorField(
+      aikido::statespace::dart::MetaSkeletonStateSpacePtr _stateSpace,
+      dart::dynamics::BodyNodePtr _bodyNode);


A generic vector field can be defined on any state space, not only MetaSkeletonStateSpaces. Also, it is not necessarily parameterized by a BodyNode. Those are implementation details of how some vector fields are implemented, e.g. those that move a BodyNode along a constraint. I think we should replace MetaSkeletonStateSpace with StateSpace and remove all of the reference to BodyNodes from this base class.

It would also be useful to introduce a subclass that handles the conversion from task space to configuration space. That class could take a BodyNodePtr and handle the necessary Jacobian calculations to convert a twist in SE(3) to the tangent space of the MetaSkeleton's configuration space.

mkoval · 2017-11-29T23:51:04Z

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

+
+/// This class defines a vector field in a configuration space
+///
+/// Any vector field should inherit this class to implememnt functions


Typo: "implememnt" -> "implement"

mkoval · 2017-11-29T23:53:06Z

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

+///
+/// Any vector field should inherit this class to implememnt functions
+/// for calculating joint velocities giving joint positions and time,
+/// and checking planning status.


I don't know what "and time" means here. I think this should just say "joint positions."

To be pedantic, we shouldn't be referring to joints at all in this class. A vector field is defined over an abstract state space - it does not technically even have to be a MetaSkeletonStateSpace!

mkoval · 2017-11-29T23:54:49Z

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

+  ///
+  /// \param[out] _qd Joint velocities
+  /// \return Whether joint velocities are successfully computed
+  virtual bool getJointVelocities(Eigen::VectorXd& _qd) const = 0;


This should take a StateSpace::State * output parameter, not an Eigen::VectorXd.

mkoval · 2017-11-29T23:57:14Z

include/aikido/planner/vectorfield/ConfigurationSpaceVectorField.hpp

+  /// Vectorfield planning status callback function
+  ///
+  /// \return Status of planning
+  virtual VectorFieldPlannerStatus checkPlanningStatus() const = 0;


Where does this function get it's input from? This function should take a const StateSpace::State * as an argument at a minimum. Ideally, we would pass the entire trajectory that has already been integrated - but that would be significantly more difficult to implement.

mkoval · 2017-12-02T18:51:28Z

include/aikido/planner/vectorfield/VectorFieldPlanner.hpp

+    double jointLimitTolerance = 3e-2,
+    double optimizationTolerance = 1.,
+    double timelimit = 5.0,
+    double integralTimeInterval = 10.0);


This helper function doesn't provide too much value over the functions that it wraps. I'd slightly prefer to remove function from Aikido and move it to libherb, where we can specify good defaults for HERB.

mkoval · 2017-12-02T18:53:19Z

include/aikido/planner/vectorfield/VectorFieldPlanner.hpp

+  /// in planning.
+  /// \return A trajectory following the vector field.
+  std::unique_ptr<aikido::trajectory::Spline> plan(
+      double integrationTimeInterval,


The integration time parameter is meaningless (see my comment above), so it does not make sense to specifiy a limit here.

mkoval · 2017-12-02T18:56:00Z

include/aikido/planner/vectorfield/VectorFieldPlanner.hpp

+  /// \return A trajectory following the vector field.
+  std::unique_ptr<aikido::trajectory::Spline> plan(
+      double integrationTimeInterval,
+      double timelimit,


We generally prefer to use std::chrono types to represent time durations. Consider using std::duration<T> or - if you don't want to template this function - std::duration<double>.

mkoval · 2017-12-02T19:08:42Z

include/aikido/planner/vectorfield/VectorFieldPlannerExceptions.hpp

+/// Define termination of vectorfield planner.
+// When VectorFieldTerminated is throws, it means
+// that planning is finished.
+class VectorFieldTerminated : public std::exception


This class is an implementation detail - it is only necessary because we use an exception as a workaround to prematurely terminate the integration. This belongs in the .cpp file or a detail namespace.

mkoval · 2017-12-02T19:13:39Z

include/aikido/planner/vectorfield/VectorFieldUtil.hpp

-/// \param[in] _cache_index Total cache index number
-/// \param[in] _stateSpace MetaSkeleton state space
+/// \param[in] knots A sequence of knots.
+/// \param[in] cache_index Total cache index number.


I don't know what this means. If it's the length of the trajectory to keep, then I suggest removing the parameter entirely relying on the user to truncate knots. E.g. they could call knots.resize(cache_index) before calling this function

dqyi11 · 2017-12-04T20:08:28Z

@mkoval How about the below design of classes to support vector field planner in arbitrary StateSpace?

VectorField class provides evaluateVelocity() and evaluateStatus().
VectorFieldPlanner class provides evaluateTrajectory() and followVectorField().

Define an abstract vector field for any state space.

class VectorField
{
public:
  bool evaluateVelocity(const StateSpace::State* _state,
                        Eigen::VectorXd* _velocity) = 0;

  // Alternative: Status is only a function of the current state.
  VectorFieldPlannerStatus evaluateStatus(
    const StateSpace::State* _state) = 0;
};

Define a MetaSkeletonStateSpaceVectorField in robot's configuration space.
(Currently we do not have one, but it is possible that there will be applications that is directly defined in MetaSkeletonStateSpace, for example, something similar with planToConfiguration?)

class MetaSkeletonStateSpaceVectorField : public VectorField
{
public:
  MetaSkeletonStateSpaceVectorField(
      aikido::statespace::dart::MetaSkeletonStateSpacePtr stateSpace);
  bool evaluateVelocity(const StateSpace::State* _state,
                        Eigen::VectorXd* _velocity);

  // Alternative: Status is only a function of the current state.
  VectorFieldPlannerStatus evaluateStatus(
    const StateSpace::State* _state);
};

Define a BodyNodePoseVectorField for a type of vector field in robot's configuration space that is created from workspace.

class BodyNodePoseVectorField : public MetaSkeletonStateSpaceVectorField
{
BodyNodePoseVectorField(
      aikido::statespace::dart::MetaSkeletonStateSpacePtr stateSpace,
      dart::dynamics::BodyNodePtr bodyNode);
};

For example, define a MoveEndEffectorOffsetVectorField for a specific BodyNodePoseVectorField.

class MoveEndEffectorOffsetVectorField : public BodyNodePoseVectorField
{
};

Correspondingly, we could have classes of VectorFieldPlanner (will be in details) as below.
VectorFieldPlanner classes provides evalutateTrajectory() and followVectorField().

class VectorFieldPlanner
{
public:
  VectorFieldPlanner(
      const VectorFieldPtr vectorField,
      const aikido::constraint::TestablePtr constraint);
              
  bool evaluateTrajectory(
      const aikido::trajectory::Trajectory& _trajectory,
      const aikido::constraint::TestablePtr constraint,
      double evalTimeStep);

  std::unique_ptr<aikido::trajectory::Spline> followVectorField(
      const aikido::statespace::StateSpace::State* startState,
      double timelimit);
};

A MetaSkeletonStateSpaceVectorFieldPlanner for all the vector fields defined in robot's configuration space.
The evaluateTrajector() accepts only timed trajectory.
It checks the satisfactions of not only CollisionFree constraint and DOF limit constraint, but also joint velocity limit constraint.

class MetaSkeletonStateSpaceVectorFieldPlanner : public VectorFieldPlanner
{
public:
  VectorFieldPlanner(
      const MetaSkeletonStateSpaceVectorFieldPtr vectorField,
      const aikido::constraint::TestablePtr constraint);
};

It seems that Testable works with a State in state space. What is the best practice of using it to check velocity limit violation, the information of which is not expressed in a State?

…utput

dqyi11 · 2017-12-05T21:01:48Z

Changes include:

Integration time is no longer a parameter but a constant. Customizing stepper() will be addressed in Remove integration time as a termination condition in vector field planner #275.
Non-Rn State Space Support will be addressed in Support arbitrary state spaces in the VectorField planner #274
Non-MetaSkeletonStateSpace Support is done. New classes are added for vector fields in arbitrary state space.
Joint Limits and Collision Checking are now called by using Testable constraints.
Eigen::VectorXd is replaced with aikido::statespace::StateSpace::State.
Constraint Validation is done using a different resolution parameter with integration step size.
Our original plan was using a trajectory-wide constraint validation after vector field planner generates a path, so the constraint validation is following a rough observation interval (defined by the stepper()). But now evaludateTrajectory() is added to support trajectory-wide constraint validation. It is used to check in new segment in the planning process (needs a user-defined resolution parameter).

mkoval

Thanks for making those changes - I think we're converging towards a solid implementation. 😅 I think you are taking the correct approach by filing issues for difficult-to-implement features that we don't need immediately.

I am trying to focus only on issues that affect the public API and inheritance tree. These are the remaining major issues that I see:

Missing functions on BodyNodePoseVectorField. The intent of this class was to encapsulate the boilerplate required to convert a vector field over a BodyNode's pose into configuration space. To do that, the class needs to implement evaluateVelocity() and evaluateStatus() in terms of equivalent functions that operate on the BodyNode's pose.

This change should significantly simplify the implementations of MoveEndEffectorOffsetVectorField and MoveEndEffectorPoseVectorField.
MetaSkeletonStateSpaceVectorField is unnecessary because it doesn't seem to provide any functionality unless combined with BodyNodePoseVectorField. If that is the case, we should merge that functionality directly into BodyNodePoseVectorField.
MetaSkeletonStateSpaceVectorFieldIntegrator is unnecessary** because all of the pure-virtual functions on VectorFieldIntegrator may be implemented for an arbitrary StateSpace. This is nice because it means that the vast majority of the planner can operate on an arbitrary state space.
Collision Free and Bounds Constraints. The vector field planner should operate on an opaque Testable constraint - it does not need access to individual collision and bounds constraints. If the goal is only to improve error messages, then that is something we can tackle using an approach similar to Testable Constraint Output #266.
Incremental Constraint Validation. The current constraint validation logic validates the trajectory only after the integration has terminated. This is suboptimal because we won't "fail fast" if we quickly encounter a constraint violation. This also completely breaks the "move as far as possible" paradigm that we often use on HERB, e.g. to move straight as far as possible to contact the environment.
Incorrect Constraint Validation. The constraint validation currently discretizes the trajectory using an integration time resolution. This is incorrect because integration time is meaningless - we should be collision checking at a fixed DOF resolution. The correct implementation requires Implement collision checking for Spline trajectories #212, so I proposed a few short-term workarounds.
Documentation. It would be helpful to do a pass and update docstrings, particularly on classes that the user will have to interact with (e.g. by overriding functions in subclasses). A number of docstrings are missing or outdated pending the most recent refactors.

mkoval · 2017-12-06T02:27:31Z

include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp

+///
+/// Any vector field should inherit this class to implememnt functions
+/// for calculating joint velocities giving joint positions and time,
+/// and checking planning status.


This description is no longer correct. It should say something like this:

This class is a vector field over a MetaSkeletonStateSpace that is defined in terms of a Cartesian vector field over the pose of a BodyNode in that MetaSkeleton. The Jacobian pseudoinverse is used to map velocities in se(3) to velocities in configuration space. If multiple solutions exist, i.e. the kinematic chain leading to the BodyNode in the MetaSkeleton is redundant, then a heuristic is used to select velocities from the null space to avoid violating joint limits.

mkoval · 2017-12-06T02:31:59Z

include/aikido/planner/vectorfield/MetaSkeletonStateSpaceVectorFieldIntegrator.hpp

+  /// \param[in] initialStepSize Initial step size in integation.
+  MetaSkeletonStateSpaceVectorFieldIntegrator(
+      const MetaSkeletonStateSpaceVectorFieldPtr vectorField,
+      const aikido::constraint::TestablePtr constraint,


Nit: These arguments are passed by value, so it is meaningless to flag them as const.

mkoval · 2017-12-06T02:37:28Z

include/aikido/planner/vectorfield/MetaSkeletonStateSpaceVectorFieldIntegrator.hpp

+  // Documentation inherited.
+  bool evaluateTrajectory(
+      const aikido::trajectory::Trajectory* trajectory,
+      const aikido::constraint::TestablePtr collisionFreeConstraint,


Nits:

This function does not support trajectory being nullptr, so it would be cleaner to take that argument const & instead of a const *.

This function does not take ownership of collisionFreeConstraint , it would be cleaner to take that argument by const & instead of a shared_ptr.

This collisionFreeConstraint is currently passed by value, so it is meaningless to flag it is as const.

The name collisionFreeConstraint is misleading because it can be any constraint, not just a CollisionFree constraint.

mkoval · 2017-12-06T02:39:23Z

include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp

+  /// than distance.
+  /// (2) When maxDistance is not set to InvalidMaxDistance,
+  /// planning will terminate and return a path, the path length
+  /// of which is [distance, maxDistance].


If I understand this correctly, it may be simpler to say that:

If not specified, maxDistance defaults to distance.

mkoval · 2017-12-06T02:39:54Z

include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp

+      double angularTolerance = 0.15,
+      double linearVelocityGain = 1.0,
+      double initialStepSize = 5e-2,
+      double jointLimitPadding = 3e-2);


I suggest moving all of these defaults to libherb since they are highly robot specific.

mkoval · 2017-12-06T03:17:26Z

include/aikido/planner/vectorfield/MetaSkeletonStateSpaceVectorField.hpp

+/// Any vector field should inherit this class to implememnt functions
+/// for calculating joint velocities giving joint positions and time,
+/// and checking planning status.
+class MetaSkeletonStateSpaceVectorField : public VectorField


I don't think this class is necessary - it doesn't seem to provide any value over VectorField itself. I think we can safely roll all of this functionality into BodyNodePoseVectorField.

mkoval · 2017-12-06T03:24:57Z

include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp

+  // Documentation inherited.
+  virtual bool evaluateVelocity(
+      const aikido::statespace::StateSpace::State* state,
+      Eigen::VectorXd& qd) const override = 0;


I was expecting this class to define evaluateVelocity() in terms of another function that returns the Cartesian velocity of the specified BodyNode. E.g.:

bool BodyNodePoseVectorField::evaluateVelocity( const aikido::statespace::StateSpace::State* state, Eigen::VectorXd& qd) const override = 0) override { // Pseudocode. Actual implementation will require some massaging of data types. const auto cartesianState = mBodyNode->getPose(state); Eigen::Vector6d cartesianVelocity; if (!evaluateCartesianVelocity(cartesianState, cartesianVelocity)) return false; return computeJointVelocityFromTwist(mBodyNode, cartesianVelocity, &qd); } // Implemented by the user: virtual bool BodyNodePoseVectorField::evaluateCartesianVelocity( const Eigen::Isometry3d& _pose, Eigen::Vector6d& _cartesianVelocity) = 0;

In my opinion, the whole point of this class is to allow the user to define a vector field over the pose of a BodyNode without worrying about the boilerplate required to call `computeJointVelocityFromTwist().

mkoval · 2017-12-06T03:26:01Z

include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp

+
+  // Documentation inherited.
+  virtual VectorFieldPlannerStatus evaluateStatus(
+      const aikido::statespace::StateSpace::State* state) const override = 0;


I was expecting this class to define evaluateStatus() in terms of another evaluateCartesianStatus() function that accepts the pose of the BodyNode. See my comment on evaluateVelocity() for details.

mkoval · 2017-12-06T03:27:07Z

src/planner/vectorfield/MetaSkeletonStateSpaceVectorField.cpp

+      == mMetaSkeleton->getVelocityLowerLimits())
+  {
+    throw std::invalid_argument("Velocity space volume zero");
+  }


I don't think these checks are necessary. Bounds should be handled entirely by the Testable passed by the user.

mkoval · 2017-12-06T03:28:55Z

include/aikido/planner/vectorfield/MetaSkeletonStateSpaceVectorFieldIntegrator.hpp

+/// This class defines two callback functions for a integrator.
+/// step() provides joint velocities the vector field planner should follow,
+/// check() is called after each integration step to check planner status.
+class MetaSkeletonStateSpaceVectorFieldIntegrator : public VectorFieldIntegrator


I am not sure why this class is necessary. I think we can safely delete this once we implement convertStateToPositions() and convertPositionsToState() in the base VectorField class.

dqyi11 · 2017-12-07T03:22:38Z

Missing functions on BodyNodePoseVectorField are added. evaluateVelocity() and evaluateStatus() are added that operate on the BodyNode's pose.
MetaSkeletonStateSpaceVectorField and MetaSkeletonStateSpaceVectorFieldIntegrator are removed.
Incremental Constraint Validation is supported. Now a trajectory is created from a new knot to the last knot in list, and is sent to evaluateTrajectory for constraint satisfaction.
Docstrings are updated.

TODO:
-Collision Free and Bounds Constraints. This will be updated after #266 is merged.
-Incorrect Constraint Validation.
@mkoval suggested

The constraint validation currently discretizes the trajectory using an integration time resolution. This is incorrect because integration time is meaningless - we should be collision checking at a fixed DOF resolution.

This will be updated after #212 is merged. The current solution is adding a user-defined check resolution in followVectorField().

mkoval · 2017-12-08T03:35:34Z

src/planner/vectorfield/BodyNodePoseVectorField.cpp

+  using aikido::constraint::createTestableBounds;
+  auto boundConstraint = createTestableBounds(mMetaSkeletonStateSpace);
+  for (double t = trajectory.getStartTime(); t <= trajectory.getEndTime();
+       t += evalStepSize)


Two issues:

This will not evaluate the endpoint of the trajectory if getEndTime() is not an integral multiple of evalStepTime.

This will reevaluate the trajectory from the start each call. This function should take the start time for t to avoid re-evaluating that part of the trajectory.

mkoval · 2017-12-08T03:35:50Z

src/planner/vectorfield/BodyNodePoseVectorField.cpp

+  for (double t = trajectory.getStartTime(); t <= trajectory.getEndTime();
+       t += evalStepSize)
+  {
+    auto state = mMetaSkeletonStateSpace->createState();


Nit: I suggest moving this outside the loop, so we don't do unnecessary state allocations.

mkoval · 2017-12-08T03:36:25Z

src/planner/vectorfield/BodyNodePoseVectorField.cpp

+    if (!constraint->isSatisfied(state))
+    {
+      throw ConstraintViolatedError();
+    }


See my previous comments. boundConstraint and constraint should be merged. The user can pass a CompoundTestable if they wish to check more than one constraint.

mkoval · 2017-12-08T03:38:21Z

src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp


 namespace aikido {
 namespace planner {
 namespace vectorfield {

+//==============================================================================
+double MoveEndEffectorOffsetVectorField::InvalidMaxDistance
+    = std::numeric_limits<double>::infinity();


infinity() seems like a perfectly valid value for the maximum distance if there is no limit. We commonly use this in PrPy when planning "move-until-touch" actions. I suggest using a negative value instead.

mkoval · 2017-12-08T03:43:59Z

src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp

+  Eigen::Vector6d desiredTwist = computeGeodesicTwist(pose, mStartPose);
+  desiredTwist.tail<3>() = mDirection * mLinearVelocityGain;
+  cartesianVelocity = desiredTwist;
+  return true;


This seems quite a bit different than the logic that it replaced. A few key differences: (1) the linear proportional controller is not additive and (2) there is no angular proportional controller. Were these changes intended?

mkoval · 2017-12-08T04:08:19Z

include/aikido/planner/vectorfield/detail/VectorFieldUtil.hpp

+///
+/// \param[in] fromTrans Current transformation.
+/// \param[in] toTrans Goal transformation.
+/// \return Geodesic twist.


Document the convention about how to interpret the elements of this vector. Which elements represent linear versus angular velocities? What coordinate frame is this expressed in?

mkoval · 2017-12-08T04:08:36Z

include/aikido/planner/vectorfield/detail/VectorFieldUtil.hpp

+///
+/// \param[in] fromTrans Current transformation.
+/// \param[in] toTrans Goal transformation.
+/// \return Geodesic error.


Document the convention about how to interpret the elements of this vector. Which elements represent linear versus angular velocities? What coordinate frame is this expressed in?

mkoval · 2017-12-08T04:08:52Z

include/aikido/planner/vectorfield/detail/VectorFieldUtil.hpp

+/// \param[in] fromTrans Current transformation.
+/// \param[in] toTrans Goal transformation.
+/// \param[in] r In units of meters/radians converts radians to meters.
+/// \return Geodesic distance.


What are the units on the return value?

mkoval · 2017-12-08T04:09:38Z

include/aikido/planner/vectorfield/detail/VectorFieldUtil.hpp

+/// \param[in] jointVelocityUpperLimits Joint velocity upper bounds.
+/// \param[in] jointVelocityLimited Whether joint velocity bounds are
+/// considered in optimization.
+/// \param[in] stepSize Step size in second.


What is this used for? Can we remove it?

mkoval · 2017-12-08T04:14:25Z

include/aikido/planner/vectorfield/detail/VectorFieldUtil.hpp

+/// \param[in] jointLimitPadding If less then this distance to joint
+/// limit, velocity is bounded in that direction to 0.
+/// \param[in] jointVelocityLowerLimits Joint velocity lower bounds.
+/// \param[in] jointVelocityUpperLimits Joint velocity upper bounds.


These arguments should only be required if jointVelocityLimited is true. Are they ignored otherwise?

Instead of having a jointVelocityLimitReached flag, consider passing those arguments by const * and ignoring the velocity limits if they are nullptr.

…com/personalrobotics/aikido into enhancement/dqyi/vectorFieldPlanner

mkoval

This looks great. Very nice work @dqyi11 - thank you for putting up with all of my comments. Bonus points for updating the docstrings - they help a lot when trying to understand how all the pieces fit together.

At this point, all of the major issues have been addressed. I left a handful of minor comments, most of which just just involve documenting limitations of the current implementation, so we remember to come back and fix them later.

Once those comments are addressed, I am happy to merge this. 👍

mkoval · 2017-12-09T02:16:46Z

include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp

+  /// Evaluate Cartesian velocity by current pose of body node.
+  ///
+  /// \param[in] pose Current pose of body node.
+  /// \param[out] cartesianVelocity Cartesian velocity defined in SE(3).


Pedantic Nit: Velocities live in se(3), not SE(3).

mkoval · 2017-12-09T02:18:15Z

include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp

+  /// could be found.
+  /// \param[in] positionTolerance Constraint tolerance in meters.
+  /// \param[in] angularTolerance Constraint tolerance in radians.
+  /// \param[in] initialStepSize Initial step size.


Clarify that this is the maximum step size used to guarantee that the integrator does not step out of joint limits, not the initial step size.

mkoval · 2017-12-09T02:19:01Z

include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp

+      double r,
+      double linearVelocityGain,
+      double angularVelocityGain,
+      double initialStepSize,


See my comment on BodyNodePoseVectorField. This is a confusing parameter.

mkoval · 2017-12-09T02:20:44Z

include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp

+
+  /// Angular velocit gain.
+  double mAngularVelocityGain;
+};


This object contains a Isometry3d member variable, which is a fixed-size vectorizable type. Therefore, we should flag this class with EIGEN_MAKE_ALIGNED_OPERATOR_NEW.

mkoval · 2017-12-09T02:20:52Z

include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp

+
+protected:
+  /// Goal pose.
+  Eigen::Isometry3d mGoalPose;


This object contains a Isometry3d member variable, which is a fixed-size vectorizable type. Therefore, we should flag this class with EIGEN_MAKE_ALIGNED_OPERATOR_NEW.

mkoval · 2017-12-09T03:29:54Z

src/planner/vectorfield/detail/VectorFieldIntegrator.cpp

+    {
+      throw VectorFieldTerminated("State not in constraint.");
+    }
+  }


This constraint validation logic is correct, but will result in a lot of extra constraint validation checks if the the integration step size is significantly smaller than the collision checking resolution. This is common if the robot is near a singularity because the ODE is relatively stiff.

A better way to implement this is to maintain the time mLastEvaluation at which the last constraint validation check was performed. In this function, we could collision check the trajectory from mLastEvaluation to t excluding the endpoint, then update mLastEvaluation = t. Finally, when planning terminates, we need to check the unevaluated portion [mLastEvaluation, getEndTime()] of the trajectory.

I think this is fine for now - this doesn't affect correctness. If you don't want to make this change now, then just file an issue so it doesn't slip through the cracks. 😉

mkoval · 2017-12-09T03:30:13Z

src/planner/vectorfield/detail/VectorFieldIntegrator.cpp

+      trajEvalStartTime = mConstraintCheckResolution;
+    }
+    if (!mVectorField->evaluateTrajectory(
+            *trajSegment.get(),


Nit: This is equivalent to *trajSegment.

mkoval · 2017-12-09T03:30:56Z

src/planner/vectorfield/detail/VectorFieldIntegrator.cpp

+    }
+  }
+  mKnots.push_back(knot);
+  mIndex += 1;


It seems like mIndex always equals mKnots.size(). Is the variable necessary? 🤔

mkoval · 2017-12-09T03:33:00Z

include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp

+  /// distance.
+  /// \param[in] linearVelocityGain Linear velocity gain in workspace.
+  /// \param[in] angularVelocityGain Angular velocity gain in workspace.
+  /// \param[in] initialStepSize Initial step size.


Clarify that this is the maximum step size used to guarantee that the integrator does not step out of joint limits, not the initial step size.

mkoval · 2017-12-09T03:38:54Z

src/planner/vectorfield/detail/VectorFieldIntegrator.cpp

+            mConstraintCheckResolution,
+            trajEvalStartTime))
+    {
+      throw VectorFieldTerminated("State not in constraint.");


This should be a ConstraintViolatedError.

mkoval

This looks great - this is a huge improvement over the vector field planner that currently lives in master. Thank you for your patience addressing all of the comments @dqyi11!

There are three follow-up issues from this pull request that we should address at some point: #274, #275, and #285.

mkoval · 2017-12-09T21:46:08Z

include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp

+  /// \param[in] maxStepSize The maximum step size used to guarantee
+  /// that the integrator does not step out of joint limits.
+  /// \param[in] jointLimitpadding If less then this distance to joint
+  /// limit, velocity is bounded in that direction to 0.


Nit: Missing docstring for the enforceJointVelocityLimits flag. I'd also suggest we make that flag default to false, since we typically use the vector field planner as a kinematic planner.

mkoval · 2017-12-09T22:06:44Z

include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp

+  /// BodyNode
+  dart::dynamics::BodyNodePtr mBodyNode;
+
+  /// Initial step size of integrator.


Nit: This comment should be updated to reflect the fact that this is a maximum step size.

mkoval · 2017-12-09T22:13:45Z

src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp

-  return result;
+  Eigen::Vector6d desiredTwist = computeGeodesicTwist(pose, mStartPose);
+  desiredTwist.tail<3>() = mDirection;
+  cartesianVelocity = desiredTwist;


Yes, that makes sense. 👍 Thanks for the explanation.

mkoval · 2017-12-09T22:17:07Z

src/planner/vectorfield/VectorFieldPlanner.cpp

-      throw std::invalid_argument(ss.str());
-    }
-    if (skeleton->getVelocityLowerLimit(i) > skeleton->getVelocityUpperLimit(i))
+    // dtwarn << e.what() << std::endl;


Remove dead code.

* new vector field planner and added move end effector pose vector field * add move to end effector pose test cases * refactor code and format * add new tests to plan to end effector pose * add functions of enabling collision checking and dof limit checking * fix doc strings * add velocity gain to support controlling in complex scenarios * add new class for collision error * address JS's comments * address JS's comments and add doc strings for some member variables * Fix const-correctness in ConfigurationSpaceVectorField * Fix code style * fix doc string style * Add invalid max distance variable * update cpde style * fix typos * address Mike's comments * add parameter for constraint check resolution * add doc strings, moved implementation details, add plannerStatus as output * address Mike's comments * update for formatting * address Mike's comments * address Mike's comments * change to use TestableIntersection * fix doc strings * add changelog.md

dqyi11 added 7 commits November 24, 2017 12:44

new vector field planner and added move end effector pose vector field

5a30b77

add move to end effector pose test cases

15e7fe0

merge from master

57d342e

refactor code and format

b7fc1b5

add new tests to plan to end effector pose

99a3706

add functions of enabling collision checking and dof limit checking

8870a96

fix doc strings

a6f9ace

dqyi11 added this to the Aikido 0.2.0 milestone Nov 26, 2017

dqyi11 requested review from brianhou and jslee02 November 26, 2017 20:01

jslee02 self-assigned this Nov 27, 2017

add velocity gain to support controlling in complex scenarios

b471f86

jslee02 reviewed Nov 28, 2017

View reviewed changes

jslee02 and others added 3 commits November 27, 2017 19:01

Merge branch 'master' into enhancement/dqyi/vectorFieldPlanner

2c80af7

add new class for collision error

5aa1f22

Merge branch 'enhancement/dqyi/vectorFieldPlanner' of https://github.…

e7f506a

…com/personalrobotics/aikido into enhancement/dqyi/vectorFieldPlanner

jslee02 suggested changes Nov 29, 2017

View reviewed changes

address JS's comments

3d501b6

jslee02 reviewed Nov 29, 2017

View reviewed changes

dqyi11 and others added 7 commits November 29, 2017 12:15

address JS's comments and add doc strings for some member variables

cec4bf3

Fix const-correctness in ConfigurationSpaceVectorField

0c89b74

Fix code style

c35fc80

fix doc string style

28dc85f

Add invalid max distance variable

45ee24a

update cpde style

ced7229

fix code style

2ef5f78

jslee02 approved these changes Nov 30, 2017

View reviewed changes

jslee02 assigned dqyi11 and unassigned jslee02 Nov 30, 2017

mkoval requested changes Dec 3, 2017

View reviewed changes

dqyi11 added 4 commits December 5, 2017 02:19

address Mike's comments

74272f6

Merge branch 'master' into enhancement/dqyi/vectorFieldPlanner

a5221d6

add parameter for constraint check resolution

81f3ba2

add doc strings, moved implementation details, add plannerStatus as o…

5be7e08

…utput

dqyi11 mentioned this pull request Dec 5, 2017

Support arbitrary state spaces in the VectorField planner #274

Open

brianhou requested a review from sniyaz December 5, 2017 23:02

mkoval mentioned this pull request Dec 6, 2017

Remove integration time as a termination condition in vector field planner #275

Open

mkoval requested changes Dec 6, 2017

View reviewed changes

address Mike's comments

cda62da

dqyi11 and others added 2 commits December 6, 2017 19:31

update for formatting

fdd6562

Merge branch 'master' into enhancement/dqyi/vectorFieldPlanner

85c93d7

mkoval reviewed Dec 8, 2017

View reviewed changes

brianhou and others added 3 commits December 8, 2017 02:39

Merge branch 'master' into enhancement/dqyi/vectorFieldPlanner

b2dc8f2

address Mike's comments

46c0bae

Merge branch 'enhancement/dqyi/vectorFieldPlanner' of https://github.…

8ba6b05

…com/personalrobotics/aikido into enhancement/dqyi/vectorFieldPlanner

mkoval requested changes Dec 9, 2017

View reviewed changes

Daqing Yi and others added 2 commits December 8, 2017 23:18

address Mike's comments

daf3120

change to use TestableIntersection

a4c3fd1

mkoval mentioned this pull request Dec 9, 2017

Constrain step size in VectorField planner #285

Open

mkoval approved these changes Dec 9, 2017

View reviewed changes

dqyi11 added 2 commits December 9, 2017 16:53

fix doc strings

79e9f1d

add changelog.md

cbca871

dqyi11 merged commit 2a5aa04 into master Dec 10, 2017

dqyi11 mentioned this pull request Dec 10, 2017

New vector field planner implementation #286

Merged

5 tasks

jslee02 mentioned this pull request Jan 6, 2018

VectorFieldPlanner #267

Open

3 tasks

		@@ -52,6 +53,7 @@ std::unique_ptr<aikido::trajectory::Spline> convertToSpline(
		}

		//==============================================================================

		@@ -62,26 +64,32 @@ DesiredTwistFunction::DesiredTwistFunction(
		}

		//==============================================================================

New vector field planner impelementation #268

New vector field planner impelementation #268

Conversation

dqyi11 commented Nov 26, 2017 • edited by jslee02 Loading

codecov bot commented Nov 26, 2017 • edited Loading

Codecov Report

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jslee02 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jslee02 Nov 28, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jslee02 Nov 28, 2017 • edited Loading

Choose a reason for hiding this comment

dqyi11 Nov 29, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jslee02 left a comment

Choose a reason for hiding this comment

mkoval left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dqyi11 commented Dec 4, 2017 • edited Loading

dqyi11 commented Dec 5, 2017 • edited by mkoval Loading

mkoval left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dqyi11 commented Dec 7, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dqyi11 commented Nov 26, 2017 •

edited by jslee02

Loading

codecov bot commented Nov 26, 2017 •

edited

Loading

jslee02 Nov 28, 2017 •

edited

Loading

jslee02 Nov 28, 2017 •

edited

Loading

dqyi11 Nov 29, 2017 •

edited

Loading

dqyi11 commented Dec 4, 2017 •

edited

Loading

dqyi11 commented Dec 5, 2017 •

edited by mkoval

Loading

dqyi11 commented Dec 7, 2017 •

edited

Loading