Commit 7a8e77f9 authored by stevet's avatar stevet Committed by Pierre Fernbach
Browse files

remove cubic and quintic classes

parent 2f385541
Pipeline #9444 passed with stage
in 5 minutes and 18 seconds
......@@ -58,9 +58,7 @@ SET(${PROJECT_NAME}_HEADERS
include/${PROJECT_NAME}/MathDefs.h
include/${PROJECT_NAME}/polynomial.h
include/${PROJECT_NAME}/bezier_curve.h
include/${PROJECT_NAME}/cubic_spline.h
include/${PROJECT_NAME}/curve_constraint.h
include/${PROJECT_NAME}/quintic_spline.h
include/${PROJECT_NAME}/linear_variable.h
include/${PROJECT_NAME}/quadratic_variable.h
include/${PROJECT_NAME}/cubic_hermite_spline.h
......
/**
* \file cubic_spline.h
* \brief Definition of a cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the CubicFunction struct.
* It allows the creation and evaluation of natural
* smooth cubic splines of arbitrary dimension
*/
#ifndef _STRUCT_CUBICSPLINE
#define _STRUCT_CUBICSPLINE
#include "MathDefs.h"
#include "polynomial.h"
#include <stdexcept>
namespace curves {
/// \brief Creates coefficient vector of a cubic spline defined on the interval
/// \f$[t_{min}, t_{max}]\f$. It follows the equation : <br>
/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$ t \in [t_{min}, t_{max}] \f$
/// with a, b, c and d the control points.
///
template <typename Point, typename T_Point>
T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point const& d) {
T_Point res;
res.push_back(a);
res.push_back(b);
res.push_back(c);
res.push_back(d);
return res;
}
template <typename Time, typename Numeric, bool Safe, typename Point, typename T_Point>
polynomial<Time, Numeric, Safe, Point, T_Point> create_cubic(Point const& a, Point const& b, Point const& c,
Point const& d, const Time t_min, const Time t_max) {
T_Point coeffs = make_cubic_vector<Point, T_Point>(a, b, c, d);
return polynomial<Time, Numeric, Safe, Point, T_Point>(coeffs.begin(), coeffs.end(), t_min, t_max);
}
} // namespace curves
#endif //_STRUCT_CUBICSPLINE
......@@ -20,10 +20,9 @@
#define _CLASS_EXACTCUBIC
#include "curve_abc.h"
#include "cubic_spline.h"
#include "quintic_spline.h"
#include "curve_constraint.h"
#include "piecewise_curve.h"
#include "polynomial.h"
#include "MathDefs.h"
......@@ -41,6 +40,7 @@ template <typename Time = double, typename Numeric = Time, bool Safe = false,
typename SplineBase = polynomial<Time, Numeric, Safe, Point, T_Point> >
struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
typedef Point point_t;
typedef const Eigen::Ref<const point_t> point_ref_t;
typedef T_Point t_point_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef Eigen::Matrix<Numeric, 3, 3> Matrix3;
......@@ -52,9 +52,10 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
typedef typename t_spline_t::const_iterator cit_spline_t;
typedef curve_constraints<Point> spline_constraints;
typedef exact_cubic<Time, Numeric, Safe, Point, T_Point, SplineBase> exact_cubic_t;
typedef exact_cubic<Time, Numeric, Safe, point_t, T_Point, SplineBase> exact_cubic_t;
typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t; // parent class
typedef piecewise_curve<Time, Numeric, Safe, Point> piecewise_curve_t;
typedef piecewise_curve<Time, Numeric, Safe, point_t> piecewise_curve_t;
typedef polynomial<Time, Numeric, Safe, point_t> polynomial_t;
typedef typename piecewise_curve_t::t_curve_ptr_t t_curve_ptr_t;
/* Constructors - destructors */
......@@ -128,6 +129,29 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
}
private:
static polynomial_t create_cubic(point_ref_t a,point_ref_t b, point_ref_t c, point_ref_t d,
const time_t t_min, const time_t t_max){
typename polynomial_t::t_point_t coeffs;
coeffs.push_back(a);
coeffs.push_back(b);
coeffs.push_back(c);
coeffs.push_back(d);
return polynomial_t(coeffs.begin(), coeffs.end(), t_min, t_max);
}
static polynomial_t create_quintic(point_ref_t a,point_ref_t b, point_ref_t c, point_ref_t d,
point_ref_t e, point_ref_t f,
const time_t t_min, const time_t t_max){
typename polynomial_t::t_point_t coeffs;
coeffs.push_back(a);
coeffs.push_back(b);
coeffs.push_back(c);
coeffs.push_back(d);
coeffs.push_back(e);
coeffs.push_back(f);
return polynomial_t(coeffs.begin(), coeffs.end(), t_min, t_max);
}
/// \brief Compute polynom of exact cubic spline from waypoints.
/// Compute the coefficients of polynom as in paper : "Task-Space Trajectories via Cubic Spline Optimization".<br>
/// \f$x_i(t)=a_i+b_i(t-t_i)+c_i(t-t_i)^2\f$<br>
......@@ -198,7 +222,7 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
it = wayPointsBegin, next = wayPointsBegin;
++next;
for (int i = 0; next != wayPointsEnd; ++i, ++it, ++next) {
subSplines.push_back(create_cubic<Time, Numeric, Safe, Point, T_Point>(a.row(i), b.row(i), c.row(i), d.row(i),
subSplines.push_back(create_cubic(a.row(i), b.row(i), c.row(i), d.row(i),
(*it).first, (*next).first));
}
return subSplines;
......@@ -230,7 +254,7 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
subSplines.push_back(create_cubic<Time, Numeric, Safe, Point, T_Point>(a0, b0, c0, d0, init_t, end_t));
subSplines.push_back(create_cubic(a0, b0, c0, d0, init_t, end_t));
constraints.init_vel = subSplines.back().derivate(end_t, 1);
constraints.init_acc = subSplines.back().derivate(end_t, 2);
}
......@@ -270,7 +294,7 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
d = rhs.row(0);
e = rhs.row(1);
f = rhs.row(2);
subSplines.push_back(create_quintic<Time, Numeric, Safe, Point, T_Point>(a0, b0, c0, d, e, f, init_t, end_t));
subSplines.push_back(create_quintic(a0, b0, c0, d, e, f, init_t, end_t));
}
public:
......
/**
* \file OptimizeSpline.h
* \brief Optimization loop for cubic spline generations
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file uses the mosek library to optimize the waypoints location
* to generate exactCubic spline
*/
#ifndef _CLASS_SPLINEOPTIMIZER
#define _CLASS_SPLINEOPTIMIZER
#include "curves/MathDefs.h"
#include "curves/exact_cubic.h"
#include "mosek/mosek.h"
#include <Eigen/SparseCore>
#include <utility>
namespace curves {
/// \class SplineOptimizer
/// \brief Mosek connection to produce optimized splines
template <typename Time = double, typename Numeric = Time, std::size_t Dim = 3, bool Safe = false,
typename Point = Eigen::Matrix<Numeric, Dim, 1> >
struct SplineOptimizer {
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef Point point_t;
typedef Time time_t;
typedef Numeric num_t;
typedef exact_cubic<time_t, Numeric, Dim, Safe, Point> exact_cubic_t;
typedef SplineOptimizer<time_t, Numeric, Dim, Safe, Point> splineOptimizer_t;
/* Constructors - destructors */
public:
///\brief Initializes optimizer environment.
SplineOptimizer() {
MSKrescodee r_ = MSK_makeenv(&env_, NULL);
if (r_ != MSK_RES_OK) {
throw std::runtime_error("Issue initializing MSK_makeenv");
}
}
///\brief Destructor.
~SplineOptimizer() { MSK_deleteenv(&env_); }
private:
SplineOptimizer(const SplineOptimizer&);
SplineOptimizer& operator=(const SplineOptimizer&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Start an optimization loop to create curve.
/// \param waypoints : a list containing at least 2 waypoints in ascending time order.
/// \return An Optimised curve.
template <typename In>
exact_cubic_t* GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const;
/*Operations*/
private:
template <typename In>
void ComputeHMatrices(In wayPointsBegin, In wayPointsEnd, MatrixX& h1, MatrixX& h2, MatrixX& h3, MatrixX& h4) const;
/*Attributes*/
MSKenv_t env_;
/*Attributes*/
private:
typedef std::pair<time_t, Point> waypoint_t;
typedef std::vector<waypoint_t> T_waypoints_t;
}; // End struct SplineOptimizer
template <typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point>
template <typename In>
inline void SplineOptimizer<Time, Numeric, Dim, Safe, Point>::ComputeHMatrices(In wayPointsBegin, In wayPointsEnd,
MatrixX& h1, MatrixX& h2, MatrixX& h3,
MatrixX& h4) const {
std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
assert((!Safe) || (size > 1));
In it(wayPointsBegin), next(wayPointsBegin);
++next;
Numeric t_previous((*it).first);
for (std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) {
num_t const dTi((*next).first - (*it).first);
num_t const dTi_sqr(dTi * dTi);
// filling matrices values
h3(i, i) = -3 / dTi_sqr;
h3(i, i + 1) = 3 / dTi_sqr;
h4(i, i) = -2 / dTi;
h4(i, i + 1) = -1 / dTi;
if (i + 2 < size) {
In it2(next);
++it2;
num_t const dTi_1((*it2).first - (*next).first);
num_t const dTi_1sqr(dTi_1 * dTi_1);
// this can be optimized but let's focus on clarity as long as not needed
h1(i + 1, i) = 2 / dTi;
h1(i + 1, i + 1) = 4 / dTi + 4 / dTi_1;
h1(i + 1, i + 2) = 2 / dTi_1;
h2(i + 1, i) = -6 / dTi_sqr;
h2(i + 1, i + 1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
h2(i + 1, i + 2) = 6 / dTi_1sqr;
}
}
}
template <typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point>
template <typename In>
inline exact_cubic<Time, Numeric, Dim, Safe, Point>*
SplineOptimizer<Time, Numeric, Dim, Safe, Point>::GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const {
exact_cubic_t* res = 0;
int const size((int)std::distance(wayPointsBegin, wayPointsEnd));
if (Safe && size < 1) {
throw std::length_error("can not generate optimizedCurve, number of waypoints should be superior to one");
; // TODO
}
// refer to the paper to understand all this.
MatrixX h1 = MatrixX::Zero(size, size);
MatrixX h2 = MatrixX::Zero(size, size);
MatrixX h3 = MatrixX::Zero(size, size);
MatrixX h4 = MatrixX::Zero(size, size);
// remove this for the time being
/*MatrixX g1 = MatrixX::Zero(size, size);
MatrixX g2 = MatrixX::Zero(size, size);
MatrixX g3 = MatrixX::Zero(size, size);
MatrixX g4 = MatrixX::Zero(size, size);*/
ComputeHMatrices(wayPointsBegin, wayPointsEnd, h1, h2, h3, h4);
// number of Waypoints : T + 1 => T mid points. Dim variables per points, + acceleration + derivations
// (T * t+ 1 ) * Dim * 3 = nb var
// NOG const MSKint32t numvar = ( size + size - 1) * 3 * 3;
const MSKint32t numvar = (size)*Dim * 3;
/*
We store the variables in that order to simplifly matrix computation ( see later )
// NOG [ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn.. x0' --- zn..' ] T
[ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn..] T
*/
/*the first constraint is H1x. = H2x => H2x - H1x. = 0
this will give us size * 3 inequalities constraints
So this line of A will be writen
H2 -H1 0 0 0 0
*/
int ptOff = (int)Dim * size; // . offest
int ptptOff = (int)Dim * 2 * size; // .. offest
int prOff = (int)3 * ptOff; // ' offest
// NOG int prptOff = (int) prOff + ptOff; // '. offest
// NOG int prptptOff = (int) prptOff + ptOff; // '.. offest
MatrixX h2x_h1x = MatrixX::Zero(size * Dim, numvar);
/**A looks something like that : (n = size)
[H2(0) 0 0 -H1(0) 0-------------------0]
[ 0 0 H2(0) 0 0 -H1(0)---------------0]
[ 0 0 0 H2(0) 0 0 H1(0)--------0]
...
[ 0 0 0 0 H2(n) 0 0 0 -H1(n)-0] // row n
Overall it's fairly easy to fill
*/
for (int i = 0; i < size * Dim; i = i + 3) {
for (int j = 0; j < Dim; ++j) {
int id = i + j;
h2x_h1x.block(id, j * size, 1, size) = h2.row(i % 3);
h2x_h1x.block(id, j * size + ptOff, 1, size) -= h1.row(i % 3);
}
}
/*the second constraint is x' = G1x + G2x. => G1x + G2x. - x' = 0
this will give us size * 3 inequalities constraints
So this line of A will be writen
H2 -H1 0 0 0 0
*/
MatrixX g1x_g2x = MatrixX::Zero(size * Dim, numvar);
/**A looks something like that : (n = size)
[G1(0) 0 0 G2(0) 0-----------------------0 -1 0]
[ 0 0 G1(0) 0 0 G2(0)-------------------0 -1 0]
[ 0 0 0 G1(0) 0 0 G2(0)-----------0 -1 0]
...
[ 0 0 0 0 G1(n) 0 0 0 G2(n)-0 -1 0] // row n
Overall it's fairly easy to fill
*/
// NOG
/*for(int j = 0; j<3; ++j)
{
for(int j =0; j<3; ++j)
{
int id = i + j;
g1x_g2x.block(id, j*size , 1, size) = g1.row(i);
g1x_g2x.block(id, j*size + ptOff, 1, size) = g2.row(i);
g1x_g2x.block(id, j*size + prOff, 1, size) -= MatrixX::Ones(1, size);
}
}*/
/*the third constraint is x.' = G3x + G4x. => G3x + G4x. - x.' = 0
this will give us size * 3 inequalities constraints
So this line of A will be writen
H2 -H1 0 0 0 0
*/
MatrixX g3x_g4x = MatrixX::Zero(size * Dim, numvar);
/**A looks something like that : (n = size)
[G3(0) 0 0 G4(0) 0-------------------0 -1 0]
[ 0 0 G3(0) 0 0 G4(0)---------------0 -1 0]
[ 0 0 0 G3(0) 0 0 G4(0)--------0 -1 0]
...
[ 0 0 0 0 G3(n) 0 0 0 G4(n)-0 -1 0] // row n
Overall it's fairly easy to fill
*/
// NOG
/*for(int j = 0; j<3; ++j)
{
for(int j =0; j<3; ++j)
{
int id = i + j;
g3x_g4x.block(id, j*size , 1, size) = g3.row(i);
g3x_g4x.block(id, j*size + ptOff, 1, size) = g4.row(i);
g3x_g4x.block(id, j*size + prptOff, 1, size) -= MatrixX::Ones(1, size);
}
}
*/
/*the fourth constraint is x.. = 1/2(H3x + H4x.) => 1/2(H3x + H4x.) - x.. = 0
=> H3x + H4x. - 2x.. = 0
this will give us size * 3 inequalities constraints
So this line of A will be writen
H2 -H1 0 0 0 0
*/
MatrixX h3x_h4x = MatrixX::Zero(size * Dim, numvar);
/**A looks something like that : (n = size)
[H3(0) 0 0 H4(0) 0-------------------0 -2 0]
[ 0 0 H3(0) 0 0 H4(0)---------------0 -2 0]
[ 0 0 0 H3(0) 0 0 H4(0)-------0 -2 0]
...
[ 0 0 0 0 H3(n) 0 0 0 H4(n)-0 -2 0] // row n
Overall it's fairly easy to fill
*/
for (int i = 0; i < size * Dim; i = i + Dim) {
for (int j = 0; j < Dim; ++j) {
int id = i + j;
h3x_h4x.block(id, j * size, 1, size) = h3.row(i % 3);
h3x_h4x.block(id, j * size + ptOff, 1, size) = h4.row(i % 3);
h3x_h4x.block(id, j * size + ptptOff, 1, size) = MatrixX::Ones(1, size) * -2;
}
}
/*the following constraints are easy to understand*/
/*x0,: = x^0*/
MatrixX x0_x0 = MatrixX::Zero(Dim, numvar);
for (int j = 0; j < Dim; ++j) {
x0_x0(0, 0) = 1;
x0_x0(1, size) = 1;
x0_x0(2, size * 2) = 1;
}
/*x0.,: = 0*/
MatrixX x0p_0 = MatrixX::Zero(Dim, numvar);
for (int j = 0; j < Dim; ++j) {
x0p_0(0, ptOff) = 1;
x0p_0(1, ptOff + size) = 1;
x0p_0(2, ptOff + size * 2) = 1;
}
/*xt,: = x^t*/
MatrixX xt_xt = MatrixX::Zero(Dim, numvar);
for (int j = 0; j < Dim; ++j) {
xt_xt(0, size - 1) = 1;
xt_xt(1, 2 * size - 1) = 1;
xt_xt(2, 3 * size - 1) = 1;
}
/*xT.,: = 0*/
MatrixX xtp_0 = MatrixX::Zero(Dim, numvar);
for (int j = 0; j < Dim; ++j) {
xtp_0(0, ptOff + size - 1) = 1;
xtp_0(1, ptOff + size + size - 1) = 1;
xtp_0(2, ptOff + size * 2 + size - 1) = 1;
}
// skipping constraints on x and y accelerations for the time being
// to compute A i'll create an eigen matrix, then i'll convert it to a sparse one and fill those tables
// total number of constraints
// NOG h2x_h1x (size * Dim) + h3x_h4x (size * Dim ) + g1x_g2x (size * Dim ) + g3x_g4x (size*Dim)
// NOG + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 (Dim) = 4 * Dim * size + 4 * Dim
// h2x_h1x (size * Dim) + h3x_h4x (size * Dim )
// + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 (Dim) = 2 * Dim * size + 4 * Dim
// NOG const MSKint32t numcon = 12 * size + 12;
const MSKint32t numcon = Dim * 2 * size + 4 * Dim; // TODO
// this gives us the matrix A of size numcon * numvaar
MatrixX a = MatrixX::Zero(numcon, numvar);
a.block(0, 0, size * Dim, numvar) = h2x_h1x;
a.block(size * Dim, 0, size * Dim, numvar) = h3x_h4x;
a.block(size * Dim * 2, 0, Dim, numvar) = x0p_0;
a.block(size * Dim * 2 + Dim, 0, Dim, numvar) = xtp_0;
a.block(size * Dim * 2 + Dim * 2, 0, Dim, numvar) = x0_x0;
a.block(size * Dim * 2 + Dim * 3, 0, Dim, numvar) = xt_xt;
// convert to sparse representation
Eigen::SparseMatrix<Numeric> spA;
spA = a.sparseView();
// convert to sparse representation using column
// http://docs.mosek.com/7.0/capi/Conventions_employed_in_the_API.html#sec-intro-subsubsec-cmo-rmo-matrix
int nonZeros = spA.nonZeros();
/* Below is the sparse representation of the A
matrix stored by column. */
double* aval = new double[nonZeros];
MSKint32t* asub = new MSKint32t[nonZeros];
MSKint32t* aptrb = new MSKint32t[numvar];
MSKint32t* aptre = new MSKint32t[numvar];
int currentIndex = 0;
for (int j = 0; j < numvar; ++j) {
bool nonZeroAtThisCol = false;
for (int i = 0; i < numcon; ++i) {
if (a(i, j) != 0) {
if (!nonZeroAtThisCol) {
aptrb[j] = currentIndex;
nonZeroAtThisCol = true;
}
aval[currentIndex] = a(i, j);
asub[currentIndex] = i;
aptre[j] = currentIndex + 1; // overriding previous value
++currentIndex;
}
}
}
/*Q looks like this
0 0 0 0 0 0 | -> size * 3
0 0 2 0 0 0 | -> size *3
0 0 0 0 0 0
0 0 0 0 2 0
0 0 0 0 0 0
*/
/* Number of non-zeros in Q.*/
const MSKint32t numqz = size * Dim * 2;
MSKint32t* qsubi = new MSKint32t[numqz];
MSKint32t* qsubj = new MSKint32t[numqz];
double* qval = new double[numqz];
for (int id = 0; id < numqz; ++id) {
qsubi[id] = id + ptOff; // we want the x.
qsubj[id] = id + ptOff;
qval[id] = 2;
}
/* Bounds on constraints. */
MSKboundkeye* bkc = new MSKboundkeye[numcon];
double* blc = new double[numcon];
double* buc = new double[numcon];
for (int i = 0; i < numcon - Dim * 2; ++i) {
bkc[i] = MSK_BK_FX;
blc[i] = 0;
buc[i] = 0;
}
for (int i = numcon - Dim * 2; i < numcon - Dim; ++i) // x0 = x^0
{
bkc[i] = MSK_BK_FX;
blc[i] = wayPointsBegin->second(i - (numcon - Dim * 2));
buc[i] = wayPointsBegin->second(i - (numcon - Dim * 2));
}
In last(wayPointsEnd);
--last;
for (int i = numcon - 3; i < numcon; ++i) // xT = x^T
{
bkc[i] = MSK_BK_FX;
blc[i] = last->second(i - (numcon - Dim));
buc[i] = last->second(i - (numcon - Dim));
}
///*No Bounds on variables. */
MSKboundkeye* bkx = new MSKboundkeye[numvar];
double* blx = new double[numvar];
double* bux = new double[numvar];
for (int i = 0; i < numvar; ++i) {
bkx[i] = MSK_BK_FR;
blx[i] = -MSK_INFINITY;
bux[i] = +MSK_INFINITY;
}
MSKrescodee r;
MSKtask_t task = NULL;
/* Create the optimization task. */
r = MSK_maketask(env_, numcon, numvar, &task);
/* Directs the log task stream to the 'printstr' function. */
/*if ( r==MSK_RES_OK )
r = MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);*/
/* Append 'numcon' empty constraints.
The constraints will initially have no bounds. */
if (r == MSK_RES_OK) {
r = MSK_appendcons(task, numcon);
}
/* Append 'numvar' variables.
The variables will initially be fixed at zero (x=0). */
if (r == MSK_RES_OK) {
r = MSK_appendvars(task, numvar);
}
for (int j = 0; j < numvar && r == MSK_RES_OK; ++j) {
/* Set the linear term c_j in the objective.*/
if (r == MSK_RES_OK) {
r = MSK_putcj(task, j, 0);
}
/* Set the bounds on variable j.
blx[j] <= x_j <= bux[j] */
if (r == MSK_RES_OK) {
r = MSK_putvarbound(task, j, /* Index of variable.*/
bkx[j], /* Bound key.*/
blx[j], /* Numerical value of lower bound.*/
bux[j]); /* Numerical value of upper bound.*/
}
/* Input column j of A */
if (r == MSK_RES_OK) {
r = MSK_putacol(task, j, /* Variable (column) index.*/
aptre[j] - aptrb[j], /* Number of non-zeros in column j.*/
asub + aptrb[j], /* Pointer to row indexes of column j.*/
aval + aptrb[j]); /* Pointer to Values of column j.*/
}
}
/* Set the bounds on constraints.
for i=1, ...,numcon : blc[i] <= constraint i <= buc[i] */
for (int i = 0; i < numcon && r == MSK_RES_OK; ++i) {
r = MSK_putconbound(task, i, /* Index of constraint.*/
bkc[i], /* Bound key.*/