#include <feasibility_driven_ddp_solver.h>

Inheritance diagram for exotica::AbstractFeasibilityDrivenDDPSolver:

[legend]

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

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

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

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...

Public Member Functions inherited from exotica::AbstractDDPSolver
std::vector< double >	get_control_cost_evolution () const

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

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

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

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

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

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

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

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

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

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

std::vector< double >	get_regularization_evolution () const

std::vector< double >	get_steplength_evolution () const

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

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

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

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

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

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

Eigen::VectorXd	GetFeedbackControl (Eigen::VectorXdRefConst x, int t) const override

void	set_control_cost_evolution (const int index, const double cost)

Public Member Functions inherited from exotica::MotionSolver
int	GetNumberOfMaxIterations ()

double	GetPlanningTime ()

PlanningProblemPtr	GetProblem () const

void	InstantiateBase (const Initializer &init) override

	MotionSolver ()=default

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

void	SetNumberOfMaxIterations (int max_iter)

virtual	~MotionSolver ()=default

Public Member Functions inherited from exotica::Object
std::string	GetObjectName ()

void	InstantiateObject (const Initializer &init)

	Object ()

virtual std::string	type () const

virtual	~Object ()

Public Member Functions inherited from exotica::InstantiableBase
virtual std::vector< Initializer >	GetAllTemplates () const =0

virtual Initializer	GetInitializerTemplate ()=0

	InstantiableBase ()=default

virtual void	InstantiateInternal (const Initializer &init)=0

virtual	~InstantiableBase ()=default

Protected Member Functions
virtual void	AllocateData ()

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

virtual bool	BackwardPassFDDP ()

double	CalcDiff ()

double	CheckStoppingCriteria ()

bool	ComputeDirection (const bool recalcDiff)

virtual void	ComputeGains (const int t)

void	DecreaseRegularization () override

const Eigen::Vector2d &	ExpectedImprovement ()

void	ForwardPass (const double steplength)

void	IncreaseRegularization () override

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	TryStep (const double steplength)

void	UpdateExpectedImprovement ()

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
bool	clamp_to_control_limits_in_forward_pass_ = false

Eigen::MatrixXd	control_limits_

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

double	dg_ = 0.

double	dq_ = 0.

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

double	dv_ = 0.

double	dVexp_
	Expected cost reduction. More...

std::vector< Eigen::VectorXd >	dx_

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

Eigen::VectorXd	fTVxx_p_

std::vector< Eigen::MatrixXd >	FuTVxx_p_

Eigen::MatrixXd	FxTVxx_p_

double	initial_regularization_rate_ = 1e-9

bool	is_feasible_ = false

int	last_T_ = -1

int	NDX_

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

std::vector< Eigen::VectorXd >	Quuk_

std::vector< Eigen::MatrixXd >	Qxu_

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

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

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

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

double	stop_
	Value computed by CheckStoppingCriteria. More...

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

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

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

double	th_gradient_tolerance_ = 0.
	Gradient tolerance. 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...

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

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

std::vector< Eigen::VectorXd >	us_

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

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

Eigen::VectorXd	xnext_

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

std::vector< Eigen::VectorXd >	xs_

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

Protected Attributes inherited from exotica::AbstractDDPSolver
double	alpha_best_
	Line-search step taken. More...

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

AbstractDDPSolverInitializer	base_parameters_

double	control_cost_
	Control cost during iteration. More...

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

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

double	cost_
	Cost during iteration. More...

double	cost_prev_
	Cost during previous iteration. More...

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

double	dt_
	Integration time-step. More...

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

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

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

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

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

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

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

int	NU_
	Size of control vector. More...

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

int	NX_
	Size of state vector. More...

DynamicTimeIndexedShootingProblemPtr	prob_
	Shared pointer to the planning problem. More...

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

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

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

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

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

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

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

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

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

double	time_taken_backward_pass_

double	time_taken_forward_pass_

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

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

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

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

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

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

Protected Attributes inherited from exotica::MotionSolver
int	max_iterations_

double	planning_time_

