Configuration space cost. More...

#include <configuration_space_cost.hpp>

Inheritance diagram for robotoc::ConfigurationSpaceCost:

Collaboration diagram for robotoc::ConfigurationSpaceCost:

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

Public Member Functions
	ConfigurationSpaceCost (const Robot &robot)
	Constructor. More...

	ConfigurationSpaceCost (const Robot &robot, const std::shared_ptr< ConfigurationSpaceRefBase > &ref)
	Constructor. More...

	ConfigurationSpaceCost ()
	Default constructor. More...

	~ConfigurationSpaceCost ()
	Destructor. More...

	ConfigurationSpaceCost (const ConfigurationSpaceCost &)=default
	Default copy constructor. More...

ConfigurationSpaceCost &	operator= (const ConfigurationSpaceCost &)=default
	Default copy operator. More...

	ConfigurationSpaceCost (ConfigurationSpaceCost &&) noexcept=default
	Default move constructor. More...

ConfigurationSpaceCost &	operator= (ConfigurationSpaceCost &&) noexcept=default
	Default move assign operator. More...

void	set_ref (const std::shared_ptr< ConfigurationSpaceRefBase > &ref)
	Sets the reference configuration. More...

void	set_q_ref (const Eigen::VectorXd &q_ref)
	Sets the const reference configuration q. More...

void	set_v_ref (const Eigen::VectorXd &v_ref)
	Sets the const reference velocity v. More...

void	set_u_ref (const Eigen::VectorXd &u_ref)
	Sets the const reference control input torques u. More...

void	set_q_weight (const Eigen::VectorXd &q_weight)
	Sets the weight vector on the configuration q. More...

void	set_v_weight (const Eigen::VectorXd &v_weight)
	Sets the weight on the velocity v. More...

void	set_a_weight (const Eigen::VectorXd &a_weight)
	Sets the weight on the acceleration a. More...

void	set_u_weight (const Eigen::VectorXd &u_weight)
	Sets the weight on the control input torques u. More...

void	set_q_weight_terminal (const Eigen::VectorXd &q_weight_terminal)
	Sets the weight vector on the configuration q at the terminal stage. More...

void	set_v_weight_terminal (const Eigen::VectorXd &v_weight_terminal)
	Sets the weight vector on the velocity v at the terminal stage. More...

void	set_q_weight_impact (const Eigen::VectorXd &q_weight_impact)
	Sets the weight vector on the configuration q at impact stages. More...

void	set_v_weight_impact (const Eigen::VectorXd &v_weight_impact)
	Sets the weight vector on the velocity v at the impact stages. More...

void	set_dv_weight_impact (const Eigen::VectorXd &dv_weight_impact)
	Sets the weight vector on the impact change in the velocity dv at the impact stages. More...

bool	isCostConfigActive (const GridInfo &grid_info) const
	Evaluate if the cost on the configuration q is active for given grid_info. More...

void	evalConfigDiff (const Robot &robot, CostFunctionData &data, const GridInfo &grid_info, const Eigen::VectorXd &q) const
	Evaluate the difference between the configuration and the reference configuration. More...

void	evalConfigDiffJac (const Robot &robot, CostFunctionData &data, const GridInfo &grid_info, const Eigen::VectorXd &q) const
	Evaluate the Jacobian of the difference between the configuration and the reference configuration. More...

double	evalStageCost (Robot &robot, const ContactStatus &contact_status, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s) const override
	Computes the stage cost. More...

void	evalStageCostDerivatives (Robot &robot, const ContactStatus &contact_status, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s, SplitKKTResidual &kkt_residual) const override
	Computes the first-order partial derivatives of the stage cost. This function is always called just after evalStageCost(). More...

void	evalStageCostHessian (Robot &robot, const ContactStatus &contact_status, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s, SplitKKTMatrix &kkt_matrix) const override
	Computes the Hessian, i.e., the second-order partial derivatives of the stage cost. This function is always called just after evalStageCostDerivatives(). More...

double	evalTerminalCost (Robot &robot, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s) const override
	Computes the terminal cost. More...

void	evalTerminalCostDerivatives (Robot &robot, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s, SplitKKTResidual &kkt_residual) const override
	Computes the first-order partial derivatives of the terminal cost. This function is always called just after evalTerminalCost(). More...

void	evalTerminalCostHessian (Robot &robot, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s, SplitKKTMatrix &kkt_matrix) const override
	Computes the Hessian, i.e., the second-order partial derivatives of the teminal cost. This function is always called just after evalTerminalCostDerivatives(). More...

