#include <feasibility_driven_ddp_solver.h>

Inheritance diagram for exotica::AbstractFeasibilityDrivenDDPSolver:

Collaboration diagram for exotica::AbstractFeasibilityDrivenDDPSolver:

Public Member Functions
void	Solve (Eigen::MatrixXd &solution) override
	Solves the problem. More...

void	SpecifyProblem (PlanningProblemPtr pointer) override
	Binds the solver to a specific problem which must be pre-initalised. More...

const std::vector< Eigen::VectorXd > &	get_fs () const

const std::vector< Eigen::VectorXd > &	get_xs () const

const std::vector< Eigen::VectorXd > &	get_us () const

Public Member Functions inherited from exotica::AbstractDDPSolver
Eigen::VectorXd	GetFeedbackControl (Eigen::VectorXdRefConst x, int t) const override

const std::vector< Eigen::MatrixXd > &	get_Vxx () const

const std::vector< Eigen::VectorXd > &	get_Vx () const

const std::vector< Eigen::MatrixXd > &	get_Qxx () const

const std::vector< Eigen::MatrixXd > &	get_Qux () const

const std::vector< Eigen::MatrixXd > &	get_Quu () const

const std::vector< Eigen::VectorXd > &	get_Qx () const

const std::vector< Eigen::VectorXd > &	get_Qu () const

const std::vector< Eigen::MatrixXd > &	get_K () const

const std::vector< Eigen::VectorXd > &	get_k () const

const std::vector< Eigen::VectorXd > &	get_X_try () const

const std::vector< Eigen::VectorXd > &	get_U_try () const

const std::vector< Eigen::VectorXd > &	get_X_ref () const

const std::vector< Eigen::VectorXd > &	get_U_ref () const

const std::vector< Eigen::MatrixXd > &	get_Quu_inv () const

const std::vector< Eigen::MatrixXd > &	get_fx () const

const std::vector< Eigen::MatrixXd > &	get_fu () const

std::vector< double >	get_control_cost_evolution () const

void	set_control_cost_evolution (const int index, const double cost)

std::vector< double >	get_steplength_evolution () const

std::vector< double >	get_regularization_evolution () const

Public Member Functions inherited from exotica::MotionSolver
	MotionSolver ()=default

virtual	~MotionSolver ()=default

void	InstantiateBase (const Initializer &init) override

PlanningProblemPtr	GetProblem () const

std::string	Print (const std::string &prepend) const override

void	SetNumberOfMaxIterations (int max_iter)

int	GetNumberOfMaxIterations ()

double	GetPlanningTime ()

Public Member Functions inherited from exotica::Object
	Object ()

virtual	~Object ()

virtual std::string	type () const
	Type Information wrapper: must be virtual so that it is polymorphic... More...

std::string	GetObjectName ()

void	InstantiateObject (const Initializer &init)

Public Member Functions inherited from exotica::InstantiableBase
	InstantiableBase ()=default

virtual	~InstantiableBase ()=default

virtual Initializer	GetInitializerTemplate ()=0

virtual void	InstantiateInternal (const Initializer &init)=0

virtual std::vector< Initializer >	GetAllTemplates () const =0

Protected Member Functions
void	IncreaseRegularization () override

void	DecreaseRegularization () override

const Eigen::Vector2d &	ExpectedImprovement ()

void	UpdateExpectedImprovement ()

bool	IsNaN (const double value)

void	SetCandidate (const std::vector< Eigen::VectorXd > &xs_warm, const std::vector< Eigen::VectorXd > &us_warm, const bool is_feasible)

double	CheckStoppingCriteria ()

double	CalcDiff ()

bool	ComputeDirection (const bool recalcDiff)

virtual bool	BackwardPassFDDP ()

void	BackwardPass () override
	Computes the control gains for a the trajectory in the associated DynamicTimeIndexedProblem. More...

virtual void	ComputeGains (const int t)

void	ForwardPass (const double steplength)

double	TryStep (const double steplength)

virtual void	AllocateData ()

Protected Member Functions inherited from exotica::AbstractDDPSolver
double	ForwardPass (const double alpha)
	Forward simulates the dynamics using the gains computed in the last BackwardPass;. More...

Protected Attributes
int	NDX_

int	last_T_ = -1

Eigen::MatrixXd	control_limits_

double	initial_regularization_rate_ = 1e-9

bool	clamp_to_control_limits_in_forward_pass_ = false

double	steplength_
	Current applied step-length. More...

