Public Member Functions | Protected Member Functions | Private Types | Private Member Functions | Private Attributes | List of all members
exotica::AICOSolver Class Reference

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

#include <aico_solver.h>

Inheritance diagram for exotica::AICOSolver:
Inheritance graph
[legend]

Public Member Functions

 AICOSolver ()
 
void Instantiate (const AICOSolverInitializer &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...
 
virtual ~AICOSolver ()
 
- 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< AICOSolverInitializer >
std::vector< InitializerGetAllTemplates () const override
 
Initializer GetInitializerTemplate () override
 
const AICOSolverInitializer & GetParameters () const
 
void InstantiateInternal (const Initializer &init) override
 

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

double EvaluateTrajectory (const std::vector< Eigen::VectorXd > &x, bool skip_update=false)
 Computes the cost of the trajectory. More...
 
double GetTaskCosts (int t)
 Updates the task cost terms $ R, r, \hat{r} $ for time step $t$. UnconstrainedTimeIndexedProblem::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 (int t)
 Updates the backward message at time step $t$. More...
 
void UpdateFwdMessage (int t)
 Updates the forward 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...
 

Private Attributes

std::vector< Eigen::VectorXd > b
 Belief mean. More...
 
std::vector< 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
 
std::vector< Eigen::MatrixXd > Binv
 Belief covariance inverse. More...
 
std::vector< 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...
 
Eigen::VectorXd cost_control_
 Control cost for each time step. More...
 
Eigen::VectorXd cost_control_old_
 Control cost for each time step (last most optimal value) 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...
 
Eigen::VectorXd cost_task_
 Task cost for each task for each time step. More...
 
Eigen::MatrixXd cost_task_old_
 Task cost for each task for each time step (last most optimal value) More...
 
double damping = 0.01
 Damping. More...
 
double damping_init_ = 0.0
 Damping. More...
 
std::vector< 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 last_T_
 T the last time InitMessages was called. 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...
 
UnconstrainedTimeIndexedProblemPtr prob_
 Shared pointer to the planning problem. More...
 
std::vector< Eigen::VectorXd > q
 Configuration space trajectory. More...
 
std::vector< Eigen::VectorXd > q_old
 Configuration space trajectory (last most optimal value) More...
 
std::vector< SinglePassMeanCovarianceq_stat_
 Cost weighted normal distribution of configurations across sweeps. More...
 
std::vector< Eigen::VectorXd > qhat
 Point of linearisation. More...
 
std::vector< Eigen::VectorXd > qhat_old
 Point of linearisation (last most optimal value) More...
 
std::vector< Eigen::VectorXd > r
 Task message mean. More...
 
std::vector< Eigen::MatrixXd > R
 Task message covariance. 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
 Task message point of linearisation. More...
 
Eigen::VectorXd rhat_old
 Task message point of linearisation (last most optimal value) More...
 
std::vector< Eigen::VectorXd > s
 Forward message mean. More...
 
std::vector< Eigen::VectorXd > s_old
 Forward message mean (last most optimal value) More...
 
std::vector< Eigen::MatrixXd > Sinv
 Forward message covariance inverse. More...
 
std::vector< 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...
 
std::vector< Eigen::VectorXd > v
 Backward message mean. More...
 
std::vector< Eigen::VectorXd > v_old
 Backward message mean (last most optimal value) More...
 
bool verbose_ = false
 
std::vector< Eigen::MatrixXd > Vinv
 Backward message covariance inverse. More...
 
std::vector< 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< AICOSolverInitializer >
AICOSolverInitializer parameters_
 

Detailed Description

Solves motion planning problem using Approximate Inference Control method.

Definition at line 72 of file aico_solver.h.

Member Enumeration Documentation

Enumerator
FORWARD 
SYMMETRIC 
LOCAL_GAUSS_NEWTON 
LOCAL_GAUSS_NEWTON_DAMPED 

Definition at line 158 of file aico_solver.h.

Constructor & Destructor Documentation

exotica::AICOSolver::AICOSolver ( )
default
exotica::AICOSolver::~AICOSolver ( )
virtualdefault

Member Function Documentation

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

Computes the cost of the trajectory.

Parameters
xTrajectory.
Returns
Cost of the trajectory.

Definition at line 436 of file aico_solver.cpp.

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
tTime step to be updated.

Definition at line 362 of file aico_solver.cpp.

void exotica::AICOSolver::InitMessages ( )
protected

Initializes message data.

Parameters
q0Start configuration
Returns
Indicates success

Definition at line 203 of file aico_solver.cpp.

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

Initialise AICO messages from an initial trajectory.

Parameters
q_initInitial trajectory
Returns
Indicates success

Definition at line 260 of file aico_solver.cpp.

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

Reimplemented from exotica::Instantiable< AICOSolverInitializer >.

Definition at line 45 of file aico_solver.cpp.

void exotica::AICOSolver::PerhapsUndoStep ( )
private

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

Definition at line 582 of file aico_solver.cpp.

void exotica::AICOSolver::RememberOldState ( )
private

Stores the previous state.

Definition at line 561 of file aico_solver.cpp.

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

Solves the problem.

Parameters
solutionReturned solution trajectory as a vector of joint configurations.

Implements exotica::MotionSolver.

Definition at line 85 of file aico_solver.cpp.

void exotica::AICOSolver::SpecifyProblem ( PlanningProblemPtr  pointer)
overridevirtual

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

Parameters
pointerShared pointer to the motion planning problem
Returns
Successful if the problem is a valid UnconstrainedTimeIndexedProblem

Reimplemented from exotica::MotionSolver.

Definition at line 73 of file aico_solver.cpp.

double exotica::AICOSolver::Step ( )
private

Compute one step of the AICO algorithm.

Returns
Change in cost of the trajectory.

Definition at line 474 of file aico_solver.cpp.

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

Updates the backward message at time step $t$.

Parameters
tTime 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\!}} $.

