Exotica
planning_problem.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2020, University of Edinburgh, University of Oxford
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_CORE_PLANNING_PROBLEM_H_
31 #define EXOTICA_CORE_PLANNING_PROBLEM_H_
32 
33 #include <chrono>
34 #include <map>
35 #include <string>
36 #include <vector>
37 
38 #include <exotica_core/object.h>
39 #include <exotica_core/scene.h>
40 #include <exotica_core/task_map.h>
44 
45 #define REGISTER_PROBLEM_TYPE(TYPE, DERIV) EXOTICA_CORE_REGISTER_CORE(exotica::PlanningProblem, TYPE, DERIV)
46 
47 namespace exotica
48 {
50 {
51  NotStarted = -1,
52  // Continue = 0,
58  Divergence,
61  // Condition,
62 };
63 
64 class PlanningProblem : public Object, Uncopyable, public virtual InstantiableBase, public std::enable_shared_from_this<PlanningProblem>
65 {
66 public:
68  virtual ~PlanningProblem();
69 
70  void InstantiateBase(const Initializer& init) override;
73  ScenePtr GetScene() const;
74  std::string Print(const std::string& prepend) const override;
75 
77  Eigen::VectorXd GetStartState() const;
78  virtual Eigen::VectorXd ApplyStartState(bool update_traj = true);
79 
80  void SetStartTime(double t);
81  double GetStartTime() const;
82 
83  virtual void PreUpdate();
84  unsigned int GetNumberOfProblemUpdates() const { return number_of_problem_updates_; }
86  std::pair<std::vector<double>, std::vector<double>> GetCostEvolution() const;
87  int GetNumberOfIterations() const;
88  double GetCostEvolution(int index) const;
89  void ResetCostEvolution(size_t size);
90  void SetCostEvolution(int index, double value);
93  virtual bool IsValid() { ThrowNamed("Not implemented"); };
94  double t_start;
96 
97  int N = 0;
98  [[deprecated("Replaced by Scene::get_num_positions")]] int get_num_positions() const;
99  [[deprecated("Replaced by Scene::get_num_velocities")]] int get_num_velocities() const;
100  [[deprecated("Replaced by Scene::get_num_controls")]] int get_num_controls() const;
101 
102 protected:
103  void UpdateTaskKinematics(std::shared_ptr<KinematicResponse> response);
104  void UpdateMultipleTaskKinematics(std::vector<std::shared_ptr<KinematicResponse>> responses);
105 
110  Eigen::VectorXd start_state_;
111  unsigned int number_of_problem_updates_ = 0; // Stores number of times the problem has been updated
112  std::vector<std::pair<std::chrono::high_resolution_clock::time_point, double>> cost_evolution_;
113 };
114 
116 typedef std::shared_ptr<PlanningProblem> PlanningProblemPtr;
117 typedef std::shared_ptr<const PlanningProblem> PlanningProblemConstPtr;
118 } // namespace exotica
119 
120 #endif // EXOTICA_CORE_PLANNING_PROBLEM_H_
exotica::TerminationCriterion::NotStarted
@ NotStarted
exotica::TerminationCriterion::Divergence
@ Divergence
exotica::PlanningProblem::SetCostEvolution
void SetCostEvolution(int index, double value)
exotica::PlanningProblem::number_of_problem_updates_
unsigned int number_of_problem_updates_
Definition: planning_problem.h:111
exotica::TerminationCriterion::GradientTolerance
@ GradientTolerance
exotica::TerminationCriterion
TerminationCriterion
Definition: planning_problem.h:49
exotica::PlanningProblem::PreUpdate
virtual void PreUpdate()
exotica::PlanningProblem::SetStartTime
void SetStartTime(double t)
exotica::PlanningProblem::flags_
KinematicRequestFlags flags_
Definition: planning_problem.h:109
exotica::PlanningProblem::InstantiateBase
void InstantiateBase(const Initializer &init) override
exotica::Uncopyable
Definition: uncopyable.h:35
exotica::TaskMapVec
std::vector< TaskMapPtr > TaskMapVec
Definition: task_map.h:94
exotica::PlanningProblem::t_start
double t_start
Definition: planning_problem.h:93
exotica::PlanningProblemFac
Factory< PlanningProblem > PlanningProblemFac
Definition: planning_problem.h:115
exotica::TerminationCriterion::Convergence
@ Convergence
exotica::PlanningProblem::SetStartState
void SetStartState(Eigen::VectorXdRefConst x)
exotica::PlanningProblem::cost_evolution_
std::vector< std::pair< std::chrono::high_resolution_clock::time_point, double > > cost_evolution_
Definition: planning_problem.h:112
exotica::PlanningProblem::ResetCostEvolution
void ResetCostEvolution(size_t size)
exotica::PlanningProblem::GetFlags
KinematicRequestFlags GetFlags() const
Definition: planning_problem.h:91
task_space_vector.h
exotica::PlanningProblem::get_num_controls
int get_num_controls() const
exotica::PlanningProblem::UpdateMultipleTaskKinematics
void UpdateMultipleTaskKinematics(std::vector< std::shared_ptr< KinematicResponse >> responses)
exotica::InstantiableBase
Definition: property.h:98
exotica::TerminationCriterion::BacktrackIterationLimit
@ BacktrackIterationLimit
exotica::PlanningProblem::start_state_
Eigen::VectorXd start_state_
Definition: planning_problem.h:110
exotica::Factory< PlanningProblem >
exotica::PlanningProblem::N
int N
Definition: planning_problem.h:97
exotica::PlanningProblem::GetTasks
TaskMapVec & GetTasks()
exotica::PlanningProblem::termination_criterion
TerminationCriterion termination_criterion
Definition: planning_problem.h:95
exotica
Definition: cartpole_dynamics_solver.h:38
conversions.h
exotica::PlanningProblem::GetNumberOfProblemUpdates
unsigned int GetNumberOfProblemUpdates() const
Definition: planning_problem.h:84
exotica::PlanningProblem::tasks_
TaskMapVec tasks_
Definition: planning_problem.h:108
exotica::PlanningProblem::~PlanningProblem
virtual ~PlanningProblem()
exotica::PlanningProblem::ResetNumberOfProblemUpdates
void ResetNumberOfProblemUpdates()
Definition: planning_problem.h:85
exotica::KIN_FK
@ KIN_FK
Definition: kinematic_tree.h:58
scene.h
exotica::PlanningProblem::scene_
ScenePtr scene_
Definition: planning_problem.h:106
exotica::TerminationCriterion::StepTolerance
@ StepTolerance
exotica::PlanningProblem::PlanningProblem
PlanningProblem()
exotica::PlanningProblem::ApplyStartState
virtual Eigen::VectorXd ApplyStartState(bool update_traj=true)
exotica::PlanningProblem::Print
std::string Print(const std::string &prepend) const override
exotica::ScenePtr
std::shared_ptr< Scene > ScenePtr
Definition: scene.h:246
exotica::Object
Definition: object.h:44
Eigen::VectorXdRefConst
const typedef Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
Definition: conversions.h:52
exotica::KinematicRequestFlags
KinematicRequestFlags
Definition: kinematic_tree.h:56
exotica::TerminationCriterion::IterationLimit
@ IterationLimit
exotica::PlanningProblem::get_num_positions
int get_num_positions() const
! Dimension of planning problem. TODO: Update from positions/velocities/controls and make private.
exotica::Initializer
Definition: property.h:70
exotica::PlanningProblem::GetStartState
Eigen::VectorXd GetStartState() const
exotica::PlanningProblem::GetCostEvolution
std::pair< std::vector< double >, std::vector< double > > GetCostEvolution() const
exotica::PlanningProblem::UpdateTaskKinematics
void UpdateTaskKinematics(std::shared_ptr< KinematicResponse > response)
exotica::TerminationCriterion::FunctionTolerance
@ FunctionTolerance
exotica::PlanningProblemPtr
std::shared_ptr< PlanningProblem > PlanningProblemPtr
Definition: planning_problem.h:116
exotica::PlanningProblemConstPtr
std::shared_ptr< const PlanningProblem > PlanningProblemConstPtr
Definition: planning_problem.h:117
exotica::PlanningProblem::GetStartTime
double GetStartTime() const
exotica::PlanningProblem::GetNumberOfIterations
int GetNumberOfIterations() const
exotica::PlanningProblem
Definition: planning_problem.h:64
exotica::PlanningProblem::task_maps_
TaskMapMap task_maps_
Definition: planning_problem.h:107
exotica::TaskMapMap
std::map< std::string, TaskMapPtr > TaskMapMap
The mapping by name of TaskMaps.
Definition: task_map.h:93
exotica::TerminationCriterion::UserDefined
@ UserDefined
exotica::PlanningProblem::get_num_velocities
int get_num_velocities() const
task_map.h
uncopyable.h
exotica::PlanningProblem::IsValid
virtual bool IsValid()
Evaluates whether the problem is valid.
Definition: planning_problem.h:93
exotica::PlanningProblem::GetTaskMaps
TaskMapMap & GetTaskMaps()
exotica::PlanningProblem::GetScene
ScenePtr GetScene() const
object.h
ThrowNamed
#define ThrowNamed(m)
Definition: exception.h:42