Eigen::Vector2d	d_
	LQ approximation of the expected improvement. More...

double	dV_
	Cost reduction obtained by TryStep. More...

double	dVexp_
	Expected cost reduction. More...

double	th_acceptstep_ = 0.1
	Threshold used for accepting step. More...

double	th_stop_ = 1e-9
	Tolerance for stopping the algorithm. More...

double	th_gradient_tolerance_ = 0.
	Gradient tolerance. More...

double	stop_
	Value computed by CheckStoppingCriteria. More...

double	dg_ = 0.

double	dq_ = 0.

double	dv_ = 0.

double	th_acceptnegstep_ = 2.
	Threshold used for accepting step along ascent direction. More...

std::vector< Eigen::VectorXd >	us_

std::vector< Eigen::VectorXd >	xs_

bool	is_feasible_ = false

double	xreg_ = 1e-9
	State regularization. More...

double	ureg_ = 1e-9
	Control regularization. More...

double	regmin_ = 1e-9
	Minimum regularization (will not decrease lower) More...

double	regmax_ = 1e9
	Maximum regularization (to exit by divergence) More...

double	regfactor_ = 10.
	Factor by which the regularization gets increased/decreased. More...

std::vector< Eigen::VectorXd >	xs_try_
	State trajectory computed by line-search procedure. More...

std::vector< Eigen::VectorXd >	us_try_
	Control trajectory computed by line-search procedure. More...

std::vector< Eigen::VectorXd >	dx_

std::vector< Eigen::VectorXd >	fs_
	Gaps/defects between shooting nodes. More...

Eigen::VectorXd	xnext_

Eigen::MatrixXd	FxTVxx_p_

std::vector< Eigen::MatrixXd >	FuTVxx_p_

std::vector< Eigen::MatrixXd >	Qxu_

Eigen::VectorXd	fTVxx_p_

std::vector< Eigen::LDLT< Eigen::MatrixXd > >	Quu_ldlt_

std::vector< Eigen::VectorXd >	Quuk_

double	th_grad_ = 1e-12
	Tolerance of the expected gradient used for testing the step. More...

double	th_stepdec_ = 0.5
	Step-length threshold used to decrease regularization. More...

double	th_stepinc_ = 0.01
	Step-length threshold used to increase regularization. More...

bool	was_feasible_ = false
	Label that indicates in the previous iterate was feasible. More...

Protected Attributes inherited from exotica::AbstractDDPSolver
DynamicTimeIndexedShootingProblemPtr	prob_
	Shared pointer to the planning problem. More...

DynamicsSolverPtr	dynamics_solver_
	Shared pointer to the dynamics solver. More...

AbstractDDPSolverInitializer	base_parameters_

Eigen::VectorXd	alpha_space_
	Backtracking line-search steplengths. More...

double	lambda_
	Regularization (Vxx, Quu) More...

int	T_
	Length of shooting problem, i.e., state trajectory. The control trajectory has length T_-1. More...

int	NU_
	Size of control vector. More...

int	NX_
	Size of state vector. More...

int	NDX_
	Size of tangent vector to the state vector. More...

int	NV_
	Size of velocity vector (tangent vector to the configuration) More...

double	dt_
	Integration time-step. More...

double	cost_
	Cost during iteration. More...

double	control_cost_
	Control cost during iteration. More...

double	cost_try_
	Total cost computed by line-search procedure. More...

double	control_cost_try_
	Total control cost computed by line-search procedure. More...

double	cost_prev_
	Cost during previous iteration. More...

double	alpha_best_
	Line-search step taken. More...

double	time_taken_forward_pass_

double	time_taken_backward_pass_

std::vector< Eigen::MatrixXd >	Vxx_
	Hessian of the Value function. More...

std::vector< Eigen::VectorXd >	Vx_
	Gradient of the Value function. More...

std::vector< Eigen::MatrixXd >	Qxx_
	Hessian of the Hamiltonian. More...

std::vector< Eigen::MatrixXd >	Qux_
	Hessian of the Hamiltonian. More...

std::vector< Eigen::MatrixXd >	Quu_
	Hessian of the Hamiltonian. More...

std::vector< Eigen::VectorXd >	Qx_
	Gradient of the Hamiltonian. More...

std::vector< Eigen::VectorXd >	Qu_
	Gradient of the Hamiltonian. More...

std::vector< Eigen::MatrixXd >	K_
	Feedback gains. More...

std::vector< Eigen::VectorXd >	k_
	Feed-forward terms. More...

