Solves motion planning problem using Approximate Inference Control method. More...

#include <aico_solver.h>

Inheritance diagram for exotica::AICOSolver:

Collaboration diagram for exotica::AICOSolver:

Public Member Functions
	AICOSolver ()

void	Instantiate (const AICOSolverInitializer &init) override

virtual	~AICOSolver ()

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::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 std::vector< Initializer >	GetAllTemplates () const =0

Public Member Functions inherited from exotica::Instantiable< AICOSolverInitializer >
void	InstantiateInternal (const Initializer &init) override

Initializer	GetInitializerTemplate () override

std::vector< Initializer >	GetAllTemplates () const override

const AICOSolverInitializer &	GetParameters () const

Protected Member Functions
void	InitMessages ()
	Initializes message data. More...

void	InitTrajectory (const std::vector< Eigen::VectorXd > &q_init)
	Initialise AICO messages from an initial trajectory. More...

Private Types
enum	SweepMode { FORWARD = 0, SYMMETRIC, LOCAL_GAUSS_NEWTON, LOCAL_GAUSS_NEWTON_DAMPED }

Private Member Functions
void	UpdateFwdMessage (int t)
	Updates the forward message at time step $t$. More...

void	UpdateBwdMessage (int t)
	Updates the backward message at time step $t$. More...

void	UpdateTaskMessage (int t, const Eigen::Ref< const Eigen::VectorXd > &qhat_t, double tolerance, double max_step_size=-1.)

void	UpdateTimestep (int t, bool update_fwd, bool update_bwd, int max_relocation_iterations, double tolerance, bool force_relocation, double max_step_size=-1.)
	Update messages for given time step. More...

void	UpdateTimestepGaussNewton (int t, bool update_fwd, bool update_bwd, int max_relocation_iterations, double tolerance, double max_step_size=-1.)
	Update messages for given time step using the Gauss Newton method. More...

double	EvaluateTrajectory (const std::vector< Eigen::VectorXd > &x, bool skip_update=false)
	Computes the cost of the trajectory. More...

void	RememberOldState ()
	Stores the previous state. More...

void	PerhapsUndoStep ()
	Reverts back to previous state if the cost of the current state is higher. More...

double	GetTaskCosts (int t)
	Updates the task cost terms $R, r, \hat{r}$ for time step . UnconstrainedTimeIndexedProblem::Update() has to be called before calling this function. More...

double	Step ()
	Compute one step of the AICO algorithm. More...

Private Attributes
UnconstrainedTimeIndexedProblemPtr	prob_
	Shared pointer to the planning problem. More...

double	damping = 0.01
	Damping. More...

double	damping_init_ = 0.0
	Damping. More...

double	minimum_step_tolerance_ = 1e-5
	Update tolerance to stop update of messages if change of maximum coefficient is less than this tolerance. More...

double	step_tolerance_ = 1e-5
	Relative step tolerance (termination criterion) More...

double	function_tolerance_ = 1e-5
	Relative function tolerance/first-order optimality criterion. More...

int	max_backtrack_iterations_ = 10
	Max. number of sweeps without improvement before terminating (= line-search) More...

bool	use_bwd_msg_ = false
	Flag for using backward message initialisation. More...

Eigen::VectorXd	bwd_msg_v_
	Backward message initialisation mean. More...

Eigen::MatrixXd	bwd_msg_Vinv_
	Backward message initialisation covariance. More...

bool	sweep_improved_cost_
	Whether the last sweep improved the cost (for backtrack iterations count) More...

int	iteration_count_
	Iteration counter. More...

std::vector< SinglePassMeanCovariance >	q_stat_
	Cost weighted normal distribution of configurations across sweeps. More...

std::vector< Eigen::VectorXd >	s
	Forward message mean. More...

std::vector< Eigen::MatrixXd >	Sinv
	Forward message covariance inverse. More...

std::vector< Eigen::VectorXd >	v
	Backward message mean. More...

