Cost on the task space placement (SE(3)). More...

#include <task_space_6d_cost.hpp>

Inheritance diagram for robotoc::TaskSpace6DCost:

Collaboration diagram for robotoc::TaskSpace6DCost:

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

Public Member Functions
	TaskSpace6DCost (const Robot &robot, const int frame_id)
	Constructor. More...

	TaskSpace6DCost (const Robot &robot, const std::string &frame_name)
	Constructor. More...

	TaskSpace6DCost (const Robot &robot, const int frame_id, const std::shared_ptr< TaskSpace6DRefBase > &ref)
	Constructor. More...

	TaskSpace6DCost (const Robot &robot, const int frame_id, const SE3 &const_ref)
	Constructor. More...

	TaskSpace6DCost (const Robot &robot, const int frame_id, const Eigen::Vector3d &const_position_ref, const Eigen::Matrix3d &const_rotation_ref)
	Constructor. More...

	TaskSpace6DCost (const Robot &robot, const std::string &frame_name, const std::shared_ptr< TaskSpace6DRefBase > &ref)
	Constructor. More...

	TaskSpace6DCost (const Robot &robot, const std::string &frame_name, const SE3 &const_ref)
	Constructor. More...

	TaskSpace6DCost (const Robot &robot, const std::string &frame_name, const Eigen::Vector3d &const_position_ref, const Eigen::Matrix3d &const_rotation_ref)
	Constructor. More...

	TaskSpace6DCost ()
	Default constructor. More...

	~TaskSpace6DCost ()
	Destructor. More...

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

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

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

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

void	set_ref (const std::shared_ptr< TaskSpace6DRefBase > &ref)
	Sets the reference task-space placement. More...

void	set_const_ref (const SE3 &const_ref)
	Sets the const reference task-space placement. More...

void	set_const_ref (const Eigen::Vector3d &const_position_ref, const Eigen::Matrix3d &const_rotation_ref)
	Sets the const reference task-space placement. More...

void	set_weight (const Eigen::Vector3d &weight_position, const Eigen::Vector3d &weight_rotation)
	Sets the weight vector. More...

void	set_weight_terminal (const Eigen::Vector3d &weight_position_terminal, const Eigen::Vector3d &weight_rotation_terminal)
	Sets the weight vector at the terminal stage. More...

void	set_weight_impact (const Eigen::Vector3d &weight_position_impact, const Eigen::Vector3d &weight_rotation_impact)
	Sets the weight vector at the impact stage. More...

bool	isCostActive (const GridInfo &grid_info) const
	Evaluate if the cost is active for given grid_info. More...

void	evalDiff (const Robot &robot, CostFunctionData &data, const GridInfo &grid_info) const
	Evaluate the difference between the robot's task-space position status and reference. 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

Cost on the task space placement (SE(3)).

Member Typedef Documentation

◆ Vector6d

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

Constructor & Destructor Documentation

◆ TaskSpace6DCost() [1/11]

robotoc::TaskSpace6DCost::TaskSpace6DCost	(	const Robot &	robot,
		const int	frame_id
	)

Constructor.

Parameters

[in]	robot	Robot model.
[in]	frame_id	Frame of interest.

◆ TaskSpace6DCost() [2/11]

robotoc::TaskSpace6DCost::TaskSpace6DCost	(	const Robot &	robot,
		const std::string &	frame_name
	)

Constructor.

Parameters

[in]	robot	Robot model.
[in]	frame_name	Frame of interest.

◆ TaskSpace6DCost() [3/11]

robotoc::TaskSpace6DCost::TaskSpace6DCost	(	const Robot &	robot,
		const int	frame_id,
		const std::shared_ptr< TaskSpace6DRefBase > &	ref
	)

Constructor.

Parameters

[in]	robot	Robot model.
[in]	frame_id	Frame of interest.
[in]	ref	Reference task-space placement.

◆ TaskSpace6DCost() [4/11]

robotoc::TaskSpace6DCost::TaskSpace6DCost	(	const Robot &	robot,
		const int	frame_id,
		const SE3 &	const_ref
	)

Constructor.

Parameters

[in]	robot	Robot model.
[in]	frame_id	Frame of interest.
[in]	const_ref	Const reference task-space placement.

◆ TaskSpace6DCost() [5/11]

robotoc::TaskSpace6DCost::TaskSpace6DCost	(	const Robot &	robot,
		const int	frame_id,
		const Eigen::Vector3d &	const_position_ref,
		const Eigen::Matrix3d &	const_rotation_ref
	)

Constructor.

Parameters

[in]	robot	Robot model.
[in]	frame_id	Frame of interest.
[in]	const_position_ref	Const reference task-space position.
[in]	const_rotation_ref	Const reference task-space rotation.

◆ TaskSpace6DCost() [6/11]

robotoc::TaskSpace6DCost::TaskSpace6DCost	(	const Robot &	robot,
		const std::string &	frame_name,
		const std::shared_ptr< TaskSpace6DRefBase > &	ref
	)

Constructor.

Parameters

[in]	robot	Robot model.
[in]	frame_name	Frame of interest.
[in]	ref	Reference task-space placement.

◆ TaskSpace6DCost() [7/11]

robotoc::TaskSpace6DCost::TaskSpace6DCost	(	const Robot &	robot,
		const std::string &	frame_name,
		const SE3 &	const_ref
	)

Constructor.

Parameters

[in]	robot	Robot model.
[in]	frame_name	Frame of interest.
[in]	const_ref	Const reference task-space placement.

◆ TaskSpace6DCost() [8/11]