double	evalImpactCost (Robot &robot, const ImpactStatus &impact_status, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s) const override
	Computes the impact cost. More...

void	evalImpactCostDerivatives (Robot &robot, const ImpactStatus &impact_status, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s, SplitKKTResidual &kkt_residual) const override
	Computes the first-order partial derivatives of the impact cost. This function is always called just after evalImpactCost(). More...

void	evalImpactCostHessian (Robot &robot, const ImpactStatus &impact_status, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s, SplitKKTMatrix &kkt_matrix) const override
	Computes the Hessian, i.e., the second-order partial derivatives of the impact cost. This function is always called just after evalImpactCostDerivatives(). More...

Public Member Functions inherited from robotoc::CostFunctionComponentBase
	CostFunctionComponentBase ()
	Default constructor. More...

virtual	~CostFunctionComponentBase ()
	Destructor. More...

	CostFunctionComponentBase (const CostFunctionComponentBase &)=default
	Default copy constructor. More...

CostFunctionComponentBase &	operator= (const CostFunctionComponentBase &)=default
	Default copy operator. More...

	CostFunctionComponentBase (CostFunctionComponentBase &&) noexcept=default
	Default move constructor. More...

CostFunctionComponentBase &	operator= (CostFunctionComponentBase &&) noexcept=default
	Default move assign operator. More...

virtual double	evalStageCost (Robot &robot, const ContactStatus &contact_status, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s) const =0
	Computes the stage cost. More...

virtual void	evalStageCostDerivatives (Robot &robot, const ContactStatus &contact_status, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s, SplitKKTResidual &kkt_residual) const =0
	Computes the first-order partial derivatives of the stage cost. This function is always called just after evalStageCost(). More...

virtual void	evalStageCostHessian (Robot &robot, const ContactStatus &contact_status, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s, SplitKKTMatrix &kkt_matrix) const =0
	Computes the Hessian, i.e., the second-order partial derivatives of the stage cost. This function is always called just after evalStageCostDerivatives(). More...

virtual double	evalTerminalCost (Robot &robot, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s) const =0
	Computes the terminal cost. More...

virtual void	evalTerminalCostDerivatives (Robot &robot, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s, SplitKKTResidual &kkt_residual) const =0
	Computes the first-order partial derivatives of the terminal cost. This function is always called just after evalTerminalCost(). More...

virtual void	evalTerminalCostHessian (Robot &robot, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s, SplitKKTMatrix &kkt_matrix) const =0
	Computes the Hessian, i.e., the second-order partial derivatives of the teminal cost. This function is always called just after evalTerminalCostDerivatives(). More...

virtual double	evalImpactCost (Robot &robot, const ImpactStatus &impact_status, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s) const =0
	Computes the impact cost. More...

virtual void	evalImpactCostDerivatives (Robot &robot, const ImpactStatus &impact_status, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s, SplitKKTResidual &kkt_residual) const =0
	Computes the first-order partial derivatives of the impact cost. This function is always called just after evalImpactCost(). More...

virtual void	evalImpactCostHessian (Robot &robot, const ImpactStatus &impact_status, CostFunctionData &data, const GridInfo &grid_info, const SplitSolution &s, SplitKKTMatrix &kkt_matrix) const =0
	Computes the Hessian, i.e., the second-order partial derivatives of the impact cost. This function is always called just after evalImpactCostDerivatives(). More...

template<typename Derived >
std::shared_ptr< Derived >	as_shared_ptr ()
	Gets the shared ptr of this object as the specified type. If this fails in dynamic casting, throws an exception. More...

Detailed Description

Configuration space cost.

Member Typedef Documentation

◆ Vector6d

using robotoc::ConfigurationSpaceCost::Vector6d = Eigen::Matrix<double, 6, 1>

Constructor & Destructor Documentation

◆ ConfigurationSpaceCost() [1/5]

robotoc::ConfigurationSpaceCost::ConfigurationSpaceCost ( const Robot & robot )

Constructor.

Parameters

[in] robot Robot model.

◆ ConfigurationSpaceCost() [2/5]

robotoc::ConfigurationSpaceCost::ConfigurationSpaceCost	(	const Robot &	robot,
		const std::shared_ptr< ConfigurationSpaceRefBase > &	ref
	)

Constructor.

Parameters

[in]	robot	Robot model.
[in]	ref	Reference configuraton.

◆ ConfigurationSpaceCost() [3/5]

robotoc::ConfigurationSpaceCost::ConfigurationSpaceCost ( )

Default constructor.

◆ ~ConfigurationSpaceCost()

robotoc::ConfigurationSpaceCost::~ConfigurationSpaceCost ( )

