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 = current.split(tmp - current_time);
244  res.push_back(pairsplit.first);
245  current = pairsplit.second;
246  current_time += tmp - current_time;
247  }
248  res.push_back(current);
249  return res;
250 }
251 
252 template <typename Point, typename Numeric>
256  const std::size_t& Dim = pData.dim_;
257  typedef problem_definition<Point, Numeric> problem_definition_t;
258  typedef typename problem_definition_t::matrix_x_t matrix_x_t;
259  typedef typename problem_definition_t::vector_x_t vector_x_t;
261  bezier_t;
262  typedef std::vector<bezier_t> T_bezier_t;
263  typedef typename T_bezier_t::const_iterator CIT_bezier_t;
264  typedef typename bezier_t::t_point_t t_point;
265  typedef typename bezier_t::t_point_t::const_iterator cit_point;
266 
267  long cols = pData.numVariables * Dim;
268  long rows = compute_num_ineq_control_points<Point, Numeric>(pDef, pData);
269  prob.ineqMatrix = matrix_x_t::Zero(rows, cols);
270  prob.ineqVector = vector_x_t::Zero(rows);
271 
272  if (pDef.inequalityMatrices_.size() == 0) return;
273 
274  // compute sub-bezier curves
275  T_bezier_t beziers = split<Point, Numeric>(pDef, pData);
276 
277  if (pDef.inequalityMatrices_.size() != pDef.inequalityVectors_.size()) {
278  throw std::invalid_argument(
279  "The sizes of the inequality matrices and vectors do not match.");
280  }
281  if (pDef.inequalityMatrices_.size() != beziers.size()) {
282  throw std::invalid_argument(
283  "The sizes of the inequality matrices and the bezier degree do not "
284  "match.");
285  }
286 
287  long currentRowIdx = 0;
288  typename problem_definition_t::CIT_matrix_x_t cmit =
289  pDef.inequalityMatrices_.begin();
290  typename problem_definition_t::CIT_vector_x_t cvit =
291  pDef.inequalityVectors_.begin();
292  // for each bezier split ..
293  for (CIT_bezier_t bit = beziers.begin(); bit != beziers.end();
294  ++bit, ++cvit, ++cmit) {
295  // compute vector of linear expressions of each control point
296  const t_point& wps = bit->waypoints();
297  // each control has a linear expression depending on all variables
298  for (cit_point cit = wps.begin(); cit != wps.end(); ++cit) {
299  prob.ineqMatrix.block(currentRowIdx, 0, cmit->rows(), cols) =
300  (*cmit) * (cit->B()); // constraint inequality for current bezier *
301  // expression of control point
302  prob.ineqVector.segment(currentRowIdx, cmit->rows()) =
303  *cvit - (*cmit) * (cit->c());
304  currentRowIdx += cmit->rows();
305  }
306  }
307  assert(rows == currentRowIdx); // we filled all the constraints - NB: leave
308  // assert for Debug tests
309 }
310 
311 template <typename Point, typename Numeric, typename In>
312 quadratic_variable<Numeric> bezier_product(In PointsBegin1, In PointsEnd1,
313  In PointsBegin2, In PointsEnd2,
314  const std::size_t /*Dim*/) {
315  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
316  unsigned int nPoints1 =
317  (unsigned int)(std::distance(PointsBegin1, PointsEnd1)),
318  nPoints2 =
319  (unsigned int)(std::distance(PointsBegin2, PointsEnd2));
320  if (nPoints1 <= 0 || nPoints2 <= 0) {
321  throw std::runtime_error(
322  "This should never happen because an unsigned int cannot go negative "
323  "without underflowing.");
324  }
325  unsigned int deg1 = nPoints1 - 1, deg2 = nPoints2 - 1;
326  unsigned int newDeg = (deg1 + deg2);
327  // the integral of the primitive will simply be the last control points of the
328  // primitive, divided by the degree of the primitive, newDeg. We will store
329  // this in matrices for bilinear terms, and a vector for the linear terms, as
330  // well as another one for the constants.
331  quadratic_variable<Numeric> res(vector_x_t::Zero(PointsBegin1->B().cols()));
332  // depending on the index, the fraction coefficient of the bernstein polynom
333  // is either the fraction given by (i+j)/ (deg1+deg2), or 1 - (i+j)/
334  // (deg1+deg2). The trick is that the condition is given by whether the
335  // current index in the combinatorial is odd or even. time parametrization is
336  // not relevant for the cost
337 
338  Numeric ratio;
339  for (unsigned int i = 0; i < newDeg + 1; ++i) {
340  unsigned int j = i > deg2 ? i - deg2 : 0;
341  for (; j < std::min(deg1, i) + 1; ++j) {
342  ratio = (Numeric)(bin(deg1, j) * bin(deg2, i - j)) /
343  (Numeric)(bin(newDeg, i));
344  In itj = PointsBegin1 + j;
345  In iti = PointsBegin2 + (i - j);
346  res += ((*itj) * (*iti)) * ratio;
347  }
348  }
349  return res / (newDeg + 1);
350 }
351 
353  return static_cast<constraint_flag>(~static_cast<const int>(a));
354 }
355 
357  return static_cast<constraint_flag>(static_cast<const int>(a) |
358  static_cast<const int>(b));
359 }
360 
362  return static_cast<constraint_flag>(static_cast<const int>(a) &
363  static_cast<const int>(b));
364 }
365 
367  return static_cast<constraint_flag>(static_cast<const int>(a) ^
368  static_cast<const int>(b));
369 }
370 
372  return (constraint_flag&)((int&)(a) |= static_cast<const int>(b));
373 }
374 
376  return (constraint_flag&)((int&)(a) &= static_cast<const int>(b));
377 }
378 
380  return (constraint_flag&)((int&)(a) ^= static_cast<const int>(b));
381 }
382 
383 } // namespace optimization
384 } // namespace ndcurves
385 #endif //_CLASS_LINEAR_PROBLEM_DETAILS
Definition: bernstein.h:20
Definition: definitions.h:21
num_t totalTime
Definition: definitions.h:78
Definition: definitions.h:28
std::size_t numVariables
Definition: details.h:33
constraint_flag
Definition: definitions.h:20
quadratic_variable< Numeric > bezier_product(In PointsBegin1, In PointsEnd1, In PointsBegin2, In PointsEnd2, const std::size_t)
Definition: details.h:312
constraint_flag operator &(constraint_flag a, constraint_flag b)
Definition: details.h:361
problem_data(const problem_data &other)
Definition: details.h:41
point_t end_pos
Definition: definitions.h:76
~problem_data()
Definition: details.h:23
constraint_flag & operator &=(constraint_flag &a, constraint_flag b)
Definition: details.h:375
Definition: definitions.h:41
constraint_flag operator~(constraint_flag a)
Definition: details.h:352
const std::size_t dim_
Definition: definitions.h:82
T_vector_x_t inequalityVectors_
Definition: definitions.h:81
long compute_num_ineq_control_points(const problem_definition< Point, Numeric > &pDef, const problem_data< Point, Numeric > &pData)
Definition: details.h:215
bezier_curve< double, double, true, pointX_t > bezier_t
Definition: fwd.h:99
linear_variable< Numeric > var_t
Definition: details.h:27
std::size_t num_active_constraints(const constraint_flag &flag)
Definition: details.h:54
Bezier * compute_linear_control_points(const problem_data< Point, Numeric > &pData, const std::vector< LinearVar > &linearVars, const Numeric totalTime)
Definition: details.h:79
void initInequalityMatrix(const problem_definition< Point, Numeric > &pDef, problem_data< Point, Numeric > &pData, quadratic_problem< Point, Numeric > &prob)
Definition: details.h:253
Definition: definitions.h:27
constraint_flag & operator|=(constraint_flag &a, constraint_flag b)
Definition: details.h:371
point_t end_jerk
Definition: curve_constraint.h:63
point_t init_pos
Definition: definitions.h:75
std::size_t startVariableIndex
Definition: details.h:36
constraint_flag operator|(constraint_flag a, constraint_flag b)
Definition: details.h:356
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > ineqMatrix
Definition: definitions.h:35
std::vector< var_t > T_var_t
Definition: details.h:28
std::size_t numStateConstraints
Definition: details.h:37
constraint_flag flag
Definition: definitions.h:74
problem_data< Point, Numeric, Safe > setup_control_points(const problem_definition< Point, Numeric > &pDef)
Definition: details.h:93
class allowing to create a Bezier curve of dimension 1 <= n <= 3.
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:38
utils for defining optimization problems
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
problem_data(const std::size_t dim)
Definition: details.h:22
std::size_t numControlPoints
Definition: details.h:34
linear_variable< double, true > linear_variable_t
Definition: fwd.h:100
Definition: definitions.h:26
Definition: definitions.h:25
point_t init_acc
Definition: curve_constraint.h:59
Definition: fwd.h:60
constraint_flag & operator^=(constraint_flag &a, constraint_flag b)
Definition: details.h:379
point_t end_acc
Definition: curve_constraint.h:62
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
Definition: bezier_curve.h:31
vector_x_t splitTimes_
Definition: definitions.h:79
struct to define constraints on start / end velocities and acceleration on a curve ...
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
Definition: definitions.h:22
double Numeric
Definition: effector_spline.h:26
point_t init_vel
Definition: curve_constraint.h:58
Definition: details.h:21
Definition: fwd.h:63
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > ineqVector
Definition: definitions.h:36
bezier_t * bezier
Definition: details.h:38
Definition: definitions.h:23
const std::size_t dim_
Definition: details.h:39
std::size_t degree
Definition: definitions.h:77
point_t end_vel
Definition: curve_constraint.h:61
std::vector< var_t > variables_
Definition: details.h:32
bezier_curve< Numeric, Numeric, true, linear_variable< Numeric > > bezier_t
Definition: details.h:30
storage for variable points of the form p_i = B_i x + c_i
Definition: definitions.h:34
point_t init_jerk
Definition: curve_constraint.h:60
Definition: definitions.h:24
constraint_flag operator^(constraint_flag a, constraint_flag b)
Definition: details.h:366
Definition: definitions.h:30