std::vector< Eigen::MatrixXd >	Vinv
	Backward message covariance inverse. More...

std::vector< Eigen::VectorXd >	r
	Task message mean. More...

std::vector< Eigen::MatrixXd >	R
	Task message covariance. More...

Eigen::VectorXd	rhat
	Task message point of linearisation. More...

std::vector< Eigen::VectorXd >	b
	Belief mean. More...

std::vector< Eigen::MatrixXd >	Binv
	Belief covariance inverse. More...

std::vector< Eigen::VectorXd >	q
	Configuration space trajectory. More...

std::vector< Eigen::VectorXd >	qhat
	Point of linearisation. More...

Eigen::VectorXd	cost_control_
	Control cost for each time step. More...

Eigen::VectorXd	cost_task_
	Task cost for each task for each time step. More...

std::vector< Eigen::VectorXd >	s_old
	Forward message mean (last most optimal value) More...

std::vector< Eigen::MatrixXd >	Sinv_old
	Forward message covariance inverse (last most optimal value) More...

std::vector< Eigen::VectorXd >	v_old
	Backward message mean (last most optimal value) More...

std::vector< Eigen::MatrixXd >	Vinv_old
	Backward message covariance inverse (last most optimal value) More...

std::vector< Eigen::VectorXd >	r_old
	Task message mean (last most optimal value) More...

std::vector< Eigen::MatrixXd >	R_old
	Task message covariance (last most optimal value) More...

Eigen::VectorXd	rhat_old
	Task message point of linearisation (last most optimal value) More...

std::vector< Eigen::VectorXd >	b_old
	Belief mean (last most optimal value) More...

std::vector< Eigen::MatrixXd >	Binv_old
	Belief covariance inverse (last most optimal value) More...

std::vector< Eigen::VectorXd >	q_old
	Configuration space trajectory (last most optimal value) More...

std::vector< Eigen::VectorXd >	qhat_old
	Point of linearisation (last most optimal value) More...

Eigen::VectorXd	cost_control_old_
	Control cost for each time step (last most optimal value) More...

Eigen::MatrixXd	cost_task_old_
	Task cost for each task for each time step (last most optimal value) More...

std::vector< Eigen::VectorXd >	damping_reference_
	Damping reference point. More...

double	cost_ = 0.0
	cost of MAP trajectory More...

double	cost_old_ = std::numeric_limits<double>::max()
	cost of MAP trajectory (last most optimal value) More...

double	cost_prev_ = std::numeric_limits<double>::max()
	previous iteration cost More...

double	b_step_ = 0.0
	Squared configuration space step. More...

double	b_step_old_

Eigen::MatrixXd	W
	Configuration space weight matrix inverse. More...

Eigen::MatrixXd	Winv
	Configuration space weight matrix inverse. More...

int	last_T_
	T the last time InitMessages was called. More...

int	sweep_ = 0
	Sweeps so far. More...

int	best_sweep_ = 0

int	best_sweep_old_ = 0

int	sweep_mode_ = 0
	Sweep mode. More...

int	update_count_ = 0

bool	verbose_ = false

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

std::string	object_name_

bool	debug_

Protected Attributes inherited from exotica::MotionSolver
PlanningProblemPtr	problem_

double	planning_time_ = -1

int	max_iterations_ = 100

Protected Attributes inherited from exotica::Instantiable< AICOSolverInitializer >
AICOSolverInitializer	parameters_

Detailed Description

Solves motion planning problem using Approximate Inference Control method.

Member Enumeration Documentation

◆ SweepMode

enum exotica::AICOSolver::SweepMode

private

Enumerator
FORWARD
SYMMETRIC
LOCAL_GAUSS_NEWTON
LOCAL_GAUSS_NEWTON_DAMPED

Constructor & Destructor Documentation

◆ AICOSolver()

exotica::AICOSolver::AICOSolver ( )

◆ ~AICOSolver()