Destructor.

◆ ConfigurationSpaceCost() [4/5]

robotoc::ConfigurationSpaceCost::ConfigurationSpaceCost ( const ConfigurationSpaceCost & )

default

Default copy constructor.

◆ ConfigurationSpaceCost() [5/5]

robotoc::ConfigurationSpaceCost::ConfigurationSpaceCost ( ConfigurationSpaceCost && )

defaultnoexcept

Default move constructor.

Member Function Documentation

◆ evalConfigDiff()

void robotoc::ConfigurationSpaceCost::evalConfigDiff	(	const Robot &	robot,
		CostFunctionData &	data,
		const GridInfo &	grid_info,
		const Eigen::VectorXd &	q
	)		const

inline

Evaluate the difference between the configuration and the reference configuration.

Parameters

[in]	robot	Robot model.
[in,out]	data	Cost funciton data.
[in]	grid_info	Grid info
[in]	q	Current configuration

◆ evalConfigDiffJac()

void robotoc::ConfigurationSpaceCost::evalConfigDiffJac	(	const Robot &	robot,
		CostFunctionData &	data,
		const GridInfo &	grid_info,
		const Eigen::VectorXd &	q
	)		const

inline

Evaluate the Jacobian of the difference between the configuration and the reference configuration.

Parameters

[in]	robot	Robot model.
[in,out]	data	Cost funciton data.
[in]	grid_info	Grid info
[in]	q	Current configuration

◆ evalImpactCost()

double robotoc::ConfigurationSpaceCost::evalImpactCost	(	Robot &	robot,
		const ImpactStatus &	impact_status,
		CostFunctionData &	data,
		const GridInfo &	grid_info,
		const SplitSolution &	s
	)		const

overridevirtual

Computes the impact cost.

Parameters

[in]	robot	Robot model.
[in]	impact_status	Impact status.
[in]	data	Cost function data.
[in]	grid_info	Grid info.
[in]	s	Split solution.

Returns: Impact cost.

Implements robotoc::CostFunctionComponentBase.

◆ evalImpactCostDerivatives()

void robotoc::ConfigurationSpaceCost::evalImpactCostDerivatives	(	Robot &	robot,
		const ImpactStatus &	impact_status,
		CostFunctionData &	data,
		const GridInfo &	grid_info,
		const SplitSolution &	s,
		SplitKKTResidual &	kkt_residual
	)		const

overridevirtual

Computes the first-order partial derivatives of the impact cost. This function is always called just after evalImpactCost().

Parameters

[in]	robot	Robot model.
[in]	impact_status	Impact status.
[in]	data	Cost function data.
[in]	grid_info	Grid info.
[in]	s	Split solution.
[in,out]	kkt_residual	Split KKT residual. The partial derivatives are added to this object.

Implements robotoc::CostFunctionComponentBase.

◆ evalImpactCostHessian()

void robotoc::ConfigurationSpaceCost::evalImpactCostHessian	(	Robot &	robot,
		const ImpactStatus &	impact_status,
		CostFunctionData &	data,
		const GridInfo &	grid_info,
		const SplitSolution &	s,
		SplitKKTMatrix &	kkt_matrix
	)		const

overridevirtual

Computes the Hessian, i.e., the second-order partial derivatives of the impact cost. This function is always called just after evalImpactCostDerivatives().

Parameters

[in]	robot	Robot model.
[in]	impact_status	Impact status.
[in]	data	Cost function data.
[in]	grid_info	Grid info.
[in]	s	Split solution.
[in,out]	kkt_matrix	Impact split KKT matrix. The Hessians are added to this object.

Implements robotoc::CostFunctionComponentBase.

◆ evalStageCost()

double robotoc::ConfigurationSpaceCost::evalStageCost	(	Robot &	robot,
		const ContactStatus &	contact_status,
		CostFunctionData &	data,
		const GridInfo &	grid_info,
		const SplitSolution &	s
	)		const

overridevirtual

Computes the stage cost.

Parameters

[in]	robot	Robot model.
[in]	contact_status	Contact status.
[in]	data	Cost function data.
[in]	grid_info	Grid info.
[in]	s	Split solution.

Returns: Stage cost.

Implements robotoc::CostFunctionComponentBase.

◆ evalStageCostDerivatives()

void robotoc::ConfigurationSpaceCost::evalStageCostDerivatives	(	Robot &	robot,
		const ContactStatus &	contact_status,
		CostFunctionData &	data,
		const GridInfo &	grid_info,
		const SplitSolution &	s,
		SplitKKTResidual &	kkt_residual
	)		const