PlanningProblemPtr	problem_

Additional Inherited Members
Public Attributes inherited from exotica::Object
bool	debug_

std::string	ns_

std::string	object_name_

Detailed Description

Definition at line 41 of file feasibility_driven_ddp_solver.h.

Member Function Documentation

void exotica::AbstractFeasibilityDrivenDDPSolver::AllocateData ( )

protectedvirtual

Reimplemented in exotica::ControlLimitedFeasibilityDrivenDDPSolver.

Definition at line 52 of file feasibility_driven_ddp_solver.cpp.

void exotica::AbstractFeasibilityDrivenDDPSolver::BackwardPass ( )

inlineoverrideprotectedvirtual

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

Implements exotica::AbstractDDPSolver.

Definition at line 76 of file feasibility_driven_ddp_solver.h.

bool exotica::AbstractFeasibilityDrivenDDPSolver::BackwardPassFDDP ( )

protectedvirtual

Definition at line 586 of file feasibility_driven_ddp_solver.cpp.

double exotica::AbstractFeasibilityDrivenDDPSolver::CalcDiff ( )

protected

Definition at line 555 of file feasibility_driven_ddp_solver.cpp.

double exotica::AbstractFeasibilityDrivenDDPSolver::CheckStoppingCriteria ( )

protected

Definition at line 386 of file feasibility_driven_ddp_solver.cpp.

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

protected

Definition at line 546 of file feasibility_driven_ddp_solver.cpp.

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

protectedvirtual

Reimplemented in exotica::ControlLimitedFeasibilityDrivenDDPSolver.

Definition at line 671 of file feasibility_driven_ddp_solver.cpp.

void exotica::AbstractFeasibilityDrivenDDPSolver::DecreaseRegularization ( )

overrideprotectedvirtual

Reimplemented from exotica::AbstractDDPSolver.

Definition at line 376 of file feasibility_driven_ddp_solver.cpp.

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

protected

Definition at line 396 of file feasibility_driven_ddp_solver.cpp.

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

protected

Definition at line 480 of file feasibility_driven_ddp_solver.cpp.

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

inline

Definition at line 47 of file feasibility_driven_ddp_solver.h.

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

inline

Definition at line 49 of file feasibility_driven_ddp_solver.h.

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

inline

Definition at line 48 of file feasibility_driven_ddp_solver.h.

void exotica::AbstractFeasibilityDrivenDDPSolver::IncreaseRegularization ( )

overrideprotectedvirtual

Reimplemented from exotica::AbstractDDPSolver.

Definition at line 366 of file feasibility_driven_ddp_solver.cpp.

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

inlineprotected

Definition at line 59 of file feasibility_driven_ddp_solver.h.

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

protected

Definition at line 436 of file feasibility_driven_ddp_solver.cpp.

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.

Definition at line 142 of file feasibility_driven_ddp_solver.cpp.

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.

Definition at line 128 of file feasibility_driven_ddp_solver.cpp.

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

protected

Definition at line 474 of file feasibility_driven_ddp_solver.cpp.

void exotica::AbstractFeasibilityDrivenDDPSolver::UpdateExpectedImprovement ( )

protected

Definition at line 413 of file feasibility_driven_ddp_solver.cpp.

Member Data Documentation

bool exotica::AbstractFeasibilityDrivenDDPSolver::clamp_to_control_limits_in_forward_pass_ = false

protected

Definition at line 85 of file feasibility_driven_ddp_solver.h.

Eigen::MatrixXd exotica::AbstractFeasibilityDrivenDDPSolver::control_limits_

protected

Definition at line 83 of file feasibility_driven_ddp_solver.h.

Eigen::Vector2d exotica::AbstractFeasibilityDrivenDDPSolver::d_

protected

LQ approximation of the expected improvement.

Definition at line 88 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::dg_ = 0.

protected

Definition at line 96 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::dq_ = 0.

protected

Definition at line 97 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::dV_

protected

Cost reduction obtained by TryStep.

Definition at line 89 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::dv_ = 0.

protected

Definition at line 98 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::dVexp_

