details.h
Go to the documentation of this file.
1 
9 #ifndef _CLASS_LINEAR_PROBLEM_DETAILS
10 #define _CLASS_LINEAR_PROBLEM_DETAILS
11 
12 #include <ndcurves/bernstein.h>
13 #include <ndcurves/bezier_curve.h>
17 
18 namespace ndcurves {
19 namespace optimization {
20 template <typename Point, typename Numeric, bool Safe = true>
21 struct problem_data {
22  problem_data(const std::size_t dim) : bezier(0), dim_(dim) {}
24  if (bezier) delete bezier;
25  }
26 
28  typedef std::vector<var_t> T_var_t;
31 
32  std::vector<var_t> variables_; // includes constant variables
33  std::size_t numVariables; // total number of variable (/ DIM for total size)
34  std::size_t numControlPoints; // total number of control Points (variables +
35  // waypoints) / DIM )
36  std::size_t startVariableIndex; // before that index, variables are constant
37  std::size_t numStateConstraints;
39  const std::size_t dim_;
40 
41  problem_data(const problem_data& other)
42  : variables_(other.variables_),
47  dim_(other.dim_) {
48  const bezier_t& b = *other.bezier;
49  bezier = new bezier_t(b.waypoints().begin(), b.waypoints().end(), b.T_min_,
50  b.T_max_, b.mult_T_);
51  }
52 };
53 
54 inline std::size_t num_active_constraints(const constraint_flag& flag) {
55  long lValue = (long)(flag);
56  std::size_t iCount = 0;
57  while (lValue != 0) {
58  lValue = lValue & (lValue - 1);
59  iCount++;
60  }
61  return (flag & NONE) ? iCount - 1 : iCount;
62 }
63 
64 template <typename Numeric, typename LinearVar>
65 LinearVar fill_with_zeros(const LinearVar& var, const std::size_t i,
66  const std::size_t startVariableIndex,
67  const std::size_t numVariables,
68  const std::size_t Dim) {
69  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
70  typename LinearVar::matrix_x_t B;
71  B = matrix_t::Zero(Dim, numVariables * Dim);
72  if (startVariableIndex <= i && i <= startVariableIndex + numVariables - 1 &&
73  var.size() > 0)
74  B.block(0, Dim * (i - startVariableIndex), Dim, Dim) = var.B();
75  return LinearVar(B, var.c());
76 }
77 
78 template <typename Point, typename Numeric, typename Bezier, typename LinearVar>
80  const std::vector<LinearVar>& linearVars,
81  const Numeric totalTime) {
82  std::vector<LinearVar> res;
83  // now need to fill all this with zeros...
84  std::size_t totalvar = linearVars.size();
85  for (std::size_t i = 0; i < totalvar; ++i)
86  res.push_back(fill_with_zeros<Numeric, LinearVar>(
87  linearVars[i], i, pData.startVariableIndex, pData.numVariables,
88  pData.dim_));
89  return new Bezier(res.begin(), res.end(), 0., totalTime);
90 }
91 
92 template <typename Point, typename Numeric, bool Safe>
95  typedef Numeric num_t;
96  typedef Point point_t;
97  typedef linear_variable<Numeric> var_t;
98  typedef problem_data<Point, Numeric> problem_data_t;
99 
100  const std::size_t& degree = pDef.degree;
101  const constraint_flag& flag = pDef.flag;
102 
103  const std::size_t numControlPoints = pDef.degree + 1;
104  const std::size_t numActiveConstraints = num_active_constraints(flag);
105  if (numActiveConstraints >= numControlPoints)
106  throw std::runtime_error(
107  "In setup_control_points; too many constraints for the considered "
108  "degree");
109 
110  problem_data_t problemData(pDef.dim_);
111  typename problem_data_t::T_var_t& variables_ = problemData.variables_;
112 
113  std::size_t numConstants = 0;
114  std::size_t i = 0;
115  if (flag & INIT_POS) {
116  variables_.push_back(var_t(pDef.init_pos));
117  ++numConstants;
118  ++i;
119  if (flag & INIT_VEL) {
120  point_t vel =
121  pDef.init_pos + (pDef.init_vel / (num_t)degree) / pDef.totalTime;
122  variables_.push_back(var_t(vel));
123  ++numConstants;
124  ++i;
125  if (flag & INIT_ACC) {
126  point_t acc = (pDef.init_acc / (num_t)(degree * (degree - 1))) /
127  (pDef.totalTime * pDef.totalTime) +
128  2 * vel - pDef.init_pos;
129  ;
130  variables_.push_back(var_t(acc));
131  ++numConstants;
132  ++i;
133  if (flag & INIT_JERK) {
134  point_t jerk = pDef.init_jerk * pDef.totalTime * pDef.totalTime *
135  pDef.totalTime /
136  (num_t)(degree * (degree - 1) * (degree - 2)) +
137  3 * acc - 3 * vel + pDef.init_pos;
138  variables_.push_back(var_t(jerk));
139  ++numConstants;
140  ++i;
141  }
142  }
143  }
144  }
145  const std::size_t first_variable_idx = i;
146  // variables
147  for (; i + 4 < numControlPoints; ++i)
148  variables_.push_back(var_t::X(pDef.dim_));
149  // end constraints
150  if (flag & END_POS) {
151  if (flag & END_VEL) {
152  point_t vel =
153  pDef.end_pos - (pDef.end_vel / (num_t)degree) / pDef.totalTime;
154  if (flag & END_ACC) {
155  point_t acc = (pDef.end_acc / (num_t)(degree * (degree - 1))) /
156  (pDef.totalTime) * (pDef.totalTime) +
157  2 * vel - pDef.end_pos;
158  if (flag & END_JERK) {
159  point_t jerk = -pDef.end_jerk * pDef.totalTime * pDef.totalTime *
160  pDef.totalTime /
161  (num_t)(degree * (degree - 1) * (degree - 2)) +
162  3 * acc - 3 * vel + pDef.end_pos;
163  variables_.push_back(var_t(jerk));
164  ++numConstants;
165  ++i;
166  } else
167  while (i < numControlPoints - 3) {
168  variables_.push_back(var_t::X(pDef.dim_));
169  ++i;
170  }
171  variables_.push_back(var_t(acc));
172  ++numConstants;
173  ++i;
174  } else
175  while (i < numControlPoints - 2) {
176  variables_.push_back(var_t::X(pDef.dim_));
177  ++i;
178  }
179  variables_.push_back(var_t(vel));
180  ++numConstants;
181  ++i;
182  } else {
183  while (i < numControlPoints - 1) {
184  variables_.push_back(var_t::X(pDef.dim_));
185  ++i;
186  }
187  }
188  variables_.push_back(var_t(pDef.end_pos));
189  ++numConstants;
190  ++i;
191  }
192  // add remaining variables (only if no end_pos constraints)
193  for (; i < numControlPoints; ++i) variables_.push_back(var_t::X(pDef.dim_));
194 
195  if (numControlPoints <= numConstants) {
196  throw std::runtime_error("numControlPoints < numConstants");
197  }
198  if (numControlPoints != variables_.size()) {
199  throw std::runtime_error("numControlPoints != variables_.size()");
200  }
201 
202  problemData.numControlPoints = numControlPoints;
203  problemData.numVariables = numControlPoints - numConstants;
204  problemData.startVariableIndex = first_variable_idx;
205  problemData.numStateConstraints =
206  numActiveConstraints - problemData.numVariables;
207  problemData.bezier = compute_linear_control_points<
209  problemData, variables_, pDef.totalTime);
210  return problemData;
211 }
212 
213 // TODO assumes constant are inside constraints...
214 template <typename Point, typename Numeric>
217  const problem_data<Point, Numeric>& pData) {
218  typedef problem_definition<Point, Numeric> problem_definition_t;
219  long rows(0);
220  // rows depends on each constraint size, and the number of waypoints
221  for (typename problem_definition_t::CIT_vector_x_t cit =
222  pDef.inequalityVectors_.begin();
223  cit != pDef.inequalityVectors_.end(); ++cit)
224  rows += cit->rows() * pData.numControlPoints;
225  return rows;
226 }
227 
228 template <typename Point, typename Numeric>
229 std::vector<bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> > >
234  typedef std::vector<bezier_t> T_bezier_t;
235 
236  const Eigen::VectorXd& times = pDef.splitTimes_;
237  T_bezier_t res;
238  bezier_t* current = pData.bezier;
239  Numeric current_time = 0.;
240  Numeric tmp;
241  for (int i = 0; i < times.rows(); ++i) {
242  tmp = times[i];
243  std::pair<bezier_t, bezier_t> pairsplit =
244  current->split(tmp - current_time);
245  res.push_back(pairsplit.first);
246  current = &(pairsplit.second);
247  current_time += tmp - current_time;
248  }
249  res.push_back(*current);
250  return res;
251 }
252 
253 template <typename Point, typename Numeric>
257  const std::size_t& Dim = pData.dim_;
258  typedef problem_definition<Point, Numeric> problem_definition_t;
259  typedef typename problem_definition_t::matrix_x_t matrix_x_t;
260  typedef typename problem_definition_t::vector_x_t vector_x_t;
262  bezier_t;
263  typedef std::vector<bezier_t> T_bezier_t;
264  typedef typename T_bezier_t::const_iterator CIT_bezier_t;
265  typedef typename bezier_t::t_point_t t_point;
266  typedef typename bezier_t::t_point_t::const_iterator cit_point;
267 
268  long cols = pData.numVariables * Dim;
269  long rows = compute_num_ineq_control_points<Point, Numeric>(pDef, pData);
270  prob.ineqMatrix = matrix_x_t::Zero(rows, cols);
271  prob.ineqVector = vector_x_t::Zero(rows);
272 
273  if (pDef.inequalityMatrices_.size() == 0) return;
274 
275  // compute sub-bezier curves
276  T_bezier_t beziers = split<Point, Numeric>(pDef, pData);
277 
278  if (pDef.inequalityMatrices_.size() != pDef.inequalityVectors_.size()) {
279  throw std::invalid_argument(
280  "The sizes of the inequality matrices and vectors do not match.");
281  }
282  if (pDef.inequalityMatrices_.size() != beziers.size()) {
283  throw std::invalid_argument(
284  "The sizes of the inequality matrices and the bezier degree do not "
285  "match.");
286  }
287 
288  long currentRowIdx = 0;
289  typename problem_definition_t::CIT_matrix_x_t cmit =
290  pDef.inequalityMatrices_.begin();
291  typename problem_definition_t::CIT_vector_x_t cvit =
292  pDef.inequalityVectors_.begin();
293  // for each bezier split ..
294  for (CIT_bezier_t bit = beziers.begin(); bit != beziers.end();
295  ++bit, ++cvit, ++cmit) {
296  // compute vector of linear expressions of each control point
297  const t_point& wps = bit->waypoints();
298  // each control has a linear expression depending on all variables
299  for (cit_point cit = wps.begin(); cit != wps.end(); ++cit) {
300  prob.ineqMatrix.block(currentRowIdx, 0, cmit->rows(), cols) =
301  (*cmit) * (cit->B()); // constraint inequality for current bezier *
302  // expression of control point
303  prob.ineqVector.segment(currentRowIdx, cmit->rows()) =
304  *cvit - (*cmit) * (cit->c());
305  currentRowIdx += cmit->rows();
306  }
307  }
308  assert(rows == currentRowIdx); // we filled all the constraints - NB: leave
309  // assert for Debug tests
310 }
311 
312 template <typename Point, typename Numeric, typename In>
313 quadratic_variable<Numeric> bezier_product(In PointsBegin1, In PointsEnd1,
314  In PointsBegin2, In PointsEnd2,
315  const std::size_t /*Dim*/) {
316  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
317  unsigned int nPoints1 =
318  (unsigned int)(std::distance(PointsBegin1, PointsEnd1)),
319  nPoints2 =
320  (unsigned int)(std::distance(PointsBegin2, PointsEnd2));
321  if (nPoints1 <= 0 || nPoints2 <= 0) {
322  throw std::runtime_error(
323  "This should never happen because an unsigned int cannot go negative "
324  "without underflowing.");
325  }
326  unsigned int deg1 = nPoints1 - 1, deg2 = nPoints2 - 1;
327  unsigned int newDeg = (deg1 + deg2);
328  // the integral of the primitive will simply be the last control points of the
329  // primitive, divided by the degree of the primitive, newDeg. We will store
330  // this in matrices for bilinear terms, and a vector for the linear terms, as
331  // well as another one for the constants.
332  quadratic_variable<Numeric> res(vector_x_t::Zero(PointsBegin1->B().cols()));
333  // depending on the index, the fraction coefficient of the bernstein polynom
334  // is either the fraction given by (i+j)/ (deg1+deg2), or 1 - (i+j)/
335  // (deg1+deg2). The trick is that the condition is given by whether the
336  // current index in the combinatorial is odd or even. time parametrization is
337  // not relevant for the cost
338 
339  Numeric ratio;
340  for (unsigned int i = 0; i < newDeg + 1; ++i) {
341  unsigned int j = i > deg2 ? i - deg2 : 0;
342  for (; j < std::min(deg1, i) + 1; ++j) {
343  ratio = (Numeric)(bin(deg1, j) * bin(deg2, i - j)) /
344  (Numeric)(bin(newDeg, i));
345  In itj = PointsBegin1 + j;
346  In iti = PointsBegin2 + (i - j);
347  res += ((*itj) * (*iti)) * ratio;
348  }
349  }
350  return res / (newDeg + 1);
351 }
352 
354  return static_cast<constraint_flag>(~static_cast<int>(a));
355 }
356 
358  return static_cast<constraint_flag>(static_cast<int>(a) |
359  static_cast<int>(b));
360 }
361 
363  return static_cast<constraint_flag>(static_cast<int>(a) &
364  static_cast<int>(b));
365 }
366 
368  return static_cast<constraint_flag>(static_cast<int>(a) ^
369  static_cast<int>(b));
370 }
371 
373  return (constraint_flag&)((int&)(a) |= static_cast<int>(b));
374 }
375 
377  return (constraint_flag&)((int&)(a) &= static_cast<int>(b));
378 }
379 
381  return (constraint_flag&)((int&)(a) ^= static_cast<int>(b));
382 }
383 
384 } // namespace optimization
385 } // namespace ndcurves
386 #endif //_CLASS_LINEAR_PROBLEM_DETAILS
class allowing to create a Bezier curve of dimension 1 <= n <= 3.
struct to define constraints on start / end velocities and acceleration on a curve
utils for defining optimization problems
storage for variable points of the form p_i = B_i x + c_i
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
double Numeric
Definition: effector_spline.h:26
constraint_flag & operator|=(constraint_flag &a, constraint_flag b)
Definition: details.h:372
constraint_flag operator&(constraint_flag a, constraint_flag b)
Definition: details.h:362
Bezier * compute_linear_control_points(const problem_data< Point, Numeric > &pData, const std::vector< LinearVar > &linearVars, const Numeric totalTime)
Definition: details.h:79
constraint_flag operator|(constraint_flag a, constraint_flag b)
Definition: details.h:357
constraint_flag operator^(constraint_flag a, constraint_flag b)
Definition: details.h:367
long compute_num_ineq_control_points(const problem_definition< Point, Numeric > &pDef, const problem_data< Point, Numeric > &pData)
Definition: details.h:215
constraint_flag
Definition: definitions.h:20
@ INIT_ACC
Definition: definitions.h:23
@ NONE
Definition: definitions.h:30
@ INIT_JERK
Definition: definitions.h:24
@ END_POS
Definition: definitions.h:25
@ END_ACC
Definition: definitions.h:27
@ END_JERK
Definition: definitions.h:28
@ INIT_POS
Definition: definitions.h:21
@ INIT_VEL
Definition: definitions.h:22
@ END_VEL
Definition: definitions.h:26
quadratic_variable< Numeric > bezier_product(In PointsBegin1, In PointsEnd1, In PointsBegin2, In PointsEnd2, const std::size_t)
Definition: details.h:313
constraint_flag & operator^=(constraint_flag &a, constraint_flag b)
Definition: details.h:380
std::vector< bezier_curve< Numeric, Numeric, true, linear_variable< Numeric > > > split(const problem_definition< Point, Numeric > &pDef, problem_data< Point, Numeric > &pData)
Definition: details.h:230
constraint_flag & operator&=(constraint_flag &a, constraint_flag b)
Definition: details.h:376
void initInequalityMatrix(const problem_definition< Point, Numeric > &pDef, problem_data< Point, Numeric > &pData, quadratic_problem< Point, Numeric > &prob)
Definition: details.h:254
problem_data< Point, Numeric, Safe > setup_control_points(const problem_definition< Point, Numeric > &pDef)
Definition: details.h:93
std::size_t num_active_constraints(const constraint_flag &flag)
Definition: details.h:54
LinearVar fill_with_zeros(const LinearVar &var, const std::size_t i, const std::size_t startVariableIndex, const std::size_t numVariables, const std::size_t Dim)
Definition: details.h:65
constraint_flag operator~(constraint_flag a)
Definition: details.h:353
Definition: bernstein.h:20
bezier_curve< double, double, true, pointX_t > bezier_t
Definition: fwd.h:107
linear_variable< double, true > linear_variable_t
Definition: fwd.h:108
Definition: bezier_curve.h:31
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:38
point_t init_acc
Definition: curve_constraint.h:59
point_t end_acc
Definition: curve_constraint.h:62
point_t init_jerk
Definition: curve_constraint.h:60
point_t end_jerk
Definition: curve_constraint.h:63
point_t init_vel
Definition: curve_constraint.h:58
point_t end_vel
Definition: curve_constraint.h:61
Definition: linear_variable.h:26
Definition: details.h:21
std::size_t startVariableIndex
Definition: details.h:36
bezier_curve< Numeric, Numeric, true, linear_variable< Numeric > > bezier_t
Definition: details.h:30
std::size_t numControlPoints
Definition: details.h:34
std::size_t numVariables
Definition: details.h:33
linear_variable< Numeric > var_t
Definition: details.h:27
std::vector< var_t > variables_
Definition: details.h:32
std::vector< var_t > T_var_t
Definition: details.h:28
problem_data(const std::size_t dim)
Definition: details.h:22
bezier_t * bezier
Definition: details.h:38
problem_data(const problem_data &other)
Definition: details.h:41
const std::size_t dim_
Definition: details.h:39
std::size_t numStateConstraints
Definition: details.h:37
~problem_data()
Definition: details.h:23
Definition: definitions.h:41
num_t totalTime
Definition: definitions.h:78
point_t end_pos
Definition: definitions.h:76
point_t init_pos
Definition: definitions.h:75
std::size_t degree
Definition: definitions.h:77
T_vector_x_t inequalityVectors_
Definition: definitions.h:81
T_matrix_x_t inequalityMatrices_
Definition: definitions.h:80
const std::size_t dim_
Definition: definitions.h:82
vector_x_t splitTimes_
Definition: definitions.h:79
constraint_flag flag
Definition: definitions.h:74
Definition: definitions.h:34
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > ineqMatrix
Definition: definitions.h:35
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > ineqVector
Definition: definitions.h:36