std::vector< Eigen::VectorXd >	X_try_
	Updated state trajectory during iteration (computed by line-search) More...

std::vector< Eigen::VectorXd >	U_try_
	Updated control trajectory during iteration (computed by line-search) More...

std::vector< Eigen::VectorXd >	X_ref_
	Reference state trajectory for feedback control. More...

std::vector< Eigen::VectorXd >	U_ref_
	Reference control trajectory for feedback control. More...

std::vector< Eigen::MatrixXd >	Quu_inv_
	Inverse of the Hessian of the Hamiltonian. More...

std::vector< Eigen::MatrixXd >	fx_
	Derivative of the dynamics f w.r.t. x. More...

std::vector< Eigen::MatrixXd >	fu_
	Derivative of the dynamics f w.r.t. u. More...

std::vector< double >	control_cost_evolution_
	Evolution of the control cost (control regularization) More...

std::vector< double >	steplength_evolution_
	Evolution of the steplength. More...

std::vector< double >	regularization_evolution_
	Evolution of the regularization (xreg/ureg) More...

Protected Attributes inherited from exotica::MotionSolver
PlanningProblemPtr	problem_

double	planning_time_ = -1

int	max_iterations_ = 100

Additional Inherited Members
Public Attributes inherited from exotica::Object
std::string	ns_

std::string	object_name_

bool	debug_

Member Function Documentation

◆ AllocateData()

virtual void exotica::AbstractFeasibilityDrivenDDPSolver::AllocateData ( )

protectedvirtual

Reimplemented in exotica::ControlLimitedFeasibilityDrivenDDPSolver.

◆ BackwardPass()

void exotica::AbstractFeasibilityDrivenDDPSolver::BackwardPass ( )

inlineoverrideprotectedvirtual

Computes the control gains for a the trajectory in the associated DynamicTimeIndexedProblem.

Implements exotica::AbstractDDPSolver.

◆ BackwardPassFDDP()

virtual bool exotica::AbstractFeasibilityDrivenDDPSolver::BackwardPassFDDP ( )

protectedvirtual

◆ CalcDiff()

double exotica::AbstractFeasibilityDrivenDDPSolver::CalcDiff ( )

protected

◆ CheckStoppingCriteria()

double exotica::AbstractFeasibilityDrivenDDPSolver::CheckStoppingCriteria ( )

protected

◆ ComputeDirection()

bool exotica::AbstractFeasibilityDrivenDDPSolver::ComputeDirection ( const bool recalcDiff )

protected

◆ ComputeGains()

virtual void exotica::AbstractFeasibilityDrivenDDPSolver::ComputeGains ( const int t )

protectedvirtual

Reimplemented in exotica::ControlLimitedFeasibilityDrivenDDPSolver.

◆ DecreaseRegularization()

void exotica::AbstractFeasibilityDrivenDDPSolver::DecreaseRegularization ( )

overrideprotectedvirtual

Reimplemented from exotica::AbstractDDPSolver.

◆ ExpectedImprovement()

const Eigen::Vector2d& exotica::AbstractFeasibilityDrivenDDPSolver::ExpectedImprovement ( )

protected

◆ ForwardPass()

void exotica::AbstractFeasibilityDrivenDDPSolver::ForwardPass ( const double steplength )

protected

◆ get_fs()

const std::vector<Eigen::VectorXd>& exotica::AbstractFeasibilityDrivenDDPSolver::get_fs ( ) const

inline

◆ get_us()

const std::vector<Eigen::VectorXd>& exotica::AbstractFeasibilityDrivenDDPSolver::get_us ( ) const

inline

◆ get_xs()

const std::vector<Eigen::VectorXd>& exotica::AbstractFeasibilityDrivenDDPSolver::get_xs ( ) const

inline

◆ IncreaseRegularization()

void exotica::AbstractFeasibilityDrivenDDPSolver::IncreaseRegularization ( )

overrideprotectedvirtual

Reimplemented from exotica::AbstractDDPSolver.

◆ IsNaN()

bool exotica::AbstractFeasibilityDrivenDDPSolver::IsNaN ( const double value )

inlineprotected

◆ SetCandidate()

void exotica::AbstractFeasibilityDrivenDDPSolver::SetCandidate	(	const std::vector< Eigen::VectorXd > &	xs_warm,
		const std::vector< Eigen::VectorXd > &	us_warm,
		const bool	is_feasible
	)

protected

◆ Solve()

