Exotica
tasks.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_TASKS_H_
31 #define EXOTICA_CORE_TASKS_H_
32 
33 #include <Eigen/Dense>
34 #include <map>
35 #include <memory>
36 #include <string>
37 #include <vector>
38 
40 #include <exotica_core/property.h>
43 
44 namespace exotica
45 {
47 {
48  int id;
49  int start;
50  int length;
53 };
54 
55 struct Task
56 {
57  Task() = default;
58  virtual ~Task() = default;
59 
60  virtual void Initialize(const std::vector<exotica::Initializer>& inits, std::shared_ptr<PlanningProblem> prob, TaskSpaceVector& Phi);
61 
64  std::vector<TaskIndexing> indexing;
65 
68  int num_tasks;
69  double tolerance = 0.0; // To avoid numerical issues consider all values below this as 0.0.
70 
71 protected:
72  std::vector<TaskInitializer> task_initializers_;
73 };
74 
75 struct TimeIndexedTask : public Task
76 {
77  TimeIndexedTask() = default;
78  virtual ~TimeIndexedTask() = default;
79 
80  virtual void Initialize(const std::vector<exotica::Initializer>& inits, std::shared_ptr<PlanningProblem> prob, TaskSpaceVector& Phi);
81  void UpdateS();
82 
83  void Update(const TaskSpaceVector& big_Phi,
84  Eigen::MatrixXdRefConst big_dPhi_dx,
85  Eigen::MatrixXdRefConst big_dPhi_du,
86  HessianRefConst big_ddPhi_ddx,
87  HessianRefConst big_ddPhi_ddu,
88  HessianRefConst big_ddPhi_dxdu,
89  int t);
90  void Update(const TaskSpaceVector& big_Phi,
91  Eigen::MatrixXdRefConst big_dPhi_dx,
92  Eigen::MatrixXdRefConst big_dPhi_du,
93  int t);
94 
95  void Update(const TaskSpaceVector& big_Phi, Eigen::MatrixXdRefConst big_jacobian, HessianRefConst big_hessian, int t);
96  void Update(const TaskSpaceVector& big_Phi, Eigen::MatrixXdRefConst big_jacobian, int t);
97  void Update(const TaskSpaceVector& big_Phi, int t);
98 
99  void ReinitializeVariables(int _T, std::shared_ptr<PlanningProblem> _prob, const TaskSpaceVector& _Phi);
100 
101  inline void ValidateTimeIndex(int& t_in) const;
102 
103  void SetGoal(const std::string& task_name, Eigen::VectorXdRefConst goal, int t);
104  Eigen::VectorXd GetGoal(const std::string& task_name, int t) const;
105 
106  void SetRho(const std::string& task_name, const double rho_in, int t);
107  double GetRho(const std::string& task_name, int t) const;
108 
109  Eigen::VectorXd GetTaskError(const std::string& task_name, int t) const;
110  Eigen::MatrixXd GetS(const std::string& task_name, int t) const;
111 
112  std::vector<Eigen::VectorXd> rho;
113  std::vector<TaskSpaceVector> y;
114  std::vector<Eigen::VectorXd> ydiff;
115  std::vector<TaskSpaceVector> Phi;
116  std::vector<Hessian> hessian;
117  std::vector<Hessian> ddPhi_ddx;
118  std::vector<Hessian> ddPhi_ddu;
119  std::vector<Hessian> ddPhi_dxdu;
120  std::vector<Eigen::MatrixXd> jacobian;
121  std::vector<Eigen::MatrixXd> dPhi_dx;
122  std::vector<Eigen::MatrixXd> dPhi_du;
123  std::vector<Eigen::MatrixXd> S;
124  int T;
125 };
126 
127 struct EndPoseTask : public Task
128 {
129  EndPoseTask() = default;
130  virtual ~EndPoseTask() = default;
131 
132  virtual void Initialize(const std::vector<exotica::Initializer>& inits, std::shared_ptr<PlanningProblem> prob, TaskSpaceVector& Phi);
133  void UpdateS();
134  void Update(const TaskSpaceVector& big_Phi, Eigen::MatrixXdRefConst big_jacobian, HessianRefConst big_hessian);
135  void Update(const TaskSpaceVector& big_Phi, Eigen::MatrixXdRefConst big_jacobian);
136  void Update(const TaskSpaceVector& big_Phi);
137 
138  void SetGoal(const std::string& task_name, Eigen::VectorXdRefConst goal);
139  Eigen::VectorXd GetGoal(const std::string& task_name) const;
140 
141  void SetRho(const std::string& task_name, const double rho_in);
142  double GetRho(const std::string& task_name) const;
143 
144  Eigen::MatrixXd GetS(const std::string& task_name) const;
145  Eigen::VectorXd GetTaskError(const std::string& task_name) const;
146  Eigen::MatrixXd GetTaskJacobian(const std::string& task_name) const;
147 
148  Eigen::VectorXd rho;
150  Eigen::VectorXd ydiff;
152  Eigen::MatrixXd jacobian;
154  Eigen::MatrixXd S;
155 };
156 
157 struct SamplingTask : public Task
158 {
159  SamplingTask() = default;
160  virtual ~SamplingTask() = default;
161 
162  virtual void Initialize(const std::vector<exotica::Initializer>& inits, std::shared_ptr<PlanningProblem> prob, TaskSpaceVector& Phi);
163  void UpdateS();
164  void Update(const TaskSpaceVector& big_Phi);
165 
166  void SetGoal(const std::string& task_name, Eigen::VectorXdRefConst goal);
167  Eigen::VectorXd GetGoal(const std::string& task_name) const;
168 
169  void SetRho(const std::string& task_name, const double rho);
170  double GetRho(const std::string& task_name) const;
171 
172  Eigen::VectorXd rho;
174  Eigen::VectorXd ydiff;
176  Eigen::MatrixXd S;
177 };
178 } // namespace exotica
179 
180 #endif // EXOTICA_CORE_TASKS_H_
exotica::SamplingTask::~SamplingTask
virtual ~SamplingTask()=default
exotica::Task
Definition: tasks.h:55
exotica::TaskIndexing::id
int id
Definition: tasks.h:48
exotica::EndPoseTask::EndPoseTask
EndPoseTask()=default
exotica::TimeIndexedTask::ReinitializeVariables
void ReinitializeVariables(int _T, std::shared_ptr< PlanningProblem > _prob, const TaskSpaceVector &_Phi)
exotica::SamplingTask::SamplingTask
SamplingTask()=default
exotica::TimeIndexedTask::ddPhi_ddu
std::vector< Hessian > ddPhi_ddu
Definition: tasks.h:118
exotica::EndPoseTask::Update
void Update(const TaskSpaceVector &big_Phi, Eigen::MatrixXdRefConst big_jacobian, HessianRefConst big_hessian)
exotica::Task::Task
Task()=default
exotica::EndPoseTask::SetGoal
void SetGoal(const std::string &task_name, Eigen::VectorXdRefConst goal)
exotica::EndPoseTask::rho
Eigen::VectorXd rho
Definition: tasks.h:148
exotica::EndPoseTask::GetRho
double GetRho(const std::string &task_name) const
exotica::EndPoseTask::S
Eigen::MatrixXd S
Definition: tasks.h:154
exotica::TimeIndexedTask::hessian
std::vector< Hessian > hessian
Definition: tasks.h:116
exotica::TaskMapVec
std::vector< TaskMapPtr > TaskMapVec
Definition: task_map.h:94
exotica::EndPoseTask::hessian
Hessian hessian
Definition: tasks.h:153
planning_problem.h
exotica::Task::length_jacobian
int length_jacobian
Definition: tasks.h:67
exotica::TimeIndexedTask::Phi
std::vector< TaskSpaceVector > Phi
Definition: tasks.h:115
exotica::EndPoseTask::GetGoal
Eigen::VectorXd GetGoal(const std::string &task_name) const
property.h
exotica::TimeIndexedTask::Update
void Update(const TaskSpaceVector &big_Phi, Eigen::MatrixXdRefConst big_dPhi_dx, Eigen::MatrixXdRefConst big_dPhi_du, HessianRefConst big_ddPhi_ddx, HessianRefConst big_ddPhi_ddu, HessianRefConst big_ddPhi_dxdu, int t)
task_space_vector.h
exotica::EndPoseTask::jacobian
Eigen::MatrixXd jacobian
Definition: tasks.h:152
exotica::TimeIndexedTask::GetS
Eigen::MatrixXd GetS(const std::string &task_name, int t) const
exotica::Task::~Task
virtual ~Task()=default
exotica::TimeIndexedTask::GetTaskError
Eigen::VectorXd GetTaskError(const std::string &task_name, int t) const
exotica::EndPoseTask::GetS
Eigen::MatrixXd GetS(const std::string &task_name) const
exotica::EndPoseTask::~EndPoseTask
virtual ~EndPoseTask()=default
exotica::EndPoseTask::UpdateS
void UpdateS()
exotica::SamplingTask::SetRho
void SetRho(const std::string &task_name, const double rho)
exotica::TimeIndexedTask::SetGoal
void SetGoal(const std::string &task_name, Eigen::VectorXdRefConst goal, int t)
exotica::EndPoseTask::GetTaskJacobian
Eigen::MatrixXd GetTaskJacobian(const std::string &task_name) const
exotica
Definition: cartpole_dynamics_solver.h:38
exotica::TaskIndexing::start_jacobian
int start_jacobian
Definition: tasks.h:51
exotica::Hessian
Eigen::Array< Eigen::MatrixXd, Eigen::Dynamic, 1 > Hessian
Definition: conversions.h:154
conversions.h
exotica::SamplingTask::Initialize
virtual void Initialize(const std::vector< exotica::Initializer > &inits, std::shared_ptr< PlanningProblem > prob, TaskSpaceVector &Phi)
exotica::TimeIndexedTask::ValidateTimeIndex
void ValidateTimeIndex(int &t_in) const
exotica::EndPoseTask::Initialize
virtual void Initialize(const std::vector< exotica::Initializer > &inits, std::shared_ptr< PlanningProblem > prob, TaskSpaceVector &Phi)
exotica::EndPoseTask::Phi
TaskSpaceVector Phi
Definition: tasks.h:151
exotica::TimeIndexedTask::~TimeIndexedTask
virtual ~TimeIndexedTask()=default
exotica::TimeIndexedTask::SetRho
void SetRho(const std::string &task_name, const double rho_in, int t)
exotica::EndPoseTask::y
TaskSpaceVector y
Definition: tasks.h:149
exotica::SamplingTask::rho
Eigen::VectorXd rho
Definition: tasks.h:172
exotica::TimeIndexedTask::ddPhi_dxdu
std::vector< Hessian > ddPhi_dxdu
Definition: tasks.h:119
exotica::SamplingTask::Update
void Update(const TaskSpaceVector &big_Phi)
exotica::Task::Initialize
virtual void Initialize(const std::vector< exotica::Initializer > &inits, std::shared_ptr< PlanningProblem > prob, TaskSpaceVector &Phi)
exotica::Task::task_initializers_
std::vector< TaskInitializer > task_initializers_
Definition: tasks.h:72
exotica::Task::num_tasks
int num_tasks
Definition: tasks.h:68
Eigen::VectorXdRefConst
const typedef Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
Definition: conversions.h:52
exotica::TimeIndexedTask::dPhi_dx
std::vector< Eigen::MatrixXd > dPhi_dx
Definition: tasks.h:121
exotica::TaskSpaceVector
Definition: task_space_vector.h:50
exotica::TimeIndexedTask::dPhi_du
std::vector< Eigen::MatrixXd > dPhi_du
Definition: tasks.h:122
exotica::SamplingTask::y
TaskSpaceVector y
Definition: tasks.h:173
exotica::EndPoseTask::GetTaskError
Eigen::VectorXd GetTaskError(const std::string &task_name) const
Eigen::MatrixXdRefConst
const typedef Eigen::Ref< const Eigen::MatrixXd > & MatrixXdRefConst
Definition: conversions.h:53
exotica::SamplingTask::ydiff
Eigen::VectorXd ydiff
Definition: tasks.h:174
exotica::TimeIndexedTask
Definition: tasks.h:75
exotica::HessianRefConst
Eigen::internal::ref_selector< Hessian >::type HessianRefConst
Definition: conversions.h:166
exotica::SamplingTask::GetRho
double GetRho(const std::string &task_name) const
exotica::Task::length_Phi
int length_Phi
Definition: tasks.h:66
exotica::TimeIndexedTask::GetGoal
Eigen::VectorXd GetGoal(const std::string &task_name, int t) const
exotica::Task::indexing
std::vector< TaskIndexing > indexing
Definition: tasks.h:64
exotica::SamplingTask
Definition: tasks.h:157
exotica::TimeIndexedTask::GetRho
double GetRho(const std::string &task_name, int t) const
exotica::Task::task_maps
TaskMapMap task_maps
Definition: tasks.h:62
exotica::SamplingTask::SetGoal
void SetGoal(const std::string &task_name, Eigen::VectorXdRefConst goal)
exotica::TimeIndexedTask::rho
std::vector< Eigen::VectorXd > rho
Definition: tasks.h:112
exotica::TimeIndexedTask::jacobian
std::vector< Eigen::MatrixXd > jacobian
Definition: tasks.h:120
exotica::SamplingTask::Phi
TaskSpaceVector Phi
Definition: tasks.h:175
exotica::TaskIndexing::length_jacobian
int length_jacobian
Definition: tasks.h:52
exotica::SamplingTask::GetGoal
Eigen::VectorXd GetGoal(const std::string &task_name) const
exotica::TimeIndexedTask::S
std::vector< Eigen::MatrixXd > S
Definition: tasks.h:123
exotica::TimeIndexedTask::ydiff
std::vector< Eigen::VectorXd > ydiff
Definition: tasks.h:114
exotica::EndPoseTask
Definition: tasks.h:127
exotica::TimeIndexedTask::Initialize
virtual void Initialize(const std::vector< exotica::Initializer > &inits, std::shared_ptr< PlanningProblem > prob, TaskSpaceVector &Phi)
exotica::TimeIndexedTask::ddPhi_ddx
std::vector< Hessian > ddPhi_ddx
Definition: tasks.h:117
exotica::Task::tolerance
double tolerance
Definition: tasks.h:69
exotica::TaskMapMap
std::map< std::string, TaskMapPtr > TaskMapMap
The mapping by name of TaskMaps.
Definition: task_map.h:93
exotica::TaskIndexing::length
int length
Definition: tasks.h:50
exotica::TimeIndexedTask::UpdateS
void UpdateS()
exotica::EndPoseTask::SetRho
void SetRho(const std::string &task_name, const double rho_in)
exotica::Task::tasks
TaskMapVec tasks
Definition: tasks.h:63
exotica::TimeIndexedTask::y
std::vector< TaskSpaceVector > y
Definition: tasks.h:113
exotica::TimeIndexedTask::TimeIndexedTask
TimeIndexedTask()=default
exotica::TaskIndexing::start
int start
Definition: tasks.h:49
exotica::SamplingTask::UpdateS
void UpdateS()
exotica::TimeIndexedTask::T
int T
Definition: tasks.h:124
exotica::TaskIndexing
Definition: tasks.h:46
exotica::EndPoseTask::ydiff
Eigen::VectorXd ydiff
Definition: tasks.h:150
exotica::SamplingTask::S
Eigen::MatrixXd S
Definition: tasks.h:176