Exotica
bayesian_ik_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 #ifndef EXOTICA_AICO_SOLVER_BAYESIAN_IK_SOLVER_H_
31 #define EXOTICA_AICO_SOLVER_BAYESIAN_IK_SOLVER_H_
32 
33 #include <iostream>
34 
37 
40 
41 #include <exotica_aico_solver/bayesian_ik_solver_initializer.h>
42 
43 namespace exotica
44 {
47 class BayesianIKSolver : public MotionSolver, public Instantiable<BayesianIKSolverInitializer>
48 {
49 public:
50  void Instantiate(const BayesianIKSolverInitializer& init) override;
51 
54  void Solve(Eigen::MatrixXd& solution) override;
55 
59  void SpecifyProblem(PlanningProblemPtr pointer) override;
60 
61 protected:
65  void InitMessages();
66 
70  void InitTrajectory(const Eigen::VectorXd& q_init);
71 
72 private:
74  double damping = 0.01;
75  double damping_init_ = 100.0;
76  double minimum_step_tolerance_ = 1e-5;
77  double step_tolerance_ = 1e-5;
78  double function_tolerance_ = 1e-5;
80  bool use_bwd_msg_ = false;
81  Eigen::VectorXd bwd_msg_v_;
82  Eigen::MatrixXd bwd_msg_Vinv_;
85 
86  Eigen::VectorXd s;
87  Eigen::MatrixXd Sinv;
88  Eigen::VectorXd v;
89  Eigen::MatrixXd Vinv;
90  Eigen::VectorXd r;
91  Eigen::MatrixXd R;
92  double rhat;
93  Eigen::VectorXd b;
94  Eigen::MatrixXd Binv;
95  Eigen::VectorXd q;
96  Eigen::VectorXd qhat;
97 
98  Eigen::VectorXd s_old;
99  Eigen::MatrixXd Sinv_old;
100  Eigen::VectorXd v_old;
101  Eigen::MatrixXd Vinv_old;
102  Eigen::VectorXd r_old;
103  Eigen::MatrixXd R_old;
104  double rhat_old;
105  Eigen::VectorXd b_old;
106  Eigen::MatrixXd Binv_old;
107  Eigen::VectorXd q_old;
108  Eigen::VectorXd qhat_old;
109 
110  Eigen::VectorXd damping_reference_;
111  double cost_ = 0.0;
112  double cost_old_ = std::numeric_limits<double>::max();
113  double cost_prev_ = std::numeric_limits<double>::max();
114  double b_step_ = 0.0;
115  double b_step_old_;
116 
117  Eigen::MatrixXd W;
118  Eigen::MatrixXd Winv;
119 
120  int sweep_ = 0;
121  int best_sweep_ = 0;
124  {
125  FORWARD = 0,
129  };
130  int sweep_mode_ = 0;
131  int update_count_ = 0;
132 
133  bool verbose_ = false;
134 
142  void UpdateFwdMessage();
143 
151  void UpdateBwdMessage();
152 
159  void UpdateTaskMessage(const Eigen::Ref<const Eigen::VectorXd>& qhat_t, double tolerance,
160  double max_step_size = -1.);
161 
170  void UpdateTimestep(bool update_fwd, bool update_bwd,
171  int max_relocation_iterations, double tolerance, bool force_relocation,
172  double max_step_size = -1.);
173 
188  void UpdateTimestepGaussNewton(bool update_fwd, bool update_bwd,
189  int max_relocation_iterations, double tolerance, double max_step_size = -1.);
193  double EvaluateTrajectory(const Eigen::VectorXd& x, bool skip_update = false);
194 
196  void RememberOldState();
197 
199  void PerhapsUndoStep();
200 
202  void GetTaskCosts();
203 
206  double Step();
207 };
208 } // namespace exotica
209 
210 #endif // EXOTICA_AICO_SOLVER_BAYESIAN_IK_SOLVER_H_
exotica::BayesianIKSolver::prob_
UnconstrainedEndPoseProblemPtr prob_
Shared pointer to the planning problem.
Definition: bayesian_ik_solver.h:73
exotica::BayesianIKSolver::Step
double Step()
Compute one step of the AICO algorithm.
exotica::BayesianIKSolver::LOCAL_GAUSS_NEWTON_DAMPED
@ LOCAL_GAUSS_NEWTON_DAMPED
Definition: bayesian_ik_solver.h:128
exotica::BayesianIKSolver::sweep_improved_cost_
bool sweep_improved_cost_
Whether the last sweep improved the cost (for backtrack iterations count)
Definition: bayesian_ik_solver.h:83
exotica::BayesianIKSolver::max_backtrack_iterations_
int max_backtrack_iterations_
Max. number of sweeps without improvement before terminating (= line-search)
Definition: bayesian_ik_solver.h:79
exotica::BayesianIKSolver::qhat_old
Eigen::VectorXd qhat_old
Point of linearisation (last most optimal value)
Definition: bayesian_ik_solver.h:108
exotica::BayesianIKSolver::SweepMode
SweepMode
Definition: bayesian_ik_solver.h:123
exotica::BayesianIKSolver::function_tolerance_
double function_tolerance_
Relative function tolerance/first-order optimality criterion.
Definition: bayesian_ik_solver.h:78
exotica::BayesianIKSolver::b_old
Eigen::VectorXd b_old
Belief mean (last most optimal value)
Definition: bayesian_ik_solver.h:105
exotica::BayesianIKSolver::Binv
Eigen::MatrixXd Binv
Belief covariance inverse.
Definition: bayesian_ik_solver.h:94
exotica::BayesianIKSolver::UpdateTimestep
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.
exotica::BayesianIKSolver::RememberOldState
void RememberOldState()
Stores the previous state.
exotica::BayesianIKSolver::q
Eigen::VectorXd q
Configuration space trajectory.
Definition: bayesian_ik_solver.h:95
exotica::BayesianIKSolver::b_step_old_
double b_step_old_
Definition: bayesian_ik_solver.h:115
exotica::BayesianIKSolver::W
Eigen::MatrixXd W
Configuration space weight matrix inverse.
Definition: bayesian_ik_solver.h:117
exotica::BayesianIKSolver::Instantiate
void Instantiate(const BayesianIKSolverInitializer &init) override
exotica::BayesianIKSolver::qhat
Eigen::VectorXd qhat
Point of linearisation.
Definition: bayesian_ik_solver.h:96
exotica::BayesianIKSolver::Winv
Eigen::MatrixXd Winv
Configuration space weight matrix inverse.
Definition: bayesian_ik_solver.h:118
exotica::BayesianIKSolver::EvaluateTrajectory
double EvaluateTrajectory(const Eigen::VectorXd &x, bool skip_update=false)
Computes the cost of the trajectory.
exotica::BayesianIKSolver::GetTaskCosts
void GetTaskCosts()
Updates the task cost terms . UnconstrainedEndPoseProblem::Update() has to be called before calling t...
unconstrained_end_pose_problem.h
exotica::BayesianIKSolver::UpdateFwdMessage
void UpdateFwdMessage()
Updates the forward message Updates the mean and covariance of the forward message using: ,...
exotica::BayesianIKSolver::sweep_mode_
int sweep_mode_
Sweep mode.
Definition: bayesian_ik_solver.h:130
exotica::BayesianIKSolver::Sinv_old
Eigen::MatrixXd Sinv_old
Forward message covariance inverse (last most optimal value)
Definition: bayesian_ik_solver.h:99
exotica::Instantiable
Definition: property.h:110
exotica::BayesianIKSolver::b
Eigen::VectorXd b
Belief mean.
Definition: bayesian_ik_solver.h:93
exotica
Definition: cartpole_dynamics_solver.h:38
exotica::BayesianIKSolver::r
Eigen::VectorXd r
Task message mean.
Definition: bayesian_ik_solver.h:90
exotica::BayesianIKSolver::SpecifyProblem
void SpecifyProblem(PlanningProblemPtr pointer) override
Binds the solver to a specific problem which must be pre-initalised.
exotica::BayesianIKSolver::InitMessages
void InitMessages()
Initializes message data.
exotica::BayesianIKSolver::s
Eigen::VectorXd s
Forward message mean.
Definition: bayesian_ik_solver.h:86
exotica::BayesianIKSolver::v
Eigen::VectorXd v
Backward message mean.
Definition: bayesian_ik_solver.h:88
exotica::BayesianIKSolver::Vinv_old
Eigen::MatrixXd Vinv_old
Backward message covariance inverse (last most optimal value)
Definition: bayesian_ik_solver.h:101
exotica::BayesianIKSolver::Vinv
Eigen::MatrixXd Vinv
Backward message covariance inverse.
Definition: bayesian_ik_solver.h:89
exotica::BayesianIKSolver
Solves motion planning problem using Approximate Inference Control method.
Definition: bayesian_ik_solver.h:47
exotica::BayesianIKSolver::cost_old_
double cost_old_
cost of MAP trajectory (last most optimal value)
Definition: bayesian_ik_solver.h:112
exotica::BayesianIKSolver::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: bayesian_ik_solver.h:76
exotica::UnconstrainedEndPoseProblemPtr
std::shared_ptr< exotica::UnconstrainedEndPoseProblem > UnconstrainedEndPoseProblemPtr
Definition: unconstrained_end_pose_problem.h:81
exotica::BayesianIKSolver::iteration_count_
int iteration_count_
Iteration counter.
Definition: bayesian_ik_solver.h:84
exotica::BayesianIKSolver::UpdateTimestepGaussNewton
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.
exotica::BayesianIKSolver::damping_init_
double damping_init_
Damping.
Definition: bayesian_ik_solver.h:75
exotica::BayesianIKSolver::Sinv
Eigen::MatrixXd Sinv
Forward message covariance inverse.
Definition: bayesian_ik_solver.h:87
incremental_gaussian.h
exotica::BayesianIKSolver::cost_
double cost_
cost of MAP trajectory
Definition: bayesian_ik_solver.h:111
exotica::BayesianIKSolver::InitTrajectory
void InitTrajectory(const Eigen::VectorXd &q_init)
Initialise AICO messages from an initial trajectory.
exotica::BayesianIKSolver::verbose_
bool verbose_
Definition: bayesian_ik_solver.h:133
exotica::BayesianIKSolver::step_tolerance_
double step_tolerance_
Relative step tolerance (termination criterion)
Definition: bayesian_ik_solver.h:77
exotica::BayesianIKSolver::r_old
Eigen::VectorXd r_old
Task message mean (last most optimal value)
Definition: bayesian_ik_solver.h:102
motion_solver.h
exotica::BayesianIKSolver::PerhapsUndoStep
void PerhapsUndoStep()
Reverts back to previous state if the cost of the current state is higher.
exotica::MotionSolver
Definition: motion_solver.h:42
exotica::BayesianIKSolver::best_sweep_
int best_sweep_
Definition: bayesian_ik_solver.h:121
exotica::BayesianIKSolver::s_old
Eigen::VectorXd s_old
Forward message mean (last most optimal value)
Definition: bayesian_ik_solver.h:98
exotica::PlanningProblemPtr
std::shared_ptr< PlanningProblem > PlanningProblemPtr
Definition: planning_problem.h:116
exotica::BayesianIKSolver::use_bwd_msg_
bool use_bwd_msg_
Flag for using backward message initialisation.
Definition: bayesian_ik_solver.h:80
exotica::BayesianIKSolver::rhat_old
double rhat_old
Task message point of linearisation (last most optimal value)
Definition: bayesian_ik_solver.h:104
exotica::BayesianIKSolver::UpdateTaskMessage
void UpdateTaskMessage(const Eigen::Ref< const Eigen::VectorXd > &qhat_t, double tolerance, double max_step_size=-1.)
Updates the task message.
exotica::BayesianIKSolver::SYMMETRIC
@ SYMMETRIC
Definition: bayesian_ik_solver.h:126
exotica::BayesianIKSolver::sweep_
int sweep_
Sweeps so far.
Definition: bayesian_ik_solver.h:120
exotica::BayesianIKSolver::v_old
Eigen::VectorXd v_old
Backward message mean (last most optimal value)
Definition: bayesian_ik_solver.h:100
exotica::BayesianIKSolver::damping
double damping
Damping.
Definition: bayesian_ik_solver.h:74
exotica::BayesianIKSolver::R
Eigen::MatrixXd R
Task message covariance.
Definition: bayesian_ik_solver.h:91
exotica::BayesianIKSolver::b_step_
double b_step_
Squared configuration space step.
Definition: bayesian_ik_solver.h:114
exotica::BayesianIKSolver::R_old
Eigen::MatrixXd R_old
Task message covariance (last most optimal value)
Definition: bayesian_ik_solver.h:103
exotica::BayesianIKSolver::LOCAL_GAUSS_NEWTON
@ LOCAL_GAUSS_NEWTON
Definition: bayesian_ik_solver.h:127
exotica::BayesianIKSolver::rhat
double rhat
Task message point of linearisation.
Definition: bayesian_ik_solver.h:92
exotica::BayesianIKSolver::bwd_msg_v_
Eigen::VectorXd bwd_msg_v_
Backward message initialisation mean.
Definition: bayesian_ik_solver.h:81
exotica::BayesianIKSolver::damping_reference_
Eigen::VectorXd damping_reference_
Damping reference point.
Definition: bayesian_ik_solver.h:110
exotica::BayesianIKSolver::Binv_old
Eigen::MatrixXd Binv_old
Belief covariance inverse (last most optimal value)
Definition: bayesian_ik_solver.h:106
exotica::BayesianIKSolver::bwd_msg_Vinv_
Eigen::MatrixXd bwd_msg_Vinv_
Backward message initialisation covariance.
Definition: bayesian_ik_solver.h:82
exotica::BayesianIKSolver::q_old
Eigen::VectorXd q_old
Configuration space trajectory (last most optimal value)
Definition: bayesian_ik_solver.h:107
exotica::BayesianIKSolver::best_sweep_old_
int best_sweep_old_
Definition: bayesian_ik_solver.h:122
exotica::BayesianIKSolver::Solve
void Solve(Eigen::MatrixXd &solution) override
Solves the problem.
exotica::BayesianIKSolver::cost_prev_
double cost_prev_
previous iteration cost
Definition: bayesian_ik_solver.h:113
exotica::BayesianIKSolver::update_count_
int update_count_
Definition: bayesian_ik_solver.h:131
exotica::BayesianIKSolver::UpdateBwdMessage
void UpdateBwdMessage()
Updates the backward message Updates the mean and covariance of the backward message using: ,...
math_operations.h
exotica::BayesianIKSolver::FORWARD
@ FORWARD
Definition: bayesian_ik_solver.h:125