Adding another kinodynamic timer #443

dqyi11 · 2018-06-11T20:39:05Z

This PR introduces another kinodynamic timer by using a time-optimal trajectory generation algorithm, which is developed by Tobias Kunz.
Code: https://github.com/tobiaskunz/trajectories
Publication: T. Kunz, M. Stilman. Time-Optimal Trajectory Generation for Path Following with Bounded Acceleration and Velocity. Robotics: Science and Systems (RSS), 2012. http://www.roboticsproceedings.org/rss08/p27.html

This type of timer accepts both Interpolated and Spline paths. It firstly converts a non-differentiable linear-piecewise trajectory to a differentiable one by adding circular blends. It then applies a time-optimal exact path-following algorithm that maximizes the path velocities at every point along the path.

The preprocessing of adding circular blends might slightly change the shape of a path. However, unlike how the existing parabolic timer does, the timed trajectory does not make a complete stop (zero velocities) at each waypoint.

QUESTIONs:

Shall we create a postprocessing folder for all the postprocessing methods?
How are we going to enable Robot class call different timing/smoothing methods?
Document new methods and classes
Format code with make format
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 · 2018-06-11T21:21:27Z

Codecov Report

Merging #443 into master will decrease coverage by 0.04%.
The diff coverage is 76.57%.

@@            Coverage Diff             @@
##           master     #443      +/-   ##
==========================================
- Coverage   79.01%   78.97%   -0.05%     
==========================================
  Files         256      258       +2     
  Lines        6185     6296     +111     
==========================================
+ Hits         4887     4972      +85     
- Misses       1298     1324      +26

Impacted Files	Coverage Δ
include/aikido/planner/kunzretimer/KunzRetimer.hpp	`100% <100%> (ø)`
src/planner/kunzretimer/KunzRetimer.cpp	`76.36% <76.36%> (ø)`
src/planner/ompl/CRRT.cpp	`75.58% <0%> (-0.59%)`	⬇️
src/trajectory/Spline.cpp	`94.44% <0%> (+0.92%)`	⬆️

brianhou · 2018-06-22T23:11:27Z

I'll try to take a more detailed look over this PR in the next couple days, but I have one quick question: how does this differ from Mintos? My understanding was that we had an implementation in features/minTOS that was working on an early version of AIKIDO, so it seemed like it wouldn't have been too difficult to update.

Shall we create a postprocessing folder for all the postprocessing methods?

I think we can continue creating a namespace/directory underneath aikido::planner for each type of planner or postprocessor. If we decide we don't like it, we can always change it later.

How are we going to enable Robot class call different timing/smoothing methods?

I don't have a clear understanding of when we should use one or the other. Is it always better to use this over the parabolic smoother, which forces us to stop at waypoints? Or are there certain planning problems where we believe that the parabolic smoother would be better? If we should always use one, then I think we can continue doing what we're doing with constructing a single postprocessor in the robot constructor. Otherwise, I guess we can construct all the postprocessors in the constructor (similar to how we construct all the planners in the robot constructor) and then delegate accordingly.

dqyi11 · 2018-06-23T00:33:07Z

Thanks! @brianhou
I think that minTOS is going to be used as an alternative smoother that considers constraint manifolds and kinematic limitations. I am seeing them as different postprocessing methods by the changes in path geometry. ParabolicTimer stops at each waypoint so no geometric change is introduced. This timing method will add circular blend at each waypoint so the path geometry is adjusted to be differentiable. The change of path geometry can be kept under a tolerance threshold by the parameter maxDeviation. The ParabolicSmoother and upcoming minTOS will "optimize" the path geometry according to some criteria.

I suggest to keep different postprocessing methods. When we want a robot shall strictly follow a planned path (e.g. welding?), ParabolicTiming can be used. When we want a robot to keep the path shape with a tolerance (e.g. forward pushing?), this timing can be used. When we want a robot to generate an efficient and smooth motion, smoothers can be used.

So my original suggestion was whether we can have an aikido::planner::postprocessing namespace for these different postprocessing methods. The current aikido::planner::parabolic can be moved under that one.

brianhou · 2018-07-02T18:06:28Z

