Nodes that are associated to either swing or stance phases. More...

#include <nodes_variables_phase_based.h>

Inheritance diagram for towr::NodesVariablesPhaseBased:

[legend]

Classes
struct	PolyInfo
	Holds semantic information each polynomial in spline. More...

Public Types
using	NodeIds = std::vector< int >

using	OptIndexMap = std::map< int, std::vector< NodeValueInfo > >

using	Ptr = std::shared_ptr< NodesVariablesPhaseBased >

Public Types inherited from towr::NodesVariables
using	ObserverPtr = NodesObserver *

using	Ptr = std::shared_ptr< NodesVariables >

enum	Side { Start =0, End }

using	VecDurations = std::vector< double >

Public Types inherited from ifopt::Component
typedef Eigen::SparseMatrix< double, Eigen::RowMajor >	Jacobian

typedef std::shared_ptr< Component >	Ptr

typedef std::vector< Bounds >	VecBound

typedef Eigen::VectorXd	VectorXd

Public Member Functions
virtual VecDurations	ConvertPhaseToPolyDurations (const VecDurations &phase_durations) const
	Converts durations of swing and stance phases to polynomial durations. More...

virtual double	GetDerivativeOfPolyDurationWrtPhaseDuration (int polynomial_id) const
	How a change in the phase duration affects the polynomial duration. More...

NodeIds	GetIndicesOfNonConstantNodes () const
	The indices of those nodes that don't belong to a constant phase. More...

int	GetNodeIDAtStartOfPhase (int phase) const

virtual int	GetNumberOfPrevPolynomialsInPhase (int polynomial_id) const
	How many polynomials in the current phase come before. More...

int	GetPhase (int node_id) const

Eigen::Vector3d	GetValueAtStartOfPhase (int phase) const

virtual bool	IsConstantNode (int node_id) const
	node is constant if either left or right polynomial belongs to a constant phase. More...

virtual bool	IsInConstantPhase (int polynomial_id) const
	Is the polynomial constant, so not changing the value. More...

	NodesVariablesPhaseBased (int phase_count, bool first_phase_constant, const std::string &var_name, int n_polys_in_changing_phase)
	Constructs a variable set of node variables. More...

virtual	~NodesVariablesPhaseBased ()=default

Public Member Functions inherited from towr::NodesVariables
void	AddFinalBound (Dx deriv, const std::vector< int > &dimensions, const VectorXd &val)
	Restricts the last node in the spline. More...

void	AddObserver (ObserverPtr const spline)
	Adds a dependent observer that gets notified when the nodes change. More...

void	AddStartBound (Dx deriv, const std::vector< int > &dimensions, const VectorXd &val)
	Restricts the first node in the spline. More...

const std::vector< Node >	GetBoundaryNodes (int poly_id) const

VecBound	GetBounds () const override

int	GetDim () const

const std::vector< Node >	GetNodes () const

int	GetOptIndex (const NodeValueInfo &nvi) const
	Index in the optimization vector for a specific nodes' pos/vel. More...

int	GetPolynomialCount () const

VectorXd	GetValues () const override
	Pure optimization variables that define the nodes. More...

void	SetByLinearInterpolation (const VectorXd &initial_val, const VectorXd &final_val, double t_total)
	Sets nodes pos/vel equally spaced from initial to final position. More...

void	SetVariables (const VectorXd &x) override
	Sets some node positions and velocity from the optimization variables. More...

Public Member Functions inherited from ifopt::VariableSet
Jacobian	GetJacobian () const final

	VariableSet (int n_var, const std::string &name)

virtual	~VariableSet ()=default

Public Member Functions inherited from ifopt::Component
	Component (int num_rows, const std::string &name)

std::string	GetName () const

int	GetRows () const

virtual void	Print (double tolerance, int &index_start) const

void	SetRows (int num_rows)

virtual	~Component ()=default

Protected Member Functions
std::vector< NodeValueInfo >	GetNodeValuesInfo (int idx) const override
	Node values affected by one specific optimization variable. More...

void	SetNumberOfVariables (int n_variables)

Protected Member Functions inherited from towr::NodesVariables
	NodesVariables (const std::string &variable_name)

virtual	~NodesVariables ()=default

Protected Attributes
OptIndexMap	index_to_node_value_info_
	Assign optimization variables to the correct node values. More...

Protected Attributes inherited from towr::NodesVariables
VecBound	bounds_
	the bounds on the node values. More...

int	n_dim_

std::vector< Node >	nodes_

Private Member Functions
std::vector< int >	GetAdjacentPolyIds (int node_id) const
	IDs of the polynomial to the left and right of node_id. More...

int	GetPolyIDAtStartOfPhase (int phase) const
	ID of the first polynomial of a phase. More...

Private Attributes
std::vector< PolyInfo >	polynomial_info_
	semantic information associated with each polynomial More...

Additional Inherited Members
Static Public Member Functions inherited from towr::NodesVariables
static int	GetNodeId (int poly_id, Side side)
	The node ID that belongs to a specific side of a specific polynomial. More...