robotoc::TaskSpace6DCost::TaskSpace6DCost	(	const Robot &	robot,
		const std::string &	frame_name,
		const Eigen::Vector3d &	const_position_ref,
		const Eigen::Matrix3d &	const_rotation_ref
	)

Constructor.

Parameters

[in]	robot	Robot model.
[in]	frame_name	Frame of interest.
[in]	const_position_ref	Const reference task-space position.
[in]	const_rotation_ref	Const reference task-space rotation.

◆ TaskSpace6DCost() [9/11]

robotoc::TaskSpace6DCost::TaskSpace6DCost ( )

Default constructor.

◆ ~TaskSpace6DCost()

robotoc::TaskSpace6DCost::~TaskSpace6DCost ( )

Destructor.

◆ TaskSpace6DCost() [10/11]

robotoc::TaskSpace6DCost::TaskSpace6DCost ( const TaskSpace6DCost & )

default

Default copy constructor.

◆ TaskSpace6DCost() [11/11]

robotoc::TaskSpace6DCost::TaskSpace6DCost ( TaskSpace6DCost && )

defaultnoexcept

Default move constructor.

Member Function Documentation

◆ evalDiff()

void robotoc::TaskSpace6DCost::evalDiff	(	const Robot &	robot,
		CostFunctionData &	data,
		const GridInfo &	grid_info
	)		const

inline

Evaluate the difference between the robot's task-space position status and reference.

Parameters

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

◆ evalImpactCost()

double robotoc::TaskSpace6DCost::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::TaskSpace6DCost::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::TaskSpace6DCost::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::TaskSpace6DCost::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::TaskSpace6DCost::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::TaskSpace6DCost::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::TaskSpace6DCost::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::TaskSpace6DCost::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::TaskSpace6DCost::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.

◆ isCostActive()

bool robotoc::TaskSpace6DCost::isCostActive ( const GridInfo & grid_info ) const

inline

Evaluate if the cost 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]

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

default

Default copy operator.

◆ operator=() [2/2]

TaskSpace6DCost & robotoc::TaskSpace6DCost::operator= ( TaskSpace6DCost && )

defaultnoexcept

Default move assign operator.

◆ set_const_ref() [1/2]

void robotoc::TaskSpace6DCost::set_const_ref	(	const Eigen::Vector3d &	const_position_ref,
		const Eigen::Matrix3d &	const_rotation_ref
	)

Sets the const reference task-space placement.

Parameters

[in]	const_position_ref	Const reference task-space position.
[in]	const_rotation_ref	Const reference task-space rotation.

◆ set_const_ref() [2/2]

void robotoc::TaskSpace6DCost::set_const_ref ( const SE3 & const_ref )

Sets the const reference task-space placement.

Parameters

[in] const_ref Const reference task-space placement.

◆ set_ref()

void robotoc::TaskSpace6DCost::set_ref ( const std::shared_ptr< TaskSpace6DRefBase > & ref )

Sets the reference task-space placement.

Parameters

[in] ref Reference task-space placement.

◆ set_weight()

void robotoc::TaskSpace6DCost::set_weight	(	const Eigen::Vector3d &	weight_position,
		const Eigen::Vector3d &	weight_rotation
	)

Sets the weight vector.

Parameters

[in]	weight_position	Weight vector on the task-space position error.
[in]	weight_rotation	Weight vector on the task-space rotation error.

◆ set_weight_impact()

void robotoc::TaskSpace6DCost::set_weight_impact	(	const Eigen::Vector3d &	weight_position_impact,
		const Eigen::Vector3d &	weight_rotation_impact
	)

Sets the weight vector at the impact stage.

Parameters

[in]	weight_position_impact	Weight vector on the task-space position error at the impact stage.
[in]	weight_rotation_impact	Weight vector on the task-space rotation error at the impact stage.

◆ set_weight_terminal()

void robotoc::TaskSpace6DCost::set_weight_terminal	(	const Eigen::Vector3d &	weight_position_terminal,
		const Eigen::Vector3d &	weight_rotation_terminal
	)

Sets the weight vector at the terminal stage.

Parameters

[in]	weight_position_terminal	Weight vector on the task-space position error at the terminal stage.
[in]	weight_rotation_terminal	Weight vector on the task-space rotation error at the terminal stage.

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

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

Public Types

Public Member Functions

Detailed Description

Member Typedef Documentation

◆ Vector6d

Constructor & Destructor Documentation

◆ TaskSpace6DCost() [1/11]

◆ TaskSpace6DCost() [2/11]

◆ TaskSpace6DCost() [3/11]

◆ TaskSpace6DCost() [4/11]

◆ TaskSpace6DCost() [5/11]

◆ TaskSpace6DCost() [6/11]

◆ TaskSpace6DCost() [7/11]

◆ TaskSpace6DCost() [8/11]

◆ TaskSpace6DCost() [9/11]

◆ ~TaskSpace6DCost()

◆ TaskSpace6DCost() [10/11]

◆ TaskSpace6DCost() [11/11]

Member Function Documentation

◆ evalDiff()

◆ evalImpactCost()

◆ evalImpactCostDerivatives()

◆ evalImpactCostHessian()

◆ evalStageCost()

◆ evalStageCostDerivatives()

◆ evalStageCostHessian()

◆ evalTerminalCost()

◆ evalTerminalCostDerivatives()

◆ evalTerminalCostHessian()

◆ isCostActive()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ set_const_ref() [1/2]

◆ set_const_ref() [2/2]

◆ set_ref()

◆ set_weight()

◆ set_weight_impact()

◆ set_weight_terminal()