virtual exotica::AICOSolver::~AICOSolver ( )

virtual

Member Function Documentation

◆ EvaluateTrajectory()

double exotica::AICOSolver::EvaluateTrajectory	(	const std::vector< Eigen::VectorXd > &	x,
		bool	skip_update = `false`
	)

private

Computes the cost of the trajectory.

Parameters

x	Trajectory.

Returns: Cost of the trajectory.

◆ GetTaskCosts()

double exotica::AICOSolver::GetTaskCosts ( int t )

private

Updates the task cost terms $R, r, \hat{r}$ for time step $t$ . UnconstrainedTimeIndexedProblem::Update() has to be called before calling this function.

Parameters

t	Time step to be updated.

◆ InitMessages()

void exotica::AICOSolver::InitMessages ( )

protected

Initializes message data.

Parameters

q0	Start configuration

Returns: Indicates success

◆ InitTrajectory()

void exotica::AICOSolver::InitTrajectory ( const std::vector< Eigen::VectorXd > & q_init )

protected

Initialise AICO messages from an initial trajectory.

Parameters

q_init Initial trajectory

Returns: Indicates success

◆ Instantiate()

void exotica::AICOSolver::Instantiate ( const AICOSolverInitializer & init )

overridevirtual

Reimplemented from exotica::Instantiable< AICOSolverInitializer >.

◆ PerhapsUndoStep()

void exotica::AICOSolver::PerhapsUndoStep ( )

private

Reverts back to previous state if the cost of the current state is higher.

◆ RememberOldState()

void exotica::AICOSolver::RememberOldState ( )

private

Stores the previous state.

◆ Solve()

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

overridevirtual

Solves the problem.

Parameters

solution Returned solution trajectory as a vector of joint configurations.

Implements exotica::MotionSolver.

◆ SpecifyProblem()

void exotica::AICOSolver::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 UnconstrainedTimeIndexedProblem

Reimplemented from exotica::MotionSolver.

◆ Step()

double exotica::AICOSolver::Step ( )

private

Compute one step of the AICO algorithm.

Returns: Change in cost of the trajectory.

◆ UpdateBwdMessage()

void exotica::AICOSolver::UpdateBwdMessage ( int t )

private

Updates the backward message at time step $t$.

Parameters

t Time step Updates the mean and covariance of the backward message using: $\mu_{x_{t+1}\rightarrow x_t}(x)=\mathcal{N}(x_t|v_t,V_t)$ , where $v_t=-A_{t}^{-1}a_{t}\!\!+\!\!A_{t}^{-1}(V_{t+1}^{-1}\!\!+\!\!R_{t+1})^{-1}(V_{t+1}^{-1}v_{t+1}\!\!+\!\!r_{t+1})$ and $V_t=A_{t}^{-1}[Q+B_tH^{-1}B_t^{\!\top\!} + (V_{t+1}^{-1}+R_{t+1})^{-1}]A_{t}^{-{\!\top\!}}$ .

◆ UpdateFwdMessage()

void exotica::AICOSolver::UpdateFwdMessage ( int t )

private

Updates the forward message at time step $t$.

Parameters

t Time step Updates the mean and covariance of the forward message using: $\mu_{x_{t-1}\rightarrow x_t}(x)=\mathcal{N}(x_t|s_t,S_t)$ , where $s_t=a_{t-1}\!+\!A_{t-1}(S_{t-1}^{-1}\!+\!R_{t-1})^{-1}(S_{t-1}^{-1}s_{t-1}\!+\!r_{t-1})$ and $S_t=Q+B_tH^{-1}B_t^{\!\top\!} + A_{t-1}(S_{t-1}^{-1}+R_{t-1})^{-1}A_{t-1}^{\!\top\!}$ .

◆ UpdateTaskMessage()

void exotica::AICOSolver::UpdateTaskMessage	(	int	t,
		const Eigen::Ref< const Eigen::VectorXd > &	qhat_t,
		double	tolerance,
		double	max_step_size = `-1.`
	)