Static Public Attributes inherited from towr::NodesVariables
static const int	NodeValueNotOptimized = -1

Static Public Attributes inherited from ifopt::Component
static const int	kSpecifyLater

Detailed Description

Nodes that are associated to either swing or stance phases.

Not all node values must be optimized over. We can for example fix node derivatives to 0.0 and also use the same optimization variable to fill to node positions. If the spline represents the position of an end-effector, this means that no matter which values the the solver chooses, the foot will never move during the stance phase. This is done in NodesVariablesEEMotion. A force spline is parameterized in the reverse way, with multiple nodes during stance phase. This is implemented in NodesVariablesEEForce.

Motion (dim: x) and force (dim: z) spline for one foot

A high-level explanation of this concept can be found here: https://youtu.be/KhWuLvb934g?t=1004

Definition at line 59 of file nodes_variables_phase_based.h.

Member Typedef Documentation

using towr::NodesVariablesPhaseBased::NodeIds = std::vector<int>

Definition at line 62 of file nodes_variables_phase_based.h.

using towr::NodesVariablesPhaseBased::OptIndexMap = std::map<int, std::vector<NodeValueInfo> >

Definition at line 63 of file nodes_variables_phase_based.h.

using towr::NodesVariablesPhaseBased::Ptr = std::shared_ptr<NodesVariablesPhaseBased>

Definition at line 61 of file nodes_variables_phase_based.h.

Constructor & Destructor Documentation

towr::NodesVariablesPhaseBased::NodesVariablesPhaseBased	(	int	phase_count,
		bool	first_phase_constant,
		const std::string &	var_name,
		int	n_polys_in_changing_phase
	)

Constructs a variable set of node variables.

Parameters

phase_count	The number of phases (swing, stance) to represent.
first_phase_constant	Whether first node belongs to a constant phase.
var_name	The name given to this set of optimization variables.
n_polys_in_changing_phase	How many polynomials should be used to paramerize each non-constant phase.

Definition at line 60 of file nodes_variables_phase_based.cc.

virtual towr::NodesVariablesPhaseBased::~NodesVariablesPhaseBased ( )

virtualdefault

Member Function Documentation

NodesVariablesPhaseBased::VecDurations towr::NodesVariablesPhaseBased::ConvertPhaseToPolyDurations ( const VecDurations & phase_durations ) const

virtual

Converts durations of swing and stance phases to polynomial durations.

Parameters

phase_durations The durations of alternating swing and stance phases.

Returns: The durations of each polynomial, where multiple polynomials can be used to represent one phase.

Definition at line 74 of file nodes_variables_phase_based.cc.

std::vector< int > towr::NodesVariablesPhaseBased::GetAdjacentPolyIds ( int node_id ) const

private

IDs of the polynomial to the left and right of node_id.

Definition at line 164 of file nodes_variables_phase_based.cc.

double towr::NodesVariablesPhaseBased::GetDerivativeOfPolyDurationWrtPhaseDuration ( int polynomial_id ) const

virtual

How a change in the phase duration affects the polynomial duration.

Parameters

polynomial_id The ID of the polynomial within the spline.

If a phase is represented by multiple (3) equally timed polynomials, then T_poly = 1/3 * T_phase. The derivative of T_poly is then 1/3.

Definition at line 87 of file nodes_variables_phase_based.cc.

NodesVariablesPhaseBased::NodeIds towr::NodesVariablesPhaseBased::GetIndicesOfNonConstantNodes ( ) const

The indices of those nodes that don't belong to a constant phase.

For forces nodes these are the stance phases (can produce force), and for feet these are the swing phases (can move end-effector).

Definition at line 120 of file nodes_variables_phase_based.cc.

int towr::NodesVariablesPhaseBased::GetNodeIDAtStartOfPhase ( int phase ) const

Returns: the ID of the first node in the phase.

Definition at line 157 of file nodes_variables_phase_based.cc.

std::vector<NodeValueInfo> towr::NodesVariablesPhaseBased::GetNodeValuesInfo ( int opt_idx ) const

inlineoverrideprotectedvirtual

Node values affected by one specific optimization variable.

Parameters

opt_idx The index (=row) of the optimization variable.

Returns: All node values affected by this optimization variable.

This function determines which node values are optimized over, and which nodes values are set by the same optimization variable.

Reverse of GetOptIndex().

Implements towr::NodesVariables.

Definition at line 168 of file nodes_variables_phase_based.h.

int towr::NodesVariablesPhaseBased::GetNumberOfPrevPolynomialsInPhase ( int polynomial_id ) const

virtual

How many polynomials in the current phase come before.

Parameters

polynomial_id The ID of the polynomial within the spline.

If a phase is represented by multiple (3) polynomials, and the current polynomial corresponds to the third one in the phase, then 2 polynomials come before it.

Definition at line 94 of file nodes_variables_phase_based.cc.

int towr::NodesVariablesPhaseBased::GetPhase ( int node_id ) const