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

#include <bayesian_ik_solver.h>

Inheritance diagram for exotica::BayesianIKSolver:

[legend]

Public Member Functions
void	Instantiate (const BayesianIKSolverInitializer &init) override

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
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
	InstantiableBase ()=default

virtual	~InstantiableBase ()=default

Public Member Functions inherited from exotica::Instantiable< BayesianIKSolverInitializer >
std::vector< Initializer >	GetAllTemplates () const override

Initializer	GetInitializerTemplate () override

const BayesianIKSolverInitializer &	GetParameters () const

void	InstantiateInternal (const Initializer &init) override

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

void	InitTrajectory (const 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
double	EvaluateTrajectory (const Eigen::VectorXd &x, bool skip_update=false)
	Computes the cost of the trajectory. More...

void	GetTaskCosts ()
	Updates the task cost terms $R, r, \hat{r}$ . UnconstrainedEndPoseProblem::Update() has to be called before calling this function. More...

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

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

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

void	UpdateBwdMessage ()
	Updates the backward message 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\!}}$ . More...

void	UpdateFwdMessage ()
	Updates the forward message 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\!}$ . More...

void	UpdateTaskMessage (const Eigen::Ref< const Eigen::VectorXd > &qhat_t, double tolerance, double max_step_size=-1.)
	Updates the task message. More...

void	UpdateTimestep (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 (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...

Private Attributes
Eigen::VectorXd	b
	Belief mean. More...

Eigen::VectorXd	b_old
	Belief mean (last most optimal value) More...

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

double	b_step_old_

int	best_sweep_ = 0

int	best_sweep_old_ = 0

Eigen::MatrixXd	Binv
	Belief covariance inverse. More...

Eigen::MatrixXd	Binv_old
	Belief covariance inverse (last most optimal value) More...

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

Eigen::MatrixXd	bwd_msg_Vinv_
	Backward message initialisation covariance. 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	damping = 0.01
	Damping. More...

double	damping_init_ = 100.0
	Damping. More...

Eigen::VectorXd	damping_reference_
	Damping reference point. More...

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

int	iteration_count_
	Iteration counter. More...

int	max_backtrack_iterations_ = 10
	Max. number of sweeps without improvement before terminating (= line-search) 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...

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

Eigen::VectorXd	q
	Configuration space trajectory. More...

Eigen::VectorXd	q_old
	Configuration space trajectory (last most optimal value) More...

Eigen::VectorXd	qhat
	Point of linearisation. More...

Eigen::VectorXd	qhat_old
	Point of linearisation (last most optimal value) More...

Eigen::VectorXd	r
	Task message mean. More...

Eigen::MatrixXd	R
	Task message covariance. More...

Eigen::VectorXd	r_old
	Task message mean (last most optimal value) More...

Eigen::MatrixXd	R_old
	Task message covariance (last most optimal value) More...

double	rhat
	Task message point of linearisation. More...

double	rhat_old
	Task message point of linearisation (last most optimal value) More...

Eigen::VectorXd	s
	Forward message mean. More...

Eigen::VectorXd	s_old
	Forward message mean (last most optimal value) More...

Eigen::MatrixXd	Sinv
	Forward message covariance inverse. More...

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

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

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

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

int	sweep_mode_ = 0
	Sweep mode. More...

int	update_count_ = 0

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

Eigen::VectorXd	v
	Backward message mean. More...

Eigen::VectorXd	v_old
	Backward message mean (last most optimal value) More...

bool	verbose_ = false

Eigen::MatrixXd	Vinv
	Backward message covariance inverse. More...

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

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

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

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

std::string	ns_

std::string	object_name_

Protected Attributes inherited from exotica::MotionSolver
int	max_iterations_

double	planning_time_

PlanningProblemPtr	problem_

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

Detailed Description

Solves motion planning problem using Approximate Inference Control method.

Definition at line 47 of file bayesian_ik_solver.h.

Member Enumeration Documentation

enum exotica::BayesianIKSolver::SweepMode

private

Enumerator
FORWARD
SYMMETRIC
LOCAL_GAUSS_NEWTON
LOCAL_GAUSS_NEWTON_DAMPED

Definition at line 123 of file bayesian_ik_solver.h.

Member Function Documentation

double exotica::BayesianIKSolver::EvaluateTrajectory	(	const Eigen::VectorXd &	x,
		bool	skip_update = `false`
	)

private

Computes the cost of the trajectory.

Parameters

x	Trajecotry.

Returns: Cost of the trajectory.

Definition at line 348 of file bayesian_ik_solver.cpp.

void exotica::BayesianIKSolver::GetTaskCosts ( )

private

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

Definition at line 277 of file bayesian_ik_solver.cpp.

void exotica::BayesianIKSolver::InitMessages ( )

protected

Initializes message data.

Parameters

q0	Start configuration

Returns: Indicates success

Definition at line 168 of file bayesian_ik_solver.cpp.

void exotica::BayesianIKSolver::InitTrajectory ( const Eigen::VectorXd & q_init )

protected

Initialise AICO messages from an initial trajectory.

Parameters

q_init Initial trajectory

Returns: Indicates success

Definition at line 197 of file bayesian_ik_solver.cpp.

void exotica::BayesianIKSolver::Instantiate ( const BayesianIKSolverInitializer & init )

overridevirtual

Reimplemented from exotica::Instantiable< BayesianIKSolverInitializer >.

Definition at line 42 of file bayesian_ik_solver.cpp.

void exotica::BayesianIKSolver::PerhapsUndoStep ( )

private

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

Definition at line 433 of file bayesian_ik_solver.cpp.

void exotica::BayesianIKSolver::RememberOldState ( )

private

Stores the previous state.

Definition at line 413 of file bayesian_ik_solver.cpp.

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

overridevirtual

Solves the problem.

Parameters

solution Returned end pose solution.

Implements exotica::MotionSolver.

Definition at line 79 of file bayesian_ik_solver.cpp.

void exotica::BayesianIKSolver::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 UnconstrainedEndPoseProblem

Reimplemented from exotica::MotionSolver.

Definition at line 67 of file bayesian_ik_solver.cpp.

double exotica::BayesianIKSolver::Step ( )

private

Compute one step of the AICO algorithm.

Returns: Change in cost of the trajectory.

Definition at line 364 of file bayesian_ik_solver.cpp.

void exotica::BayesianIKSolver::UpdateBwdMessage ( )

private

Updates the backward message 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\!}}$ .