void exotica::AbstractFeasibilityDrivenDDPSolver::Solve ( Eigen::MatrixXd & solution )

overridevirtual

Solves the problem.

Parameters

solution Returned solution trajectory as a vector of joint configurations.

Reimplemented from exotica::AbstractDDPSolver.

◆ SpecifyProblem()

void exotica::AbstractFeasibilityDrivenDDPSolver::SpecifyProblem ( PlanningProblemPtr pointer )

overridevirtual

Binds the solver to a specific problem which must be pre-initalised.

Parameters

pointer Shared pointer to the motion planning problem

Returns: Successful if the problem is a valid DynamicTimeIndexedProblem

Reimplemented from exotica::AbstractDDPSolver.

◆ TryStep()

double exotica::AbstractFeasibilityDrivenDDPSolver::TryStep ( const double steplength )

protected

◆ UpdateExpectedImprovement()

void exotica::AbstractFeasibilityDrivenDDPSolver::UpdateExpectedImprovement ( )

protected

Member Data Documentation

◆ clamp_to_control_limits_in_forward_pass_

bool exotica::AbstractFeasibilityDrivenDDPSolver::clamp_to_control_limits_in_forward_pass_ = false

protected

◆ control_limits_

Eigen::MatrixXd exotica::AbstractFeasibilityDrivenDDPSolver::control_limits_

protected

◆ d_

Eigen::Vector2d exotica::AbstractFeasibilityDrivenDDPSolver::d_

protected

LQ approximation of the expected improvement.

◆ dg_

double exotica::AbstractFeasibilityDrivenDDPSolver::dg_ = 0.

protected

◆ dq_

double exotica::AbstractFeasibilityDrivenDDPSolver::dq_ = 0.

protected

◆ dV_

double exotica::AbstractFeasibilityDrivenDDPSolver::dV_

protected

Cost reduction obtained by TryStep.

◆ dv_

double exotica::AbstractFeasibilityDrivenDDPSolver::dv_ = 0.

protected

◆ dVexp_

double exotica::AbstractFeasibilityDrivenDDPSolver::dVexp_

protected

Expected cost reduction.

◆ dx_

std::vector<Eigen::VectorXd> exotica::AbstractFeasibilityDrivenDDPSolver::dx_

protected

◆ fs_

std::vector<Eigen::VectorXd> exotica::AbstractFeasibilityDrivenDDPSolver::fs_

protected

Gaps/defects between shooting nodes.

◆ fTVxx_p_

Eigen::VectorXd exotica::AbstractFeasibilityDrivenDDPSolver::fTVxx_p_

protected

◆ FuTVxx_p_

std::vector<Eigen::MatrixXd> exotica::AbstractFeasibilityDrivenDDPSolver::FuTVxx_p_

protected

◆ FxTVxx_p_

Eigen::MatrixXd exotica::AbstractFeasibilityDrivenDDPSolver::FxTVxx_p_

protected

◆ initial_regularization_rate_

double exotica::AbstractFeasibilityDrivenDDPSolver::initial_regularization_rate_ = 1e-9

protected

◆ is_feasible_

bool exotica::AbstractFeasibilityDrivenDDPSolver::is_feasible_ = false

protected

◆ last_T_

int exotica::AbstractFeasibilityDrivenDDPSolver::last_T_ = -1

protected

◆ NDX_

int exotica::AbstractFeasibilityDrivenDDPSolver::NDX_

protected

◆ Quu_ldlt_

std::vector<Eigen::LDLT<Eigen::MatrixXd> > exotica::AbstractFeasibilityDrivenDDPSolver::Quu_ldlt_

protected

◆ Quuk_

std::vector<Eigen::VectorXd> exotica::AbstractFeasibilityDrivenDDPSolver::Quuk_

protected

◆ Qxu_

std::vector<Eigen::MatrixXd> exotica::AbstractFeasibilityDrivenDDPSolver::Qxu_

protected

◆ regfactor_

double exotica::AbstractFeasibilityDrivenDDPSolver::regfactor_ = 10.

protected

Factor by which the regularization gets increased/decreased.

◆ regmax_

double exotica::AbstractFeasibilityDrivenDDPSolver::regmax_ = 1e9

protected

Maximum regularization (to exit by divergence)

◆ regmin_

double exotica::AbstractFeasibilityDrivenDDPSolver::regmin_ = 1e-9

protected

Minimum regularization (will not decrease lower)

◆ steplength_