protected

Expected cost reduction.

Definition at line 90 of file feasibility_driven_ddp_solver.h.

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

protected

Definition at line 111 of file feasibility_driven_ddp_solver.h.

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

protected

Gaps/defects between shooting nodes.

Definition at line 114 of file feasibility_driven_ddp_solver.h.

Eigen::VectorXd exotica::AbstractFeasibilityDrivenDDPSolver::fTVxx_p_

protected

Definition at line 120 of file feasibility_driven_ddp_solver.h.

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

protected

Definition at line 118 of file feasibility_driven_ddp_solver.h.

Eigen::MatrixXd exotica::AbstractFeasibilityDrivenDDPSolver::FxTVxx_p_

protected

Definition at line 117 of file feasibility_driven_ddp_solver.h.

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

protected

Definition at line 84 of file feasibility_driven_ddp_solver.h.

bool exotica::AbstractFeasibilityDrivenDDPSolver::is_feasible_ = false

protected

Definition at line 102 of file feasibility_driven_ddp_solver.h.

int exotica::AbstractFeasibilityDrivenDDPSolver::last_T_ = -1

protected

Definition at line 53 of file feasibility_driven_ddp_solver.h.

int exotica::AbstractFeasibilityDrivenDDPSolver::NDX_

protected

Definition at line 49 of file feasibility_driven_ddp_solver.h.

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

protected

Definition at line 121 of file feasibility_driven_ddp_solver.h.

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

protected

Definition at line 122 of file feasibility_driven_ddp_solver.h.

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

protected

Definition at line 119 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::regfactor_ = 10.

protected

Factor by which the regularization gets increased/decreased.

Definition at line 107 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::regmax_ = 1e9

protected

Maximum regularization (to exit by divergence)

Definition at line 106 of file feasibility_driven_ddp_solver.h.

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

protected

Minimum regularization (will not decrease lower)

Definition at line 105 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::steplength_

protected

Current applied step-length.

Definition at line 87 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::stop_

protected

Value computed by CheckStoppingCriteria.

Definition at line 94 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::th_acceptnegstep_ = 2.

protected

Threshold used for accepting step along ascent direction.

Definition at line 99 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::th_acceptstep_ = 0.1

protected

Threshold used for accepting step.

Definition at line 91 of file feasibility_driven_ddp_solver.h.

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

protected

Tolerance of the expected gradient used for testing the step.

Definition at line 123 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::th_gradient_tolerance_ = 0.

protected

Gradient tolerance.

Definition at line 93 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::th_stepdec_ = 0.5

protected

Step-length threshold used to decrease regularization.

Definition at line 124 of file feasibility_driven_ddp_solver.h.

double exotica::AbstractFeasibilityDrivenDDPSolver::th_stepinc_ = 0.01

protected

Step-length threshold used to increase regularization.

Definition at line 125 of file feasibility_driven_ddp_solver.h.

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

protected

Tolerance for stopping the algorithm.

Definition at line 92 of file feasibility_driven_ddp_solver.h.

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

protected

Control regularization.

Definition at line 104 of file feasibility_driven_ddp_solver.h.

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

protected

Definition at line 100 of file feasibility_driven_ddp_solver.h.

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

protected

Control trajectory computed by line-search procedure.

Definition at line 110 of file feasibility_driven_ddp_solver.h.

bool exotica::AbstractFeasibilityDrivenDDPSolver::was_feasible_ = false

protected

Label that indicates in the previous iterate was feasible.

Definition at line 126 of file feasibility_driven_ddp_solver.h.

Eigen::VectorXd exotica::AbstractFeasibilityDrivenDDPSolver::xnext_

protected

Definition at line 116 of file feasibility_driven_ddp_solver.h.

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

protected

State regularization.

Definition at line 103 of file feasibility_driven_ddp_solver.h.

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

protected

Definition at line 101 of file feasibility_driven_ddp_solver.h.

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

protected

State trajectory computed by line-search procedure.

Definition at line 109 of file feasibility_driven_ddp_solver.h.

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

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

Member Data Documentation