bounded_time_indexed_problem.cpp
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 
31 #include <exotica_core/setup.h>
32 
34 
35 namespace exotica
36 {
37 void BoundedTimeIndexedProblem::Instantiate(const BoundedTimeIndexedProblemInitializer& init)
38 {
39  this->parameters_ = init;
40 
41  if (init.LowerBound.rows() == N)
42  {
43  scene_->GetKinematicTree().SetJointLimitsLower(init.LowerBound);
44  }
45  else if (init.LowerBound.rows() != 0)
46  {
47  ThrowNamed("Lower bound size incorrect! Expected " << N << " got " << init.LowerBound.rows());
48  }
49  if (init.UpperBound.rows() == N)
50  {
51  scene_->GetKinematicTree().SetJointLimitsUpper(init.UpperBound);
52  }
53  else if (init.UpperBound.rows() != 0)
54  {
55  ThrowNamed("Lower bound size incorrect! Expected " << N << " got " << init.UpperBound.rows());
56  }
57 
58  cost.Initialize(this->parameters_.Cost, shared_from_this(), cost_Phi);
59 
60  T_ = this->parameters_.T;
61  tau_ = this->parameters_.tau;
62  ApplyStartState(false);
64 }
65 
67 {
69  for (int i = 0; i < tasks_.size(); ++i) tasks_[i]->is_used = false;
70  cost.UpdateS();
71 
72  // Create a new set of kinematic solutions with the size of the trajectory
73  // based on the lastest KinematicResponse in order to reflect model state
74  // updates etc.
75  kinematic_solutions_.clear();
76  kinematic_solutions_.resize(T_);
77  for (int i = 0; i < T_; ++i) kinematic_solutions_[i] = std::make_shared<KinematicResponse>(*scene_->GetKinematicTree().GetKinematicResponse());
78 }
79 
81 {
83 
84  x[t] = x_in;
85 
86  // Set the corresponding KinematicResponse for KinematicTree in order to
87  // have Kinematics elements updated based in x_in.
88  scene_->GetKinematicTree().SetKinematicResponse(kinematic_solutions_[t]);
89 
90  // Pass the corresponding number of relevant task kinematics to the TaskMaps
91  // via the PlanningProblem::UpdateMultipleTaskKinematics method. For now we
92  // support passing _two_ timesteps - this can be easily changed later on.
93  std::vector<std::shared_ptr<KinematicResponse>> kinematics_solutions{kinematic_solutions_[t]};
94 
95  // If the current timestep is 0, pass the 0th timestep's response twice.
96  // Otherwise pass the (t-1)th response.
97  kinematics_solutions.emplace_back((t == 0) ? kinematic_solutions_[t] : kinematic_solutions_[t - 1]);
98 
99  // Actually update the tasks' kinematics mappings.
100  PlanningProblem::UpdateMultipleTaskKinematics(kinematics_solutions);
101 
102  scene_->Update(x_in, static_cast<double>(t) * tau_);
103 
104  Phi[t].SetZero(length_Phi);
105  if (flags_ & KIN_J) jacobian[t].setZero();
106  if (flags_ & KIN_H)
107  for (int i = 0; i < length_jacobian; ++i) hessian[t](i).setZero();
108  for (int i = 0; i < num_tasks; ++i)
109  {
110  // Only update TaskMap if rho is not 0
111  if (tasks_[i]->is_used)
112  {
113  if (flags_ & KIN_H)
114  {
115  tasks_[i]->Update(x[t],
116  Phi[t].data.segment(tasks_[i]->start, tasks_[i]->length),
117  jacobian[t].middleRows(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian),
118  hessian[t].segment(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian));
119  }
120  else if (flags_ & KIN_J)
121  {
122  tasks_[i]->Update(x[t],
123  Phi[t].data.segment(tasks_[i]->start, tasks_[i]->length),
124  Eigen::MatrixXdRef(jacobian[t].middleRows(tasks_[i]->start_jacobian, tasks_[i]->length_jacobian)) // Adding MatrixXdRef(...) is a work-around for issue #737 when using Eigen 3.3.9
125  );
126  }
127  else
128  {
129  tasks_[i]->Update(x[t], Phi[t].data.segment(tasks_[i]->start, tasks_[i]->length));
130  }
131  }
132  }
133  if (flags_ & KIN_H)
134  {
135  cost.Update(Phi[t], jacobian[t], hessian[t], t);
136  }
137  else if (flags_ & KIN_J)
138  {
139  cost.Update(Phi[t], jacobian[t], t);
140  }
141  else
142  {
143  cost.Update(Phi[t], t);
144  }
145 
146  if (t > 0) xdiff[t] = x[t] - x[t - 1];
147 
149 }
150 
152 {
153  if (debug_) HIGHLIGHT_NAMED("BoundedTimeIndexedProblem", "Initialize problem with T=" << T_);
154 
155  num_tasks = tasks_.size();
156  length_Phi = 0;
157  length_jacobian = 0;
158  TaskSpaceVector y_ref_;
159  for (int i = 0; i < num_tasks; ++i)
160  {
161  AppendVector(y_ref_.map, tasks_[i]->GetLieGroupIndices());
162  length_Phi += tasks_[i]->length;
163  length_jacobian += tasks_[i]->length_jacobian;
164  }
165 
166  y_ref_.SetZero(length_Phi);
167  Phi.assign(T_, y_ref_);
168 
169  x.assign(T_, Eigen::VectorXd::Zero(N));
170  xdiff.assign(T_, Eigen::VectorXd::Zero(N));
171  if (flags_ & KIN_J) jacobian.assign(T_, Eigen::MatrixXd(length_jacobian, N));
172  if (flags_ & KIN_H)
173  {
174  Hessian Htmp;
175  Htmp.setConstant(length_jacobian, Eigen::MatrixXd::Zero(N, N));
176  hessian.assign(T_, Htmp);
177  }
178 
179  // Set initial trajectory with current state
180  initial_trajectory_.resize(T_, scene_->GetControlledState());
181 
182  cost.ReinitializeVariables(T_, shared_from_this(), cost_Phi);
183 
184  // Updates related to tau
185  ct = 1.0 / tau_ / T_;
187 
188  PreUpdate();
189 }
190 
192 {
193  bool succeeded = true;
194  auto bounds = scene_->GetKinematicTree().GetJointLimits();
195 
196  std::cout.precision(4);
197 
198  // Check for every state
199  for (int t = 0; t < T_; ++t)
200  {
201  // Check joint limits
202  if (use_bounds)
203  {
204  for (int i = 0; i < N; ++i)
205  {
206  constexpr double tolerance = 1.e-3;
207  if (x[t](i) < bounds(i, 0) - tolerance || x[t](i) > bounds(i, 1) + tolerance)
208  {
209  if (debug_) HIGHLIGHT_NAMED("TimeIndexedProblem::IsValid", "State at timestep " << t << " is out of bounds: joint #" << i << ": " << bounds(i, 0) << " < " << x[t](i) << " < " << bounds(i, 1));
210  succeeded = false;
211  }
212  }
213  }
214  }
215 
216  return succeeded;
217 }
218 } // namespace exotica
std::vector< TaskVectorEntry > map
KinematicRequestFlags flags_
virtual Eigen::VectorXd ApplyStartState(bool update_traj=true)
unsigned int number_of_problem_updates_
bool IsValid() override
Evaluates whether the problem is valid.
geometry_msgs::TransformStamped t
void Update(Eigen::VectorXdRefConst x, int t) override
Updates an individual timestep from a given state vector.
virtual void Initialize(const std::vector< exotica::Initializer > &inits, std::shared_ptr< PlanningProblem > prob, TaskSpaceVector &Phi)
Definition: tasks.cpp:240
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)
Definition: tasks.cpp:261
#define ThrowNamed(m)
Definition: exception.h:42
Eigen::Array< Eigen::MatrixXd, Eigen::Dynamic, 1 > Hessian
Definition: conversions.h:154
std::vector< Eigen::VectorXd > x
Current internal problem state.
bool debug_
Definition: object.h:86
void ValidateTimeIndex(int &t_in) const
Checks the desired time index for bounds and supports -1 indexing.
void PreUpdate() override
Updates internal variables before solving, e.g., after setting new values for Rho.
BoundedTimeIndexedProblemInitializer parameters_
Definition: property.h:139
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
Definition: conversions.h:52
void UpdateMultipleTaskKinematics(std::vector< std::shared_ptr< KinematicResponse >> responses)
void Instantiate(const BoundedTimeIndexedProblemInitializer &init) override
Instantiates the problem from an Initializer.
void ReinitializeVariables(int _T, std::shared_ptr< PlanningProblem > _prob, const TaskSpaceVector &_Phi)
Definition: tasks.cpp:420
#define HIGHLIGHT_NAMED(name, x)
Definition: printable.h:62
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
Definition: conversions.h:55
std::vector< std::shared_ptr< KinematicResponse > > kinematic_solutions_
Bound constrained time-indexed problem.
Eigen::VectorXd xdiff_max_
Maximum change in the variables in a single timestep tau_. Gets set/updated via SetJointVelocityLimit...
void AppendVector(std::vector< Val > &orig, const std::vector< Val > &extra)
Definition: conversions.h:223
#define REGISTER_PROBLEM_TYPE(TYPE, DERIV)
double ct
Normalisation of scalar cost and Jacobian over trajectory length.
Eigen::VectorXd q_dot_max_
Joint velocity limit (rad/s)
std::vector< Eigen::VectorXd > initial_trajectory_


exotica_core
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Mon Feb 28 2022 22:24:13