double exotica::AbstractFeasibilityDrivenDDPSolver::steplength_

protected

Current applied step-length.

◆ stop_

double exotica::AbstractFeasibilityDrivenDDPSolver::stop_

protected

Value computed by CheckStoppingCriteria.

◆ th_acceptnegstep_

double exotica::AbstractFeasibilityDrivenDDPSolver::th_acceptnegstep_ = 2.

protected

Threshold used for accepting step along ascent direction.

◆ th_acceptstep_

double exotica::AbstractFeasibilityDrivenDDPSolver::th_acceptstep_ = 0.1

protected

Threshold used for accepting step.

◆ th_grad_

double exotica::AbstractFeasibilityDrivenDDPSolver::th_grad_ = 1e-12

protected

Tolerance of the expected gradient used for testing the step.

◆ th_gradient_tolerance_

double exotica::AbstractFeasibilityDrivenDDPSolver::th_gradient_tolerance_ = 0.

protected

Gradient tolerance.

◆ th_stepdec_

double exotica::AbstractFeasibilityDrivenDDPSolver::th_stepdec_ = 0.5

protected

Step-length threshold used to decrease regularization.

◆ th_stepinc_

double exotica::AbstractFeasibilityDrivenDDPSolver::th_stepinc_ = 0.01

protected

Step-length threshold used to increase regularization.

◆ th_stop_

double exotica::AbstractFeasibilityDrivenDDPSolver::th_stop_ = 1e-9

protected

Tolerance for stopping the algorithm.

◆ ureg_

double exotica::AbstractFeasibilityDrivenDDPSolver::ureg_ = 1e-9

protected

Control regularization.

◆ us_

std::vector<Eigen::VectorXd> exotica::AbstractFeasibilityDrivenDDPSolver::us_

protected

◆ us_try_

std::vector<Eigen::VectorXd> exotica::AbstractFeasibilityDrivenDDPSolver::us_try_

protected

Control trajectory computed by line-search procedure.

◆ was_feasible_

bool exotica::AbstractFeasibilityDrivenDDPSolver::was_feasible_ = false

protected

Label that indicates in the previous iterate was feasible.

◆ xnext_

Eigen::VectorXd exotica::AbstractFeasibilityDrivenDDPSolver::xnext_

protected

◆ xreg_

double exotica::AbstractFeasibilityDrivenDDPSolver::xreg_ = 1e-9

protected

State regularization.

◆ xs_

std::vector<Eigen::VectorXd> exotica::AbstractFeasibilityDrivenDDPSolver::xs_

protected

◆ xs_try_

std::vector<Eigen::VectorXd> exotica::AbstractFeasibilityDrivenDDPSolver::xs_try_

protected

State trajectory computed by line-search procedure.

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

/tmp/exotica/exotations/solvers/exotica_ddp_solver/include/exotica_ddp_solver/feasibility_driven_ddp_solver.h

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Member Function Documentation

◆ AllocateData()

◆ BackwardPass()

◆ BackwardPassFDDP()

◆ CalcDiff()

◆ CheckStoppingCriteria()

◆ ComputeDirection()

◆ ComputeGains()

◆ DecreaseRegularization()

◆ ExpectedImprovement()

◆ ForwardPass()

◆ get_fs()

◆ get_us()

◆ get_xs()

◆ IncreaseRegularization()

◆ IsNaN()

◆ SetCandidate()

◆ Solve()

◆ SpecifyProblem()

◆ TryStep()

◆ UpdateExpectedImprovement()

Member Data Documentation

◆ clamp_to_control_limits_in_forward_pass_

◆ control_limits_

◆ d_

◆ dg_

◆ dq_

◆ dV_

◆ dv_

◆ dVexp_

◆ dx_

◆ fs_

◆ fTVxx_p_

◆ FuTVxx_p_

◆ FxTVxx_p_

◆ initial_regularization_rate_

◆ is_feasible_

◆ last_T_

◆ NDX_

◆ Quu_ldlt_

◆ Quuk_

◆ Qxu_

◆ regfactor_

◆ regmax_

◆ regmin_

◆ steplength_

◆ stop_

◆ th_acceptnegstep_

◆ th_acceptstep_

◆ th_grad_

◆ th_gradient_tolerance_

◆ th_stepdec_

◆ th_stepinc_

◆ th_stop_

◆ ureg_

◆ us_

◆ us_try_

◆ was_feasible_

◆ xnext_

◆ xreg_

◆ xs_

◆ xs_try_