Definition at line 315 of file aico_solver.cpp.

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

Updates the forward message at time step $t$.

Parameters
tTime 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\!} $.

Definition at line 306 of file aico_solver.cpp.

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
tTime step
qhat_tPoint of linearisation at time step $t$
toleranceLazy update tolerance (only update the task message if the state changed more than this value)
max_step_sizeIf 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 340 of file aico_solver.cpp.

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
tTime step.
update_fwdUpdate the forward message.
update_bwdUpdate the backward message.
max_relocation_iterationsMaximum number of relocation while searching for a good linearisation point
toleranceTolerance for for stopping the search.
force_relocationSet to true to force relocation even when the result is within tolerance.
max_step_sizeStep size for UpdateTaskMessage.

Definition at line 387 of file aico_solver.cpp.

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
tTime step.
update_fwdUpdate the forward message.
update_bwdUpdate the backward message.
max_relocation_iterationsMaximum number of relocation while searching for a good linearisation point
toleranceTolerance for for stopping the search.
max_step_sizeStep 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 428 of file aico_solver.cpp.

Member Data Documentation

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

Belief mean.

Definition at line 122 of file aico_solver.h.

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

Belief mean (last most optimal value)

Definition at line 136 of file aico_solver.h.

double exotica::AICOSolver::b_step_ = 0.0
private

Squared configuration space step.

Definition at line 147 of file aico_solver.h.

double exotica::AICOSolver::b_step_old_
private

Definition at line 148 of file aico_solver.h.

int exotica::AICOSolver::best_sweep_ = 0
private

Definition at line 156 of file aico_solver.h.

int exotica::AICOSolver::best_sweep_old_ = 0
private

Definition at line 157 of file aico_solver.h.

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

Belief covariance inverse.

Definition at line 123 of file aico_solver.h.

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

Belief covariance inverse (last most optimal value)

Definition at line 137 of file aico_solver.h.

Eigen::VectorXd exotica::AICOSolver::bwd_msg_v_
private

Backward message initialisation mean.

Definition at line 108 of file aico_solver.h.

Eigen::MatrixXd exotica::AICOSolver::bwd_msg_Vinv_
private

Backward message initialisation covariance.

Definition at line 109 of file aico_solver.h.

double exotica::AICOSolver::cost_ = 0.0
private

cost of MAP trajectory

Definition at line 144 of file aico_solver.h.

Eigen::VectorXd exotica::AICOSolver::cost_control_
private

Control cost for each time step.

Definition at line 126 of file aico_solver.h.

Eigen::VectorXd exotica::AICOSolver::cost_control_old_
private

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

Definition at line 140 of file aico_solver.h.

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

cost of MAP trajectory (last most optimal value)

Definition at line 145 of file aico_solver.h.

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

previous iteration cost

Definition at line 146 of file aico_solver.h.

