Ensure that the optimized motion complies with the system dynamics. More...

#include <dynamic_constraint.h>

Inheritance diagram for towr::DynamicConstraint:

Public Types
using	Vector6d = Eigen::Matrix< double, 6, 1 >

Public Types inherited from towr::TimeDiscretizationConstraint
using	Bounds = ifopt::Bounds

using	VecTimes = std::vector< double >

Public Types inherited from ifopt::ConstraintSet
typedef std::shared_ptr< ConstraintSet >	Ptr

typedef Composite::Ptr	VariablesPtr

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
	DynamicConstraint (const DynamicModel::Ptr &model, double T, double dt, const SplineHolder &spline_holder)
	Construct a Dynamic constraint. More...

virtual	~DynamicConstraint ()=default

Public Member Functions inherited from towr::TimeDiscretizationConstraint
void	FillJacobianBlock (std::string var_set, Jacobian &) const override

VecBound	GetBounds () const override

Eigen::VectorXd	GetValues () const override

	TimeDiscretizationConstraint (double T, double dt, std::string constraint_name)
	Constructs a constraint for ifopt. More...

	TimeDiscretizationConstraint (const VecTimes &dts, std::string name)
	construct a constraint for ifopt. More...

virtual	~TimeDiscretizationConstraint ()=default

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

Jacobian	GetJacobian () const final

void	LinkWithVariables (const VariablesPtr &x)

virtual	~ConstraintSet ()=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

Private Member Functions
int	GetRow (int k, Dim6D dimension) const
	The row in the overall constraint for this evaluation time. More...

void	UpdateBoundsAtInstance (double t, int k, VecBound &bounds) const override
	Sets upper/lower bound a specific time t, corresponding to node k. More...

void	UpdateConstraintAtInstance (double t, int k, VectorXd &g) const override
	Sets the constraint value a specific time t, corresponding to node k. More...

void	UpdateJacobianAtInstance (double t, int k, std::string, Jacobian &) const override
	Sets Jacobian rows at a specific time t, corresponding to node k. More...

void	UpdateModel (double t) const
	Updates the model with the current state and forces. More...

Private Attributes
EulerConverter	base_angular_
	angular base state More...

NodeSpline::Ptr	base_linear_
	lin. base pos/vel/acc in world frame More...

std::vector< NodeSpline::Ptr >	ee_forces_
	endeffector forces in world frame. More...

std::vector< NodeSpline::Ptr >	ee_motion_
	endeffector position in world frame. More...

DynamicModel::Ptr	model_
	the dynamic model (e.g. Centroidal) More...

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

Protected Member Functions inherited from towr::TimeDiscretizationConstraint
int	GetNumberOfNodes () const

Protected Member Functions inherited from ifopt::ConstraintSet
const VariablesPtr	GetVariables () const

Protected Attributes inherited from towr::TimeDiscretizationConstraint
VecTimes	dts_
	times at which the constraint is evaluated. More...

Detailed Description

Ensure that the optimized motion complies with the system dynamics.

At specific time instances along the trajecetory, this class checks the current value of the optimization variables and makes sure that the current robot state (pos,vel) and forces match the current acceleration defined by the system dynamics.

The physics-based acceleration is influenced by the robot state as

xdd(t) = f(x(t), xd(t), f(t))

The constraint of the optimization variables w is then:

g(t) = acc_spline(t) - xdd(t)
     = acc_spline(t) - xdd(x(t), xd(t), f(t))
     = acc_spline(w) - xdd(w)

Definition at line 63 of file dynamic_constraint.h.

Member Typedef Documentation

◆ Vector6d

using towr::DynamicConstraint::Vector6d = Eigen::Matrix<double, 6, 1>

Definition at line 65 of file dynamic_constraint.h.

Constructor & Destructor Documentation

◆ DynamicConstraint()

towr::DynamicConstraint::DynamicConstraint	(	const DynamicModel::Ptr &	model,
		double	T,
		double	dt,
		const SplineHolder &	spline_holder
	)

Construct a Dynamic constraint.

Parameters

