9 #ifndef _CLASS_LINEAR_PROBLEM_DETAILS
10 #define _CLASS_LINEAR_PROBLEM_DETAILS
19 namespace optimization {
20 template <
typename Po
int,
typename Numeric,
bool Safe = true>
49 bezier =
new bezier_t(b.waypoints().begin(), b.waypoints().end(), b.T_min_,
55 long lValue = (long)(flag);
56 std::size_t iCount = 0;
58 lValue = lValue & (lValue - 1);
61 return (flag &
NONE) ? iCount - 1 : iCount;
64 template <
typename Numeric,
typename LinearVar>
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 &&
74 B.block(0, Dim * (i - startVariableIndex), Dim, Dim) = var.B();
75 return LinearVar(B, var.c());
78 template <
typename Po
int,
typename Numeric,
typename Bezier,
typename LinearVar>
80 const std::vector<LinearVar>& linearVars,
82 std::vector<LinearVar> res;
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>(
89 return new Bezier(res.begin(), res.end(), 0., totalTime);
92 template <
typename Po
int,
typename Numeric,
bool Safe>
96 typedef Point point_t;
100 const std::size_t& degree = pDef.
degree;
103 const std::size_t numControlPoints = pDef.
degree + 1;
105 if (numActiveConstraints >= numControlPoints)
106 throw std::runtime_error(
107 "In setup_control_points; too many constraints for the considered "
110 problem_data_t problemData(pDef.
dim_);
111 typename problem_data_t::T_var_t& variables_ = problemData.variables_;
113 std::size_t numConstants = 0;
116 variables_.push_back(var_t(pDef.
init_pos));
122 variables_.push_back(var_t(vel));
126 point_t acc = (pDef.
init_acc / (num_t)(degree * (degree - 1))) /
130 variables_.push_back(var_t(acc));
136 (num_t)(degree * (degree - 1) * (degree - 2)) +
138 variables_.push_back(var_t(jerk));
145 const std::size_t first_variable_idx = i;
147 for (; i + 4 < numControlPoints; ++i)
148 variables_.push_back(var_t::X(pDef.
dim_));
155 point_t acc = (pDef.
end_acc / (num_t)(degree * (degree - 1))) /
161 (num_t)(degree * (degree - 1) * (degree - 2)) +
162 3 * acc - 3 * vel + pDef.
end_pos;
163 variables_.push_back(var_t(jerk));
167 while (i < numControlPoints - 3) {
168 variables_.push_back(var_t::X(pDef.
dim_));
171 variables_.push_back(var_t(acc));
175 while (i < numControlPoints - 2) {
176 variables_.push_back(var_t::X(pDef.
dim_));
179 variables_.push_back(var_t(vel));
183 while (i < numControlPoints - 1) {
184 variables_.push_back(var_t::X(pDef.
dim_));
188 variables_.push_back(var_t(pDef.
end_pos));
193 for (; i < numControlPoints; ++i) variables_.push_back(var_t::X(pDef.
dim_));
195 if (numControlPoints <= numConstants) {
196 throw std::runtime_error(
"numControlPoints < numConstants");
198 if (numControlPoints != variables_.size()) {
199 throw std::runtime_error(
"numControlPoints != variables_.size()");
202 problemData.numControlPoints = numControlPoints;
203 problemData.numVariables = numControlPoints - numConstants;
204 problemData.startVariableIndex = first_variable_idx;
205 problemData.numStateConstraints =
206 numActiveConstraints - problemData.numVariables;
209 problemData, variables_, pDef.
totalTime);
214 template <
typename Po
int,
typename Numeric>
221 for (
typename problem_definition_t::CIT_vector_x_t cit =
228 template <
typename Po
int,
typename Numeric>
229 std::vector<bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> > >
234 typedef std::vector<bezier_t> T_bezier_t;
241 for (
int i = 0; i < times.rows(); ++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;
249 res.push_back(*current);
253 template <
typename Po
int,
typename Numeric>
257 const std::size_t& Dim = pData.
dim_;
259 typedef typename problem_definition_t::matrix_x_t matrix_x_t;
260 typedef typename problem_definition_t::vector_x_t vector_x_t;
263 typedef std::vector<bezier_t> T_bezier_t;
264 typedef typename T_bezier_t::const_iterator CIT_bezier_t;
266 typedef typename bezier_t::t_point_t::const_iterator cit_point;
269 long rows = compute_num_ineq_control_points<Point, Numeric>(pDef, pData);
270 prob.
ineqMatrix = matrix_x_t::Zero(rows, cols);
276 T_bezier_t beziers = split<Point, Numeric>(pDef, pData);
279 throw std::invalid_argument(
280 "The sizes of the inequality matrices and vectors do not match.");
283 throw std::invalid_argument(
284 "The sizes of the inequality matrices and the bezier degree do not "
288 long currentRowIdx = 0;
289 typename problem_definition_t::CIT_matrix_x_t cmit =
291 typename problem_definition_t::CIT_vector_x_t cvit =
294 for (CIT_bezier_t bit = beziers.begin(); bit != beziers.end();
295 ++bit, ++cvit, ++cmit) {
297 const t_point& wps = bit->waypoints();
299 for (cit_point cit = wps.begin(); cit != wps.end(); ++cit) {
300 prob.
ineqMatrix.block(currentRowIdx, 0, cmit->rows(), cols) =
301 (*cmit) * (cit->B());
303 prob.
ineqVector.segment(currentRowIdx, cmit->rows()) =
304 *cvit - (*cmit) * (cit->c());
305 currentRowIdx += cmit->rows();
308 assert(rows == currentRowIdx);
312 template <
typename Po
int,
typename Numeric,
typename In>
314 In PointsBegin2, In PointsEnd2,
315 const std::size_t ) {
316 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
317 unsigned int nPoints1 =
318 (
unsigned int)(std::distance(PointsBegin1, PointsEnd1)),
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.");
326 unsigned int deg1 = nPoints1 - 1, deg2 = nPoints2 - 1;
327 unsigned int newDeg = (deg1 + deg2);
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)) /
345 In itj = PointsBegin1 + j;
346 In iti = PointsBegin2 + (i - j);
347 res += ((*itj) * (*iti)) * ratio;
350 return res / (newDeg + 1);
359 static_cast<int>(b));
364 static_cast<int>(b));
369 static_cast<int>(b));
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
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