private

brief Updates the task message at time step $t$

Parameters

t	Time step
qhat_t	Point of linearisation at time step $t$
tolerance	Lazy update tolerance (only update the task message if the state changed more than this value)
max_step_size	If step size >0, cap the motion at this step to the step size. Updates the mean and covariance of the task message using: $\mu_{z_t\rightarrow x_t}(x)=\mathcal{N}[x_t\|r_t,R_t]$

◆ UpdateTimestep()

void exotica::AICOSolver::UpdateTimestep	(	int	t,
		bool	update_fwd,
		bool	update_bwd,
		int	max_relocation_iterations,
		double	tolerance,
		bool	force_relocation,
		double	max_step_size = `-1.`
	)

private

Update messages for given time step.

Parameters

t	Time step.
update_fwd	Update the forward message.
update_bwd	Update the backward message.
max_relocation_iterations	Maximum number of relocation while searching for a good linearisation point
tolerance	Tolerance for for stopping the search.
force_relocation	Set to true to force relocation even when the result is within tolerance.
max_step_size	Step size for UpdateTaskMessage.

◆ UpdateTimestepGaussNewton()

void exotica::AICOSolver::UpdateTimestepGaussNewton	(	int	t,
		bool	update_fwd,
		bool	update_bwd,
		int	max_relocation_iterations,
		double	tolerance,
		double	max_step_size = `-1.`
	)

private

Update messages for given time step using the Gauss Newton method.

Parameters

t	Time step.
update_fwd	Update the forward message.
update_bwd	Update the backward message.
max_relocation_iterations	Maximum number of relocation while searching for a good linearisation point
tolerance	Tolerance for for stopping the search.
max_step_size	Step size for UpdateTaskMessage. First, the messages $\mu_{x_{t-1}\rightarrow x_t}(x)=\mathcal{N}(x_t\|s_t,S_t)$ , $\mu_{x_{t+1}\rightarrow x_t}(x)=\mathcal{N}(x_t\|v_t,V_t)$ and $\mu_{z_t\rightarrow x_t}(x)=\mathcal{N}[x_t\|r_t,R_t]$ are computed. Then, the belief is updated: $b_t(X_t)=\mu_{x_{t-1}\rightarrow x_t}(x) \mu_{x_{t+1}\rightarrow x_t}(x) \mu_{z_t\rightarrow x_t}(x)$ where the mean and covariance are updated as follows: $b_t(X_t)=\mathcal{N}\left(x_t\|(S_t^{-1}+V_t^{-1}+R_t)^{-1}(S_t^{-1}s_t+V_t^{-1}v_t+r_t),S_t^{-1}+V_t^{-1}+R_t \right)$ .

Member Data Documentation

◆ b

std::vector<Eigen::VectorXd> exotica::AICOSolver::b

private

Belief mean.

◆ b_old

std::vector<Eigen::VectorXd> exotica::AICOSolver::b_old

private

Belief mean (last most optimal value)

◆ b_step_

double exotica::AICOSolver::b_step_ = 0.0

private

Squared configuration space step.

◆ b_step_old_

double exotica::AICOSolver::b_step_old_

private

◆ best_sweep_

int exotica::AICOSolver::best_sweep_ = 0

private

◆ best_sweep_old_

int exotica::AICOSolver::best_sweep_old_ = 0

private

◆ Binv

std::vector<Eigen::MatrixXd> exotica::AICOSolver::Binv

private

Belief covariance inverse.

◆ Binv_old

std::vector<Eigen::MatrixXd> exotica::AICOSolver::Binv_old

private

Belief covariance inverse (last most optimal value)

◆ bwd_msg_v_

Eigen::VectorXd exotica::AICOSolver::bwd_msg_v_

private

Backward message initialisation mean.

◆ bwd_msg_Vinv_

Eigen::MatrixXd exotica::AICOSolver::bwd_msg_Vinv_