Eigen::VectorXd exotica::AICOSolver::cost_task_
private

Task cost for each task for each time step.

Definition at line 127 of file aico_solver.h.

Eigen::MatrixXd exotica::AICOSolver::cost_task_old_
private

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

Definition at line 141 of file aico_solver.h.

double exotica::AICOSolver::damping = 0.01
private

Damping.

Definition at line 101 of file aico_solver.h.

double exotica::AICOSolver::damping_init_ = 0.0
private

Damping.

Definition at line 102 of file aico_solver.h.

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

Damping reference point.

Definition at line 143 of file aico_solver.h.

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

Relative function tolerance/first-order optimality criterion.

Definition at line 105 of file aico_solver.h.

int exotica::AICOSolver::iteration_count_
private

Iteration counter.

Definition at line 111 of file aico_solver.h.

int exotica::AICOSolver::last_T_
private

T the last time InitMessages was called.

Definition at line 153 of file aico_solver.h.

int exotica::AICOSolver::max_backtrack_iterations_ = 10
private

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

Definition at line 106 of file aico_solver.h.

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.

Definition at line 103 of file aico_solver.h.

UnconstrainedTimeIndexedProblemPtr exotica::AICOSolver::prob_
private

Shared pointer to the planning problem.

Definition at line 100 of file aico_solver.h.

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

Configuration space trajectory.

Definition at line 124 of file aico_solver.h.

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

Configuration space trajectory (last most optimal value)

Definition at line 138 of file aico_solver.h.

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

Cost weighted normal distribution of configurations across sweeps.

Definition at line 113 of file aico_solver.h.

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

Point of linearisation.

Definition at line 125 of file aico_solver.h.

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

Point of linearisation (last most optimal value)

Definition at line 139 of file aico_solver.h.

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

Task message mean.

Definition at line 119 of file aico_solver.h.

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

Task message covariance.

Definition at line 120 of file aico_solver.h.

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

Task message mean (last most optimal value)

Definition at line 133 of file aico_solver.h.

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

Task message covariance (last most optimal value)

Definition at line 134 of file aico_solver.h.

Eigen::VectorXd exotica::AICOSolver::rhat
private

Task message point of linearisation.

Definition at line 121 of file aico_solver.h.

Eigen::VectorXd exotica::AICOSolver::rhat_old
private

Task message point of linearisation (last most optimal value)

Definition at line 135 of file aico_solver.h.

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

Forward message mean.

Definition at line 115 of file aico_solver.h.

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

Forward message mean (last most optimal value)

Definition at line 129 of file aico_solver.h.

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

Forward message covariance inverse.

Definition at line 116 of file aico_solver.h.

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

Forward message covariance inverse (last most optimal value)

Definition at line 130 of file aico_solver.h.

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

Relative step tolerance (termination criterion)

Definition at line 104 of file aico_solver.h.

int exotica::AICOSolver::sweep_ = 0
private

Sweeps so far.

Definition at line 155 of file aico_solver.h.

bool exotica::AICOSolver::sweep_improved_cost_
private

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

Definition at line 110 of file aico_solver.h.

int exotica::AICOSolver::sweep_mode_ = 0
private

Sweep mode.

Definition at line 165 of file aico_solver.h.

int exotica::AICOSolver::update_count_ = 0
private

Definition at line 166 of file aico_solver.h.

bool exotica::AICOSolver::use_bwd_msg_ = false
private

Flag for using backward message initialisation.

Definition at line 107 of file aico_solver.h.

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

Backward message mean.

Definition at line 117 of file aico_solver.h.

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

Backward message mean (last most optimal value)

Definition at line 131 of file aico_solver.h.

bool exotica::AICOSolver::verbose_ = false
private

Definition at line 168 of file aico_solver.h.

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

Backward message covariance inverse.

Definition at line 118 of file aico_solver.h.

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

Backward message covariance inverse (last most optimal value)

Definition at line 132 of file aico_solver.h.

Eigen::MatrixXd exotica::AICOSolver::W
private

Configuration space weight matrix inverse.

Definition at line 150 of file aico_solver.h.

Eigen::MatrixXd exotica::AICOSolver::Winv
private

Configuration space weight matrix inverse.

Definition at line 151 of file aico_solver.h.


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


exotica_aico_solver
Author(s):
autogenerated on Sat Apr 10 2021 02:35:19