Add B-spline trajectory #453

jslee02 · 2018-08-06T01:12:38Z

(This is a work-in-progress pull request to get early feedback.)

This PR adds trajectory::BSpline, implementation of B-spline trajectory, and necessary classes in common namespace.

Unlike trajectory::Spline, trajectory::BSpline guarantees class C^k continuity (k - 1 times differentiable) over the whole trajectory where k is the order of B-spline.

Eigen::Spline (renamed to aikido::common::BSpline) and Eigen::SplineTraits are copied into AIKIDO code base with some modifications. The motivation of the adjustment is that Eigen's B-spline module doesn't allow changing control points after construction while we need this feature for trajectory optimization, which we will introduce soon.

Unit tests will be added before merging this PR.

Before creating a pull request

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 · 2018-08-07T02:18:18Z

Codecov Report

Merging #453 into master will decrease coverage by 1.29%.
The diff coverage is 50%.

@@            Coverage Diff            @@
##           master     #453     +/-   ##
=========================================
- Coverage   78.88%   77.59%   -1.3%     
=========================================
  Files         258      260      +2     
  Lines        6300     6588    +288     
=========================================
+ Hits         4970     5112    +142     
- Misses       1330     1476    +146

Impacted Files	Coverage Δ
include/aikido/common/BSpline.hpp	`100% <100%> (ø)`
src/trajectory/BSpline.cpp	`37.39% <37.39%> (ø)`
src/common/StepSequence.cpp	`92.85% <90%> (-0.7%)`	⬇️
src/planner/ompl/CRRT.cpp	`75.58% <0%> (-0.59%)`	⬇️

dqyi11 · 2018-08-08T00:15:30Z

include/aikido/common/SplineFwd.hpp

+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 20010-2011 Hauke Heibel <[email protected]>


Are we going to update the head description?

I think we should keep this license as we copied this code from Eigen, and will add our license on top of this license for the part we modify/add.

dqyi11 · 2018-08-08T00:17:43Z

include/aikido/common/SplineFwd.hpp

+};
+
+/** \brief 2D float B-spline with dynamic degree. */
+typedef BSpline<float, 2> BSpline2f;


Shall we change it to using BSpline2f = BSpline<float, 2> to follow aikido style?

I'm unsure whether we want to fully change this code or keep the changes as minimum as possible. Let's leave this code as it is and then revisit.

dqyi11 · 2018-08-08T00:20:17Z

include/aikido/trajectory/BSpline.hpp

+  using ControlPointVectorType = typename SplineType::ControlPointVectorType;
+
+  /// Constructs BSpline from control points and knots. The degree is
+  /// automatically determined by the


Unfinished?

dqyi11 · 2018-08-08T00:21:56Z

include/aikido/trajectory/BSpline.hpp

+  /// Sets start point of one sub space of the state space.
+  ///
+  /// \param[in] stateSpaceIndex The index to the state space to set the start
+  /// point.


The docstring of value is missed

dqyi11 · 2018-08-08T00:26:59Z

include/aikido/trajectory/BSpline.hpp

