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>
40 : variables_(other.variables_),
41 numVariables(other.numVariables),
42 numControlPoints(other.numControlPoints),
43 startVariableIndex(other.startVariableIndex),
44 numStateConstraints(other.numStateConstraints),
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_);
52 long lValue = (long)(flag);
53 std::size_t iCount = 0;
55 lValue = lValue & (lValue - 1);
58 return (flag &
NONE) ? iCount - 1 : iCount;
61 template <
typename Numeric,
typename LinearVar>
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());
72 template <
typename Po
int,
typename Numeric,
typename Bezier,
typename LinearVar>
74 const std::vector<LinearVar>& linearVars,
const Numeric totalTime) {
75 std::vector<LinearVar> res;
77 std::size_t totalvar = linearVars.size();
78 for (std::size_t i = 0; i < totalvar; ++i)
81 return new Bezier(res.begin(), res.end(), 0., totalTime);
84 template <
typename Po
int,
typename Numeric,
bool Safe>
87 typedef Point point_t;
91 const std::size_t& degree = pDef.
degree;
96 if (numActiveConstraints >= numControlPoints)
97 throw std::runtime_error(
"In setup_control_points; too many constraints for the considered degree");
99 problem_data_t problemData(pDef.
dim_);
100 typename problem_data_t::T_var_t&
variables_ = problemData.variables_;
102 std::size_t numConstants = 0;
110 variables_.push_back(
var_t(vel));
117 variables_.push_back(var_t(acc));
122 (num_t)(degree * (degree - 1) * (degree - 2)) +
124 variables_.push_back(
var_t(jerk));
131 const std::size_t first_variable_idx = i;
143 (num_t)(degree * (degree - 1) * (degree - 2)) +
144 3 * acc - 3 * vel + pDef.
end_pos;
145 variables_.push_back(
var_t(jerk));
149 while (i < numControlPoints - 3) {
153 variables_.push_back(
var_t(acc));
157 while (i < numControlPoints - 2) {
161 variables_.push_back(
var_t(vel));
165 while (i < numControlPoints - 1) {
177 if (numControlPoints <= numConstants) {
178 throw std::runtime_error(
"numControlPoints < numConstants");
180 if (numControlPoints != variables_.size()) {
181 throw std::runtime_error(
"numControlPoints != variables_.size()");
185 problemData.numVariables = numControlPoints - numConstants;
186 problemData.startVariableIndex = first_variable_idx;
187 problemData.numStateConstraints = numActiveConstraints - problemData.numVariables;
189 compute_linear_control_points<Point, Numeric, bezier_curve<Numeric, Numeric, true, var_t>, var_t>(
195 template <
typename Po
int,
typename Numeric>
201 for (
typename problem_definition_t::CIT_vector_x_t cit = pDef.
inequalityVectors_.begin();
207 template <
typename Po
int,
typename Numeric>
208 std::vector<bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> > >
split(
212 typedef std::vector<bezier_t> T_bezier_t;
216 bezier_t& current = *pData.
bezier;
219 for (
int i = 0; i < times.rows(); ++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;
226 res.push_back(current);
230 template <
typename Po
int,
typename Numeric>
233 const std::size_t& Dim = pData.
dim_;
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;
241 typedef typename bezier_t::t_point_t::const_iterator cit_point;
244 long rows = compute_num_ineq_control_points<Point, Numeric>(pDef, pData);
245 prob.
ineqMatrix = matrix_x_t::Zero(rows, cols);
248 if (pDef.inequalityMatrices_.size() == 0)
return;
251 T_bezier_t beziers = split<Point, Numeric>(pDef, pData);
253 if (pDef.inequalityMatrices_.size() != pDef.inequalityVectors_.size()) {
254 throw std::invalid_argument(
"The sizes of the inequality matrices and vectors do not match.");
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.");
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();
264 for (CIT_bezier_t bit = beziers.begin(); bit != beziers.end(); ++bit, ++cvit, ++cmit) {
266 const t_point& wps = bit->waypoints();
268 for (cit_point cit = wps.begin(); cit != wps.end(); ++cit) {
269 prob.
ineqMatrix.block(currentRowIdx, 0, cmit->rows(), cols) =
270 (*cmit) * (cit->B());
271 prob.
ineqVector.segment(currentRowIdx, cmit->rows()) = *cvit - (*cmit) * (cit->c());
272 currentRowIdx += cmit->rows();
275 assert(rows == currentRowIdx);
278 template <
typename Po
int,
typename Numeric,
typename In>
280 const std::size_t ) {
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.");
287 unsigned int deg1 = nPoints1 - 1, deg2 = nPoints2 - 1;
288 unsigned int newDeg = (deg1 + deg2);
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;
309 return res / (newDeg + 1);
317 return static_cast<constraint_flag>(
static_cast<const int>(a) | static_cast<const int>(b));
321 return static_cast<constraint_flag>(
static_cast<const int>(a) & static_cast<const int>(b));
325 return static_cast<constraint_flag>(
static_cast<const int>(a) ^ static_cast<const int>(b));
342 #endif //_CLASS_LINEAR_PROBLEM_DETAILS constraint_flag & operator &=(constraint_flag &a, constraint_flag b)
Definition: details.h:332
Definition: definitions.h:28
Definition: definitions.h:23
std::size_t numVariables
Definition: details.h:32
constraint_flag operator|(constraint_flag a, constraint_flag b)
Definition: details.h:316
point_t init_jerk
Definition: curve_constraint.h:44
Definition: definitions.h:25
double Numeric
Definition: effector_spline.h:26
Definition: definitions.h:34
Definition: definitions.h:26
problem_data(const problem_data &other)
Definition: details.h:39
point_t end_vel
Definition: curve_constraint.h:45
constraint_flag operator^(constraint_flag a, constraint_flag b)
Definition: details.h:324
point_t end_jerk
Definition: curve_constraint.h:47
constraint_flag operator~(constraint_flag a)
Definition: details.h:312
bezier_t * bezier
Definition: details.h:36
const std::size_t dim_
Definition: details.h:37
long compute_num_ineq_control_points(const problem_definition< Point, Numeric > &pDef, const problem_data< Point, Numeric > &pData)
Definition: details.h:196
point_t init_acc
Definition: curve_constraint.h:43
Definition: definitions.h:27
point_t end_pos
Definition: definitions.h:74
static linear_variable_t Zero(size_t dim=0)
Get a linear variable equal to zero.
Definition: linear_variable.h:102
~problem_data()
Definition: details.h:23
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
num_t totalTime
Definition: definitions.h:76
Definition: bernstein.h:20
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > ineqMatrix
Definition: definitions.h:35
Definition: definitions.h:30
Definition: definitions.h:21
point_t init_vel
Definition: curve_constraint.h:42
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
point_t end_acc
Definition: curve_constraint.h:46
Definition: definitions.h:24
class allowing to create a Bezier curve of dimension 1 <= n <= 3.
problem_data< Point, Numeric, Safe > setup_control_points(const problem_definition< Point, Numeric > &pDef)
Definition: details.h:85
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
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:39
Bezier * compute_linear_control_points(const problem_data< Point, Numeric > &pData, const std::vector< LinearVar > &linearVars, const Numeric totalTime)
Definition: details.h:73
vector_x_t splitTimes_
Definition: definitions.h:77
utils for defining optimization problems
constraint_flag flag
Definition: definitions.h:72
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > ineqVector
Definition: definitions.h:36
constraint_flag operator &(constraint_flag a, constraint_flag b)
Definition: details.h:320
point_t init_pos
Definition: definitions.h:73
std::size_t startVariableIndex
Definition: details.h:34
void initInequalityMatrix(const problem_definition< Point, Numeric > &pDef, problem_data< Point, Numeric > &pData, quadratic_problem< Point, Numeric > &prob)
Definition: details.h:231
std::size_t numControlPoints
Definition: details.h:33
constraint_flag & operator^=(constraint_flag &a, constraint_flag b)
Definition: details.h:336
const std::size_t dim_
Definition: definitions.h:80
constraint_flag & operator|=(constraint_flag &a, constraint_flag b)
Definition: details.h:328
std::vector< var_t > variables_
Definition: details.h:31
struct to define constraints on start / end velocities and acceleration on a curve ...
Definition: definitions.h:22
std::vector< var_t > T_var_t
Definition: details.h:28
std::size_t num_active_constraints(const constraint_flag &flag)
Definition: details.h:51
T_vector_x_t inequalityVectors_
Definition: definitions.h:79
std::size_t numStateConstraints
Definition: details.h:35
std::size_t degree
Definition: definitions.h:75
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
problem_data(const std::size_t dim)
Definition: details.h:22
bezier_curve< Numeric, Numeric, true, linear_variable< Numeric > > bezier_t
Definition: details.h:29
Definition: definitions.h:41
linear_variable< Numeric > var_t
Definition: details.h:27
Definition: bezier_curve.h:32