overridevirtual

Computes the first-order partial derivatives of the stage cost. This function is always called just after evalStageCost().

Parameters

[in]	robot	Robot model.
[in]	contact_status	Contact status.
[in]	data	Cost function data.
[in]	grid_info	Grid info.
[in]	s	Split solution.
[in,out]	kkt_residual	Split KKT residual. The partial derivatives are added to this object.

Implements robotoc::CostFunctionComponentBase.

◆ evalStageCostHessian()

void robotoc::ConfigurationSpaceCost::evalStageCostHessian	(	Robot &	robot,
		const ContactStatus &	contact_status,
		CostFunctionData &	data,
		const GridInfo &	grid_info,
		const SplitSolution &	s,
		SplitKKTMatrix &	kkt_matrix
	)		const

overridevirtual

Computes the Hessian, i.e., the second-order partial derivatives of the stage cost. This function is always called just after evalStageCostDerivatives().

Parameters

[in]	robot	Robot model.
[in]	contact_status	Contact status.
[in]	data	Cost function data.
[in]	grid_info	Grid info.
[in]	s	Split solution.
[in,out]	kkt_matrix	Split KKT matrix. The Hessians are added to this object.

Implements robotoc::CostFunctionComponentBase.

◆ evalTerminalCost()

double robotoc::ConfigurationSpaceCost::evalTerminalCost	(	Robot &	robot,
		CostFunctionData &	data,
		const GridInfo &	grid_info,
		const SplitSolution &	s
	)		const

overridevirtual

Computes the terminal cost.

Parameters

[in]	robot	Robot model.
[in]	data	Cost function data.
[in]	grid_info	Grid info.
[in]	s	Split solution.

Returns: Terminal cost.

Implements robotoc::CostFunctionComponentBase.

◆ evalTerminalCostDerivatives()

void robotoc::ConfigurationSpaceCost::evalTerminalCostDerivatives	(	Robot &	robot,
		CostFunctionData &	data,
		const GridInfo &	grid_info,
		const SplitSolution &	s,
		SplitKKTResidual &	kkt_residual
	)		const

overridevirtual

Computes the first-order partial derivatives of the terminal cost. This function is always called just after evalTerminalCost().

Parameters

[in]	robot	Robot model.
[in]	data	Cost function data.
[in]	grid_info	Grid info.
[in]	s	Split solution.
[in,out]	kkt_residual	Split KKT residual. The partial derivatives are added to this object.

Implements robotoc::CostFunctionComponentBase.

◆ evalTerminalCostHessian()

void robotoc::ConfigurationSpaceCost::evalTerminalCostHessian	(	Robot &	robot,
		CostFunctionData &	data,
		const GridInfo &	grid_info,
		const SplitSolution &	s,
		SplitKKTMatrix &	kkt_matrix
	)		const

overridevirtual

Computes the Hessian, i.e., the second-order partial derivatives of the teminal cost. This function is always called just after evalTerminalCostDerivatives().

Parameters

[in]	robot	Robot model.
[in]	data	Cost function data.
[in]	grid_info	Grid info.
[in]	s	Split solution.
[in,out]	kkt_matrix	Split KKT matrix. The Hessians are added to this object.

Implements robotoc::CostFunctionComponentBase.

◆ isCostConfigActive()

bool robotoc::ConfigurationSpaceCost::isCostConfigActive ( const GridInfo & grid_info ) const

inline

Evaluate if the cost on the configuration q is active for given grid_info.

Parameters

[in] grid_info Grid info.

Returns: Cost status (if the cost is active or not).

◆ operator=() [1/2]

ConfigurationSpaceCost & robotoc::ConfigurationSpaceCost::operator= ( ConfigurationSpaceCost && )

defaultnoexcept

Default move assign operator.

◆ operator=() [2/2]

ConfigurationSpaceCost & robotoc::ConfigurationSpaceCost::operator= ( const ConfigurationSpaceCost & )

default

Default copy operator.

◆ set_a_weight()

void robotoc::ConfigurationSpaceCost::set_a_weight ( const Eigen::VectorXd & a_weight )

Sets the weight on the acceleration a.

Parameters

[in] a_weight Weight vector on the acceleration a. Size must be Robot::dimv().

◆ set_dv_weight_impact()

void robotoc::ConfigurationSpaceCost::set_dv_weight_impact ( const Eigen::VectorXd & dv_weight_impact )

Sets the weight vector on the impact change in the velocity dv at the impact stages.

Parameters

[in] dv_weight_impact Weight vector on the impact change in the velocity the impact stages. Size must be Robot::dimv().