+      bool withStartControlPoint = true,
+      bool withEndControlPoint = true);
+
+  const ControlPointVectorType& getControlPoints(


Docstring is missed.

dqyi11 · 2018-08-08T00:27:08Z

include/aikido/trajectory/BSpline.hpp

+  const ControlPointVectorType& getControlPoints(
+      std::size_t stateSpaceIndex) const;
+
+  ControlPointVectorType getControlPoints(


Docstring is missed.

dqyi11 · 2018-08-08T00:47:33Z

src/trajectory/BSpline.cpp

+//==============================================================================
+void BSpline::evaluate(double t, statespace::StateSpace::State* state) const
+{
+  Eigen::VectorXd values(mStateSpace->getDimension());


shall we check t is in [startTime, endTime]?

dqyi11 · 2018-08-08T00:56:15Z

src/trajectory/BSpline.cpp

+{
+  const auto numControlPoints
+      = static_cast<ControlPointVectorType::Index>(getNumControlPoints());
+


Shall stateSpaceIndex be checked?

dqyi11 · 2018-08-08T00:56:31Z

src/trajectory/BSpline.cpp

+const BSpline::ControlPointVectorType& BSpline::getControlPoints(
+    std::size_t stateSpaceIndex) const
+{
+  return mSplines[stateSpaceIndex].ctrls();


shall the stateSpaceIndex be checked?

dqyi11 · 2018-08-08T01:07:41Z

src/trajectory/BSpline.cpp

+
+  const common::StepSequence internalKnots(
+      startTime, endTime, numControlPoints + 1u - degree, true);
+  ControlPointVectorType::Index index = 1;


why index==0 is not updated?

This line should be fixed to index = degree. We use multiple knots of the number of degree at the start and end knots so that the start and end of the trajectory are placed at the first and last control points, respectively. Let me document this to be clear.

dqyi11 · 2018-08-08T01:09:06Z

src/trajectory/BSpline.cpp

+    double /*t*/, int derivative, Eigen::VectorXd& /*tangentVector*/) const
+{
+  if (derivative < 1)
+    throw std::logic_error("Derivative must be positive.");


shall we just use std::argument_error?

jslee02 · 2018-09-03T07:10:05Z

@dqyi11 I believe the comments are addressed. Could you do a second round review?

Add B-spline trajectory

7ed2455

jslee02 added the pr: work-in-progress Calls for preliminary review of the PR. Remove when PR is ready for complete review. label Aug 6, 2018

jslee02 added this to the Aikido 0.3.0 milestone Aug 6, 2018

jslee02 requested review from brianhou and dqyi11 August 6, 2018 01:12

jslee02 added 2 commits August 5, 2018 18:15

Update changelog

4833bfb

Add missing override specifiers

0dcd760

dqyi11 reviewed Aug 8, 2018

View reviewed changes

dqyi11 and others added 9 commits August 29, 2018 16:52

add unit test for B spline

4809975

Merge branch 'master' into enhance/bspline_v2

862b63a

Change exception type: logic_error --> invalid_argument

8d77919

Format code

3cd08be

Fix computing step size in StepSequence

03be7cf

Update tests for bspline trajectory

24d530c

Add exceptions to BSpline

f96f8f5

Address comments

9daccb4

Add missing document

a1ad354

jslee02 removed the pr: work-in-progress Calls for preliminary review of the PR. Remove when PR is ready for complete review. label Sep 3, 2018

Format code

8d8c379

jslee02 merged commit 1d92d1c into master Oct 13, 2018

jslee02 deleted the enhance/bspline_v2 branch October 13, 2018 18:47

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

Add B-spline trajectory (#453)

b8bcfc5

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add B-spline trajectory #453

Add B-spline trajectory #453

jslee02 commented Aug 6, 2018 •

edited

Loading

codecov bot commented Aug 7, 2018 •

edited

Loading

dqyi11 Aug 8, 2018

jslee02 Aug 8, 2018

dqyi11 Aug 8, 2018

jslee02 Aug 8, 2018

dqyi11 Aug 8, 2018

dqyi11 Aug 8, 2018

dqyi11 Aug 8, 2018

dqyi11 Aug 8, 2018

dqyi11 Aug 8, 2018

dqyi11 Aug 8, 2018

dqyi11 Aug 8, 2018

dqyi11 Aug 8, 2018

jslee02 Aug 8, 2018

dqyi11 Aug 8, 2018

jslee02 commented Sep 3, 2018

Add B-spline trajectory #453

Add B-spline trajectory #453

Conversation

jslee02 commented Aug 6, 2018 • edited Loading

codecov bot commented Aug 7, 2018 • 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

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 commented Sep 3, 2018

jslee02 commented Aug 6, 2018 •

edited

Loading

codecov bot commented Aug 7, 2018 •

edited

Loading