Add ParabolicTiming for linear spline #302

dqyi11 · 2018-01-18T22:04:26Z

This pull request adds computeParabolicTime for linear spline trajectory.

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

…ories

codecov · 2018-01-18T22:32:43Z

Codecov Report

Merging #302 into master will decrease coverage by 0.03%.
The diff coverage is 73.91%.

@@            Coverage Diff            @@
##           master    #302      +/-   ##
=========================================
- Coverage   81.14%   81.1%   -0.04%     
=========================================
  Files         204     204              
  Lines        5891    5913      +22     
=========================================
+ Hits         4780    4796      +16     
- Misses       1111    1117       +6

Impacted Files	Coverage Δ
src/planner/parabolic/ParabolicUtil.cpp	`90.75% <100%> (+0.15%)`	⬆️
src/planner/parabolic/ParabolicTimer.cpp	`76.62% <70%> (-2.33%)`	⬇️

…ories

jslee02 · 2018-01-19T05:51:34Z

CHANGELOG.md

@@ -30,6 +30,7 @@

  * Added World class: [#243](https://github.com/personalrobotics/aikido/pull/243), [#252](https://github.com/personalrobotics/aikido/pull/252), [#265](https://github.com/personalrobotics/aikido/pull/265)
  * Added vector field planner [#246](https://github.com/personalrobotics/aikido/pull/246), [#262](https://github.com/personalrobotics/aikido/pull/262), [#268](https://github.com/personalrobotics/aikido/pull/268)
+  * Added parabolic timing for linear spline [#302](https://github.com/personalrobotics/aikido/pull/302)


Please move this to 0.3.0.

jslee02 · 2018-01-19T05:56:03Z

src/planner/parabolic/ParabolicTimer.cpp

+      _inputTrajectory, _maxVelocity, _maxAcceleration, true);
+
+  auto outputTrajectory
+      = detail::convertToSpline(*dynamicPath, startTime, stateSpace);


Is dynamicPath always not nullptr? If so, please throw an exception if dynamicPath is nullptr.

Otherwise, return nullptr as output trajectory if dynamicPath is nullptr.

The current implementation of convertToDynamicPath guarantees that dynamicPath will never be nullptr.
Do you think we shall add the check in case any potential change will happen in the future?

jslee02 · 2018-01-19T05:59:40Z

src/planner/parabolic/ParabolicTimer.cpp

+      throw std::invalid_argument("Velocity limits must be finite.");
+
+    if (_maxAcceleration[i] <= 0.)
+      throw std::invalid_argument("Acceleration limits must be positive.");


Shouldn't be fine to allow zero velocity/acceleration limits? Could you explain for me why nonnegative limits aren't enough? 😅

This is following the code of computeParabolicTiming for an Interpolated.
I think the reason is that if the velocity/acceleration limit of one dimension is zero, it should not be considered in timing a trajectory.

…ories

brianhou

This is a lot of duplicated code in convertToDynamicPath and computeParabolicTiming:

computeParabolicTiming is identical except for passing a flag to convertToDynamicPath to decide whether velocities should be preserved (which is currently supported only for Spline, not Interpolated, and is always set to ignore the velocities)
convertToDynamicPath is identical except for (1) how waypoints are extracted from each trajectory type, (2) evaluating Spline derivatives to (potentially) preserve them in the DynamicPath, and (3) the flag mentioned above

It seems to me that this redundancy is mostly because we don't have a unified interface for getting waypoints from a trajectory (e.g. getNumWaypoints(), getWaypoint(i), getWaypointTime(i)) for convertToDynamicPath. I'm not sure whether the best way to add this is to add a WaypointTrajectory in the class hierarchy between Trajectory and Interpolated/Spline, or to add those methods directly to the Trajectory class. Either way, the resulting code for both types of trajectories could look something like this:

std::unique_ptr<ParabolicRamp::DynamicPath> convertToDynamicPath(
    const aikido::trajectory::WaypointTrajectory& _inputTrajectory,
    const Eigen::VectorXd& _maxVelocity,
    const Eigen::VectorXd& _maxAcceleration,
    bool preserveWaypointVelocities)
{
  const auto stateSpace = _inputTrajectory.getStateSpace();
  const auto numWaypoints = _inputTrajectory.getNumWaypoints();

  std::vector<ParabolicRamp::Vector> milestones;
  std::vector<ParabolicRamp::Vector> velocities;

  Eigen::VectorXd tangentVector, currVec;
  for (std::size_t iwaypoint = 0; iwaypoint < numWaypoints; ++iwaypoint)
  {
    auto currentState = stateSpace->createState();
    double t = _inputTrajectory.getWaypointTime(iwaypoint);

    _inputTrajectory.evaluate(t, currentState);
    stateSpace->logMap(currentState, currVec);
    milestones.emplace_back(toVector(currVec));

    if (preserveWaypointVelocities)
    {
      _inputTrajectory.evaluateDerivative(t, 1, tangentVector);
      velocities.emplace_back(toVector(tangentVector));
    }
  }

  auto outputPath = make_unique<ParabolicRamp::DynamicPath>();
  outputPath->Init(toVector(_maxVelocity), toVector(_maxAcceleration));
  if (preserveWaypointVelocities)
    outputPath->SetMilestones(milestones, velocities)
  else
    outputPath->SetMilestones(milestones)

  if (!outputPath->IsValid())
    throw std::runtime_error("Converted DynamicPath is not valid");
  return outputPath;
}

Does this change sound valuable, and would it make sense to add this interface to Trajectory or a different class?

brianhou · 2018-01-20T02:48:02Z

src/planner/parabolic/ParabolicUtil.hpp

@@ -60,7 +60,8 @@ std::unique_ptr<aikido::trajectory::Spline> convertToSpline(
 std::unique_ptr<ParabolicRamp::DynamicPath>
    convertToDynamicPath(const aikido::trajectory::Spline& _inputTrajectory,
                         const Eigen::VectorXd& _maxVelocity,
-                         const Eigen::VectorXd& _maxAcceleration);
+                         const Eigen::VectorXd& _maxAcceleration,
+                         bool untimed = false);


Can we rename untimed to timed and flip the default values? Or maybe a more informative name, like preserveWaypointVelocity, would be better?

In general, I am a fan of boolean flags to indicate input properties including an is or has prefix to help indicate their intent e.g. isTimed. But that's neither here nor there.

dqyi11 · 2018-01-24T17:56:12Z

@brianhou I agree with your suggestion on introducing a WaypointTrajectory class.
Shall we create an issue for discussion before making the changes?
My minor concern is that a Spline is a set of segments. A segment includes a start state, duration and curve coefficients.
https://github.com/personalrobotics/aikido/blob/master/include/aikido/trajectory/Spline.hpp#L132
The "waypoints" of a Spline is "indirectly" obtained from segments.
https://github.com/personalrobotics/aikido/blob/master/src/trajectory/Spline.cpp#L206
https://github.com/personalrobotics/aikido/blob/master/src/trajectory/Spline.cpp#L226

There is an alternative approach in conversion.
We could generate a time sequence from start time to end time, and call evaluate method of a Trajectory class to generate a sequence of "waypoints" for conversion.
But it will be very hard to determine a "good" time step.

brianhou · 2018-01-24T19:24:54Z

I see. Let's just merge this PR as is (to unblock @sniyaz and MAGI cleanup) and then create an issue to track this cleanup. @dqyi11, can you create the issue? I'll merge once Travis is satisfied.

dqyi11 · 2018-01-24T19:44:23Z

@brianhou Thanks!
I have created an issue #312

#312

Magi requires that we have post processors support both Interpolated and Spline trajectories. This is a small PR that makes the parabolic post processor classes support the new methods added in #302.

* add timing for spline trajectories * fix code style * add test case * add docstring * update change log * fix CHANGELOG * address Brian's comments * fix bug

Magi requires that we have post processors support both Interpolated and Spline trajectories. This is a small PR that makes the parabolic post processor classes support the new methods added in #302.

dqyi11 added 5 commits December 10, 2017 16:45

add timing for spline trajectories

b817559

fix code style

c95525c

Merge branch 'master' into enhancement/dqyi/enableTimingSplineTraject…

3666be1

…ories

add test case

4572195

add docstring

0e8e642

dqyi11 added this to the Aikido 0.3.0 milestone Jan 18, 2018

update change log

977f7f9

dqyi11 requested a review from brianhou January 18, 2018 22:14

Merge branch 'master' into enhancement/dqyi/enableTimingSplineTraject…

a64317b

…ories

jslee02 reviewed Jan 19, 2018

View reviewed changes

Daqing Yi and others added 4 commits January 19, 2018 14:57

fix CHANGELOG

c88945f

merge from master

ab6ea2c

Merge branch 'master' into enhancement/dqyi/enableTimingSplineTraject…

0ae2b3b

…ories

Merge branch 'master' into enhancement/dqyi/enableTimingSplineTraject…

c48568f

…ories

brianhou previously requested changes Jan 20, 2018

View reviewed changes

address Brian's comments

6f0da09

fix bug

9b7c843

brianhou approved these changes Jan 25, 2018

View reviewed changes

brianhou merged commit 25d00f2 into master Jan 25, 2018

brianhou deleted the enhancement/dqyi/enableTimingSplineTrajectories branch January 25, 2018 00:46

brianhou mentioned this pull request Jan 25, 2018

Demo branch #287

Closed

3 tasks

sniyaz mentioned this pull request Feb 6, 2018

Update ParabolicSmoother/ParabolicTimer to support Splines #324

Merged

gilwoolee pushed a commit that referenced this pull request Jan 21, 2019

Add ParabolicTiming for linear spline (#302)

d86462a

* add timing for spline trajectories * fix code style * add test case * add docstring * update change log * fix CHANGELOG * address Brian's comments * fix bug

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add ParabolicTiming for linear spline #302

Add ParabolicTiming for linear spline #302

dqyi11 commented Jan 18, 2018 •

edited

Loading

codecov bot commented Jan 18, 2018 •

edited

Loading

jslee02 Jan 19, 2018

jslee02 Jan 19, 2018

dqyi11 Jan 19, 2018

jslee02 Jan 19, 2018

dqyi11 Jan 19, 2018

brianhou left a comment

brianhou Jan 20, 2018

psigen Jan 20, 2018

dqyi11 commented Jan 24, 2018

brianhou commented Jan 24, 2018

dqyi11 commented Jan 24, 2018

Add ParabolicTiming for linear spline #302

Add ParabolicTiming for linear spline #302

Conversation

dqyi11 commented Jan 18, 2018 • edited Loading

codecov bot commented Jan 18, 2018 • edited Loading

Codecov Report

jslee02 Jan 19, 2018

Choose a reason for hiding this comment

jslee02 Jan 19, 2018

Choose a reason for hiding this comment

dqyi11 Jan 19, 2018

Choose a reason for hiding this comment

jslee02 Jan 19, 2018

Choose a reason for hiding this comment

dqyi11 Jan 19, 2018

Choose a reason for hiding this comment

brianhou left a comment

Choose a reason for hiding this comment

brianhou Jan 20, 2018

Choose a reason for hiding this comment

psigen Jan 20, 2018

Choose a reason for hiding this comment

dqyi11 commented Jan 24, 2018

brianhou commented Jan 24, 2018

dqyi11 commented Jan 24, 2018

dqyi11 commented Jan 18, 2018 •

edited

Loading

codecov bot commented Jan 18, 2018 •

edited

Loading