Exotica
aico_solver.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018, University of Edinburgh
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 // This code is based on algorithm developed by Marc Toussaint
31 // M. Toussaint: Robot Trajectory Optimization using Approximate Inference. In Proc. of the Int. Conf. on Machine Learning (ICML 2009), 1049-1056, ACM, 2009.
32 // http://ipvs.informatik.uni-stuttgart.de/mlr/papers/09-toussaint-ICML.pdf
33 // Original code available at http://ipvs.informatik.uni-stuttgart.de/mlr/marc/source-code/index.html
34 
37 
46 
51 
54 
55 #ifndef EXOTICA_AICO_SOLVER_AICO_SOLVER_H_
56 #define EXOTICA_AICO_SOLVER_AICO_SOLVER_H_
57 
58 #include <iostream>
59 
62 
65 
66 #include <exotica_aico_solver/aico_solver_initializer.h>
67 
68 namespace exotica
69 {
72 class AICOSolver : public MotionSolver, public Instantiable<AICOSolverInitializer>
73 {
74 public:
75  AICOSolver();
76  void Instantiate(const AICOSolverInitializer& init) override;
77  virtual ~AICOSolver();
78 
81  void Solve(Eigen::MatrixXd& solution) override;
82 
86  void SpecifyProblem(PlanningProblemPtr pointer) override;
87 
88 protected:
92  void InitMessages();
93 
97  void InitTrajectory(const std::vector<Eigen::VectorXd>& q_init);
98 
99 private:
101  double damping = 0.01;
102  double damping_init_ = 0.0;
103  double minimum_step_tolerance_ = 1e-5;
104  double step_tolerance_ = 1e-5;
105  double function_tolerance_ = 1e-5;
107  bool use_bwd_msg_ = false;
108  Eigen::VectorXd bwd_msg_v_;
109  Eigen::MatrixXd bwd_msg_Vinv_;
112 
113  std::vector<SinglePassMeanCovariance> q_stat_;
114 
115  std::vector<Eigen::VectorXd> s;
116  std::vector<Eigen::MatrixXd> Sinv;
117  std::vector<Eigen::VectorXd> v;
118  std::vector<Eigen::MatrixXd> Vinv;
119  std::vector<Eigen::VectorXd> r;
120  std::vector<Eigen::MatrixXd> R;
121  Eigen::VectorXd rhat;
122  std::vector<Eigen::VectorXd> b;
123  std::vector<Eigen::MatrixXd> Binv;
124  std::vector<Eigen::VectorXd> q;
125  std::vector<Eigen::VectorXd> qhat;
126  Eigen::VectorXd cost_control_;
127  Eigen::VectorXd cost_task_;
128 
129  std::vector<Eigen::VectorXd> s_old;
130  std::vector<Eigen::MatrixXd> Sinv_old;
131  std::vector<Eigen::VectorXd> v_old;
132  std::vector<Eigen::MatrixXd> Vinv_old;
133  std::vector<Eigen::VectorXd> r_old;
134  std::vector<Eigen::MatrixXd> R_old;
135  Eigen::VectorXd rhat_old;
136  std::vector<Eigen::VectorXd> b_old;
137  std::vector<Eigen::MatrixXd> Binv_old;
138  std::vector<Eigen::VectorXd> q_old;
139  std::vector<Eigen::VectorXd> qhat_old;
140  Eigen::VectorXd cost_control_old_;
141  Eigen::MatrixXd cost_task_old_;
142 
143  std::vector<Eigen::VectorXd> damping_reference_;
144  double cost_ = 0.0;
145  double cost_old_ = std::numeric_limits<double>::max();
146  double cost_prev_ = std::numeric_limits<double>::max();
147  double b_step_ = 0.0;
148  double b_step_old_;
149 
150  Eigen::MatrixXd W;
151  Eigen::MatrixXd Winv;
152 
153  int last_T_;
154 
155  int sweep_ = 0;
156  int best_sweep_ = 0;
159  {
160  FORWARD = 0,
164  };
165  int sweep_mode_ = 0;
166  int update_count_ = 0;
167 
168  bool verbose_ = false;
169 
178  void UpdateFwdMessage(int t);
179 
189  void UpdateBwdMessage(int t);
190 
198  void UpdateTaskMessage(int t,
199  const Eigen::Ref<const Eigen::VectorXd>& qhat_t, double tolerance,
200  double max_step_size = -1.);
201 
210  void UpdateTimestep(int t, bool update_fwd, bool update_bwd,
211  int max_relocation_iterations, double tolerance, bool force_relocation,
212  double max_step_size = -1.);
213 
228  void UpdateTimestepGaussNewton(int t, bool update_fwd, bool update_bwd,
229  int max_relocation_iterations, double tolerance, double max_step_size = -1.);
230 
234  double EvaluateTrajectory(const std::vector<Eigen::VectorXd>& x, bool skip_update = false);
235 
237  void RememberOldState();
238 
240  void PerhapsUndoStep();
241 
244  double GetTaskCosts(int t);
245 
248  double Step();
249 };
250 } // namespace exotica
251 
252 #endif // EXOTICA_AICO_SOLVER_AICO_SOLVER_H_
exotica::AICOSolver::cost_task_
Eigen::VectorXd cost_task_
Task cost for each task for each time step.
Definition: aico_solver.h:127
exotica::AICOSolver::cost_control_old_
Eigen::VectorXd cost_control_old_
Control cost for each time step (last most optimal value)
Definition: aico_solver.h:140
exotica::AICOSolver::q
std::vector< Eigen::VectorXd > q
Configuration space trajectory.
Definition: aico_solver.h:124
exotica::AICOSolver::InitTrajectory
void InitTrajectory(const std::vector< Eigen::VectorXd > &q_init)
Initialise AICO messages from an initial trajectory.
exotica::AICOSolver::bwd_msg_Vinv_
Eigen::MatrixXd bwd_msg_Vinv_
Backward message initialisation covariance.
Definition: aico_solver.h:109
exotica::AICOSolver::UpdateTimestepGaussNewton
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.
exotica::AICOSolver::EvaluateTrajectory
double EvaluateTrajectory(const std::vector< Eigen::VectorXd > &x, bool skip_update=false)
Computes the cost of the trajectory.
exotica::AICOSolver::damping_init_
double damping_init_
Damping.
Definition: aico_solver.h:102
exotica::AICOSolver
Solves motion planning problem using Approximate Inference Control method.
Definition: aico_solver.h:72
exotica::AICOSolver::UpdateTaskMessage
void UpdateTaskMessage(int t, const Eigen::Ref< const Eigen::VectorXd > &qhat_t, double tolerance, double max_step_size=-1.)
exotica::AICOSolver::PerhapsUndoStep
void PerhapsUndoStep()
Reverts back to previous state if the cost of the current state is higher.
exotica::AICOSolver::cost_task_old_
Eigen::MatrixXd cost_task_old_
Task cost for each task for each time step (last most optimal value)
Definition: aico_solver.h:141
exotica::AICOSolver::Vinv_old
std::vector< Eigen::MatrixXd > Vinv_old
Backward message covariance inverse (last most optimal value)
Definition: aico_solver.h:132
exotica::AICOSolver::SweepMode
SweepMode
Definition: aico_solver.h:158
exotica::AICOSolver::~AICOSolver
virtual ~AICOSolver()
exotica::AICOSolver::max_backtrack_iterations_
int max_backtrack_iterations_
Max. number of sweeps without improvement before terminating (= line-search)
Definition: aico_solver.h:106
exotica::AICOSolver::cost_control_
Eigen::VectorXd cost_control_
Control cost for each time step.
Definition: aico_solver.h:126
exotica::AICOSolver::R
std::vector< Eigen::MatrixXd > R
Task message covariance.
Definition: aico_solver.h:120
exotica::AICOSolver::Step
double Step()
Compute one step of the AICO algorithm.
exotica::AICOSolver::sweep_improved_cost_
bool sweep_improved_cost_
Whether the last sweep improved the cost (for backtrack iterations count)
Definition: aico_solver.h:110
exotica::AICOSolver::Binv
std::vector< Eigen::MatrixXd > Binv
Belief covariance inverse.
Definition: aico_solver.h:123
exotica::AICOSolver::minimum_step_tolerance_
double minimum_step_tolerance_
Update tolerance to stop update of messages if change of maximum coefficient is less than this tolera...
Definition: aico_solver.h:103
exotica::Instantiable
Definition: property.h:110
exotica
Definition: cartpole_dynamics_solver.h:38
exotica::AICOSolver::W
Eigen::MatrixXd W
Configuration space weight matrix inverse.
Definition: aico_solver.h:150
exotica::AICOSolver::SYMMETRIC
@ SYMMETRIC
Definition: aico_solver.h:161
exotica::AICOSolver::InitMessages
void InitMessages()
Initializes message data.
exotica::AICOSolver::qhat_old
std::vector< Eigen::VectorXd > qhat_old
Point of linearisation (last most optimal value)
Definition: aico_solver.h:139
exotica::AICOSolver::qhat
std::vector< Eigen::VectorXd > qhat
Point of linearisation.
Definition: aico_solver.h:125
exotica::AICOSolver::b_step_
double b_step_
Squared configuration space step.
Definition: aico_solver.h:147
exotica::AICOSolver::UpdateFwdMessage
void UpdateFwdMessage(int t)
Updates the forward message at time step $t$.
exotica::AICOSolver::Sinv_old
std::vector< Eigen::MatrixXd > Sinv_old
Forward message covariance inverse (last most optimal value)
Definition: aico_solver.h:130
exotica::AICOSolver::Binv_old
std::vector< Eigen::MatrixXd > Binv_old
Belief covariance inverse (last most optimal value)
Definition: aico_solver.h:137
exotica::AICOSolver::verbose_
bool verbose_
Definition: aico_solver.h:168
exotica::AICOSolver::cost_old_
double cost_old_
cost of MAP trajectory (last most optimal value)
Definition: aico_solver.h:145
unconstrained_time_indexed_problem.h
exotica::AICOSolver::Instantiate
void Instantiate(const AICOSolverInitializer &init) override
exotica::AICOSolver::q_old
std::vector< Eigen::VectorXd > q_old
Configuration space trajectory (last most optimal value)
Definition: aico_solver.h:138
exotica::AICOSolver::best_sweep_
int best_sweep_
Definition: aico_solver.h:156
exotica::AICOSolver::AICOSolver
AICOSolver()
exotica::AICOSolver::sweep_
int sweep_
Sweeps so far.
Definition: aico_solver.h:155
exotica::AICOSolver::rhat_old
Eigen::VectorXd rhat_old
Task message point of linearisation (last most optimal value)
Definition: aico_solver.h:135
exotica::AICOSolver::SpecifyProblem
void SpecifyProblem(PlanningProblemPtr pointer) override
Binds the solver to a specific problem which must be pre-initalised.
exotica::AICOSolver::Vinv
std::vector< Eigen::MatrixXd > Vinv
Backward message covariance inverse.
Definition: aico_solver.h:118
exotica::AICOSolver::v_old
std::vector< Eigen::VectorXd > v_old
Backward message mean (last most optimal value)
Definition: aico_solver.h:131
exotica::AICOSolver::bwd_msg_v_
Eigen::VectorXd bwd_msg_v_
Backward message initialisation mean.
Definition: aico_solver.h:108
incremental_gaussian.h
exotica::AICOSolver::cost_
double cost_
cost of MAP trajectory
Definition: aico_solver.h:144
exotica::AICOSolver::last_T_
int last_T_
T the last time InitMessages was called.
Definition: aico_solver.h:153
exotica::UnconstrainedTimeIndexedProblemPtr
std::shared_ptr< exotica::UnconstrainedTimeIndexedProblem > UnconstrainedTimeIndexedProblemPtr
Definition: unconstrained_time_indexed_problem.h:98
exotica::AICOSolver::b
std::vector< Eigen::VectorXd > b
Belief mean.
Definition: aico_solver.h:122
exotica::AICOSolver::s
std::vector< Eigen::VectorXd > s
Forward message mean.
Definition: aico_solver.h:115
exotica::AICOSolver::iteration_count_
int iteration_count_
Iteration counter.
Definition: aico_solver.h:111
motion_solver.h
exotica::AICOSolver::cost_prev_
double cost_prev_
previous iteration cost
Definition: aico_solver.h:146
exotica::AICOSolver::Solve
void Solve(Eigen::MatrixXd &solution) override
Solves the problem.
exotica::AICOSolver::R_old
std::vector< Eigen::MatrixXd > R_old
Task message covariance (last most optimal value)
Definition: aico_solver.h:134
exotica::AICOSolver::prob_
UnconstrainedTimeIndexedProblemPtr prob_
Shared pointer to the planning problem.
Definition: aico_solver.h:100
exotica::AICOSolver::sweep_mode_
int sweep_mode_
Sweep mode.
Definition: aico_solver.h:165
exotica::AICOSolver::b_step_old_
double b_step_old_
Definition: aico_solver.h:148
exotica::MotionSolver
Definition: motion_solver.h:42
exotica::PlanningProblemPtr
std::shared_ptr< PlanningProblem > PlanningProblemPtr
Definition: planning_problem.h:116
exotica::AICOSolver::v
std::vector< Eigen::VectorXd > v
Backward message mean.
Definition: aico_solver.h:117
exotica::AICOSolver::update_count_
int update_count_
Definition: aico_solver.h:166
exotica::AICOSolver::Winv
Eigen::MatrixXd Winv
Configuration space weight matrix inverse.
Definition: aico_solver.h:151
exotica::AICOSolver::UpdateTimestep
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.
exotica::AICOSolver::LOCAL_GAUSS_NEWTON_DAMPED
@ LOCAL_GAUSS_NEWTON_DAMPED
Definition: aico_solver.h:163
exotica::AICOSolver::GetTaskCosts
double GetTaskCosts(int t)
Updates the task cost terms for time step . UnconstrainedTimeIndexedProblem::Update() has to be call...
exotica::AICOSolver::b_old
std::vector< Eigen::VectorXd > b_old
Belief mean (last most optimal value)
Definition: aico_solver.h:136
exotica::AICOSolver::r
std::vector< Eigen::VectorXd > r
Task message mean.
Definition: aico_solver.h:119
exotica::AICOSolver::rhat
Eigen::VectorXd rhat
Task message point of linearisation.
Definition: aico_solver.h:121
exotica::AICOSolver::q_stat_
std::vector< SinglePassMeanCovariance > q_stat_
Cost weighted normal distribution of configurations across sweeps.
Definition: aico_solver.h:113
exotica::AICOSolver::function_tolerance_
double function_tolerance_
Relative function tolerance/first-order optimality criterion.
Definition: aico_solver.h:105
exotica::AICOSolver::RememberOldState
void RememberOldState()
Stores the previous state.
exotica::AICOSolver::damping
double damping
Damping.
Definition: aico_solver.h:101
exotica::AICOSolver::s_old
std::vector< Eigen::VectorXd > s_old
Forward message mean (last most optimal value)
Definition: aico_solver.h:129
exotica::AICOSolver::use_bwd_msg_
bool use_bwd_msg_
Flag for using backward message initialisation.
Definition: aico_solver.h:107
exotica::AICOSolver::r_old
std::vector< Eigen::VectorXd > r_old
Task message mean (last most optimal value)
Definition: aico_solver.h:133
exotica::AICOSolver::LOCAL_GAUSS_NEWTON
@ LOCAL_GAUSS_NEWTON
Definition: aico_solver.h:162
exotica::AICOSolver::Sinv
std::vector< Eigen::MatrixXd > Sinv
Forward message covariance inverse.
Definition: aico_solver.h:116
exotica::AICOSolver::FORWARD
@ FORWARD
Definition: aico_solver.h:160
exotica::AICOSolver::best_sweep_old_
int best_sweep_old_
Definition: aico_solver.h:157
exotica::AICOSolver::step_tolerance_
double step_tolerance_
Relative step tolerance (termination criterion)
Definition: aico_solver.h:104
exotica::AICOSolver::damping_reference_
std::vector< Eigen::VectorXd > damping_reference_
Damping reference point.
Definition: aico_solver.h:143
exotica::AICOSolver::UpdateBwdMessage
void UpdateBwdMessage(int t)
Updates the backward message at time step $t$.
math_operations.h