A spline built from node values and polynomial durations. More...
#include <phase_spline.h>
Public Types | |
using | Ptr = std::shared_ptr< PhaseSpline > |
using | VectorXd = Eigen::VectorXd |
Public Types inherited from towr::NodeSpline | |
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 Types inherited from towr::PhaseDurationsObserver | |
using | PhaseDurationsSubjectPtr = PhaseDurations * |
Public Member Functions | |
Jacobian | GetJacobianOfPosWrtDurations (double t) const override |
How the spline position changes when the polynomial durations change. More... | |
PhaseSpline (NodesVariablesPhaseBased::Ptr const node_variables, PhaseDurations *const phase_durations) | |
Constructs a spline with varying/optimized phase durations. More... | |
void | UpdatePolynomialDurations () override |
Called by subject to update polynomial durations when they changed. More... | |
~PhaseSpline ()=default | |
Public Member Functions inherited from towr::NodeSpline | |
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 |
Public Member Functions inherited from towr::PhaseDurationsObserver | |
PhaseDurationsObserver ()=default | |
PhaseDurationsObserver (PhaseDurationsSubjectPtr phase_durations) | |
Registers this observer with the subject class to receive updates. More... | |
virtual | ~PhaseDurationsObserver ()=default |
Private Member Functions | |
Eigen::VectorXd | GetDerivativeOfPosWrtPhaseDuration (double t) const |
How the position at time t changes with current phase duration. More... | |
Private Attributes | |
NodesVariablesPhaseBased::Ptr | phase_nodes_ |
Additional Inherited Members | |
Static Public Member Functions inherited from towr::Spline | |
static int | GetSegmentID (double t_global, const VecTimes &durations) |
Protected Member Functions inherited from towr::NodeSpline | |
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 inherited from towr::NodeSpline | |
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_ |
Protected Attributes inherited from towr::PhaseDurationsObserver | |
PhaseDurationsSubjectPtr | phase_durations_ |
A spline built from node values and polynomial durations.
This class is responsible for combining the optimized node values with the optimized phase durations to construct a sequence of CubicHermitePolynomial. For this it observers whether one of the quantities changed and then updates all the polynomials accordingly.
Definition at line 47 of file phase_spline.h.
using towr::PhaseSpline::Ptr = std::shared_ptr<PhaseSpline> |
Definition at line 49 of file phase_spline.h.
using towr::PhaseSpline::VectorXd = Eigen::VectorXd |
Definition at line 50 of file phase_spline.h.
towr::PhaseSpline::PhaseSpline | ( | NodesVariablesPhaseBased::Ptr const | node_variables, |
PhaseDurations *const | phase_durations | ||
) |
Constructs a spline with varying/optimized phase durations.
node_variables | The optimized node variables (pos, vel). |
phase_durations | Pointer to the changing phase duration variables. |
Definition at line 35 of file phase_spline.cc.
|
default |
|
private |
How the position at time t changes with current phase duration.
t | The global time along the spline. |
Definition at line 78 of file phase_spline.cc.
|
overridevirtual |
How the spline position changes when the polynomial durations change.
t | The time along the spline at which the sensitivity is required. |
Reimplemented from towr::NodeSpline.
Definition at line 68 of file phase_spline.cc.
|
overridevirtual |
Called by subject to update polynomial durations when they changed.
Implements towr::PhaseDurationsObserver.
Definition at line 55 of file phase_spline.cc.
|
private |
Definition at line 83 of file phase_spline.h.