private

Backward message initialisation covariance.

◆ cost_

double exotica::AICOSolver::cost_ = 0.0

private

cost of MAP trajectory

◆ cost_control_

Eigen::VectorXd exotica::AICOSolver::cost_control_

private

Control cost for each time step.

◆ cost_control_old_

Eigen::VectorXd exotica::AICOSolver::cost_control_old_

private

Control cost for each time step (last most optimal value)

◆ cost_old_

double exotica::AICOSolver::cost_old_ = std::numeric_limits<double>::max()

private

cost of MAP trajectory (last most optimal value)

◆ cost_prev_

double exotica::AICOSolver::cost_prev_ = std::numeric_limits<double>::max()

private

previous iteration cost

◆ cost_task_

Eigen::VectorXd exotica::AICOSolver::cost_task_

private

Task cost for each task for each time step.

◆ cost_task_old_

Eigen::MatrixXd exotica::AICOSolver::cost_task_old_

private

Task cost for each task for each time step (last most optimal value)

◆ damping

double exotica::AICOSolver::damping = 0.01

private

Damping.

◆ damping_init_

double exotica::AICOSolver::damping_init_ = 0.0

private

Damping.

◆ damping_reference_

std::vector<Eigen::VectorXd> exotica::AICOSolver::damping_reference_

private

Damping reference point.

◆ function_tolerance_

double exotica::AICOSolver::function_tolerance_ = 1e-5

private

Relative function tolerance/first-order optimality criterion.

◆ iteration_count_

int exotica::AICOSolver::iteration_count_

private

Iteration counter.

◆ last_T_

int exotica::AICOSolver::last_T_

private

T the last time InitMessages was called.

◆ max_backtrack_iterations_

int exotica::AICOSolver::max_backtrack_iterations_ = 10

private

Max. number of sweeps without improvement before terminating (= line-search)

◆ minimum_step_tolerance_

double exotica::AICOSolver::minimum_step_tolerance_ = 1e-5

private

Update tolerance to stop update of messages if change of maximum coefficient is less than this tolerance.

◆ prob_

UnconstrainedTimeIndexedProblemPtr exotica::AICOSolver::prob_

private

Shared pointer to the planning problem.

◆ q

std::vector<Eigen::VectorXd> exotica::AICOSolver::q

private

Configuration space trajectory.

◆ q_old

std::vector<Eigen::VectorXd> exotica::AICOSolver::q_old

private

Configuration space trajectory (last most optimal value)

◆ q_stat_

std::vector<SinglePassMeanCovariance> exotica::AICOSolver::q_stat_

private

Cost weighted normal distribution of configurations across sweeps.

◆ qhat

std::vector<Eigen::VectorXd> exotica::AICOSolver::qhat

private

Point of linearisation.

◆ qhat_old

std::vector<Eigen::VectorXd> exotica::AICOSolver::qhat_old

private

Point of linearisation (last most optimal value)

◆ r

std::vector<Eigen::VectorXd> exotica::AICOSolver::r

private

Task message mean.

◆ R

std::vector<Eigen::MatrixXd> exotica::AICOSolver::R

private

Task message covariance.

◆ r_old

std::vector<Eigen::VectorXd> exotica::AICOSolver::r_old

private

Task message mean (last most optimal value)

◆ R_old

std::vector<Eigen::MatrixXd> exotica::AICOSolver::R_old

private

Task message covariance (last most optimal value)

◆ rhat

Eigen::VectorXd exotica::AICOSolver::rhat

private

Task message point of linearisation.

◆ rhat_old

Eigen::VectorXd exotica::AICOSolver::rhat_old

private

Task message point of linearisation (last most optimal value)

◆ s

std::vector<Eigen::VectorXd> exotica::AICOSolver::s

private

Forward message mean.

◆ s_old

std::vector<Eigen::VectorXd> exotica::AICOSolver::s_old

private

Forward message mean (last most optimal value)