◆ set_q_ref()

void robotoc::ConfigurationSpaceCost::set_q_ref ( const Eigen::VectorXd & q_ref )

Sets the const reference configuration q.

Parameters

[in] q_ref Reference configuration q. Size must be Robot::dimq().

◆ set_q_weight()

void robotoc::ConfigurationSpaceCost::set_q_weight ( const Eigen::VectorXd & q_weight )

Sets the weight vector on the configuration q.

Parameters

[in] q_weight Weight vector on the configuration q. Size must be Robot::dimv().

◆ set_q_weight_impact()

void robotoc::ConfigurationSpaceCost::set_q_weight_impact ( const Eigen::VectorXd & q_weight_impact )

Sets the weight vector on the configuration q at impact stages.

Parameters

[in] q_weight_impact Weight vector on the configuration q at impact
stages. Size must be Robot::dimv().

◆ set_q_weight_terminal()

void robotoc::ConfigurationSpaceCost::set_q_weight_terminal ( const Eigen::VectorXd & q_weight_terminal )

Sets the weight vector on the configuration q at the terminal stage.

Parameters

[in] q_weight_terminal Weight vector on the configuration q at the terminal stage. Size must be Robot::dimv().

◆ set_ref()

void robotoc::ConfigurationSpaceCost::set_ref ( const std::shared_ptr< ConfigurationSpaceRefBase > & ref )

Sets the reference configuration.

Parameters

[in] ref Reference configuraton.

◆ set_u_ref()

void robotoc::ConfigurationSpaceCost::set_u_ref ( const Eigen::VectorXd & u_ref )

Sets the const reference control input torques u.

Parameters

[in] u_ref Reference control input torques u. Size must be Robot::dimu().

◆ set_u_weight()

void robotoc::ConfigurationSpaceCost::set_u_weight ( const Eigen::VectorXd & u_weight )

Sets the weight on the control input torques u.

Parameters

[in] u_weight Weight vector on the control input torques u. Size must be Robot::dimu().

◆ set_v_ref()

void robotoc::ConfigurationSpaceCost::set_v_ref ( const Eigen::VectorXd & v_ref )

Sets the const reference velocity v.

Parameters

[in] v_ref Reference velocity v. Size must be Robot::dimv().

◆ set_v_weight()

void robotoc::ConfigurationSpaceCost::set_v_weight ( const Eigen::VectorXd & v_weight )

Sets the weight on the velocity v.

Parameters

[in] v_weight Weight vector on the velocity v. Size must be Robot::dimv().

◆ set_v_weight_impact()

void robotoc::ConfigurationSpaceCost::set_v_weight_impact ( const Eigen::VectorXd & v_weight_impact )

Sets the weight vector on the velocity v at the impact stages.

Parameters

[in] v_weight_impact Weight vector on the velocity v at the impact
stages. Size must be Robot::dimv().

◆ set_v_weight_terminal()

void robotoc::ConfigurationSpaceCost::set_v_weight_terminal ( const Eigen::VectorXd & v_weight_terminal )

Sets the weight vector on the velocity v at the terminal stage.

Parameters

[in] v_weight_terminal Weight vector on the velocity v at the terminal stage. Size must be Robot::dimv().

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

/github/workspace/include/robotoc/cost/configuration_space_cost.hpp

Public Types

Public Member Functions

Detailed Description

Member Typedef Documentation

◆ Vector6d

Constructor & Destructor Documentation

◆ ConfigurationSpaceCost() [1/5]

◆ ConfigurationSpaceCost() [2/5]

◆ ConfigurationSpaceCost() [3/5]

◆ ~ConfigurationSpaceCost()

◆ ConfigurationSpaceCost() [4/5]

◆ ConfigurationSpaceCost() [5/5]

Member Function Documentation

◆ evalConfigDiff()

◆ evalConfigDiffJac()

◆ evalImpactCost()

◆ evalImpactCostDerivatives()

◆ evalImpactCostHessian()

◆ evalStageCost()

◆ evalStageCostDerivatives()

◆ evalStageCostHessian()

◆ evalTerminalCost()

◆ evalTerminalCostDerivatives()

◆ evalTerminalCostHessian()

◆ isCostConfigActive()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ set_a_weight()

◆ set_dv_weight_impact()

◆ set_q_ref()

◆ set_q_weight()

◆ set_q_weight_impact()

◆ set_q_weight_terminal()

◆ set_ref()

◆ set_u_ref()

◆ set_u_weight()

◆ set_v_ref()

◆ set_v_weight()

◆ set_v_weight_impact()

◆ set_v_weight_terminal()