forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[trajectories] Add PathParameterizedTrajectory (RobotLocomotion#17816)
- Loading branch information
1 parent
04b640f
commit a2caba1
Showing
6 changed files
with
357 additions
and
2 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,88 @@ | ||
| #include "drake/common/trajectories/path_parameterized_trajectory.h" | ||
|
|
||
| #include <algorithm> | ||
|
|
||
| #include <fmt/ostream.h> | ||
|
|
||
| #include "drake/common/drake_assert.h" | ||
|
|
||
| namespace drake { | ||
| namespace trajectories { | ||
|
|
||
| template <typename T> | ||
| PathParameterizedTrajectory<T>::PathParameterizedTrajectory( | ||
| const Trajectory<T>& path, const Trajectory<T>& time_scaling) | ||
| : path_{path.Clone()}, time_scaling_{time_scaling.Clone()} { | ||
| DRAKE_DEMAND(time_scaling.rows() == 1); | ||
| DRAKE_DEMAND(time_scaling.cols() == 1); | ||
| } | ||
|
|
||
| template <typename T> | ||
| std::unique_ptr<Trajectory<T>> PathParameterizedTrajectory<T>::Clone() const { | ||
| return std::make_unique<PathParameterizedTrajectory<T>>(*this); | ||
| } | ||
|
|
||
| template <typename T> | ||
| MatrixX<T> PathParameterizedTrajectory<T>::value(const T& t) const { | ||
| using std::max; | ||
| using std::min; | ||
| const T time = | ||
| min(max(t, time_scaling_->start_time()), time_scaling_->end_time()); | ||
| return path_->value(time_scaling_->value(time)(0, 0)); | ||
| } | ||
|
|
||
| template <typename T> | ||
| T PathParameterizedTrajectory<T>::BellPolynomial(int n, int k, | ||
| const VectorX<T>& x) const { | ||
| if (n == 0 && k == 0) { | ||
| return 1; | ||
| } else if (n == 0 || k == 0) { | ||
| return 0; | ||
| } | ||
| T polynomial = 0; | ||
| T a = 1; | ||
| for (int ii = 1; ii < n - k + 2; ++ii) { | ||
| polynomial += a * BellPolynomial(n - ii, k - 1, x) * x[ii - 1]; | ||
| a = a * (n - ii) / ii; | ||
| } | ||
| return polynomial; | ||
| } | ||
|
|
||
| template <typename T> | ||
| MatrixX<T> PathParameterizedTrajectory<T>::DoEvalDerivative( | ||
| const T& t, int derivative_order) const { | ||
| using std::max; | ||
| using std::min; | ||
| const T time = | ||
| min(max(t, time_scaling_->start_time()), time_scaling_->end_time()); | ||
| if (derivative_order == 0) { | ||
| return value(time); | ||
| } else if (derivative_order > 0) { | ||
| VectorX<T> s_derivatives(derivative_order); | ||
| for (int order = 0; order < derivative_order; ++order) { | ||
| s_derivatives(order) = | ||
| time_scaling_->EvalDerivative(time, order + 1)(0, 0); | ||
| } | ||
| // Derivative is calculated using Faà di Bruno's formula with Bell | ||
| // polynomials: https://en.wikipedia.org/wiki/Fa%C3%A0_di_Bruno%27s_formula | ||
| MatrixX<T> derivative = MatrixX<T>::Zero(rows(), cols()); | ||
| for (int order = 1; order <= derivative_order; ++order) { | ||
| MatrixX<T> path_partial = | ||
| path_->EvalDerivative(time_scaling_->value(time)(0, 0), order); | ||
| derivative += | ||
| path_partial * BellPolynomial(derivative_order, order, s_derivatives); | ||
| } | ||
| return derivative; | ||
| } else { | ||
| throw std::invalid_argument( | ||
| fmt::format("Invalid derivative order ({}). The derivative order must " | ||
| "be greater than or equal to 0.", | ||
| derivative_order)); | ||
| } | ||
| } | ||
|
|
||
| } // namespace trajectories | ||
| } // namespace drake | ||
|
|
||
| DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( | ||
| class drake::trajectories::PathParameterizedTrajectory) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,90 @@ | ||
| #pragma once | ||
|
|
||
| #include <memory> | ||
| #include <vector> | ||
|
|
||
| #include <Eigen/Core> | ||
|
|
||
| #include "drake/common/copyable_unique_ptr.h" | ||
| #include "drake/common/default_scalars.h" | ||
| #include "drake/common/trajectories/trajectory.h" | ||
|
|
||
| namespace drake { | ||
| namespace trajectories { | ||
|
|
||
| /** | ||
| * A trajectory defined by a path and timing trajectory. | ||
| * | ||
| * Using a path of form `q(s)` and a time_scaling of the form `s(t)`, a full | ||
| * trajectory of form `q(t) = q(s(t))` is modeled. | ||
| * | ||
| * @tparam_default_scalars | ||
| */ | ||
| template <typename T> | ||
| class PathParameterizedTrajectory final : public Trajectory<T> { | ||
| public: | ||
| DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PathParameterizedTrajectory) | ||
|
|
||
| /** Constructs a trajectory with the given `path` and `time_scaling`. | ||
| @pre time_scaling.rows() == time_scaling.cols() == 1 */ | ||
| PathParameterizedTrajectory(const Trajectory<T>& path, | ||
| const Trajectory<T>& time_scaling); | ||
|
|
||
| ~PathParameterizedTrajectory() final = default; | ||
|
|
||
| // Required methods for trajectories::Trajectory interface. | ||
| std::unique_ptr<trajectories::Trajectory<T>> Clone() const override; | ||
|
|
||
| /** Evaluates the PathParameterizedTrajectory at the given time t. | ||
| @param t The time at which to evaluate the %PathParameterizedTrajectory. | ||
| @return The matrix of evaluated values. | ||
| @pre If T == symbolic::Expression, `t.is_constant()` must be true. | ||
| @warning If t does not lie in the range [start_time(), end_time()], the | ||
| trajectory will silently be evaluated at the closest | ||
| valid value of time to t. For example, `value(-1)` will return | ||
| `value(0)` for a trajectory defined over [0, 1]. */ | ||
| MatrixX<T> value(const T& t) const override; | ||
|
|
||
| Eigen::Index rows() const override { return path_->rows(); } | ||
|
|
||
| Eigen::Index cols() const override { return path_->cols(); } | ||
|
|
||
| T start_time() const override { return time_scaling_->start_time(); } | ||
|
|
||
| T end_time() const override { return time_scaling_->end_time(); } | ||
|
|
||
| /** Returns the path of this trajectory. */ | ||
| const trajectories::Trajectory<T>& path() const { return *path_; } | ||
|
|
||
| /** Returns the time_scaling of this trajectory. */ | ||
| const trajectories::Trajectory<T>& time_scaling() const { | ||
| return *time_scaling_; | ||
| } | ||
|
|
||
| private: | ||
| // Evaluates the %PathParameterizedTrajectory derivative at the given time @p | ||
| // t. Returns the nth derivative, where `n` is the value of @p | ||
| // derivative_order. | ||
| // | ||
| // @warning This method comes with the same caveats as value(). See value() | ||
| // @pre derivative_order must be non-negative. | ||
| MatrixX<T> DoEvalDerivative(const T& t, int derivative_order) const override; | ||
|
|
||
| // Evaluates the Bell Polynomial B_n,k(x) for use in calculating the | ||
| // derivative. | ||
| // @pre n and k must be non-negative and the length of x must be at least n. | ||
| T BellPolynomial(int n, int k, const VectorX<T>& x) const; | ||
|
|
||
| bool do_has_derivative() const override { | ||
| return path_->has_derivative() && time_scaling_->has_derivative(); | ||
| } | ||
|
|
||
| copyable_unique_ptr<Trajectory<T>> path_; | ||
| copyable_unique_ptr<Trajectory<T>> time_scaling_; | ||
| }; | ||
|
|
||
| } // namespace trajectories | ||
| } // namespace drake | ||
|
|
||
| DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( | ||
| class drake::trajectories::PathParameterizedTrajectory) |
Oops, something went wrong.