◆ Sinv

std::vector<Eigen::MatrixXd> exotica::AICOSolver::Sinv

private

Forward message covariance inverse.

◆ Sinv_old

std::vector<Eigen::MatrixXd> exotica::AICOSolver::Sinv_old

private

Forward message covariance inverse (last most optimal value)

◆ step_tolerance_

double exotica::AICOSolver::step_tolerance_ = 1e-5

private

Relative step tolerance (termination criterion)

◆ sweep_

int exotica::AICOSolver::sweep_ = 0

private

Sweeps so far.

◆ sweep_improved_cost_

bool exotica::AICOSolver::sweep_improved_cost_

private

Whether the last sweep improved the cost (for backtrack iterations count)

◆ sweep_mode_

int exotica::AICOSolver::sweep_mode_ = 0

private

Sweep mode.

◆ update_count_

int exotica::AICOSolver::update_count_ = 0

private

◆ use_bwd_msg_

bool exotica::AICOSolver::use_bwd_msg_ = false

private

Flag for using backward message initialisation.

◆ v

std::vector<Eigen::VectorXd> exotica::AICOSolver::v

private

Backward message mean.

◆ v_old

std::vector<Eigen::VectorXd> exotica::AICOSolver::v_old

private

Backward message mean (last most optimal value)

◆ verbose_

bool exotica::AICOSolver::verbose_ = false

private

◆ Vinv

std::vector<Eigen::MatrixXd> exotica::AICOSolver::Vinv

private

Backward message covariance inverse.

◆ Vinv_old

std::vector<Eigen::MatrixXd> exotica::AICOSolver::Vinv_old

private

Backward message covariance inverse (last most optimal value)

◆ W

Eigen::MatrixXd exotica::AICOSolver::W

private

Configuration space weight matrix inverse.

◆ Winv

Eigen::MatrixXd exotica::AICOSolver::Winv

private

Configuration space weight matrix inverse.

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

/tmp/exotica/exotations/solvers/exotica_aico_solver/include/exotica_aico_solver/aico_solver.h

Public Member Functions

Protected Member Functions

Private Types

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Enumeration Documentation

◆ SweepMode

Constructor & Destructor Documentation

◆ AICOSolver()

◆ ~AICOSolver()

Member Function Documentation

◆ EvaluateTrajectory()

◆ GetTaskCosts()

◆ InitMessages()

◆ InitTrajectory()

◆ Instantiate()

◆ PerhapsUndoStep()

◆ RememberOldState()

◆ Solve()

◆ SpecifyProblem()

◆ Step()

◆ UpdateBwdMessage()

◆ UpdateFwdMessage()

◆ UpdateTaskMessage()

◆ UpdateTimestep()

◆ UpdateTimestepGaussNewton()

Member Data Documentation

◆ b

◆ b_old

◆ b_step_

◆ b_step_old_

◆ best_sweep_

◆ best_sweep_old_

◆ Binv

◆ Binv_old

◆ bwd_msg_v_

◆ bwd_msg_Vinv_

◆ cost_

◆ cost_control_

◆ cost_control_old_

◆ cost_old_

◆ cost_prev_

◆ cost_task_

◆ cost_task_old_

◆ damping

◆ damping_init_

◆ damping_reference_

◆ function_tolerance_

◆ iteration_count_

◆ last_T_

◆ max_backtrack_iterations_

◆ minimum_step_tolerance_

◆ prob_

◆ q

◆ q_old

◆ q_stat_

◆ qhat

◆ qhat_old

◆ r

◆ R

◆ r_old

◆ R_old

◆ rhat

◆ rhat_old

◆ s

◆ s_old

◆ Sinv

◆ Sinv_old

◆ step_tolerance_

◆ sweep_

◆ sweep_improved_cost_

◆ sweep_mode_

◆ update_count_

◆ use_bwd_msg_

◆ v

◆ v_old

◆ verbose_

◆ Vinv