-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
path_parameterized_trajectory.cc
86 lines (75 loc) · 2.77 KB
/
path_parameterized_trajectory.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
#include "drake/common/trajectories/path_parameterized_trajectory.h"
#include <algorithm>
#include <fmt/format.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::clamp;
const T time =
clamp(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::clamp;
const T time =
clamp(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)