model	The system dynamics to enforce (e.g. centroidal, LIP, ...)
T	The total duration of the optimization.
dt	the discretization intervall at which to enforce constraints.
spline_holder	A pointer to the current optimization variables.

Definition at line 37 of file dynamic_constraint.cc.

◆ ~DynamicConstraint()

virtual towr::DynamicConstraint::~DynamicConstraint ( )

virtualdefault

Member Function Documentation

◆ GetRow()

int towr::DynamicConstraint::GetRow	(	int	k,
		Dim6D	dimension
	)		const

private

The row in the overall constraint for this evaluation time.

Parameters

k	The index of the constraint evaluation at t=k*dt.
dimension	Which base acceleration dimension this constraint is for.

Returns: The constraint index in the overall dynamic constraint.

Definition at line 54 of file dynamic_constraint.cc.

◆ UpdateBoundsAtInstance()

void towr::DynamicConstraint::UpdateBoundsAtInstance	(	double	t,
		int	k,
		VecBound &	b
	)		const

overrideprivatevirtual

Sets upper/lower bound a specific time t, corresponding to node k.

Parameters

t	The time along the trajectory to set the bounds.
k	The index of the time t, so t=k*dt

Implements towr::TimeDiscretizationConstraint.

Definition at line 67 of file dynamic_constraint.cc.

◆ UpdateConstraintAtInstance()

void towr::DynamicConstraint::UpdateConstraintAtInstance	(	double	t,
		int	k,
		VectorXd &	g
	)		const

overrideprivatevirtual

Sets the constraint value a specific time t, corresponding to node k.

Parameters

t	The time along the trajectory to set the constraint.
k	The index of the time t, so t=k*dt

Implements towr::TimeDiscretizationConstraint.

Definition at line 60 of file dynamic_constraint.cc.

◆ UpdateJacobianAtInstance()

void towr::DynamicConstraint::UpdateJacobianAtInstance	(	double	t,
		int	k,
		std::string	var_set,
		Jacobian &	jac
	)		const

overrideprivatevirtual

Sets Jacobian rows at a specific time t, corresponding to node k.

Parameters

t	The time along the trajcetory to set the bounds.
k	The index of the time t, so t=k*dt
var_set	The name of the ifopt variables currently being queried for.

Implements towr::TimeDiscretizationConstraint.

Definition at line 74 of file dynamic_constraint.cc.

◆ UpdateModel()

void towr::DynamicConstraint::UpdateModel ( double t ) const

private

Updates the model with the current state and forces.

Parameters

t	Time at which to query the state and force splines.

Definition at line 120 of file dynamic_constraint.cc.

Member Data Documentation

◆ base_angular_

EulerConverter towr::DynamicConstraint::base_angular_

private

angular base state

Definition at line 81 of file dynamic_constraint.h.

◆ base_linear_

NodeSpline::Ptr towr::DynamicConstraint::base_linear_

private

lin. base pos/vel/acc in world frame

Definition at line 80 of file dynamic_constraint.h.

◆ ee_forces_

std::vector<NodeSpline::Ptr> towr::DynamicConstraint::ee_forces_

private

endeffector forces in world frame.

Definition at line 82 of file dynamic_constraint.h.

◆ ee_motion_

std::vector<NodeSpline::Ptr> towr::DynamicConstraint::ee_motion_

private

endeffector position in world frame.

Definition at line 83 of file dynamic_constraint.h.

◆ model_

DynamicModel::Ptr towr::DynamicConstraint::model_

mutableprivate

the dynamic model (e.g. Centroidal)

Definition at line 85 of file dynamic_constraint.h.

The documentation for this class was generated from the following files:

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ Vector6d

Constructor & Destructor Documentation

◆ DynamicConstraint()

◆ ~DynamicConstraint()

Member Function Documentation

◆ GetRow()

◆ UpdateBoundsAtInstance()

◆ UpdateConstraintAtInstance()

◆ UpdateJacobianAtInstance()

◆ UpdateModel()

Member Data Documentation

◆ base_angular_

◆ base_linear_

◆ ee_forces_

◆ ee_motion_

◆ model_