A spline built from node values and fixed polynomial durations. More...
#include <node_spline.h>
Public Types | |
using | Jacobian = Eigen::SparseMatrix< double, Eigen::RowMajor > |
using | Ptr = std::shared_ptr< NodeSpline > |
Public Types inherited from towr::Spline | |
using | VecPoly = std::vector< CubicHermitePolynomial > |
using | VecTimes = std::vector< double > |
Public Types inherited from towr::NodesObserver | |
using | NodeSubjectPtr = NodesVariables * |
Public Member Functions | |
virtual Jacobian | GetJacobianOfPosWrtDurations (double t) const |
How the spline position changes when the polynomial durations change. More... | |
Jacobian | GetJacobianWrtNodes (double t, Dx dxdt) const |
How the spline changes when the node values change. More... | |
Jacobian | GetJacobianWrtNodes (int poly_id, double t_local, Dx dxdt) const |
How the spline changes when the node values change. More... | |
int | GetNodeVariablesCount () const |
NodeSpline (NodeSubjectPtr const node_variables, const VecTimes &polynomial_durations) | |
Constructs a spline with constant durations. More... | |
void | UpdateNodes () |
Called by subject to update the polynomials with new node values. More... | |
~NodeSpline ()=default | |
Public Member Functions inherited from towr::Spline | |
const State | GetPoint (double t) const |
const State | GetPoint (int poly_id, double t_local) const |
VecTimes | GetPolyDurations () const |
int | GetPolynomialCount () const |
double | GetTotalTime () const |
Spline (const VecTimes &poly_durations, int n_dim) | |
virtual | ~Spline ()=default |
Public Member Functions inherited from towr::NodesObserver | |
NodesObserver (NodeSubjectPtr node_values) | |
Registers this observer with the subject class to receive updates. More... | |
virtual | ~NodesObserver ()=default |
Protected Member Functions | |
void | FillJacobianWrtNodes (int poly_id, double t_local, Dx dxdt, Jacobian &jac, bool fill_with_zeros) const |
Fills specific elements of the Jacobian with respect to nodes. More... | |
Protected Member Functions inherited from towr::Spline | |
std::pair< int, double > | GetLocalTime (double t_global, const VecTimes &d) const |
How much time of the current segment has passed at t_global. More... | |
void | UpdatePolynomialCoeff () |
Updates the cubic-Hermite polynomial coefficients using the currently set nodes values and durations. More... | |
Protected Attributes | |
Jacobian | jac_wrt_nodes_structure_ |
Protected Attributes inherited from towr::Spline | |
VecPoly | cubic_polys_ |
the sequence of polynomials making up the spline. More... | |
Protected Attributes inherited from towr::NodesObserver | |
NodeSubjectPtr | node_values_ |
Additional Inherited Members | |
Static Public Member Functions inherited from towr::Spline | |
static int | GetSegmentID (double t_global, const VecTimes &durations) |
A spline built from node values and fixed polynomial durations.
This class is responsible for combining the optimized node values with fixed polynomial durations to construct a sequence of CubicHermitePolynomial. For this it observes whether the node values change and then updates all the polynomials accordingly.
Definition at line 49 of file node_spline.h.
using towr::NodeSpline::Jacobian = Eigen::SparseMatrix<double, Eigen::RowMajor> |
Definition at line 52 of file node_spline.h.
using towr::NodeSpline::Ptr = std::shared_ptr<NodeSpline> |
Definition at line 51 of file node_spline.h.
towr::NodeSpline::NodeSpline | ( | NodeSubjectPtr const | node_variables, |
const VecTimes & | polynomial_durations | ||
) |
Constructs a spline with constant durations.
nodes_variables | The optimized node variables (pos, vel). |
phase_durations | The fixed duration of each phase. |
Definition at line 36 of file node_spline.cc.
|
default |
|
protected |
Fills specific elements of the Jacobian with respect to nodes.
poly_id | The ID of the polynomial for which to get the sensitivity. |
t_local | The time passed in the polynomial with poly_id. |
dxdt | Whether the function is position, velocity or acceleration. |
jac[in/out] | The correctly sized Jacobian to fill. |
fill_with_zeros | True if only sparsity pattern should be set. |
Definition at line 85 of file node_spline.cc.
|
inlinevirtual |
How the spline position changes when the polynomial durations change.
t | The time along the spline at which the sensitivity is required. |
Reimplemented in towr::PhaseSpline.
Definition at line 102 of file node_spline.h.
NodeSpline::Jacobian towr::NodeSpline::GetJacobianWrtNodes | ( | double | t, |
Dx | dxdt | ||
) | const |
How the spline changes when the node values change.
t | The time along the spline at which the sensitivity is required. |
dxdt | Whether the derivative of the pos, vel or acc is desired. |
Definition at line 63 of file node_spline.cc.
NodeSpline::Jacobian towr::NodeSpline::GetJacobianWrtNodes | ( | int | poly_id, |
double | t_local, | ||
Dx | dxdt | ||
) | const |
How the spline changes when the node values change.
poly_id | Polynomial for which the sensitivity is desired. |
t_local | Local time in that specific polynomial. |
dxdt | Whether the derivative of the pos, vel or acc is desired. |
Definition at line 72 of file node_spline.cc.
int towr::NodeSpline::GetNodeVariablesCount | ( | ) | const |
Definition at line 57 of file node_spline.cc.
|
virtual |
Called by subject to update the polynomials with new node values.
Implements towr::NodesObserver.
Definition at line 46 of file node_spline.cc.
|
mutableprotected |
The size and non-zero elements of the Jacobian of the position w.r.t nodes.
Definition at line 108 of file node_spline.h.