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