CHANGELOG.md

@@ -43,6 +43,7 @@
  * Added flags to WorldStateSaver to specify what to save: [#339](https://github.com/personalrobotics/aikido/pull/339)
  * 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)
+  * Add a kinodynamic timer that generates a time-optimal smooth trajectory without completely stopping at each waypoint: [#443}(https://github.com/personalrobotics/aikido/pull/443)


Typo: [#433]

brianhou · 2018-07-02T18:10:58Z

src/planner/kinodynamic/KinodynamicTimer.cpp

+#include <aikido/common/Spline.hpp>
+#include <aikido/common/StepSequence.hpp>
+
+#include "Path.h"


@jslee02 Are relative paths to the external files okay here or should this be something else?

I would also like to know the standard. This one is following the style in HauserParabolicTimer.

It seems this is the current convention unless we change the CMakeLists.txt files in the external directories because the include directories of the targets are exactly the directory where the files are located (e.g., <aikido_dir>/src/external/hauserparabolicsmoother/).

One potential problem with this approach is that the file name can be ambiguous when there are two files with the same name in the different external libraries. I have a proposal to resolve this problem:

src/ external/ lib1/ CMakeLists.txt # include ./include include/ lib1/ src/ lib2/ CMakeLists.txt # include ./include include/ lib2/ src/

then we can include the files without conflicts over external libraries as

#include "lib1/same_name.h" #include "lib2/same_name.h"

With this proposal, the path to the external headers in the external sources should be changed as the same way above. There is a way to avoid this modification, but I personally prefer this because the leading project name in the header inclusion would also allow the externals to avoid potential name conflicts when they also have dependency.

TL;DR: This is our current convention. I have a proposal, which shouldn't block this PR anyway.

brianhou · 2018-07-02T18:23:59Z

src/planner/kinodynamic/KinodynamicTimer.cpp

+{
+  Trajectory trajectory(
+      Path(waypoints, maxDeviation), maxVelocities, maxAccelerations, timeStep);
+  if (trajectory.isValid())


Let's flip this to something like

if (!trajectory.isValid()) return nullptr; ...

brianhou · 2018-07-02T18:26:18Z

include/aikido/planner/kinodynamic/KinodynamicTimer.hpp

+      const aikido::constraint::TestablePtr& constraint = nullptr) override;
+
+  /// Returns the velocity limits of the dimensions
+  const Eigen::VectorXd& getVelocityLimits() const;


Are all these getters and setters useful? I'm not sure if they're necessary.

I think the getters are useful. For example, when we want to validate whether a timed trajectory violates the limits but only have the access to the timer handle. I prefer to keep the setters as well. It provides a way of adjusting timer parameters without reconstructing a new timer object.

brianhou · 2018-07-02T18:27:13Z

src/planner/kinodynamic/KinodynamicTimer.cpp

+//==============================================================================
+std::unique_ptr<aikido::trajectory::Spline>
+createSplineFromWaypointsAndConstraints(
+    const std::list<Eigen::VectorXd>& waypoints,


Why a list over a vector?

Because the external codes take in a list of Eigen::VectorXd. If we use vector, we will either have to do an extra conversion or hack the external code.

brianhou · 2018-07-02T18:41:25Z

src/planner/kinodynamic/KinodynamicTimer.cpp

+  }
+
+  double startTime = inputTrajectory.getStartTime();
+  // create waypoints from Interpolated path


I think it would be nice to create Eigen::VectorXd getWaypoints(trajectory::Interpolated traj) (and for splines), then call the appropriate function from here.

Done! Two new functions are added.

brianhou · 2018-07-02T18:43:18Z

src/planner/kinodynamic/KinodynamicTimer.cpp

+std::unique_ptr<aikido::trajectory::Spline> KinodynamicTimer::postprocess(
+    const aikido::trajectory::Spline& inputTraj,
+    const aikido::common::RNG& /*rng*/,
+    const aikido::constraint::TestablePtr& /*constraint*/)


I think we need to test that this constraint isn't being violated.

brianhou · 2018-07-02T18:43:25Z

src/planner/kinodynamic/KinodynamicTimer.cpp

+std::unique_ptr<aikido::trajectory::Spline> KinodynamicTimer::postprocess(
+    const aikido::trajectory::Interpolated& inputTraj,
+    const aikido::common::RNG& /*rng*/,
+    const aikido::constraint::TestablePtr& /*constraint*/)


I think we need to test that this constraint isn't being violated.

brianhou · 2018-07-02T18:49:18Z

tests/planner/kinodynamic/test_KinodynamicTimer.cpp

+      inputTrajectory, Vector2d::Constant(2.), Vector2d::Constant(1.));
+
+  timedTrajectory->evaluate(1., state);
+  EXPECT_TRUE(Vector2d(1.0, 2.0).isApprox(state.getValue()));


I think there are some Eigen things in https://github.com/personalrobotics/aikido/blob/master/tests/eigen_tests.hpp that we could use instead? The tests would be more descriptive when they fail.

brianhou · 2018-07-02T18:50:13Z

tests/planner/kinodynamic/test_KinodynamicTimer.cpp

+  EXPECT_TRUE(Vector2d(0.5, 0.5).isApprox(tangentVector));
+
+  timedTrajectory->evaluateDerivative(3.0, 1, tangentVector);
+  // TODO: isApprox does not work when comparing against zero.


I think the same eigen_tests.hpp might also help here.

brianhou · 2018-07-19T16:22:26Z

@dqyi11 It seems that #446 depends on this PR being merged first. Would you mind addressing the comments when you have the chance? Thanks!

dqyi11 · 2018-07-19T17:19:11Z

@brianhou Yes. I have started working on that. Will get that done soon.

…sonalrobotics/aikido into enhancement/dqyi/addRetiming

aditya-vk · 2018-07-20T22:44:01Z

include/aikido/planner/kinodynamic/KinodynamicTimer.hpp

+/// profiles that implement bang-bang control. This function curently only
+/// supports \c RealVector, \c SO2, and compound state spaces of those types.
+/// Additionally, this function requires that \c inputTrajectory to be
+/// interpolated using a \c GeodesicInterpolator.


Why does the input trajectory need to have been interpolated using GeodesicInterpolator? Why the explicit mention? Just curious. Any sequence of waypoints should be fine right?

I think this is necessary. The interpolation implies the shape of a path given a sequence of waypoints. If you choose a different interpolator, the shape of a path will be different (length might also be changed). The timing algorithm works on the assumption of the geodesic interpolation.

aditya-vk · 2018-07-20T22:44:56Z

include/aikido/planner/kinodynamic/KinodynamicTimer.hpp

+/// \param[in] maxDeviation maximum deviation from a waypoint in doing circular
+/// blending around the waypoint
+/// \param[in] timeStep time step in following the path
+/// \return time optimal trajectory that satisfies acceleration constraints


Nit: mention velocity constraints too? 🤔

dqyi11 · 2018-07-20T23:01:07Z

I would like to have suggestions about
(1) the current namespace aikido::planner::kinodynamic (or where the source codes shall be in); and
(2) the current name of the external source folder name optimal_time_path_following in aikido/src/external.

brianhou · 2018-07-21T06:54:58Z

(1) I think kinodynamic might not be a good namespace, since we might want to have actual kinodynamic planners. What about optimal_retimer?

I agree that eventually it makes sense to move all the postprocessing methods into their own namespace, although we'll also have to decide whether that would be aikido::postprocessing or aikido::planner::postprocessing or something else.

(2) How about kunz_optimal_retimer?

dqyi11 · 2018-07-25T22:22:44Z

@brianhou I have updated the namespace and folder name as suggested.
I would suggest aikido::planner::postprocessing. My reason is that Planning, Control and Perception are currently three main modules in aikido. Postprocessing seems part of Planning. I think prpy uses the same structure.

brianhou

Since we're now putting this in the optimalretimer namespace, do you think it would make more sense to rename KinodynamicTimer to OptimalRetimer and computeKinodynamicTiming to computeOptimalTiming?

brianhou · 2018-07-26T03:00:31Z

src/planner/optimalretimer/KinodynamicTimer.cpp

+
+//==============================================================================
+std::unique_ptr<aikido::trajectory::Spline>
+createSplineFromWaypointsAndConstraints(


In order to match the parabolic smoother implementation more closely, I think it would be helpful to replace this function with three functions in the detail namespace:

convertToKunzTrajectory(Interpolated traj, VectorXd maxVelocity, VectorXd maxAcceleration, double maxDeviation, double timestep)

convertToKunzTrajectory(Spline traj, VectorXd maxVelocity, VectorXd maxAcceleration, double maxDeviation, double timestep)

convertToSpline(Trajectory traj, double startTime, StateSpace stateSpace)

Then, computeOptimalTiming should basically look like computeParabolicTiming.

Great idea! I have refactored the functions.

brianhou · 2018-07-26T03:03:49Z

Also, @jslee02 if you could take a look over this PR to check if there are any nits that we should fix before merging, that would be great! (I know you're super busy, so just whenever you get a chance 😄)

jslee02

LGTM! Just a few nitpicks.

jslee02 · 2018-07-26T03:09:49Z

CHANGELOG.md

@@ -47,6 +47,7 @@
  * Added flags to WorldStateSaver to specify what to save: [#339](https://github.com/personalrobotics/aikido/pull/339)
  * 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)
+  * Add a kinodynamic timer that generates a time-optimal smooth trajectory without completely stopping at each waypoint: [#443](https://github.com/personalrobotics/aikido/pull/443)


Nit: Added ~

jslee02 · 2018-07-26T03:15:01Z

src/external/kunz_optimal_retimer/CMakeLists.txt

@@ -0,0 +1,18 @@
+find_package(EIGEN3 REQUIRED)
+
+add_library("${PROJECT_NAME}_external_optimaltimepathfollowing" STATIC


Match the directory name and the library name?

jslee02 · 2018-07-26T03:17:00Z

src/planner/optimalretimer/CMakeLists.txt

+set(sources
+  KinodynamicTimer.cpp)
+
+add_library("${PROJECT_NAME}_planner_kinodynamic" SHARED ${sources})


Match the directory name and the library name?

jslee02 · 2018-07-26T03:20:28Z

include/aikido/planner/optimalretimer/KinodynamicTimer.hpp

+/// blending around the waypoint
+/// \param[in] timeStep time step in following the path
+/// \return time optimal trajectory that satisfies velocity and acceleration
+/// constraints


Nit: Please start the description with a capital letter. For example, \param[in] inputTrajectory Input piecewise geodesic trajectory.

jslee02 · 2018-07-26T03:21:55Z

include/aikido/planner/optimalretimer/KinodynamicTimer.hpp

+  double mTimeStep;
+};
+
+} // namespace optimal_retimer


Nit: } // namespace optimalretimer

Reducing the maxDeviation from 10 to 0.1 brings the runtime down from 8 seconds to 3 seconds.

brianhou · 2018-08-02T05:57:55Z

After discussing with @aditya-vk, I've renamed OptimalRetimer to KunzRetimer. This is just a cosmetic change, so I believe the tests should still pass.

However, one of the tests (KunzRetimerTests.timingArbitraryMultipleWaypoints) is quite slow, initially taking 8 seconds on my laptop with the original parameter settings. I brought it down to 3 seconds by tweaking the maxDeviation parameter, but I'd still like it to be below the 1 second mark.

@dqyi11 do those waypoint values come from somewhere? If we're just choosing arbitrary points, maybe we can choose arbitrary ones that we can retime more quickly 😅

dqyi11 · 2018-08-02T06:43:51Z

@brianhou Thanks! Yes. They are just arbitrarily chosen ones. I will improve the test speed before merging it tomorrow morning.

* add kinodynamic retiming * integrate retiming code * add unit tests * finish unit tests * add docstrings and tests for post-processing * code format * fix tests for post processor * fix code style * code clean * add CHANGELOG * address Brian's comments * address AVK's comments * rename folder * rename namespace * Fix formatting. * rename functions * fix doc strings * refactor the functions in detail namespace * Rename OptimalRetimer to KunzRetimer. * Speed up KunzRetimer tests. Reducing the maxDeviation from 10 to 0.1 brings the runtime down from 8 seconds to 3 seconds. * modify test values for speeding

Daqing Yi added 10 commits June 7, 2018 17:58

add kinodynamic retiming

15afcfe

integrate retiming code

1a141c4

add unit tests

aaa4526

finish unit tests

eb10f6a

add docstrings and tests for post-processing

0af3405

Merge branch 'master' into enhancement/dqyi/addRetiming

aa05d22

code format

21c76ee

fix tests for post processor

de10fd8

fix code style

6cfa562

code clean

c2fb8e4

dqyi11 added this to the Aikido 0.3.0 milestone Jun 11, 2018

dqyi11 requested review from mkoval, brianhou, jslee02 and gilwoolee June 11, 2018 20:39

add CHANGELOG

56cdff4

brianhou mentioned this pull request Jul 2, 2018

Fix issues discovered during demo. #442

Merged

5 tasks

brianhou suggested changes Jul 2, 2018

View reviewed changes

Merge branch 'master' into enhancement/dqyi/addRetiming

52262c0

Merge branch 'master' into enhancement/dqyi/addRetiming

d70bbe9

brianhou and others added 3 commits July 19, 2018 12:47

Merge branch 'master' into enhancement/dqyi/addRetiming

4513327

address Brian's comments

ce1d6b5

Merge branch 'enhancement/dqyi/addRetiming' of https://github.com/per…

9994f9d

…sonalrobotics/aikido into enhancement/dqyi/addRetiming

aditya-vk reviewed Jul 20, 2018

View reviewed changes

address AVK's comments

42d9402

Daqing Yi added 2 commits July 25, 2018 14:55

rename folder

af7936a

rename namespace

e3c3a77

Fix formatting.

fff7bde

brianhou suggested changes Jul 26, 2018

View reviewed changes

jslee02 previously approved these changes Jul 26, 2018

View reviewed changes

rename functions

1e221a0

dqyi11 dismissed jslee02’s stale review via 1e221a0 July 30, 2018 21:03

Daqing Yi and others added 5 commits August 1, 2018 16:14

fix doc strings

aad12a1

Merge branch 'master' into enhancement/dqyi/addRetiming

7fcf90d

refactor the functions in detail namespace

3292ac8

Rename OptimalRetimer to KunzRetimer.

637dfde

Speed up KunzRetimer tests.

079bbac

Reducing the maxDeviation from 10 to 0.1 brings the runtime down from 8 seconds to 3 seconds.

brianhou previously approved these changes Aug 2, 2018

View reviewed changes

jslee02 previously approved these changes Aug 2, 2018

View reviewed changes

modify test values for speeding

70175fd

dqyi11 dismissed stale reviews from jslee02 and brianhou via 70175fd August 2, 2018 19:56

brianhou approved these changes Aug 2, 2018

View reviewed changes

brianhou merged commit e827fac into master Aug 2, 2018

brianhou deleted the enhancement/dqyi/addRetiming branch August 2, 2018 21:36

		@@ -0,0 +1,18 @@
		find_package(EIGEN3 REQUIRED)

		add_library("${PROJECT_NAME}_external_optimaltimepathfollowing" STATIC

Adding another kinodynamic timer #443

Adding another kinodynamic timer #443

Conversation

dqyi11 commented Jun 11, 2018 • edited by brianhou Loading

codecov bot commented Jun 11, 2018 • edited Loading

Codecov Report

brianhou commented Jun 22, 2018

dqyi11 commented Jun 23, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jslee02 Jul 20, 2018 • 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

brianhou commented Jul 19, 2018

dqyi11 commented Jul 19, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dqyi11 commented Jul 20, 2018

brianhou commented Jul 21, 2018

dqyi11 commented Jul 25, 2018

brianhou left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

brianhou commented Jul 26, 2018

jslee02 left a comment • 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

brianhou commented Aug 2, 2018

dqyi11 commented Aug 2, 2018

dqyi11 commented Jun 11, 2018 •

edited by brianhou

Loading

codecov bot commented Jun 11, 2018 •

edited

Loading

jslee02 Jul 20, 2018 •

edited

Loading

jslee02 left a comment •

edited

Loading