Definition at line 240 of file bayesian_ik_solver.cpp.

void exotica::BayesianIKSolver::UpdateFwdMessage ( )

private

Updates the forward message 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\!}$ .

Definition at line 231 of file bayesian_ik_solver.cpp.

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

private

Updates the task message.

Parameters

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]$

Definition at line 256 of file bayesian_ik_solver.cpp.

void exotica::BayesianIKSolver::UpdateTimestep	(	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.

Definition at line 299 of file bayesian_ik_solver.cpp.

void exotica::BayesianIKSolver::UpdateTimestepGaussNewton	(	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)$ .

Definition at line 340 of file bayesian_ik_solver.cpp.

Member Data Documentation

Eigen::VectorXd exotica::BayesianIKSolver::b

private

Belief mean.

Definition at line 93 of file bayesian_ik_solver.h.

Eigen::VectorXd exotica::BayesianIKSolver::b_old

private

Belief mean (last most optimal value)

Definition at line 105 of file bayesian_ik_solver.h.

double exotica::BayesianIKSolver::b_step_ = 0.0

private

Squared configuration space step.

Definition at line 114 of file bayesian_ik_solver.h.

double exotica::BayesianIKSolver::b_step_old_

private

Definition at line 115 of file bayesian_ik_solver.h.

int exotica::BayesianIKSolver::best_sweep_ = 0

private

Definition at line 121 of file bayesian_ik_solver.h.

int exotica::BayesianIKSolver::best_sweep_old_ = 0

private

Definition at line 122 of file bayesian_ik_solver.h.

Eigen::MatrixXd exotica::BayesianIKSolver::Binv

private

Belief covariance inverse.

Definition at line 94 of file bayesian_ik_solver.h.

Eigen::MatrixXd exotica::BayesianIKSolver::Binv_old

private

Belief covariance inverse (last most optimal value)

Definition at line 106 of file bayesian_ik_solver.h.

Eigen::VectorXd exotica::BayesianIKSolver::bwd_msg_v_

private

Backward message initialisation mean.

Definition at line 81 of file bayesian_ik_solver.h.

Eigen::MatrixXd exotica::BayesianIKSolver::bwd_msg_Vinv_

private

Backward message initialisation covariance.

Definition at line 82 of file bayesian_ik_solver.h.

double exotica::BayesianIKSolver::cost_ = 0.0

private

cost of MAP trajectory

Definition at line 111 of file bayesian_ik_solver.h.

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

private

cost of MAP trajectory (last most optimal value)

Definition at line 112 of file bayesian_ik_solver.h.

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

private

previous iteration cost

Definition at line 113 of file bayesian_ik_solver.h.

double exotica::BayesianIKSolver::damping = 0.01

private

Damping.

Definition at line 74 of file bayesian_ik_solver.h.

double exotica::BayesianIKSolver::damping_init_ = 100.0

private

Damping.

Definition at line 75 of file bayesian_ik_solver.h.

Eigen::VectorXd exotica::BayesianIKSolver::damping_reference_

private

Damping reference point.

Definition at line 110 of file bayesian_ik_solver.h.

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

private

Relative function tolerance/first-order optimality criterion.

Definition at line 78 of file bayesian_ik_solver.h.

int exotica::BayesianIKSolver::iteration_count_

private

Iteration counter.

Definition at line 84 of file bayesian_ik_solver.h.

int exotica::BayesianIKSolver::max_backtrack_iterations_ = 10

private

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

Definition at line 79 of file bayesian_ik_solver.h.

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

private

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

Definition at line 76 of file bayesian_ik_solver.h.

UnconstrainedEndPoseProblemPtr exotica::BayesianIKSolver::prob_

private

Shared pointer to the planning problem.

Definition at line 73 of file bayesian_ik_solver.h.

Eigen::VectorXd exotica::BayesianIKSolver::q

private

Configuration space trajectory.

Definition at line 95 of file bayesian_ik_solver.h.

Eigen::VectorXd exotica::BayesianIKSolver::q_old

private

Configuration space trajectory (last most optimal value)

Definition at line 107 of file bayesian_ik_solver.h.

Eigen::VectorXd exotica::BayesianIKSolver::qhat

private

Point of linearisation.

Definition at line 96 of file bayesian_ik_solver.h.

Eigen::VectorXd exotica::BayesianIKSolver::qhat_old

private

Point of linearisation (last most optimal value)

Definition at line 108 of file bayesian_ik_solver.h.

Eigen::VectorXd exotica::BayesianIKSolver::r

private

Task message mean.

Definition at line 90 of file bayesian_ik_solver.h.

Eigen::MatrixXd exotica::BayesianIKSolver::R

private

Task message covariance.

Definition at line 91 of file bayesian_ik_solver.h.

Eigen::VectorXd exotica::BayesianIKSolver::r_old

private

Task message mean (last most optimal value)

Definition at line 102 of file bayesian_ik_solver.h.

Eigen::MatrixXd exotica::BayesianIKSolver::R_old

private

Task message covariance (last most optimal value)

Definition at line 103 of file bayesian_ik_solver.h.

double exotica::BayesianIKSolver::rhat

private

Task message point of linearisation.

Definition at line 92 of file bayesian_ik_solver.h.

double exotica::BayesianIKSolver::rhat_old

private

Task message point of linearisation (last most optimal value)

Definition at line 104 of file bayesian_ik_solver.h.

Eigen::VectorXd exotica::BayesianIKSolver::s

private

Forward message mean.

Definition at line 86 of file bayesian_ik_solver.h.

Eigen::VectorXd exotica::BayesianIKSolver::s_old

private

Forward message mean (last most optimal value)

Definition at line 98 of file bayesian_ik_solver.h.

Eigen::MatrixXd exotica::BayesianIKSolver::Sinv

private

Forward message covariance inverse.

Definition at line 87 of file bayesian_ik_solver.h.

Eigen::MatrixXd exotica::BayesianIKSolver::Sinv_old

private

Forward message covariance inverse (last most optimal value)

Definition at line 99 of file bayesian_ik_solver.h.

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

private

Relative step tolerance (termination criterion)

Definition at line 77 of file bayesian_ik_solver.h.

int exotica::BayesianIKSolver::sweep_ = 0

private

Sweeps so far.

Definition at line 120 of file bayesian_ik_solver.h.

bool exotica::BayesianIKSolver::sweep_improved_cost_

private

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

Definition at line 83 of file bayesian_ik_solver.h.

int exotica::BayesianIKSolver::sweep_mode_ = 0

private

Sweep mode.

Definition at line 130 of file bayesian_ik_solver.h.

int exotica::BayesianIKSolver::update_count_ = 0

private

Definition at line 131 of file bayesian_ik_solver.h.

bool exotica::BayesianIKSolver::use_bwd_msg_ = false

private

Flag for using backward message initialisation.

Definition at line 80 of file bayesian_ik_solver.h.

Eigen::VectorXd exotica::BayesianIKSolver::v

private

Backward message mean.

Definition at line 88 of file bayesian_ik_solver.h.

Eigen::VectorXd exotica::BayesianIKSolver::v_old

private

Backward message mean (last most optimal value)

Definition at line 100 of file bayesian_ik_solver.h.

bool exotica::BayesianIKSolver::verbose_ = false

private

Definition at line 133 of file bayesian_ik_solver.h.

Eigen::MatrixXd exotica::BayesianIKSolver::Vinv

private

Backward message covariance inverse.

Definition at line 89 of file bayesian_ik_solver.h.

Eigen::MatrixXd exotica::BayesianIKSolver::Vinv_old

private

Backward message covariance inverse (last most optimal value)

Definition at line 101 of file bayesian_ik_solver.h.

Eigen::MatrixXd exotica::BayesianIKSolver::W

private

Configuration space weight matrix inverse.

Definition at line 117 of file bayesian_ik_solver.h.

Eigen::MatrixXd exotica::BayesianIKSolver::Winv

private

Configuration space weight matrix inverse.

Definition at line 118 of file bayesian_ik_solver.h.

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

Public Member Functions

Protected Member Functions

Private Types

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Enumeration Documentation

Member Function Documentation

Member Data Documentation