├── technical
    └── .gitignore
├── AUTHORS
├── .gitmodules
├── c++
    ├── tests
    │   ├── data
    │   │   ├── Problem01.txt
    │   │   ├── Problem03.txt
    │   │   ├── Problem02.txt
    │   │   └── Problem04.txt
    │   ├── misc.h
    │   ├── SQPTestCommon.h
    │   ├── bindings.py
    │   ├── SQPTestCommon.cpp
    │   ├── ProblemTest.cpp
    │   ├── timings.h
    │   ├── CMakeLists.txt
    │   ├── misc.cpp
    │   ├── main.cpp
    │   ├── SQPTest.cpp
    │   ├── LeastSquareTest.cpp
    │   ├── ObjectiveTest.cpp
    │   ├── MatrixComputationsTest.cpp
    │   └── LinearConstraintsTest.cpp
    ├── bindings
    │   ├── __init__.py
    │   ├── CMakeLists.txt
    │   └── PyCaptureProblemSolver.cpp
    ├── src
    │   ├── GivensSequence.cpp
    │   ├── Givens.cpp
    │   ├── QRAlgorithms.cpp
    │   ├── CondensedOrthogonalMatrix.cpp
    │   ├── BoundenessConstraint.cpp
    │   ├── CMakeLists.txt
    │   ├── ProblemMatrices.cpp
    │   ├── CaptureSolver.cpp
    │   ├── LeastSquare.cpp
    │   ├── Problem.cpp
    │   └── SQP.cpp
    ├── include
    │   └── cps
    │   │   ├── defs.h
    │   │   ├── BoundenessConstraint.h
    │   │   ├── ProblemMatrices.h
    │   │   ├── toMatlab.h
    │   │   ├── cps_api.h
    │   │   ├── Statistics.h
    │   │   ├── GivensSequence.h
    │   │   ├── CaptureSolver.h
    │   │   ├── Givens.h
    │   │   ├── Problem.h
    │   │   ├── LeastSquare.h
    │   │   ├── SQP.h
    │   │   ├── QuadraticObjective.h
    │   │   └── CondensedOrthogonalMatrix.h
    └── CMakeLists.txt
├── matlab
    ├── QR
    │   ├── g2mat.m
    │   ├── applyGivensOnLeft.m
    │   ├── hessenbergQR.m
    │   ├── expandGivens.m
    │   ├── specialGivensCons.m
    │   ├── givensCons.m
    │   ├── specialRext.m
    │   └── specialR.m
    ├── clean.m
    ├── illustration
    │   ├── evalBoundedness.m
    │   ├── solveBoundedness.m
    │   └── display3dCase.m
    ├── lpz.m
    ├── buildObjMatrix.m
    ├── matrix2latex.m
    ├── objMPC.m
    ├── buildLPZ.m
    ├── constrMPC.m
    ├── applyNullspace.m
    ├── maxQPsteplength.m
    ├── universalJj.m
    ├── initialPoint.m
    ├── balanceMPC.m
    ├── multByPinvCaT.m
    ├── feasibilityLS.m
    ├── dedicatedNullspace.m
    ├── directJN.m
    └── feasibilitySQP.m
├── README.md
└── COPYING.LESSER


/technical/.gitignore:
--------------------------------------------------------------------------------
1 | *.aux
2 | *.log
3 | *.pdf
4 | 


--------------------------------------------------------------------------------
/AUTHORS:
--------------------------------------------------------------------------------
1 | Adrien Escande <adrien dot escande at gmail dot com>
2 | Stéphane Caron <stephane dot caron at lirmm dot fr>


--------------------------------------------------------------------------------
/.gitmodules:
--------------------------------------------------------------------------------
1 | [submodule "c++/cmake"]
2 | 	path = c++/cmake
3 | 	url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
4 | 


--------------------------------------------------------------------------------
/c++/tests/data/Problem01.txt:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jrl-umi3218/CaptureProblemSolver/HEAD/c++/tests/data/Problem01.txt


--------------------------------------------------------------------------------
/c++/tests/data/Problem03.txt:
--------------------------------------------------------------------------------
 1 | Delta = [0.010000000000000002, 0.030000000000000006, 0.04999999999999999, 0.07000000000000003, 0.08999999999999997, 0.10999999999999999, 0.12999999999999995, 0.1500000000000002, 0.16999999999999993, 0.18999999999999995];
 2 | 
 3 | g = 9.806650;
 4 | 
 5 | lambda_max = 19.613300;
 6 | 
 7 | lambda_min = 0.980665;
 8 | 
 9 | omega_i_max = 5.943262;
10 | 
11 | omega_i_min = 4.411492;
12 | 
13 | s = [0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0];
14 | 
15 | z_bar = 0.913078;
16 | 
17 | z_f = 0.800000;
18 | 
19 | zd_bar = -0.351191;
20 | 
21 | 
22 | Phi = [0.0, 0.12258312499969491, 0.63249400705140146, 1.6131590170054571, 2.9860900307008413, 4.751287048322923, 6.9087500698697122, 9.4584790953404045, 12.400474124734597, 15.734735158052045, 19.461262389885704];


--------------------------------------------------------------------------------
/c++/tests/data/Problem02.txt:
--------------------------------------------------------------------------------
 1 | Delta = [0.010000000000000002, 0.030000000000000006, 0.04999999999999999, 0.07000000000000003, 0.08999999999999997, 0.10999999999999999, 0.12999999999999995, 0.1500000000000002, 0.16999999999999993, 0.18999999999999995];
 2 | 
 3 | g = 9.806650;
 4 | 
 5 | lambda_max = 19.613300;
 6 | 
 7 | lambda_min = 0.980665;
 8 | 
 9 | omega_i_max = 5.866853;
10 | 
11 | omega_i_min = 4.084177;
12 | 
13 | s = [0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0];
14 | 
15 | z_bar = 0.913078;
16 | 
17 | z_f = 0.800000;
18 | 
19 | zd_bar = -0.329088;
20 | 
21 | 
22 | 
23 | Phi = [0.0, 0.12258312500000001, 0.19028904181864373, 0.25189765210910997, 0.75510636284695154, 2.0373885765819693, 4.1279874579133482, 6.6777164834105038, 9.6197115128303459, 12.953972546172848, 16.68049975024293];


--------------------------------------------------------------------------------
/matlab/QR/g2mat.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %g = (i,n,c,s) describes a nxn matrix that can be obtained with
20 | %G = eye(n); G(i:i+1,i:i+1) = [c s; -s c]
21 | function G = g2mat(g)
22 | G = eye(g.n);
23 | r = g.i + (0:1);
24 | G(r,r) = [g.c g.s; -g.s g.c];
25 | end


--------------------------------------------------------------------------------
/matlab/clean.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %remove elements with absolute value lower than threshold
20 | function Y = clean(X,threshold)
21 | if nargin<2
22 |   threshold = 1e-14;
23 | end
24 | I = abs(X)>threshold;
25 | Y = zeros(size(X));
26 | Y(I) = X(I);
27 | end


--------------------------------------------------------------------------------
/c++/tests/data/Problem04.txt:
--------------------------------------------------------------------------------
 1 | Problem not solved by code version of 17/11/10
 2 | 
 3 | Delta = [0.010000000000000002, 0.030000000000000006, 0.04999999999999999, 0.07000000000000003, 0.08999999999999997, 0.10999999999999999, 0.12999999999999995, 0.1500000000000002, 0.16999999999999993, 0.18999999999999995];
 4 | 
 5 | g = 9.80664999999999942303929856279865;
 6 | 
 7 | lambda_max = 19.61329999999999884607859712559730;
 8 | 
 9 | lambda_min = 0.98066500000000000891731133378926;
10 | 
11 | omega_i_max = 4.42869055139326661674203933216631;
12 | 
13 | omega_i_min = 1.97538235694632402505988011398586;
14 | 
15 | s = [0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0];
16 | 
17 | z_bar = 0.77476050205895352629426042767591;
18 | 
19 | z_f = 0.80000000000000004440892098500626;
20 | 
21 | zd_bar = 0.06934560350799771899499290839231;
22 | 
23 | Phi = [0.0, 0.12258312500000001, 0.4911426569583498, 1.1066066122608129, 1.9697213958163866, 3.0810527370839793, 4.4409864744005922, 6.0497292432247232, 7.9073091000060414, 10.013576102267566, 12.368202859354648];


--------------------------------------------------------------------------------
/c++/tests/misc.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 3 |  *
 4 |  * This file is part of CPS.
 5 |  *
 6 |  * CPS is free software: you can redistribute it and/or modify
 7 |  * it under the terms of the GNU Lesser General Public License as published by
 8 |  * the Free Software Foundation, either version 3 of the License, or
 9 |  * (at your option) any later version.
10 |  *
11 |  * CPS is distributed in the hope that it will be useful,
12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 |  * GNU Lesser General Public License for more details.
15 |  *
16 |  * You should have received a copy of the GNU Lesser General Public License
17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
18 |  */
19 | 
20 | /** Compute a discrete view of the feasible space in (z_i, dz_i, z_f), i.e. the
21 |   * set of inputs in this space for which the problem is feasible.
22 |   * This is output in points.m.
23 |   */
24 | void mapFeasibleInputs();


--------------------------------------------------------------------------------
/matlab/illustration/evalBoundedness.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | function b = evalBoundedness(x,delta,alpha,beta)
20 | n = length(x);
21 | assert(length(delta) == n);
22 | b = delta(1)/sqrt(x(1)) - beta;
23 | for i=1:n-1
24 |   b = b + delta(i+1)/(sqrt(x(i))+sqrt(x(i+1)));
25 | end
26 | b = b - alpha*sqrt(x(end));
27 | end


--------------------------------------------------------------------------------
/matlab/QR/applyGivensOnLeft.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | % perform Y = G_k^T G_{k-1}^T... G_1^T X, where G_i is the Givens rotation 
20 | % described by g(i) (see g2mat).
21 | function Y = applyGivensOnLeft(X,g)
22 | Y = X;
23 | for i=1:length(g)
24 |   gi = g(i);
25 |   Y(gi.i+(0:1),:) = [gi.c -gi.s; gi.s gi.c]* Y(gi.i+(0:1),:);
26 | end
27 | end


--------------------------------------------------------------------------------
/matlab/QR/hessenbergQR.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %perform a QR decomposition on an upper Hessenberg matrix
20 | function [g,R] = hessenbergQR(H)
21 | n = size(H,1);
22 | g(n-1,1) = struct('i',0,'n',0,'c',0.,'s',0.);
23 | R = H;
24 | for i=1:n-1
25 |   disp(i)
26 |   g(i) = givensCons(R,i,i);
27 |   R = applyGivensOnLeft(R,g(i));
28 | end
29 | end


--------------------------------------------------------------------------------
/c++/tests/SQPTestCommon.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 3 |  *
 4 |  * This file is part of CPS.
 5 |  *
 6 |  * CPS is free software: you can redistribute it and/or modify
 7 |  * it under the terms of the GNU Lesser General Public License as published by
 8 |  * the Free Software Foundation, either version 3 of the License, or
 9 |  * (at your option) any later version.
10 |  *
11 |  * CPS is distributed in the hope that it will be useful,
12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 |  * GNU Lesser General Public License for more details.
15 |  *
16 |  * You should have received a copy of the GNU Lesser General Public License
17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
18 |  */
19 | 
20 | #include <cps/Problem.h>
21 | #include <cps/SQP.h>
22 | 
23 | using namespace Eigen;
24 | 
25 | namespace cps
26 | {
27 |   RawProblem resampleProblem(const RawProblem& raw, int n);
28 |   RawProblem resampleProblem(const std::string& filepath, int n);
29 | }
30 | 


--------------------------------------------------------------------------------
/c++/bindings/__init__.py:
--------------------------------------------------------------------------------
 1 | # Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | #
 3 | # This file is part of CPS.
 4 | #
 5 | # CPS is free software: you can redistribute it and/or modify
 6 | # it under the terms of the GNU Lesser General Public License as published by
 7 | # the Free Software Foundation, either version 3 of the License, or
 8 | # (at your option) any later version.
 9 | #
10 | # CPS is distributed in the hope that it will be useful,
11 | # but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | # GNU Lesser General Public License for more details.
14 | #
15 | # You should have received a copy of the GNU Lesser General Public License
16 | # along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | #
18 | 
19 | from .PyCaptureProblemSolver import Problem
20 | from .PyCaptureProblemSolver import RawProblem
21 | from .PyCaptureProblemSolver import SolverStatus
22 | from .PyCaptureProblemSolver import SQP
23 | 
24 | __all__ = [
25 |     "Problem",
26 |     "RawProblem",
27 |     "SolverStatus",
28 |     "SQP",
29 | ]
30 | 


--------------------------------------------------------------------------------
/matlab/QR/expandGivens.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %Considering that G is defined with respect to the submatrix S of a matrix 
20 | %A, return G' such that G'^T A has the same effect on S than G^T S.
21 | %A has n rows, and S begin at row i
22 | function g = expandGivens(g,i,n)
23 | for k=1:length(g)
24 |   g(k).i = g(k).i+i-1;
25 |   g(k).n = n;
26 | end
27 | end


--------------------------------------------------------------------------------
/matlab/lpz.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %solve the lp
20 | % min. c'x
21 | % s.t. l_i <= x_i-x_{i-1} <= u_i for i=1..n
22 | %      xl_j <= x_j <= xu_j for j in idx
23 | %(by convention x_0 = 0, n = length(c))
24 | function x = lpz(c,l,u,idx,xl,xu)
25 | n = length(c);
26 | assert(length(l)==n);
27 | [A,b,lb,ub] = buildLPZ(l,u,idx,xl,xu);
28 | x = linprog(c,A,b,[],[],lb,ub);
29 | end


--------------------------------------------------------------------------------
/matlab/buildObjMatrix.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %Build the objective matrix of the original problem
20 | %
21 | %Example
22 | % delta = [0.01, 0.03, 0.05, 0.07, 0.09, 0.11, 0.13, 0.15, 0.17, 0.19];
23 | % J = buildObjMatrix(delta)
24 | function J = buildObjMatrix(delta)
25 | n = length(delta);
26 | J = [diag(-1./delta(2:end)-1./delta(1:end-1)) zeros(n-1,1)]... 
27 |   + [zeros(n-1,1) diag(1./delta(2:end))]...
28 |   + [diag(1./delta(2:end-1),-1) zeros(n-1,1)];
29 | end


--------------------------------------------------------------------------------
/c++/src/GivensSequence.cpp:
--------------------------------------------------------------------------------
 1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 |  *
 3 |  * This file is part of CPS.
 4 |  *
 5 |  * CPS is free software: you can redistribute it and/or modify
 6 |  * it under the terms of the GNU Lesser General Public License as published by
 7 |  * the Free Software Foundation, either version 3 of the License, or
 8 |  * (at your option) any later version.
 9 |  *
10 |  * CPS is distributed in the hope that it will be useful,
11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 |  * GNU Lesser General Public License for more details.
14 |  *
15 |  * You should have received a copy of the GNU Lesser General Public License
16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 |  */
18 | 
19 | #include <cps/GivensSequence.h>
20 | 
21 | namespace cps
22 | {
23 |   void GivensSequence::extend(int incr)
24 |   {
25 |     for (auto& G : *this)
26 |       G.extend(incr);
27 |   }
28 | 
29 |   Eigen::MatrixXd GivensSequence::matrix(int n)
30 |   {
31 |     Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(n,n);
32 |     for (const auto& G : *this)
33 |       G.applyOnTheRightTo(Q);
34 | 
35 |     return Q;
36 |   }
37 | }
38 | 


--------------------------------------------------------------------------------
/matlab/matrix2latex.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %A small utility function to write latex code from a matrix
20 | %\BIN and \BOUT, are shortcuts for \begin{bmatrix} and \end{bmatrix}
21 | function s = matrix2latex(M)
22 | [m,n] = size(M);
23 | S = num2str(M(:,1));
24 | delim = repmat(' & ',[m,1]);
25 | for i=2:n
26 |   S = [S delim num2str(M(:,i))];
27 | end
28 | l = size(S,2);
29 | s = ['\BIN' repmat(' ',[1,l+1]);
30 |      repmat(' ',[m,2]) S repmat(' \\',[m,1]); 
31 |      '\BOUT' repmat(' ',[1,l])];
32 | end


--------------------------------------------------------------------------------
/c++/src/Givens.cpp:
--------------------------------------------------------------------------------
 1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 |  *
 3 |  * This file is part of CPS.
 4 |  *
 5 |  * CPS is free software: you can redistribute it and/or modify
 6 |  * it under the terms of the GNU Lesser General Public License as published by
 7 |  * the Free Software Foundation, either version 3 of the License, or
 8 |  * (at your option) any later version.
 9 |  *
10 |  * CPS is distributed in the hope that it will be useful,
11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 |  * GNU Lesser General Public License for more details.
14 |  *
15 |  * You should have received a copy of the GNU Lesser General Public License
16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 |  */
18 | 
19 | #include <cps/Givens.h>
20 | 
21 | namespace cps
22 | {
23 |   Givens::Givens()
24 |     : Givens(0,0,1,0)
25 |   {
26 |   }
27 | 
28 |   Givens::Givens(Index i, Index j, double c, double s)
29 |     : i_(i), j_(j), Jt_(c,-s)
30 |   {
31 |   }
32 | 
33 |   Givens::Givens(Index i, double c, double s)
34 |     : Givens(i,i+1,c,s)
35 |   {
36 |   }
37 | 
38 |   void Givens::extend(Index incr)
39 |   {
40 |     i_ += incr;
41 |     j_ += incr;
42 |   }
43 | }
44 | 


--------------------------------------------------------------------------------
/matlab/QR/specialGivensCons.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | % return the nxn Givens rotations G such that 
20 | % G(i:i+1,i:i+1)^T [-sqrt(i)/i;1] = [X;0]
21 | % G is encoded as g (see g2mat)
22 | % if i is a vector, return a vector of structures.
23 | function g = specialGivensCons(i,n)
24 | %we go downward only to be memory efficient: the size of g is fixed after
25 | %the first iteration
26 | for k=length(i):-1:1
27 |   j = i(k);
28 |   s.i = j;
29 |   s.n = n;
30 |   s.c = 1/sqrt(j+1);
31 |   s.s = sqrt(j)*s.c;
32 |   g(k) = s;
33 | end
34 | end


--------------------------------------------------------------------------------
/c++/tests/bindings.py:
--------------------------------------------------------------------------------
 1 | # Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | #
 3 | # This file is part of CPS.
 4 | #
 5 | # CPS is free software: you can redistribute it and/or modify
 6 | # it under the terms of the GNU Lesser General Public License as published by
 7 | # the Free Software Foundation, either version 3 of the License, or
 8 | # (at your option) any later version.
 9 | #
10 | # CPS is distributed in the hope that it will be useful,
11 | # but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | # GNU Lesser General Public License for more details.
14 | #
15 | # You should have received a copy of the GNU Lesser General Public License
16 | # along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | #
18 | 
19 | import os
20 | 
21 | from CaptureProblemSolver import Problem, RawProblem, SQP
22 | 
23 | 
24 | def load_problem(number):
25 |     dirname = os.path.dirname(os.path.realpath(__file__)) + "/data/"
26 |     basename = "Problem%02d.txt" % number
27 |     raw_pb = RawProblem()
28 |     raw_pb.read(dirname + basename)
29 |     return Problem(raw_pb)
30 | 
31 | 
32 | if __name__ == "__main__":
33 |     pb = load_problem(1)
34 |     sqp = SQP(pb.size)
35 |     sqp.solve(pb)
36 |     print "SQP solution:", sqp.x()
37 | 


--------------------------------------------------------------------------------
/matlab/QR/givensCons.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %return the Givens rotation zeroing A(i+1,j) with A(i,j)
20 | %g is a structure (i,n,c,s)
21 | %g described a nxn matrix that can be obtained with
22 | %G = eye(n); G(i:i+1,i:i+1) = [c s; -s c]
23 | function g = givensCons(A,i,j)
24 | g.i = i;
25 | g.n = size(A,1);
26 | a = A(i,j);
27 | b = A(i+1,j);
28 | if b==0
29 |   g.c = 1;
30 |   g.s = 0;
31 | else
32 |   if abs(b)>abs(a)
33 |     t = -a/b;
34 |     s = 1/sqrt(1+t^2);
35 |     g.c = s*t;
36 |     g.s = s;
37 |   else
38 |     t = -b/a;
39 |     g.c = 1/sqrt(1+t^2);
40 |     g.s = g.c*t;
41 |   end
42 | end
43 | end


--------------------------------------------------------------------------------
/matlab/illustration/solveBoundedness.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %given the equation 
20 | %sum (delta(i)/(sqrt(x(j)) + sqrt(x(j+1))) - alpha * sqrt(x(end)) = beta
21 | %find the positive solution in x(end), given the values x(1:end-1)
22 | function z = solveBoundedness(delta,alpha,beta,x)
23 | n = length(delta);
24 | a = delta(1)/sqrt(x(1)) - beta;
25 | for i=1:n-2
26 |   a = a + delta(i+1)/(sqrt(x(i)) + sqrt(x(i+1)));
27 | end
28 | b = sqrt(x(n-1));
29 | discr = (a-alpha*b)^2 + 4*alpha*(a*b+delta(n));
30 | if discr<0
31 |   z = NaN;
32 | else
33 |   z = ((-(a-alpha*b) - sqrt(discr))/(-2*alpha))^2;
34 | end
35 | end


--------------------------------------------------------------------------------
/c++/include/cps/defs.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 3 |  *
 4 |  * This file is part of CPS.
 5 |  *
 6 |  * CPS is free software: you can redistribute it and/or modify
 7 |  * it under the terms of the GNU Lesser General Public License as published by
 8 |  * the Free Software Foundation, either version 3 of the License, or
 9 |  * (at your option) any later version.
10 |  *
11 |  * CPS is distributed in the hope that it will be useful,
12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 |  * GNU Lesser General Public License for more details.
15 |  *
16 |  * You should have received a copy of the GNU Lesser General Public License
17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
18 |  */
19 | 
20 | #include <Eigen/Core>
21 | 
22 | namespace cps
23 | {
24 |   typedef Eigen::Ref<const Eigen::MatrixXd> MatrixConstRef;
25 |   typedef Eigen::Ref<Eigen::MatrixXd>       MatrixRef;
26 |   typedef Eigen::Ref<const Eigen::VectorXd> VectorConstRef;
27 |   typedef Eigen::Ref<Eigen::VectorXd>       VectorRef;
28 | 
29 |   /** Enums for specifying the shape of the top-left and bottom-right corners
30 |     * of some matrices. 
31 |     * See LeastSquareObjective::buildJj and the class SpecialQR
32 |     */
33 |   enum class StartType { Case1, Case2, Case3 };
34 |   enum class EndType { Case1, Case2, Case3, Case4 };
35 | }


--------------------------------------------------------------------------------
/c++/tests/SQPTestCommon.cpp:
--------------------------------------------------------------------------------
 1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 |  *
 3 |  * This file is part of CPS.
 4 |  *
 5 |  * CPS is free software: you can redistribute it and/or modify
 6 |  * it under the terms of the GNU Lesser General Public License as published by
 7 |  * the Free Software Foundation, either version 3 of the License, or
 8 |  * (at your option) any later version.
 9 |  *
10 |  * CPS is distributed in the hope that it will be useful,
11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 |  * GNU Lesser General Public License for more details.
14 |  *
15 |  * You should have received a copy of the GNU Lesser General Public License
16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 |  */
18 | 
19 | #include "SQPTestCommon.h"
20 | 
21 | namespace cps
22 | {
23 |   RawProblem resampleProblem(const RawProblem& raw, int n)
24 |   {
25 |     auto raw2 = raw;
26 |     VectorXd s = VectorXd::LinSpaced(n + 1, 0, 1);
27 |     raw2.delta.resize(n);
28 |     raw2.delta.array() = s.tail(n).array()*s.tail(n).array() - s.head(n).array()*s.head(n).array();
29 | 
30 |     //we invalidate the solution
31 |     raw2.Phi_.resize(0);
32 | 
33 |     return raw2;
34 |   }
35 | 
36 |   RawProblem resampleProblem(const std::string& filepath, int n)
37 |   {
38 |     RawProblem raw;
39 |     std::string base = TESTS_DIR;
40 |     raw.read(base + "/" + filepath);
41 | 
42 |     return resampleProblem(raw, n);
43 |   }
44 | }


--------------------------------------------------------------------------------
/c++/src/QRAlgorithms.cpp:
--------------------------------------------------------------------------------
 1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 |  *
 3 |  * This file is part of CPS.
 4 |  *
 5 |  * CPS is free software: you can redistribute it and/or modify
 6 |  * it under the terms of the GNU Lesser General Public License as published by
 7 |  * the Free Software Foundation, either version 3 of the License, or
 8 |  * (at your option) any later version.
 9 |  *
10 |  * CPS is distributed in the hope that it will be useful,
11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 |  * GNU Lesser General Public License for more details.
14 |  *
15 |  * You should have received a copy of the GNU Lesser General Public License
16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 |  */
18 | 
19 | #include <cps/QRAlgorithms.h>
20 | 
21 | using namespace Eigen;
22 | 
23 | namespace cps
24 | {
25 |   SpecialQR::SpecialQR(DenseIndex kmax)
26 |     : e_(kmax + 1)
27 |     , c1_(kmax)
28 |     , c2_(kmax)
29 |     , d_(kmax)
30 |     , l_(kmax)
31 |     , dprecomp_(-VectorXd::LinSpaced(static_cast<int>(kmax),1, static_cast<int>(kmax)))
32 |     , lprecomp_((ArrayXd::LinSpaced(static_cast<int>(kmax), 1, static_cast<int>(kmax)).sqrt()*ArrayXd::LinSpaced(static_cast<int>(kmax), 2, static_cast<int>(kmax) + 1).sqrt()).inverse().matrix())
33 |   {
34 |     G_.reserve(kmax);
35 |     for (DenseIndex i = 0; i < kmax; ++i)
36 |     {
37 |       double c = 1 / sqrt(double(i) + 2.);
38 |       G_.emplace_back(i, c, sqrt(double(i) + 1.)*c);
39 |     }
40 |   }
41 | }
42 | 


--------------------------------------------------------------------------------
/matlab/QR/specialRext.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %Extension of specialR to more types of ending
20 | %See universalJj for the meaning of endType
21 | function R = specialRext(e,endType)
22 | n = length(e);
23 | d = -(1:n)';
24 | l = 1./sqrt((1:n).*(2:n+1))';
25 | if endType==2 || endType==3
26 |   d(end) = 0;
27 |   l(end) = 1/sqrt(n);
28 | end
29 | c1 = l.*(d-1).*e;
30 | c2 = l.*d.*[e(2:end);0];
31 | switch endType
32 |   case 1
33 |     R = [diag(c1) - diag(c1(1:end-1)+c2(1:end-1),1) + diag(c2(1:end-2),2);zeros(1,n)];
34 |   case 2
35 |     R = diag(c1) - diag(c1(1:end-1)+c2(1:end-1),1) + diag(c2(1:end-2),2);
36 |   case 3
37 |     R = [diag(c1) zeros(n,1)] - [zeros(n,1) diag(c1+c2)] + [zeros(n,1) diag(c2(1:end-1),1)];
38 |   case 4
39 |     R = [[diag(c1) zeros(n,1)] - [zeros(n,1) diag(c1+c2)] + [zeros(n,1) diag(c2(1:end-1),1)]; zeros(1,n+1)]; 
40 | end
41 | end


--------------------------------------------------------------------------------
/matlab/objMPC.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %Objective function of the original capture problem
20 | %sum(((x[i+1]-x[i])\delta[i] - (x[i]-x[i-1])\delta[i-1])^2) (with x[0] = 0)
21 | function [f,gradf] = objMPC(delta, x)
22 | n = length(x);
23 | assert(length(delta) == n)
24 | y = [0;x];
25 | f = 0;
26 | for i=1:n-1
27 |   f = f + ((y(i+2)-y(i+1))/delta(i+1) - (y(i+1)-y(i+0))/delta(i+0))^2;
28 | end
29 | if nargout > 1
30 |   gradf = zeros(n,1);
31 |   for j=1:n-1
32 |     if j>1
33 |       gradf(j-1) = gradf(j-1) - 2*(y(j+1)-y(j+0))/delta(j)^2 + 2*(y(j+2)-y(j+1))/(delta(j+1)*delta(j));
34 |     end
35 |     gradf(j) = gradf(j) - 2*(y(j+2)-y(j+1))/delta(j+1)^2 - 2*(y(j+2)-2*y(j+1)+y(j+0))/(delta(j+1)*delta(j)) + 2*(y(j+1)-y(j))/delta(j)^2;
36 |     gradf(j+1) = gradf(j+1) + 2*(y(j+2)-y(j+1))/delta(j+1)^2 - 2*(y(j+1)-y(j))/(delta(j+1)*delta(j));
37 |   end
38 | end
39 | end


--------------------------------------------------------------------------------
/c++/tests/ProblemTest.cpp:
--------------------------------------------------------------------------------
 1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 |  *
 3 |  * This file is part of CPS.
 4 |  *
 5 |  * CPS is free software: you can redistribute it and/or modify
 6 |  * it under the terms of the GNU Lesser General Public License as published by
 7 |  * the Free Software Foundation, either version 3 of the License, or
 8 |  * (at your option) any later version.
 9 |  *
10 |  * CPS is distributed in the hope that it will be useful,
11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 |  * GNU Lesser General Public License for more details.
14 |  *
15 |  * You should have received a copy of the GNU Lesser General Public License
16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 |  */
18 | 
19 | #include <iostream>
20 | 
21 | #include <cps/Problem.h>
22 | #include <cps/BoundenessConstraint.h>
23 | #include <cps/SQP.h>
24 | 
25 | // boost
26 | #define BOOST_TEST_MODULE ProblemTest
27 | #include <boost/test/unit_test.hpp>
28 | 
29 | using namespace Eigen;
30 | using namespace cps;
31 | 
32 | BOOST_AUTO_TEST_CASE(BoundenessConstraintTest)
33 | {
34 |   VectorXd delta = VectorXd::LinSpaced(10, 0.01, 0.19);
35 |   BoundenessConstraint bc(delta, 1, 1);
36 | 
37 |   VectorXd x = VectorXd::Ones(10);
38 |   VectorXd grad0(10);
39 |   grad0 << -0.00875, -0.01, -0.015, -0.02, -0.025, -0.03, -0.035, -0.04, -0.045, -0.52375;
40 | 
41 |   double y;
42 |   VectorXd grad(10);
43 |   bc.compute(y, grad, x);
44 | 
45 |   BOOST_CHECK(std::abs(y + 1.495) <= 1e-15);
46 |   BOOST_CHECK(grad.isApprox(grad0, 1e-15));
47 | }
48 | 


--------------------------------------------------------------------------------
/matlab/buildLPZ.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %build the data for the feasibility problem 
20 | % find x
21 | % s.t. l_i <= x_i-x_{i-1} <= u_i for i=1..n
22 | %      xl_j <= x_j <= xu_j for j in idx
23 | function [A,b,lb,ub] = buildLPZ(l,u,idx,xl,xu)
24 | if nargin<3
25 |   idx=[];
26 |   xl=[];
27 |   xu=[];
28 | end
29 | n = length(l);
30 | assert(length(u)==n);
31 | assert(length(idx)<=n);
32 | assert(all(idx)>0 && all(idx)<=n);
33 | assert(length(idx)==length(xl));
34 | assert(length(idx)==length(xu));
35 | 
36 | lb=-1e4*ones(n,1);
37 | lb(idx) = xl;
38 | ub=1e4*ones(n,1);
39 | ub(idx) = xu;
40 | A = zeros(2*n,n);
41 | b = zeros(2*n,1);
42 | A(1,1) = -1; b(1) = -l(1); %l(1) <= x_1
43 | A(2,1) = 1; b(2) = u(1); % x_1 <= u(1)
44 | for i=2:n
45 |   % l_i <= x_i-x_{i-1}
46 |   A(2*i-1,i-1) = 1;
47 |   A(2*i-1,i) = -1;
48 |   b(2*i-1) = -l(i);
49 |   % x_i-x_{i-1} <= u_i
50 |   A(2*i,i-1) = -1;
51 |   A(2*i,i) = 1;
52 |   b(2*i) = u(i);
53 | end
54 | end


--------------------------------------------------------------------------------
/matlab/constrMPC.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %The non-linear equality constraint
20 | %sum(delta_j/(sqrt(x(j+1))+sqrt(x(j))) - alpha sqrt(x(n)) - b
21 | function [cineq,c,gradcineq,gradc] = constrMPC(delta, x, alpha, b, relax)
22 | if nargin < 5, relax = false; end
23 | n = length(x);
24 | assert(length(delta) == n)
25 | y = [0;x];
26 | cineq=[];
27 | c = -alpha*sqrt(x(n))-b;
28 | for i=0:n-1
29 |   c = c + delta(i+1)/(sqrt(y(i+2))+sqrt(y(i+1)));
30 | end
31 | if nargout > 3
32 |   gradcineq = [];
33 |   gradc = zeros(n,1);
34 |   gradc(n) = -alpha/(2*sqrt(x(n)));
35 |   for i=0:n-1
36 |     if (i>0)
37 |       gradc(i) = gradc(i) - delta(i+1)/(2*sqrt(y(i+1))*(sqrt(y(i+2))+sqrt(y(i+1)))^2);
38 |     end
39 |     gradc(i+1) = gradc(i+1) - delta(i+1)/(2*sqrt(y(i+2))*(sqrt(y(i+2))+sqrt(y(i+1)))^2);
40 |   end
41 |   if relax
42 |     gradcineq = gradc;
43 |     gradc = [];
44 |   end
45 | end
46 | if relax
47 |   cineq = c;
48 |   c = [];
49 | end
50 | end


--------------------------------------------------------------------------------
/c++/bindings/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | # Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | #
 3 | # This file is part of CPS.
 4 | #
 5 | # CPS is free software: you can redistribute it and/or modify
 6 | # it under the terms of the GNU Lesser General Public License as published by
 7 | # the Free Software Foundation, either version 3 of the License, or
 8 | # (at your option) any later version.
 9 | #
10 | # CPS is distributed in the hope that it will be useful,
11 | # but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | # GNU Lesser General Public License for more details.
14 | #
15 | # You should have received a copy of the GNU Lesser General Public License
16 | # along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | #
18 | 
19 | INCLUDE(../cmake/python.cmake)
20 | FINDPYTHON()
21 | INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH})
22 | 
23 | set(MODULE_NAME "CaptureProblemSolver")
24 | 
25 | configure_file(__init__.py ${CMAKE_CURRENT_BINARY_DIR}/${MODULE_NAME}/__init__.py COPYONLY)
26 | 
27 | set(PYCPS_SOURCES PyCaptureProblemSolver.cpp)
28 | 
29 | set(CPS_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../include)
30 | set(PYCPS_HEADERS converters.h ${CPS_INCLUDE_DIR}/cps/cps_api.h)
31 | 
32 | add_library(PyCaptureProblemSolver SHARED ${PYCPS_SOURCES} ${PYCPS_HEADERS})
33 | target_link_libraries(PyCaptureProblemSolver CaptureProblemSolver boost_numpy)
34 | TARGET_LINK_BOOST_PYTHON(PyCaptureProblemSolver)
35 | set_target_properties(PyCaptureProblemSolver PROPERTIES PREFIX "")
36 | install(TARGETS PyCaptureProblemSolver DESTINATION "${PYTHON_SITELIB}/${MODULE_NAME}")
37 | 
38 | PYTHON_INSTALL_BUILD(${MODULE_NAME} __init__.py "${PYTHON_SITELIB}")
39 | 


--------------------------------------------------------------------------------
/c++/include/cps/BoundenessConstraint.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 3 |  *
 4 |  * This file is part of CPS.
 5 |  *
 6 |  * CPS is free software: you can redistribute it and/or modify
 7 |  * it under the terms of the GNU Lesser General Public License as published by
 8 |  * the Free Software Foundation, either version 3 of the License, or
 9 |  * (at your option) any later version.
10 |  *
11 |  * CPS is distributed in the hope that it will be useful,
12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 |  * GNU Lesser General Public License for more details.
15 |  *
16 |  * You should have received a copy of the GNU Lesser General Public License
17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
18 |  */
19 | 
20 | #include <Eigen/Core>
21 | 
22 | #include <cps/cps_api.h>
23 | #include <cps/defs.h>
24 | 
25 | namespace cps
26 | {
27 |   /** The function sum(delta_j/(sqrt(x(j+1))+sqrt(x(j))) - alpha sqrt(x(n)) - b*/
28 |   class CPS_DLLAPI BoundenessConstraint
29 |   {
30 |   public:
31 |     BoundenessConstraint(const Eigen::VectorXd& delta, double alpha, double b);
32 | 
33 |     void compute(double& val, const VectorConstRef& x) const;
34 |     void compute(double& val, VectorRef grad, const VectorConstRef& x) const;
35 | 
36 |     // alternative version
37 |     double compute(const VectorConstRef& x) const;
38 | 
39 |     void setAlpha(double alpha);
40 |     void setb(double b);
41 | 
42 |   private:
43 |     Eigen::DenseIndex n_;
44 |     double alpha_;
45 |     double b_;
46 |     Eigen::VectorXd delta_;
47 | 
48 |     //intermediate data
49 |     mutable Eigen::VectorXd y_;
50 |   };
51 | }
52 | 


--------------------------------------------------------------------------------
/matlab/applyNullspace.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %multiply efficiently the matrix A by the nullspace basis N that 
20 | %dedicatedNullspace(az, an) would have returned (see dedicatedNullspace for
21 | %more details).
22 | %idx is such that N*B = B(idx,:)
23 | %
24 | %Example
25 | % MN = applyNullspace([1 1 0 0 1 0 0 1 1 1],false)
26 | function [AN,idx,activeIndex] = applyNullspace(A,az,an)
27 | n = length(az);
28 | na = sum(az) +an;
29 | AN = zeros(size(A,1),n-na);
30 | idx = zeros(n,1);
31 | activeIndex = zeros(na,1);
32 | 
33 | c = 0;
34 | k = 1;
35 | %skip first group of activated constraints
36 | while k<=n && az(k)
37 |   activeIndex(k) = k;
38 |   k = k+1;
39 | end
40 | ca = k;
41 | for i=k:n
42 |   if az(i)
43 |     AN(:,c) = AN(:,c) + A(:,i);
44 |     idx(i) = c;
45 |     activeIndex(ca) = i;
46 |     ca = ca+1;
47 |   else
48 |     c = c+1;
49 |     if (c>n-na)
50 |       break;
51 |     end
52 |     AN(:,c) = A(:,i);
53 |     idx(i) = c;
54 |   end
55 | end
56 | activeIndex(ca:end) = n+ca+1-na:n+1;


--------------------------------------------------------------------------------
/matlab/maxQPsteplength.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | % Given the current point x and a step p, find a maximal such that x+ap
20 | % verify the constraints
21 | % l_i <= x_i-x_{i-1} <= u_i for i=1..n  (x_0 = 0)
22 | % xln <= x_n <= xun
23 | %
24 | % i is the number of a constraint that prevents a to be bigger.
25 | % It is 0 if p=1, and n+1 if the blocking constraints is the last one.
26 | % type=1 (resp. -1) if the upper (resp. lower) bound is blocking and 0 if 
27 | % p=1. 
28 | function [a,i,type] = maxQPsteplength(x,p,l,u,xln,xun,activeZ,activeN)
29 | act = [activeZ;activeN];
30 | 
31 | Ax = [x(1);x(2:end)-x(1:end-1);x(end)];
32 | Ap = [p(1);p(2:end)-p(1:end-1);p(end)];
33 | 
34 | L = ([l;xln]-Ax)./Ap;
35 | U = ([u;xun]-Ax)./Ap;
36 | sup = Ap>0 & ~act;
37 | inf = Ap<0 & ~act;
38 | 
39 | alpha(sup) = U(sup);
40 | alpha(inf) = L(inf);
41 | alpha(act) = 1e10;
42 | types(sup) = 1;
43 | types(inf) = -1;
44 | 
45 | [a,i] = min(alpha);
46 | type = types(i);
47 | a = min(a,1);
48 | if a==1
49 |   i=0;
50 |   type = 0;
51 | end
52 | end


--------------------------------------------------------------------------------
/c++/tests/timings.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 3 |  *
 4 |  * This file is part of CPS.
 5 |  *
 6 |  * CPS is free software: you can redistribute it and/or modify
 7 |  * it under the terms of the GNU Lesser General Public License as published by
 8 |  * the Free Software Foundation, either version 3 of the License, or
 9 |  * (at your option) any later version.
10 |  *
11 |  * CPS is distributed in the hope that it will be useful,
12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 |  * GNU Lesser General Public License for more details.
15 |  *
16 |  * You should have received a copy of the GNU Lesser General Public License
17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
18 |  */
19 | 
20 | 
21 | #include <chrono>
22 | #include <fstream>
23 | #include <iostream>
24 | #include <vector>
25 | 
26 | #include <Eigen/Core>
27 | #include <Eigen/Jacobi>
28 | #include <Eigen/QR>
29 | 
30 | #include <cps/LeastSquare.h>
31 | #include <cps/Problem.h>
32 | #include <cps/ProblemMatrices.h>
33 | #include <cps/QRAlgorithms.h>
34 | #include <cps/QuadraticObjective.h>
35 | #include <cps/SQP.h>
36 | 
37 | #include "SQPTestCommon.h"
38 | 
39 | 
40 | /** n: size of matrices, N: number of tests*/
41 | void QRPerformances(int n, const int N);
42 | void LSPerformance(int n, const int N);
43 | void QRJAPerformance(int n, const int N);
44 | void SQPPerformance(const std::string& filepath, int n, const int N);
45 | 
46 | /** Test SQP timings
47 |  *
48 |  * \param filepath Paths to problem files
49 |  *
50 |  * \param n Problem sizes (-1: original, otherwise resampled to the given value)
51 |  *
52 |  * \param N Number of test samples
53 |  */
54 | void SQPTimings(const std::vector<std::string>& filepath, const std::vector<int>& n, const int N);


--------------------------------------------------------------------------------
/matlab/QR/specialR.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %Compute the triangular matrix of the QR decomposition of A, where A has
20 | %the form
21 | % | -e0    e0                                      |
22 | % |  e0  -e0-e1   e1                               |
23 | % |        e1   -e1-e2  e2                         |
24 | % |               ...                              |
25 | % |                     e_{k-1}  -e_{k-1}-e_k  e_k |
26 | % |                                  e_k      -e_k |
27 | % when ext is false (default) and the last element -e_k is replaced by
28 | % -e_k-e_{k+1} if ext is true
29 | %
30 | % The matrix R is such that Q*R = A where Q is obtained with
31 | % specialGivensCons
32 | function R = specialR(A,ext)
33 | if nargin<2
34 |   ext = false;
35 | end
36 | e = [diag(A,-1);0];
37 | if ext
38 |  e(end) = -A(end,end)-A(end-1,end);
39 | end
40 | d = -(1:length(e)-1)';
41 | l = 1./sqrt((1:length(e)-1).*(2:length(e)))';
42 | c1 = l.*(d-1).*e(1:end-1);
43 | c2 = l.*d.*e(2:end);
44 | R = diag([c1;0])-diag(c1+c2,1) + diag(c2(1:end-1),2);
45 | if ext
46 |  R(end) = (A(end,end)+A(end-1,end))/sqrt(length(e));
47 | end
48 | end


--------------------------------------------------------------------------------
/c++/include/cps/ProblemMatrices.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 3 |  *
 4 |  * This file is part of CPS.
 5 |  *
 6 |  * CPS is free software: you can redistribute it and/or modify
 7 |  * it under the terms of the GNU Lesser General Public License as published by
 8 |  * the Free Software Foundation, either version 3 of the License, or
 9 |  * (at your option) any later version.
10 |  *
11 |  * CPS is distributed in the hope that it will be useful,
12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 |  * GNU Lesser General Public License for more details.
15 |  *
16 |  * You should have received a copy of the GNU Lesser General Public License
17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
18 |  */
19 | 
20 | #include <Eigen/Core>
21 | 
22 | #include <cps/cps_api.h>
23 | 
24 | namespace cps
25 | {
26 |   /** A collection of functions to generate the problem matrices.
27 |     *
28 |     * Mostly for debugging purposes
29 |     */
30 | 
31 |   /** Build J such that the objective is ||Jx||^2 where d = 1/delta */
32 |   CPS_DLLAPI Eigen::MatrixXd buildJ(const Eigen::VectorXd& d);
33 | 
34 |   /** Build J_0 without the initial 0 rows (see doc)*/
35 |   CPS_DLLAPI Eigen::MatrixXd buildJ0(const Eigen::VectorXd& e);
36 |   
37 |   /** Build J_j (see doc)*/
38 |   CPS_DLLAPI Eigen::MatrixXd buildJj(const Eigen::VectorXd& e);
39 |   
40 |   /** Build J_{p-1} (see doc)*/
41 |   CPS_DLLAPI Eigen::MatrixXd buildJpm1(const Eigen::VectorXd& e);
42 | 
43 |   /** Build the matrix of zonotopique constraints l_i <= x_i - x_{i-1} <= u_i
44 |     * n is the size of x
45 |     */
46 |   CPS_DLLAPI Eigen::MatrixXd buildCZ(int n);
47 | 
48 |   /** [C_Z;e_n^T] where e_n is the n-th column of the nxn identity matrix.*/
49 |   CPS_DLLAPI Eigen::MatrixXd buildC(int n);
50 | }
51 | 


--------------------------------------------------------------------------------
/c++/include/cps/toMatlab.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 3 |  *
 4 |  * This file is part of CPS.
 5 |  *
 6 |  * CPS is free software: you can redistribute it and/or modify
 7 |  * it under the terms of the GNU Lesser General Public License as published by
 8 |  * the Free Software Foundation, either version 3 of the License, or
 9 |  * (at your option) any later version.
10 |  *
11 |  * CPS is distributed in the hope that it will be useful,
12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 |  * GNU Lesser General Public License for more details.
15 |  *
16 |  * You should have received a copy of the GNU Lesser General Public License
17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
18 |  */
19 | 
20 | #include <Eigen/Dense>
21 | #include <iosfwd>
22 | 
23 | 
24 | /** A small utility class to write Eigen matrices in a stream with a matlab-readable format.
25 |   *
26 |   * Example of use:
27 |   * Eigen::MatrixXd M = Eigen::MatrixXd::Random(5,6);
28 |   * std::cout << (toMatlab)M << std::endl;
29 |   *
30 |   * Inspired from a code from Nicolas Mansard.
31 |   */
32 | class toMatlab
33 | {
34 | public:
35 |   template<typename Derived>
36 |   toMatlab(const Eigen::DenseBase<Derived>& M)
37 |     : mat(M)
38 |   {}
39 | 
40 | private:
41 |   Eigen::MatrixXd mat;
42 | 
43 |   friend std::ostream& operator<< (std::ostream&, const toMatlab&);
44 | };
45 | 
46 | inline std::ostream& operator<< (std::ostream& o, const toMatlab& tom)
47 | {
48 |   if (tom.mat.cols() == 1)
49 |   {
50 |     Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", ";", "", "", "[", "]");
51 |     o << tom.mat.format(fmt);
52 |   }
53 |   else
54 |   {
55 |     Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
56 |     o << tom.mat.format(fmt);
57 |   }
58 |   return o;
59 | }


--------------------------------------------------------------------------------
/matlab/universalJj.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | % Build a matrix with main body of the form
20 | %  S x
21 | %  x x x
22 | %    x x x
23 | %      ...
24 | %       x x x
25 | %         x E
26 | % where S and E are specified by startType and endType, and the x stands
27 | % for diagonles with terms of e
28 | % startType:
29 | % - 1: S=[e(1); -e(1)-e(2)]
30 | % - 2: S=-e(1)-e(2);
31 | % - 3: S=-e(1);
32 | % stopType:
33 | % - 1: E=[same;e(k)];
34 | % - 2: E=same;
35 | % - 3: E=[same, e(k)];
36 | % - 4: E=-e(k)
37 | function Jj = universalJj(e,startType,endType)
38 | n = length(e);
39 | e = [e(:);0];
40 | if startType==1, s=1; else s=0; end
41 | if endType==1, eb=1; else eb=0; end
42 | if endType==3, er=1; else er=0; end
43 | if startType==3, ks=1; else ks=2; end
44 | if endType==4, ke=n; else ke=n-1; end
45 |   
46 | if n==0 %n==1 && endType~=4
47 |   Jj=zeros(0,1);
48 |   return;
49 | else
50 |   Jj = [zeros(s,ke-ks+2+er); 
51 |         (diag(e(ks:ke),-1) - diag([0;e(ks:ke)+e(ks+1:ke+1)]) + diag(e(ks:ke),1)) zeros(ke-ks+2,er);
52 |         zeros(eb,ke-ks+2+er)];
53 | end
54 | switch startType
55 |   case 1, Jj(1:2,1) = [e(1);-e(1)-e(2)];
56 |   case 2, Jj(1,1) = -e(1)-e(2);
57 |   case 3, Jj(1,1) = -e(1);
58 | end
59 | if endType == 1 || endType == 3
60 |   Jj(end,end) = e(n);
61 | end
62 | end


--------------------------------------------------------------------------------
/c++/tests/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | # Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | #
 3 | # This file is part of CPS.
 4 | #
 5 | # CPS is free software: you can redistribute it and/or modify
 6 | # it under the terms of the GNU Lesser General Public License as published by
 7 | # the Free Software Foundation, either version 3 of the License, or
 8 | # (at your option) any later version.
 9 | #
10 | # CPS is distributed in the hope that it will be useful,
11 | # but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | # GNU Lesser General Public License for more details.
14 | #
15 | # You should have received a copy of the GNU Lesser General Public License
16 | # along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | #
18 | 
19 | add_definitions(-DBOOST_TEST_DYN_LINK)
20 | include_directories(${Boost_INCLUDE_DIRS})
21 | if(WIN32)
22 |   #This is one of the way to avoid link errors related to static variables in program_options
23 |   add_definitions( -DBOOST_ALL_DYN_LINK )
24 | endif(WIN32)
25 | 
26 | add_definitions(-DTESTS_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
27 | 
28 | macro(addUnitTest name)
29 |   add_executable(${name} ${name}.cpp ${ARGN})
30 |   target_link_libraries(${name} ${Boost_LIBRARIES} CaptureProblemSolver)
31 |   add_test(${name}Unit ${name})
32 |   # Adding a project configuration file (for MSVC only)
33 |   GENERATE_MSVC_DOT_USER_FILE(${name})
34 | endmacro(addUnitTest)
35 | 
36 | addUnitTest(MatrixComputationsTest)
37 | addUnitTest(LeastSquareTest)
38 | addUnitTest(LinearConstraintsTest)
39 | addUnitTest(ProblemTest)
40 | addUnitTest(ObjectiveTest)
41 | addUnitTest(SQPTest SQPTestCommon.h SQPTestCommon.cpp)
42 | 
43 | set(TESTSOURCES
44 |   main.cpp
45 |   misc.cpp
46 |   SQPTestCommon.cpp
47 |   timings.cpp
48 | )
49 | 
50 | set(TESTHEADERS
51 |   misc.h
52 |   SQPTestCommon.h
53 |   timings.h
54 | )
55 | 
56 | add_executable(main ${TESTSOURCES} ${TESTHEADERS})
57 | target_link_libraries(main PUBLIC CaptureProblemSolver)
58 | GENERATE_MSVC_DOT_USER_FILE(main)
59 | 
60 | 


--------------------------------------------------------------------------------
/c++/include/cps/cps_api.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 3 |  *
 4 |  * This file is part of CPS.
 5 |  *
 6 |  * CPS is free software: you can redistribute it and/or modify
 7 |  * it under the terms of the GNU Lesser General Public License as published by
 8 |  * the Free Software Foundation, either version 3 of the License, or
 9 |  * (at your option) any later version.
10 |  *
11 |  * CPS is distributed in the hope that it will be useful,
12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 |  * GNU Lesser General Public License for more details.
15 |  *
16 |  * You should have received a copy of the GNU Lesser General Public License
17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
18 |  */
19 | 
20 | # if defined _WIN32 || defined __CYGWIN__
21 | // On Microsoft Windows, use dllimport and dllexport to tag symbols.
22 | #  define CPS_DLLIMPORT __declspec(dllimport)
23 | #  define CPS_DLLEXPORT __declspec(dllexport)
24 | #  define CPS_DLLLOCAL
25 | # else
26 | // On Linux, for GCC >= 4, tag symbols using GCC extension.
27 | #  if __GNUC__ >= 4
28 | #   define CPS_DLLIMPORT __attribute__ ((visibility("default")))
29 | #   define CPS_DLLEXPORT __attribute__ ((visibility("default")))
30 | #   define CPS_DLLLOCAL  __attribute__ ((visibility("hidden")))
31 | #  else
32 | // Otherwise (GCC < 4 or another compiler is used), export everything.
33 | #   define CPS_DLLIMPORT
34 | #   define CPS_DLLEXPORT
35 | #   define CPS_DLLLOCAL
36 | #  endif // __GNUC__ >= 4
37 | # endif // defined _WIN32 || defined __CYGWIN__
38 | 
39 | # ifdef CPS_STATIC
40 | // If one is using the library statically, get rid of
41 | // extra information.
42 | #  define CPS_DLLAPI
43 | #  define CPS_LOCAL
44 | # else
45 | // Depending on whether one is building or using the
46 | // library define DLLAPI to import or export.
47 | #  ifdef CPS_EXPORTS
48 | #   define CPS_DLLAPI CPS_DLLEXPORT
49 | #  else
50 | #   define CPS_DLLAPI CPS_DLLIMPORT
51 | #  endif // CPS_EXPORTS
52 | #  define CPS_LOCAL CPS_DLLLOCAL
53 | # endif // CPS_STATIC
54 | 


--------------------------------------------------------------------------------
/matlab/initialPoint.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %find a point x in the polytope
20 | % l_i <= x_i-x_{i-1} <= u_i for i=1..n
21 | % xln <= x_n <= xun
22 | %
23 | %b indicates if the constraints are feasible
24 | %
25 | %The points verifying the first n equalities are exactly
26 | %x = L*l + sum(lambda_i*(u_i-l_i)*L_i) with lambda_i in [0,1], L the lower
27 | %triangular matrix filled by ones, and L_i its i-th column.
28 | %This describes a zonotope Z.
29 | %
30 | %We thus have x_n = sum(l_i) + sum(lambda_i*(u_i-l_i)).
31 | %Let x(a) = L*l + sum(a*(u_i-l_i)*L_i). For a in [0,1], x(a) is in Z
32 | %We have x_n(a) =  sum(l_i) + sum(a*(u_i-l_i)) = sum(l_i) + a*sum(u_i-l_i)
33 | %x(0) is the point of Z with the lowest x_n, while x(1) has the highest 
34 | %x_n.
35 | %We compute al and au such that x_n(al) = xln and x_n(au) = xun
36 | %The intersection of Z with the last constraint is non empty if [0,1]
37 | %intersects [al,au]. Let a0 be the value in the middle of this
38 | %intersection, we return x(a0).
39 | 
40 | function [x,b] = initialPoint(l,u,xln,xun)
41 | assert(xln<=xun);
42 | assert(all(l<=u));
43 | s = sum(l);
44 | d = u(:)-l(:);
45 | t = sum(d);
46 | if abs(t) < 1e-15
47 |   b = xln<=s && s<=xun;
48 |   x = l(:);
49 | else
50 |   al = (xln-s)/t;
51 |   au = (xun-s)/t;
52 |   b = al<=1 && au>=0;
53 |   L = tril(ones(length(l)));
54 |   x = L*(l(:) + (max(al,0)+min(au,1))/2*d);
55 | end
56 | end


--------------------------------------------------------------------------------
/c++/src/CondensedOrthogonalMatrix.cpp:
--------------------------------------------------------------------------------
 1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 |  *
 3 |  * This file is part of CPS.
 4 |  *
 5 |  * CPS is free software: you can redistribute it and/or modify
 6 |  * it under the terms of the GNU Lesser General Public License as published by
 7 |  * the Free Software Foundation, either version 3 of the License, or
 8 |  * (at your option) any later version.
 9 |  *
10 |  * CPS is distributed in the hope that it will be useful,
11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 |  * GNU Lesser General Public License for more details.
14 |  *
15 |  * You should have received a copy of the GNU Lesser General Public License
16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 |  */
18 | 
19 | #include <cps/CondensedOrthogonalMatrix.h>
20 | 
21 | using namespace Eigen;
22 | 
23 | namespace cps
24 | {
25 |   CondensedOrthogonalMatrix::CondensedOrthogonalMatrix(int n, int kmax, int pmax, bool Ptranspose)
26 |     : ptranspose_(Ptranspose)
27 |     , n_(n)
28 |     , sequences_(kmax)
29 |     , transpositions_(VectorXi::LinSpaced(n,0,n-1))
30 |     , identityTransposition_(VectorXi::LinSpaced(n, 0, n - 1))
31 |   {
32 |     for (auto& s : sequences_)
33 |       s.reserve(pmax);
34 |     Qh_.reserve(pmax);
35 |   }
36 | 
37 |   void CondensedOrthogonalMatrix::reset(bool Ptranspose)
38 |   {
39 |     for (auto& s : sequences_)
40 |       s.clear();
41 |     Qh_.clear();
42 |     transpositions_.indices() = identityTransposition_;
43 |     ptranspose_ = Ptranspose;
44 |   }
45 | 
46 |   void CondensedOrthogonalMatrix::resize(int n, int kmax, int pmax)
47 |   {
48 |     n_ = n;
49 |     sequences_.resize(kmax);
50 |     for (auto& s : sequences_)
51 |       s.reserve(pmax);
52 |     Qh_.reserve(pmax);
53 |     transpositions_.indices() = VectorXi::LinSpaced(n, 0, n - 1);
54 |     identityTransposition_ = VectorXi::LinSpaced(n, 0, n - 1);
55 |   }
56 | 
57 |   Eigen::MatrixXd CondensedOrthogonalMatrix::matrix()
58 |   {
59 |     MatrixXd M = MatrixXd::Identity(n_, n_);
60 |     this->applyOnTheRightTo(M);
61 | 
62 |     return M;
63 |   }
64 | }
65 | 


--------------------------------------------------------------------------------
/c++/include/cps/Statistics.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 3 |  *
 4 |  * This file is part of CPS.
 5 |  *
 6 |  * CPS is free software: you can redistribute it and/or modify
 7 |  * it under the terms of the GNU Lesser General Public License as published by
 8 |  * the Free Software Foundation, either version 3 of the License, or
 9 |  * (at your option) any later version.
10 |  *
11 |  * CPS is distributed in the hope that it will be useful,
12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 |  * GNU Lesser General Public License for more details.
15 |  *
16 |  * You should have received a copy of the GNU Lesser General Public License
17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
18 |  */
19 | 
20 | #include <vector>
21 | 
22 | #ifdef USE_STATS
23 | # define STATISTICS(x) x
24 | #else
25 | # define STATISTICS(x) (void)0
26 | #endif
27 | 
28 | namespace cps
29 | {
30 |   namespace stats
31 |   {
32 |     /** A structure gathering statistics about a LS run.*/
33 |     struct LSStats
34 |     {
35 |       void reset();
36 | 
37 |       int iter;                 //number of iterations
38 |       int activation;           //number of activations
39 |       int deactivation;         //number of deactivations
40 |       int rankLoss;             //number of time a rank loss was detected in the objective matrix
41 |       int activeConstraints;    //number of active constraints
42 |     };
43 | 
44 |     /** A structure gathering statistics about a SQP run.*/
45 |     struct SQPStats
46 |     {
47 |       void reset();
48 | 
49 |       std::vector<int> lineSearchSteps; //number of steps in the line search for each iteration
50 |       std::vector<LSStats> lsStats;     //statistics of the LS runs for each iterations
51 |     };
52 | 
53 |     inline void LSStats::reset()
54 |     {
55 |       iter = 0;
56 |       activation = 0;
57 |       deactivation = 0;
58 |       rankLoss = 0;
59 |       activeConstraints = 0;
60 |     }
61 | 
62 |     inline void SQPStats::reset()
63 |     {
64 |       lineSearchSteps.clear();
65 |       lsStats.clear();
66 |     }
67 |   }
68 | }


--------------------------------------------------------------------------------
/c++/src/BoundenessConstraint.cpp:
--------------------------------------------------------------------------------
 1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 |  *
 3 |  * This file is part of CPS.
 4 |  *
 5 |  * CPS is free software: you can redistribute it and/or modify
 6 |  * it under the terms of the GNU Lesser General Public License as published by
 7 |  * the Free Software Foundation, either version 3 of the License, or
 8 |  * (at your option) any later version.
 9 |  *
10 |  * CPS is distributed in the hope that it will be useful,
11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 |  * GNU Lesser General Public License for more details.
14 |  *
15 |  * You should have received a copy of the GNU Lesser General Public License
16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 |  */
18 | 
19 | #include <cps/BoundenessConstraint.h>
20 | 
21 | using namespace Eigen;
22 | 
23 | namespace cps
24 | {
25 |   BoundenessConstraint::BoundenessConstraint(const Eigen::VectorXd& delta, double alpha, double b)
26 |     : n_(delta.size()), alpha_(alpha), b_(b), delta_(delta), y_(delta.size()+1)
27 |   {
28 |     y_[0] = 0;
29 |   }
30 | 
31 |   void BoundenessConstraint::compute(double& val, const VectorConstRef& x) const
32 |   {
33 |     y_.tail(n_) = x.cwiseSqrt();
34 |     val = -alpha_*y_[n_] - b_;
35 |     for (DenseIndex i = 0; i<n_; ++i)
36 |       val += delta_[i] / (y_[i + 1] + y_[i]);
37 |   }
38 | 
39 |   void BoundenessConstraint::compute(double& val, VectorRef grad, const VectorConstRef& x) const
40 |   {
41 |     compute(val, x);
42 |     grad[0] = -delta_[0] / (2 * y_[1] * y_[1] * y_[1]);
43 |     for (DenseIndex i = 1; i < n_; ++i)
44 |     {
45 |       double d = y_[i + 1] + y_[i];
46 |       double d2 = d*d;
47 |       grad[i-1] -= delta_[i] / (2 * y_[i] * d2);
48 |       grad[i] = -delta_[i] / (2 * y_[i+1] * d2);
49 |     }
50 |     grad[n_ - 1] -= alpha_ / (2 * y_[n_]);
51 |   }
52 | 
53 |   double BoundenessConstraint::compute(const VectorConstRef & x) const
54 |   {
55 |     double v;
56 |     compute(v, x);
57 |     return v;
58 |   }
59 | 
60 |   void BoundenessConstraint::setAlpha(double alpha)
61 |   {
62 |     alpha_ = alpha;
63 |   }
64 | 
65 |   void BoundenessConstraint::setb(double b)
66 |   {
67 |     b_ = b;
68 |   }
69 | }
70 | 


--------------------------------------------------------------------------------
/c++/src/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | # Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | #
 3 | # This file is part of CPS.
 4 | #
 5 | # CPS is free software: you can redistribute it and/or modify
 6 | # it under the terms of the GNU Lesser General Public License as published by
 7 | # the Free Software Foundation, either version 3 of the License, or
 8 | # (at your option) any later version.
 9 | #
10 | # CPS is distributed in the hope that it will be useful,
11 | # but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | # GNU Lesser General Public License for more details.
14 | #
15 | # You should have received a copy of the GNU Lesser General Public License
16 | # along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | #
18 | 
19 | set(CPS_SOURCES
20 |   BoundenessConstraint.cpp
21 |   CaptureSolver.cpp
22 |   CondensedOrthogonalMatrix.cpp
23 |   Givens.cpp
24 |   GivensSequence.cpp
25 |   LeastSquare.cpp
26 |   LinearConstraints.cpp
27 |   Problem.cpp
28 |   ProblemMatrices.cpp
29 |   QRAlgorithms.cpp
30 |   QuadraticObjective.cpp
31 |   SQP.cpp)
32 | 
33 | set(CPS_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../include)
34 | set(CPS_HEADERS
35 |   ${CPS_INCLUDE_DIR}/cps/cps_api.h
36 |   ${CPS_INCLUDE_DIR}/cps/BoundenessConstraint.h
37 |   ${CPS_INCLUDE_DIR}/cps/CaptureSolver.h
38 |   ${CPS_INCLUDE_DIR}/cps/CondensedOrthogonalMatrix.h
39 |   ${CPS_INCLUDE_DIR}/cps/defs.h
40 |   ${CPS_INCLUDE_DIR}/cps/Givens.h
41 |   ${CPS_INCLUDE_DIR}/cps/GivensSequence.h
42 |   ${CPS_INCLUDE_DIR}/cps/LeastSquare.h
43 |   ${CPS_INCLUDE_DIR}/cps/LinearConstraints.h
44 |   ${CPS_INCLUDE_DIR}/cps/Problem.h
45 |   ${CPS_INCLUDE_DIR}/cps/ProblemMatrices.h
46 |   ${CPS_INCLUDE_DIR}/cps/QRAlgorithms.h
47 |   ${CPS_INCLUDE_DIR}/cps/QuadraticObjective.h
48 |   ${CPS_INCLUDE_DIR}/cps/SQP.h
49 |   ${CPS_INCLUDE_DIR}/cps/Statistics.h
50 |   ${CPS_INCLUDE_DIR}/cps/toMatlab.h)
51 |   
52 | add_library(CaptureProblemSolver SHARED ${CPS_SOURCES} ${CPS_HEADERS})
53 | set_target_properties(CaptureProblemSolver PROPERTIES COMPILE_FLAGS "-DCPS_EXPORTS")
54 | 
55 | install(TARGETS CaptureProblemSolver
56 |   RUNTIME DESTINATION bin
57 |   LIBRARY DESTINATION lib
58 |   ARCHIVE DESTINATION lib)
59 | 
60 | install(DIRECTORY ${CPS_INCLUDE_DIR}/ DESTINATION include
61 |   FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp")
62 | 


--------------------------------------------------------------------------------
/c++/include/cps/GivensSequence.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 3 |  *
 4 |  * This file is part of CPS.
 5 |  *
 6 |  * CPS is free software: you can redistribute it and/or modify
 7 |  * it under the terms of the GNU Lesser General Public License as published by
 8 |  * the Free Software Foundation, either version 3 of the License, or
 9 |  * (at your option) any later version.
10 |  *
11 |  * CPS is distributed in the hope that it will be useful,
12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 |  * GNU Lesser General Public License for more details.
15 |  *
16 |  * You should have received a copy of the GNU Lesser General Public License
17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
18 |  */
19 | 
20 | #include <vector>
21 | 
22 | #include <cps/cps_api.h>
23 | #include <cps/Givens.h>
24 | 
25 | namespace cps
26 | {
27 |   /** A sequence of Givens rotations G_0*G_1*...
28 |     *
29 |     * Note: it is ok to derive publicly from std::vector here as we do not have
30 |     * any additional data.
31 |     */
32 |   class CPS_DLLAPI GivensSequence final : public std::vector<Givens>
33 |   {
34 |   public:
35 |     using std::vector<Givens>::vector;
36 | 
37 |     /** M = G_{k-1}^T G_{k-2}^T .... G_0^T M*/
38 |     template<typename Derived>
39 |     void applyTo(const Eigen::MatrixBase<Derived>& M) const;
40 | 
41 |     /** Computes M = M * G_0 G_1 ... G_{k-1}*/
42 |     template<typename Derived>
43 |     void applyOnTheRightTo(const Eigen::MatrixBase<Derived>& M) const;
44 | 
45 |     /** Perform G.extend(incr) on all Givens rotations in the sequence*/
46 |     void extend(int incr);
47 | 
48 |     /** Return the corresponding nxn orthogonal matrix
49 |       *
50 |       * Use only for debugging purposes.
51 |       */
52 |     Eigen::MatrixXd matrix(int n);
53 |   };
54 | 
55 | 
56 |   template<typename Derived>
57 |   inline void GivensSequence::applyTo(const Eigen::MatrixBase<Derived>& M) const
58 |   {
59 |     for (const auto& G : *this)
60 |       G.applyTo(M);
61 |   }
62 | 
63 |   template<typename Derived>
64 |   inline void GivensSequence::applyOnTheRightTo(const Eigen::MatrixBase<Derived>& M) const
65 |   {
66 |     for (const auto& G : *this)
67 |       G.applyOnTheRightTo(M);
68 |   }
69 | }
70 | 


--------------------------------------------------------------------------------
/matlab/illustration/display3dCase.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %Draw the problem geometry in the case n=3, in the plane phi_1 = delta(1)*g/zf;
20 | function display3dCase(zf,zi,dzi,wimin,wimax)
21 | g = 9.80665;
22 | lambdamin = g/10;
23 | lambdamax = 2*g;
24 | s = [0, 1/3, 2/3, 1];
25 | delta = (s(2:end).^2 - s(1:end-1).^2);
26 | d = 1./delta;
27 | db = d(1:end-1);
28 | de = d(2:end);
29 | z = zeros(length(db),1);
30 | J = [-diag(db+de)+diag(de(1:end-1),-1) z] + [z diag(de)];
31 | x1 = delta(1)*g/zf;
32 | alpha = zi/g;
33 | beta = dzi/g;
34 | 
35 | %draw in the 2d plane phi1 = x1
36 | figure();
37 | %  draw the boundedness constraint
38 | phi3 = [];
39 | r2 = 0:0.025:10;
40 | for phi2=r2
41 |   z = solveBoundedness(delta,alpha,beta,[x1,phi2]);
42 |   phi3(end+1) = z;
43 | end
44 | plot(r2,phi3);
45 | hold on
46 | %  display min value for phi3
47 | % lim = ((delta(1)/sqrt(x1)-beta)/alpha)^2;
48 | % plot([r2(1),r2(end)],[lim,lim]);
49 | 
50 | %  draw zonotope
51 | l = [x1;lambdamin*delta(2:end)'];
52 | u = [x1;lambdamax*delta(2:end)'];
53 | D = diag(u-l);
54 | L = [1 0 0; 1 1 0; 1 1 1];
55 | Z = L*(repmat(l,[1,5]) + D*[0 0 0 0 0; 0 0 1 1 0; 0 1 1 0 0]);
56 | plot(Z(2,:),Z(3,:));
57 | 
58 | %  draw limits on phi3
59 | plot([r2(1),r2(end)], [wimin^2 wimin^2])
60 | plot([r2(1),r2(end)], [wimax^2 wimax^2])
61 | 
62 | %  draw minimum value of objective
63 | p = -J(:,2:end)\(x1*J(:,1));
64 | scatter(p(1),p(2),'+');
65 | 
66 | %  draw solution
67 | x = balanceMPC(g,delta,zi,dzi,zf,lambdamin,lambdamax,wimin,wimax);
68 | scatter(x(2),x(3),'x');
69 | 
70 | %  draw relaxed solution
71 | x = balanceMPC(g,delta,zi,dzi,zf,lambdamin,lambdamax,wimin,wimax, true);
72 | scatter(x(2),x(3),'o');
73 | end


--------------------------------------------------------------------------------
/c++/tests/misc.cpp:
--------------------------------------------------------------------------------
 1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 |  *
 3 |  * This file is part of CPS.
 4 |  *
 5 |  * CPS is free software: you can redistribute it and/or modify
 6 |  * it under the terms of the GNU Lesser General Public License as published by
 7 |  * the Free Software Foundation, either version 3 of the License, or
 8 |  * (at your option) any later version.
 9 |  *
10 |  * CPS is distributed in the hope that it will be useful,
11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 |  * GNU Lesser General Public License for more details.
14 |  *
15 |  * You should have received a copy of the GNU Lesser General Public License
16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 |  */
18 | 
19 | #include "misc.h"
20 | 
21 | #include <cps/Problem.h>
22 | #include <cps/SQP.h>
23 | #include <cps/toMatlab.h>
24 | 
25 | #include <fstream>
26 | #include <vector>
27 | 
28 | using namespace Eigen;
29 | using namespace cps;
30 | 
31 | void mapFeasibleInputs()
32 | {
33 |   RawProblem raw;
34 |   std::string base = TESTS_DIR;
35 |   raw.read(base + "/data/Problem01.txt");
36 | 
37 |   Problem pb(raw);
38 |   SQP sqp(static_cast<int>(raw.delta.size()));
39 |   std::vector<Vector3d> points;
40 |   points.reserve(500000);
41 | 
42 |   for (double target_height = 0.0; target_height <= 2; target_height += 0.04)
43 |   {
44 |     for (double init_zbar = 0.0; init_zbar <= 2; init_zbar += 0.04)
45 |     {
46 |       std::cout << init_zbar << std::endl;
47 |       for (double init_zbar_deriv = -5; init_zbar_deriv <= 5; init_zbar_deriv += 0.2)
48 |       {
49 |         pb.set_init_zbar(init_zbar);
50 |         pb.set_init_zbar_deriv(init_zbar_deriv);
51 |         pb.set_target_height(target_height);
52 |         auto s = sqp.solveFeasibility(pb);
53 |         if (s == SolverStatus::Converge)
54 |         {
55 |           double v;
56 |           pb.nonLinearConstraint().compute(v, sqp.x());
57 |           if (std::abs(v) < 1e-5)
58 |             points.push_back({init_zbar, init_zbar_deriv, target_height});
59 |         }
60 |       }
61 |     }
62 |   }
63 | 
64 |   Eigen::MatrixXd P(3, static_cast<DenseIndex>(points.size()));
65 |   for (DenseIndex i = 0; i < P.cols(); ++i)
66 |     P.col(i) = points[static_cast<size_t>(i)];
67 | 
68 |   std::ofstream aof("points.m");
69 |   aof << "P = " << (toMatlab)P << ";" << std::endl;
70 | }


--------------------------------------------------------------------------------
/matlab/balanceMPC.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %Build and solve the capture problem
20 | %If relax is true (false by default), build a relaxed version in which the
21 | %non-linear equlity constraint c=0 is turned into the convex constraint
22 | %c<=0%
23 | %
24 | %Example
25 | % delta = [0.01, 0.03, 0.05, 0.07, 0.09, 0.11, 0.13, 0.15, 0.17, 0.19];
26 | % g = 9.80665;
27 | % lambda_max = 19.6133;
28 | % lambda_min = 0.980665;
29 | % omega_i_max = 3.5004454562949268;
30 | % omega_i_min = 2.7492602295351802;
31 | % z_bar = 0.91307774471737957;
32 | % z_f = 0.8;
33 | % zd_bar = -0.17413705390465162;
34 | % balanceMPC(g,delta,z_bar,zd_bar,z_f,lambda_min,lambda_max, omega_i_min, omega_i_max)
35 | function phi = balanceMPC(g,delta,zi,dzi,zf,lmin,lmax,wimin,wimax,relax)
36 | assert(nargin>=8)
37 | twoD = nargin==8;
38 | delta=delta(:);
39 | n = length(delta);
40 | 
41 | if twoD
42 |   alpha = 0;
43 |   b = (dzi+wimin*zi)/g;
44 | else
45 |   alpha = zi/g;
46 |   b = dzi/g;
47 | end
48 | 
49 | if nargin<10
50 |   relax = false;
51 | end
52 | 
53 | o = @(x) objMPC(delta,x);
54 | c = @(x) constrMPC(delta,x,alpha,b,relax);
55 | 
56 | A = diag(ones(n,1))+diag(-ones(n-1,1),-1);
57 | A = [-A;A];
58 | b = [-lmin*delta; lmax*delta];
59 | xl = zeros(n,1);
60 | xu = 1000*ones(n,1);
61 | if twoD
62 |   Aeq = zeros(2,n);
63 |   Aeq(1,1) = 1; Aeq(2,n) = 1;
64 |   beq = [delta(1)*g/zf;wimin^2];
65 | else
66 |   Aeq = zeros(1,n);
67 |   Aeq(1,1) = 1;
68 |   beq = delta(1)*g/zf;
69 |   xl(n) = wimin^2;
70 |   xu(n) = wimax^2;
71 | end
72 | L = tril(ones(n,n));
73 | phi0 = (lmin+lmax)*L*delta/2;
74 | opt = optimset('GradObj','on','GradConstr','on');
75 | phi = fmincon(o,phi0,A,b,Aeq,beq,xl,xu,c,opt);
76 | 
77 | end
78 | 


--------------------------------------------------------------------------------
/matlab/multByPinvCaT.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %Given the following constraints
20 | % l_i <= x_i-x_{i-1} <= u_i for i=1..n  (x_0 = 0)
21 | % xln <= x_n <= xun
22 | %we note Ca the matrix corresponding to the activated constraints,
23 | %specified by  az and an.
24 | %The function computes Y = pinv(Ca^T)*X in an optimized way.
25 | function Y = multByPinvCaT(X,az,an)
26 | n = length(az);
27 | assert(size(X,1) == n);
28 | na = sum(az) + an;
29 | assert(na<=n); %we do not treat the case where all constraints are active.
30 | Y = zeros(na,size(X,2));
31 | 
32 | if az(1)
33 |   k=1;
34 |   while k<na && az(k+1)
35 |     k = k+1;
36 |   end
37 |   Y(1:k,:) = applyFirst(X,k);
38 |   k0 = k+1;
39 | else
40 |   k0 = 1;
41 | end
42 | if an
43 |   k=1;
44 |   while k<na && az(n+1-k)
45 |     k=k+1;
46 |   end
47 |   Y(na-k+1:na,:) = applyLast(X,k);
48 |   kn = n-k;
49 | else
50 |   kn = n;
51 | end
52 | k=0;
53 | for i=k0:kn
54 |   if az(i)
55 |     k = k+1;
56 |   else
57 |     if k>0
58 |       Y(k0:k0+k-1,:) = apply(X,i-k-2,k+1);
59 |       k0 = k0+k;
60 |       k=0;
61 |     end
62 |   end
63 | end
64 | if k>0
65 |   Y(k0:k0+k-1,:) = apply(X,kn-k-1,k+1);
66 | end
67 | end
68 | 
69 | function Y = applyFirst(X,k)
70 | Y = zeros(k,size(X,2));
71 | Y(k,:) = X(k,:);
72 | for i=k-1:-1:1
73 |   Y(i,:) = Y(i+1,:) + X(i,:);
74 | end
75 | end
76 | 
77 | function Y = applyLast(X,k)
78 | Y = zeros(k,size(X,2));
79 | if k==1
80 |   Y = X(end,:);
81 | else
82 |   n = size(X,1);
83 |   Y(1,:) = -X(n-k+1,:);
84 |   for i=2:k-1
85 |     Y(i,:) = Y(i-1,:) - X(n-k+i,:);
86 |   end
87 |   Y(k,:) = -Y(k-1,:) + X(end,:);
88 | end
89 | 
90 | end
91 | 
92 | %apply to X(s+1:k,:)
93 | function Y = apply(X,s,k)
94 | Y = zeros(k-1,size(X,2));
95 | for i=1:k-1
96 |   Y(i,:) = [-(k-i)*ones(1,i) i*ones(1,k-i)]*X(s+(1:k),:);
97 | end
98 | Y = Y/k;
99 | end


--------------------------------------------------------------------------------
/c++/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | # Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | #
 3 | # This file is part of CPS.
 4 | #
 5 | # CPS is free software: you can redistribute it and/or modify
 6 | # it under the terms of the GNU Lesser General Public License as published by
 7 | # the Free Software Foundation, either version 3 of the License, or
 8 | # (at your option) any later version.
 9 | #
10 | # CPS is distributed in the hope that it will be useful,
11 | # but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | # GNU Lesser General Public License for more details.
14 | #
15 | # You should have received a copy of the GNU Lesser General Public License
16 | # along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | #
18 | 
19 | cmake_minimum_required(VERSION 2.8.12)
20 | 
21 | INCLUDE(cmake/base.cmake)
22 | INCLUDE(cmake/boost.cmake)
23 | INCLUDE(cmake/eigen.cmake)
24 | INCLUDE(cmake/msvc-specific.cmake)
25 | 
26 | SET(PROJECT_NAME CaptureProblemSolver)
27 | SET(PROJECT_DESCRIPTION "Solver for capture problems when walking with variable height")
28 | SET(PROJECT_URL "https://github.com/jrl-umi3218/CaptureProblemSolver")
29 | SET(PROJECT_DEBUG_POSTFIX "_d")
30 | 
31 | # Disable -Werror on Unix for now.
32 | SET(CXX_DISABLE_WERROR True)
33 | 
34 | SETUP_PROJECT()
35 | 
36 | if(NOT WIN32)
37 |   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wno-sign-conversion -std=c++0x -pedantic")
38 | endif()
39 | 
40 | option(PYTHON_BINDINGS "Generate Python bindings" ON)
41 | option(STATISTICS "Use statistics" OFF)
42 | 
43 | #########################
44 | # External dependencies
45 | SEARCH_FOR_EIGEN()
46 | 
47 | SET(BOOST_COMPONENTS python system)
48 | IF(PYTHON_BINDINGS)
49 |     set(BOOST_COMPONENTS python unit_test_framework timer system)
50 | ELSE()
51 |     set(BOOST_COMPONENTS unit_test_framework timer system)
52 | ENDIF()
53 | 
54 | SEARCH_FOR_BOOST()
55 | 
56 | #########################
57 | 
58 | # For MSVC, set local environment variable to enable finding the built dll
59 | # of the main library when launching ctest with RUN_TESTS
60 | IF(MSVC)
61 |   SET(CMAKE_MSVCIDE_RUN_PATH "\$(SolutionDir)/src/\$(Configuration)")
62 | ENDIF(MSVC)
63 | 
64 | if(STATISTICS)
65 |   add_definitions(-DUSE_STATS)
66 | endif()
67 | 
68 | add_subdirectory(src)
69 | add_subdirectory(tests)
70 | 
71 | # Add dependency towards the library in the pkg-config file.
72 | PKG_CONFIG_APPEND_LIBS(CaptureProblemSolver)
73 | 
74 | IF(PYTHON_BINDINGS)
75 |     # Add dependency towards the python bindings in the pkg-config file.
76 |     add_subdirectory(bindings)
77 | ENDIF()
78 | 
79 | SETUP_PROJECT_FINALIZE()
80 | 


--------------------------------------------------------------------------------
/c++/src/ProblemMatrices.cpp:
--------------------------------------------------------------------------------
 1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 |  *
 3 |  * This file is part of CPS.
 4 |  *
 5 |  * CPS is free software: you can redistribute it and/or modify
 6 |  * it under the terms of the GNU Lesser General Public License as published by
 7 |  * the Free Software Foundation, either version 3 of the License, or
 8 |  * (at your option) any later version.
 9 |  *
10 |  * CPS is distributed in the hope that it will be useful,
11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 |  * GNU Lesser General Public License for more details.
14 |  *
15 |  * You should have received a copy of the GNU Lesser General Public License
16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 |  */
18 | 
19 | #include <cps/ProblemMatrices.h>
20 | 
21 | using namespace Eigen;
22 | 
23 | namespace cps
24 | {
25 |   Eigen::MatrixXd buildJ(const Eigen::VectorXd& d)
26 |   {
27 |     auto n = d.size();
28 |     MatrixXd J(n - 1, n);
29 |     J.setZero();
30 |     J.diagonal() = -d.head(n - 1) - d.tail(n - 1);
31 |     J.diagonal<-1>() = d.segment(1, n - 2);
32 |     J.diagonal<+1>() = d.segment(1, n - 1);
33 | 
34 |     return J;
35 |   }
36 | 
37 |   Eigen::MatrixXd buildJ0(const Eigen::VectorXd& e)
38 |   {
39 |     auto n = e.size();
40 |     MatrixXd J0(n+1,n);
41 |     J0.setZero();
42 |     J0.diagonal() = e;
43 |     J0.diagonal<-1>().head(n - 1) = -e.head(n - 1) - e.tail(n - 1); 
44 |     J0(n, n - 1) = -e(n - 1);
45 |     J0.diagonal<-2>() = e.tail(n - 1);
46 | 
47 |     return J0;
48 |   }
49 | 
50 |   Eigen::MatrixXd buildJj(const Eigen::VectorXd& e)
51 |   {
52 |     auto n = e.size();
53 |     MatrixXd Jj(n + 1, n + 1);
54 |     Jj.setZero();
55 |     Jj.diagonal().head(n) = -e;
56 |     Jj.diagonal().tail(n) -= e;
57 |     Jj.diagonal<-1>() = e;
58 |     Jj.diagonal<+1>() = e;
59 | 
60 |     return Jj;
61 |   }
62 | 
63 |   Eigen::MatrixXd buildJpm1(const Eigen::VectorXd& e)
64 |   {
65 |     auto n = e.size();
66 |     MatrixXd Jp(n, n);
67 |     Jp.setZero();
68 |     Jp.diagonal() = -e;
69 |     Jp.diagonal().tail(n - 1) -= e.head(n - 1);
70 |     Jp.diagonal<-1>() = e.head(n - 1);
71 |     Jp.diagonal<+1>() = e.head(n - 1);
72 |     
73 |     return Jp;
74 |   }
75 | 
76 |   Eigen::MatrixXd buildCZ(int n)
77 |   {
78 |     MatrixXd Cz(n, n);
79 |     Cz.setZero();
80 |     Cz.diagonal().setOnes();
81 |     Cz.diagonal<-1>().setConstant(-1);
82 | 
83 |     return Cz;
84 |   }
85 | 
86 |   Eigen::MatrixXd buildC(int n)
87 |   {
88 |     MatrixXd C(n+1, n);
89 |     C.setZero();
90 |     C.diagonal().setOnes();
91 |     C.diagonal<-1>().setConstant(-1);
92 |     C(n, n - 1) = 1;
93 | 
94 |     return C;
95 |   }
96 | }
97 | 


--------------------------------------------------------------------------------
/matlab/feasibilityLS.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %solve the problem 
20 | % min. ||c+j'x||^2
21 | % s.t. l_i <= x_i-x_{i-1} <= u_i for i=1..n  (x_0 = 0)
22 | %      xln <= x_n <= xun
23 | %
24 | function [x,lambda] = feasibilityLS(j,c,l,u,xln,xun)
25 | n = length(j);
26 | assert(length(l) == n);
27 | assert(length(u) == n);
28 | assert(all(l<=1e-14) && all(-1e-14<=u)); %x=0 is supposed to be a feasible point
29 | assert(xln<=0 && 0<=xun);
30 | 
31 | %zonotope constraints: -1 when active at lower bound, +1 if active at upper
32 | %bound, 2 if active as equality, 0 if inactive;
33 | activeZ = zeros(n,1);
34 | activeN = 0; %constraint on x_n: -1 if active at lower bound, +1...
35 | activeZ(abs(l-u)<=1e-14)=2;
36 | activeN(xln==xun)=2;
37 | 
38 | lambda = zeros(n+1,1);
39 | 
40 | x = zeros(n,1);
41 | %finished = checkKKT(j,c,x,lambdaZ,lambdaN,l,u,xln,xun,1e-6,1e-6);
42 | maxIter = 10*n;
43 | for k=1:maxIter
44 |   [jn,idx] = applyNullspace(j',activeZ~=0,activeN~=0);
45 |   %z = -pinv(jn)*(c+j'*x);
46 |   z = -(jn'/(jn*jn'))*(c+j'*x);
47 |   %compute p = N*z
48 |   p = zeros(n,1);
49 |   nz = idx~=0;
50 |   p(nz) = z(idx(nz));
51 |   if norm(p,Inf)<1e-12
52 |     act = [activeZ;activeN];
53 |     %[~,Cact] = dedicatedNullspace(activeZ~=0,false,activeN~=0);
54 |     %lambda(act~=0) = -(j'*x+c)*(pinv(Cact')*j);
55 |     lambda(act~=0) = -(j'*x+c)*(multByPinvCaT(j,activeZ~=0,activeN~=0));
56 |     lambda(act==0) = 0;
57 |     if all(lambda(act==-1)<=0) && all(lambda(act==1)>=0)
58 |       return;
59 |     else
60 |       %deactivate
61 |       tmp = zeros(n+1,1);
62 |       tmp(act==-1) = -lambda(act==-1);
63 |       tmp(act== 1) = lambda(act==1);
64 |       [~,i] = min(tmp);
65 |       if (i<=n)
66 |         activeZ(i) = 0;
67 |       else
68 |         activeN = 0;
69 |       end
70 |     end
71 |   else
72 |     [a,i,t] = maxQPsteplength(x,p,l,u,xln,xun,activeZ~=0,activeN~=0);
73 |     x = x + a*p;
74 |     if i>0
75 |       if i<=n
76 |         activeZ(i) = t;
77 |       else
78 |         activeN = t;
79 |       end
80 |     end
81 |   end
82 | end
83 | end
84 | 
85 | 


--------------------------------------------------------------------------------
/matlab/dedicatedNullspace.m:
--------------------------------------------------------------------------------
  1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  2 | %
  3 | % This file is part of CPS.
  4 | %
  5 | % CPS is free software: you can redistribute it and/or modify
  6 | % it under the terms of the GNU Lesser General Public License as published by
  7 | % the Free Software Foundation, either version 3 of the License, or
  8 | % (at your option) any later version.
  9 | %
 10 | % CPS is distributed in the hope that it will be useful,
 11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
 12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13 | % GNU Lesser General Public License for more details.
 14 | %
 15 | % You should have received a copy of the GNU Lesser General Public License
 16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 17 | 
 18 | 
 19 | % Compute the nullspace of active constraints among
 20 | % x_i-x_{i-1} = a_i for i=1..n  (x_0 = 0) (Z)
 21 | % x_1 = x1                                (1)
 22 | % x_n = xn                                (n)
 23 | %
 24 | % az is a n-vector of booleans specifying which constraints of Z are
 25 | % active.
 26 | % a1 is a boolean for the activity of (1), and an for the activity of (n)
 27 | %
 28 | % N is the null space basis
 29 | % Aa is the matrix of active constraints
 30 | function [N,Aa] = dedicatedNullspace(az, a1, an)
 31 | assert(~(a1 && az(1)));
 32 | n = length(az);
 33 | na = sum(az) + a1 +an;
 34 | N = zeros(n, n-na);
 35 | 
 36 | az(1) = az(1) || a1;
 37 | if (any(az) || an)
 38 |   p = processAZ(az);
 39 |   if p(1,1)==1
 40 |     c = p(1,2)+1;
 41 |     i0=2;
 42 |   else
 43 |     c=1;
 44 |     i0=1;
 45 |   end
 46 |   r=1;
 47 |   for i=i0:size(p,1)
 48 |     %free variables
 49 |     l = p(i,1)-2 - c;
 50 |     if l>=0
 51 |       N(c+[0:l],r+[0:l]) = eye(l+1);
 52 |       r = r + l+1;
 53 |     end
 54 |     %for j=c:p(i,1)-2
 55 |     %  N(j,r) = 1;
 56 |     %  r = r+1;
 57 |     %end
 58 |     c = p(i,1)-1;
 59 |     if c+p(i,2)==n && an
 60 |       c=n+1;
 61 |       break;
 62 |     end
 63 |     N(c:c+p(i,2),r) = 1;
 64 |     c = p(i,1)+ p(i,2);
 65 |     r = r+1;
 66 |   end
 67 |   for j=c:n-1
 68 |     N(j,r) = 1;
 69 |     r = r+1;
 70 |   end
 71 |   if ~an && c<=n
 72 |     N(n,r)=1;
 73 |   end
 74 | else
 75 |   N = eye(n);
 76 | end
 77 |   
 78 | if nargout>1
 79 |   Aa = zeros(na,n);
 80 |   r = 1;
 81 |   if a1 || az(1)
 82 |     Aa(1,1) = 1;
 83 |     r=r+1;
 84 |   end
 85 |   for i=2:n
 86 |     if az(i)
 87 |       Aa(r,i-1:i) = [-1 1];
 88 |       r = r+1;
 89 |     end
 90 |   end
 91 |   if an
 92 |     Aa(r,n) = 1;
 93 |   end
 94 | end
 95 | end
 96 | 
 97 | function p=processAZ(az)
 98 | p = zeros(0,2);
 99 | k=0;
100 | past = false;
101 | for i=1:length(az)
102 |   if az(i)
103 |     if past
104 |       p(k,2) = p(k,2)+1;
105 |     else
106 |       k = k+1;
107 |       past = true;
108 |       p(k,:) = [i,1];
109 |     end
110 |   else
111 |     past = false;
112 |   end
113 | end
114 | end


--------------------------------------------------------------------------------
/c++/tests/main.cpp:
--------------------------------------------------------------------------------
 1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 |  *
 3 |  * This file is part of CPS.
 4 |  *
 5 |  * CPS is free software: you can redistribute it and/or modify
 6 |  * it under the terms of the GNU Lesser General Public License as published by
 7 |  * the Free Software Foundation, either version 3 of the License, or
 8 |  * (at your option) any later version.
 9 |  *
10 |  * CPS is distributed in the hope that it will be useful,
11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 |  * GNU Lesser General Public License for more details.
14 |  *
15 |  * You should have received a copy of the GNU Lesser General Public License
16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 |  */
18 | 
19 | #include <cps/Problem.h>
20 | #include <cps/CaptureSolver.h>
21 | 
22 | #include "misc.h"
23 | #include "SQPTestCommon.h"
24 | #include "timings.h"
25 | 
26 | using namespace Eigen;
27 | using namespace cps;
28 | 
29 | /** Solve the problem specified by filepath (relatively to TEST_DIR) and display some statistics.*/
30 | void solveProblem(const std::string& filepath)
31 | {
32 |   //reading a problem from file
33 |   RawProblem raw;
34 |   std::string base = TESTS_DIR;
35 |   raw.read(base + "/" + filepath);
36 | 
37 |   //creating Problem and SQP objects
38 |   Problem pb(raw);
39 |   CaptureSolver solver(static_cast<int>(raw.delta.size()));
40 | 
41 |   //solving and displaying
42 |   std::cout << "Solving problem " << filepath << "\n" <<std::endl;
43 |   auto s = solver.solve(pb);
44 |   std::cout << "  solver status:          " << static_cast<int>(s) << std::endl;
45 |   std::cout << "  boundenednes violation: " << pb.nonLinearConstraint().compute(solver.x()) << std::endl;
46 |   std::cout << "  linear constraint:      " << (pb.linearConstraints().checkPrimal(solver.x())?"":"not ") << "satisfied" << std::endl;
47 |   std::cout << "  sqp solution:           " << solver.x().transpose() << std::endl;
48 |   if (raw.Phi_.size())
49 |     std::cout << "  raw solution:           " << raw.Phi_.tail(raw.Phi_.size()-1).transpose() << "\n" << std::endl;
50 | 
51 | #ifdef USE_STATS
52 |   auto statistics = solver.statistics();
53 |   std::cout << " (iter,\tact,\tdeact,\t#actCstr)" << std::endl;
54 |   for (size_t i = 0; i < statistics.lsStats.size(); ++i)
55 |   {
56 |     auto si = statistics.lsStats[i];
57 |     std::cout << "  " << si.iter << ",\t" << si.activation << ",\t" << si.deactivation << ",\t" << si.activeConstraints << std::endl;
58 |   }
59 |   std::cout << std::endl;
60 | #endif //USE_STATS
61 | }
62 | 
63 | // Additional arguments are paths of problem files, relative to TESTS_DIR
64 | int main(int argc, char* argv[])
65 | {
66 |   if (argc > 1)
67 |   {
68 |     for (int i = 1; i < argc; ++i)
69 |     {
70 |       solveProblem(argv[i]);
71 |     }
72 |   }
73 |   else
74 |   {
75 |     solveProblem("data/Problem01.txt");
76 |   }
77 | 
78 | #ifdef WIN32
79 |   system("pause");
80 | #endif
81 | }
82 | 


--------------------------------------------------------------------------------
/matlab/directJN.m:
--------------------------------------------------------------------------------
  1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  2 | %
  3 | % This file is part of CPS.
  4 | %
  5 | % CPS is free software: you can redistribute it and/or modify
  6 | % it under the terms of the GNU Lesser General Public License as published by
  7 | % the Free Software Foundation, either version 3 of the License, or
  8 | % (at your option) any later version.
  9 | %
 10 | % CPS is distributed in the hope that it will be useful,
 11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
 12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13 | % GNU Lesser General Public License for more details.
 14 | %
 15 | % You should have received a copy of the GNU Lesser General Public License
 16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 17 | 
 18 | 
 19 | %Build J*N directly (no multiplication)
 20 | %
 21 | %Example
 22 | % delta = [0.01, 0.03, 0.05, 0.07, 0.09, 0.11, 0.13, 0.15, 0.17, 0.19];
 23 | % directJN(Delta,[1 1 0 0 1 0 0 1 1 1],false)
 24 | function JN = directJN(delta,az,an)
 25 | n = length(az);
 26 | na = sum(az) + an;
 27 | JN = zeros(n-1,n-na);
 28 | az(end+1) = an;
 29 | d = 1./delta;
 30 | 
 31 | if na==n, return; end
 32 | %if na==0, JN = universalJj(d,2,3); return; end
 33 | 
 34 | %number of consecutive activated constraints at the end
 35 | ap=0;
 36 | while az(n+1-ap)
 37 |   ap = ap+1;
 38 | end
 39 | 
 40 | a0=0;
 41 | while az(a0+1) && a0<=n
 42 |   a0 = a0+1;
 43 | end
 44 | if a0>0
 45 |   %JN(k:k+i1-2,1:i1-1) = buildJ0(d(k+1:k+i1-1), 0);
 46 |   startType = 1;
 47 |   startOffset = a0-1;
 48 |   addDim = 1;
 49 | else
 50 |   startType = 2;
 51 |   startOffset = 0;
 52 |   addDim = 0;
 53 | end
 54 | 
 55 | if a0+ap==na
 56 |   if ap==0
 57 |     if na~=n-1
 58 |       JN(1+startOffset:end,:) = universalJj(d(1+a0:end), startType, 3);
 59 |     else
 60 |       JN(1+startOffset:end,:) = d(end);
 61 |     end
 62 |   elseif ap==1
 63 |     JN(1+startOffset:end,:) = universalJj(d(1+a0:end), startType, 2);
 64 |   else
 65 |     JN(1+startOffset:end+2-ap,:) = universalJj(d(1+a0:end+1-ap), startType, 1);
 66 |   end
 67 |   return;
 68 | end
 69 | 
 70 | k = a0;
 71 | i1=0;
 72 | while k+i1<=n && ~az(k+i1+1)
 73 |   i1 = i1+1;
 74 | end
 75 | JN(startOffset+(1:i1+addDim),1:i1) = universalJj(d(a0+(1:i1)),startType,4);
 76 | 
 77 | k = k + i1;
 78 | c = i1;
 79 | a1 = 0;
 80 | while k+a1<=n && az(k+a1+1) 
 81 |   a1 = a1+1;
 82 | end
 83 | k = k + a1;
 84 | while true
 85 |   i2=0;
 86 |   while k+i2<=n&& ~az(k+i2+1) 
 87 |     i2 = i2+1;
 88 |   end
 89 |   a2 = 0;
 90 |   while k+i2+a2<=n && az(k+i2+a2+1) 
 91 |     a2 = a2+1;
 92 |   end
 93 |   if (k+i2+a2==n+1)
 94 |     if a2==0
 95 |       JN(k:k+i2-2,c:c+i2-1) = universalJj(d(end-(i2-2):end),3,3);
 96 |     elseif a2==1
 97 |       JN(k:k+i2-1,c:c+i2-1) = universalJj(d(end-i2+1:end),3,2);
 98 |     else
 99 |       JN(k:k+i2,c:c+i2-1) = universalJj(d(end-i2-a2+2:end-a2+1),3,1);
100 |     end
101 |     break;
102 |   end
103 |   JN(k:k+i2,c:c+i2) = universalJj(d(k+1:k+i2),3,4);
104 |   c = c+i2;
105 |   k = k + i2 + a2;
106 | end
107 | end
108 | 


--------------------------------------------------------------------------------
/matlab/feasibilitySQP.m:
--------------------------------------------------------------------------------
 1 | % Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 | %
 3 | % This file is part of CPS.
 4 | %
 5 | % CPS is free software: you can redistribute it and/or modify
 6 | % it under the terms of the GNU Lesser General Public License as published by
 7 | % the Free Software Foundation, either version 3 of the License, or
 8 | % (at your option) any later version.
 9 | %
10 | % CPS is distributed in the hope that it will be useful,
11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 | % GNU Lesser General Public License for more details.
14 | %
15 | % You should have received a copy of the GNU Lesser General Public License
16 | % along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 | 
18 | 
19 | %solve the problem
20 | % min. ||c(x)||^2
21 | % s.t. l_i <= x_i-x_{i-1} <= u_i for i=1..n  (x_0 = 0)
22 | %      xln <= x_n <= xun
23 | %
24 | %where c is the boundedness constraint
25 | %
26 | %Example
27 | % delta = [0.01, 0.03, 0.05, 0.07, 0.09, 0.11, 0.13, 0.15, 0.17, 0.19];
28 | % g = 9.80665;
29 | % lambda_max = 19.6133;
30 | % lambda_min = 0.980665;
31 | % omega_i_max = 3.5004454562949268;
32 | % omega_i_min = 2.7492602295351802;
33 | % z_bar = 0.91307774471737957;
34 | % z_f = 0.8;
35 | % zd_bar = -0.17413705390465162;
36 | % feasibilitySQP(g,delta,z_bar,zd_bar,z_f,lambda_min,lambda_max, omega_i_min, omega_i_max)
37 | function x = feasibilitySQP(g,delta,zi,dzi,zf,lmin,lmax,wimin,wimax)
38 | n = length(delta);
39 | l = lmin*delta(:);
40 | u = lmax*delta(:);
41 | l(1) = delta(1)*g/zf;
42 | u(1) = l(1);
43 | xln = wimin^2;
44 | xun = wimax^2;
45 | alpha = zi/g;
46 | b = dzi/g;
47 | c = @(x) constrMPC(delta,x,alpha,b);
48 | [x,b] = initialPoint(l,u,xln,xun);
49 | assert(b, 'could not find an initial point');
50 | lambda = zeros(n+1,1);
51 | 
52 | maxIter = 100;
53 | for i=1:maxIter
54 |   [~,f,~,g] = c(x);
55 |   disp(['||c||^2 : ' num2str(f^2)])
56 |   if checkKKT(f,g,x,lambda,l,u,xln,xun,1e-6,1e-6)
57 | %    disp(norm(f));
58 |     break;
59 |   end
60 |   %solve the LS
61 |   Ax = [x(1); x(2:end)-x(1:end-1)];
62 |   [p,pl] = feasibilityLS(g,f,l-Ax,u-Ax,xln-x(n),xun-x(n));
63 |   
64 |   %line search
65 |   a = 1;
66 |   beta = 0.9;
67 |   c1 = 0.01;
68 |   cgp = c1*f*g'*p;
69 |   while 0.5*c(x+a*p)^2 > 0.5*f^2 + a*cgp
70 |     a = beta*a;
71 |     if a<1e-8
72 |       error('step is too small');
73 |     end
74 |   end
75 |   x = x+a*p;
76 |   lambda = lambda + a*(lambda-pl);
77 | end
78 | disp(norm(f));
79 | end
80 | 
81 | 
82 | function b = checkKKT(f,g,x,lambda,l,u,xln,xun,tp,td)
83 | %see Stan's thesis 4.3.5
84 | tx = tp*(1+norm(x,Inf));
85 | tl = td*(1+norm(lambda,Inf));
86 | 
87 | en = zeros(length(l),1); en(end) = 1; %n-th column of the nxn identity
88 | gradL = f*g + [lambda(1:end-2)-lambda(2:end-1);lambda(end-1)] + lambda(end)*en;
89 | 
90 | cstr = [x(1); x(2:end)-x(1:end-1);x(end)];
91 | kktcstr = all(   (abs(cstr-[l;xln])<=tx & lambda<=tl) ...
92 |                | ([l;xln]-tx<=cstr & cstr<=[u;xun]+tx & abs(lambda)<=tl) ...
93 |                | (abs(cstr-[u;xun])<=tx & lambda>=-tl));
94 | b = norm(gradL,Inf) <= tl && kktcstr; 
95 | end


--------------------------------------------------------------------------------
/c++/src/CaptureSolver.cpp:
--------------------------------------------------------------------------------
  1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  2 |  *
  3 |  * This file is part of CPS.
  4 |  *
  5 |  * CPS is free software: you can redistribute it and/or modify
  6 |  * it under the terms of the GNU Lesser General Public License as published by
  7 |  * the Free Software Foundation, either version 3 of the License, or
  8 |  * (at your option) any later version.
  9 |  *
 10 |  * CPS is distributed in the hope that it will be useful,
 11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13 |  * GNU Lesser General Public License for more details.
 14 |  *
 15 |  * You should have received a copy of the GNU Lesser General Public License
 16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 17 |  */
 18 | 
 19 | #include <cps/CaptureSolver.h>
 20 | 
 21 | using namespace Eigen;
 22 | 
 23 | namespace cps
 24 | {
 25 |   CaptureSolver::CaptureSolver(int n, double viol)
 26 |     : sqp_(n)
 27 |     , violation_(viol)
 28 |   {
 29 |   }
 30 | 
 31 |   void CaptureSolver::violation(double v)
 32 |   {
 33 |     assert(v > 0);
 34 |     violation_ = v;
 35 |   }
 36 | 
 37 |   double CaptureSolver::violation() const
 38 |   {
 39 |     return violation_;
 40 |   }
 41 | 
 42 |   void CaptureSolver::SQPParameters(const SQP::Parameters& p)
 43 |   {
 44 |     sqp_.SQPParameters(p);
 45 |   }
 46 | 
 47 |   void CaptureSolver::LSParameters(const LeastSquare::Parameters& p)
 48 |   {
 49 |     sqp_.LSParameters(p);
 50 |   }
 51 | 
 52 |   const SQP::Parameters & CaptureSolver::SQPParameters() const
 53 |   {
 54 |     return sqp_.SQPParameters();
 55 |   }
 56 | 
 57 |   const LeastSquare::Parameters & CaptureSolver::LSParameters() const
 58 |   {
 59 |     return sqp_.LSParameters();
 60 |   }
 61 | 
 62 |   SolverStatus CaptureSolver::solve(const Problem& pb)
 63 |   {
 64 |     //check input
 65 |     if (pb.init_omega_min() > pb.init_omega_max())
 66 |     {
 67 |       return SolverStatus::NoLinearlyFeasiblePoint;
 68 |     }
 69 | 
 70 |     auto s = sqp_.solve(pb);
 71 |     
 72 |     //check if the problem is feasible for the non-linear constraint
 73 |     if (s == SolverStatus::Converge)
 74 |     {
 75 |       if (std::abs(pb.nonLinearConstraint().compute(sqp_.x())) <= violation_)
 76 |       {
 77 |         return SolverStatus::Converge;
 78 |       }
 79 |       else
 80 |       {
 81 |         return SolverStatus::Unfeasible;
 82 |       }
 83 |     }
 84 |     else
 85 |     {
 86 |       return s;
 87 |     }
 88 |   }
 89 | 
 90 |   SolverStatus CaptureSolver::solveFeasibility(const Problem& pb)
 91 |   {
 92 |     return sqp_.solveFeasibility(pb);;
 93 |   }
 94 | 
 95 |   const Eigen::VectorXd & CaptureSolver::x() const
 96 |   {
 97 |     return sqp_.x();
 98 |   }
 99 | 
100 |   const Eigen::VectorXd & CaptureSolver::lambda() const
101 |   {
102 |     return sqp_.lambda();
103 |   }
104 | 
105 |   const std::vector<Activation>& CaptureSolver::activeSet() const
106 |   {
107 |     return sqp_.activeSet();
108 |   }
109 | 
110 |   int CaptureSolver::numberOfIterations() const
111 |   {
112 |     return sqp_.numberOfIterations();;
113 |   }
114 | 
115 |   const stats::SQPStats & CaptureSolver::statistics() const
116 |   {
117 |     return sqp_.statistics();
118 |   }
119 | }


--------------------------------------------------------------------------------
/c++/tests/SQPTest.cpp:
--------------------------------------------------------------------------------
  1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  2 |  *
  3 |  * This file is part of CPS.
  4 |  *
  5 |  * CPS is free software: you can redistribute it and/or modify
  6 |  * it under the terms of the GNU Lesser General Public License as published by
  7 |  * the Free Software Foundation, either version 3 of the License, or
  8 |  * (at your option) any later version.
  9 |  *
 10 |  * CPS is distributed in the hope that it will be useful,
 11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13 |  * GNU Lesser General Public License for more details.
 14 |  *
 15 |  * You should have received a copy of the GNU Lesser General Public License
 16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 17 |  */
 18 | 
 19 | #include <iostream>
 20 | 
 21 | #include <cps/SQP.h>
 22 | 
 23 | #include "SQPTestCommon.h"
 24 | 
 25 | // boost
 26 | #define BOOST_TEST_MODULE SQPTests
 27 | #include <boost/test/unit_test.hpp>
 28 | 
 29 | using namespace Eigen;
 30 | using namespace cps;
 31 | 
 32 | BOOST_AUTO_TEST_CASE(testFeasibilitySQP)
 33 | {
 34 |   RawProblem raw;
 35 |   std::string base = TESTS_DIR;
 36 |   raw.read(base + "/data/Problem01.txt");
 37 | 
 38 |   Problem pb(raw);
 39 |   SQP sqp(static_cast<int>(raw.delta.size()));
 40 | 
 41 |   auto s = sqp.solveFeasibility(pb);
 42 | 
 43 |   double c;
 44 |   pb.nonLinearConstraint().compute(c, sqp.x());
 45 |   BOOST_CHECK(s == SolverStatus::Converge);
 46 |   BOOST_CHECK(pb.linearConstraints().checkPrimal(sqp.x()));
 47 |   BOOST_CHECK(std::abs(c) <= 1e-7);
 48 | }
 49 | 
 50 | 
 51 | BOOST_AUTO_TEST_CASE(BasicSQPTest)
 52 | {
 53 |   {
 54 |     RawProblem raw;
 55 |     std::string base = TESTS_DIR;
 56 |     raw.read(base + "/data/Problem01.txt");
 57 | 
 58 |     Problem pb(raw);
 59 |     if (raw.delta.size() <= 15)
 60 |       pb.objective().precompute(1);
 61 |     SQP sqp(static_cast<int>(raw.delta.size()));
 62 | 
 63 |     auto s = sqp.solve(pb);
 64 |     BOOST_CHECK(s == SolverStatus::Converge);
 65 |     BOOST_CHECK(sqp.x().isApprox(raw.Phi_.tail(raw.Phi_.size()-1), 1e-8));
 66 |   }
 67 |   {
 68 |     RawProblem raw;
 69 |     std::string base = TESTS_DIR;
 70 |     raw.read(base + "/data/Problem02.txt");
 71 | 
 72 |     Problem pb(raw);
 73 |     if (raw.delta.size() <= 15)
 74 |       pb.objective().precompute(1);
 75 |     SQP sqp(static_cast<int>(raw.delta.size()));
 76 | 
 77 |     auto s = sqp.solve(pb);
 78 |     BOOST_CHECK(s == SolverStatus::Converge);
 79 |     BOOST_CHECK(sqp.x().isApprox(raw.Phi_.tail(raw.Phi_.size() - 1), 1e-6));
 80 |   }
 81 |   {
 82 |     RawProblem raw;
 83 |     std::string base = TESTS_DIR;
 84 |     raw.read(base + "/data/Problem03.txt");
 85 | 
 86 |     Problem pb(raw);
 87 |     if (raw.delta.size() <= 15)
 88 |       pb.objective().precompute(1);
 89 |     SQP sqp(static_cast<int>(raw.delta.size()));
 90 | 
 91 |     auto s = sqp.solve(pb);
 92 |     BOOST_CHECK(s == SolverStatus::Converge);
 93 |     BOOST_CHECK(sqp.x().isApprox(raw.Phi_.tail(raw.Phi_.size() - 1), 1e-7));
 94 |   }
 95 | }
 96 | 
 97 | BOOST_AUTO_TEST_CASE(ResampledProblems)
 98 | {
 99 |   auto raw = resampleProblem("data/Problem01.txt", 20);
100 |   SQP sqp(static_cast<int>(raw.delta.size()));
101 | 
102 |   auto s = sqp.solve(raw);
103 | 
104 |   BOOST_CHECK(s == SolverStatus::Converge);
105 | }
106 | 


--------------------------------------------------------------------------------
/c++/include/cps/CaptureSolver.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 3 |  *
 4 |  * This file is part of CPS.
 5 |  *
 6 |  * CPS is free software: you can redistribute it and/or modify
 7 |  * it under the terms of the GNU Lesser General Public License as published by
 8 |  * the Free Software Foundation, either version 3 of the License, or
 9 |  * (at your option) any later version.
10 |  *
11 |  * CPS is distributed in the hope that it will be useful,
12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 |  * GNU Lesser General Public License for more details.
15 |  *
16 |  * You should have received a copy of the GNU Lesser General Public License
17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
18 |  */
19 | 
20 | #include <cps/SQP.h>
21 | 
22 | namespace cps
23 | {
24 |   /** A solver for the capture problem.
25 |     *
26 |     * The class is a wrapper around SQP to handle the differences between the
27 |     * original capture problem and the penalty-based relaxation solved by the
28 |     * SQP. It offers two main functions:
29 |     *  - solve
30 |     *  - solveFeasibility
31 |     */
32 |   class CPS_DLLAPI CaptureSolver
33 |   {
34 |   public:
35 |     /** Build a solver for problems of size n, with an accepted violation on 
36 |       * the non-linear constraint.
37 |       */
38 |     CaptureSolver(int n, double violation = 1e-6);
39 |     /** Set the accepted violation on the non-linear constraint.*/
40 |     void violation(double v);
41 |     /** Get the current accepted violation on the non-linear constraint.*/
42 |     double violation() const;
43 |     /** Set a new set of parameters for the SQP.*/
44 |     void SQPParameters(const SQP::Parameters& p);
45 |     /** Set a new set of parameters for the SQP's least-square solver.*/
46 |     void LSParameters(const LeastSquare::Parameters& p);
47 |     /** Get the current parameters of the SQP.*/
48 |     const SQP::Parameters& SQPParameters() const;
49 |     /** Get the current parameters of the SQP's least-square solver.*/
50 |     const LeastSquare::Parameters& LSParameters() const;
51 | 
52 |     /** Solve the following problem: 
53 |       *
54 |       * min. 1/2 ||J x||^2
55 |       * s.t. b(x) = 0
56 |       *      l_i <= x_i-x_{i-1} <= u_i for i=0..n-1  (x_{-1} = 0)
57 |       *      xln <= x_{n-1} <= xun
58 |       *
59 |       * where b(x) is the boundedness constraint violation described by pb.nonLinearConstraint(),
60 |       * J is described by pb.objective() and the linear constraints by pb.linearConstraints().
61 |       */
62 |     SolverStatus solve(const Problem& pb);
63 |     /** Solve the following problem: 
64 |       *
65 |       * min. 1/2 ||b(x)||^2
66 |       * s.t. l_i <= x_i-x_{i-1} <= u_i for i=0..n-1  (x_{-1} = 0)
67 |       *      xln <= x_{n-1} <= xun
68 |       *
69 |       * where b(x) is the boundedness constraint violation described by pb.nonLinearConstraint(),
70 |       * and the constraints are described by pb.linearConstraints().
71 |       */
72 |     SolverStatus solveFeasibility(const Problem& pb);
73 | 
74 |     /** Retrieve the solution (after call to solve() or solveFeasibility())*/
75 |     const Eigen::VectorXd& x() const;
76 |     /** Retrieve the Lagrange multipliers (after call to solve() or solveFeasibility())*/
77 |     const Eigen::VectorXd& lambda() const;
78 |     /** Retrieve the current active set.*/
79 |     const std::vector<Activation>& activeSet() const;
80 |     /** Get the number of SQP iterations of the last run.*/
81 |     int numberOfIterations() const;
82 | 
83 |     /** Get stats on the last run. Only meaningful if USE_STATS is defined*/
84 |     const stats::SQPStats& statistics() const;
85 | 
86 |   private:
87 |     double violation_;
88 |     SQP sqp_;
89 |     
90 |   };
91 | }


--------------------------------------------------------------------------------
/c++/tests/LeastSquareTest.cpp:
--------------------------------------------------------------------------------
  1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  2 |  *
  3 |  * This file is part of CPS.
  4 |  *
  5 |  * CPS is free software: you can redistribute it and/or modify
  6 |  * it under the terms of the GNU Lesser General Public License as published by
  7 |  * the Free Software Foundation, either version 3 of the License, or
  8 |  * (at your option) any later version.
  9 |  *
 10 |  * CPS is distributed in the hope that it will be useful,
 11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13 |  * GNU Lesser General Public License for more details.
 14 |  *
 15 |  * You should have received a copy of the GNU Lesser General Public License
 16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 17 |  */
 18 | 
 19 | #include <iostream>
 20 | 
 21 | #include <cps/LeastSquare.h>
 22 | #include <cps/QuadraticObjective.h>
 23 | 
 24 | // boost
 25 | #define BOOST_TEST_MODULE LeastSquareTests
 26 | #include <boost/test/unit_test.hpp>
 27 | 
 28 | using namespace Eigen;
 29 | using namespace cps;
 30 | 
 31 | BOOST_AUTO_TEST_CASE(LeastSquareFeasibilityTest)
 32 | {
 33 |   VectorXd l = -VectorXd::Random(10).cwiseAbs();
 34 |   VectorXd u = VectorXd::Random(10).cwiseAbs();
 35 |   LinearConstraints lc(l, u, -1, 1);
 36 | 
 37 |   VectorXd j = VectorXd::Random(10);
 38 |   double c = -10;
 39 | 
 40 |   LeastSquare ls(10);
 41 |   auto s = ls.solveFeasibility(j, c, lc);
 42 |   auto x = ls.x();
 43 |   auto lambda = ls.lambda();
 44 | 
 45 |   BOOST_CHECK(s == SolverStatus::Converge);
 46 |   BOOST_CHECK(lc.checkPrimal(x));
 47 |   BOOST_CHECK(lc.checkDual(lambda));
 48 |   VectorXd kkt = (c + j.dot(x))*j + lc.matrix().transpose()*lambda;
 49 |   BOOST_CHECK(kkt.isZero(1e-8));
 50 |   VectorXd Cx = lc.matrix()*x;
 51 |   for (DenseIndex i = 0; i < 10; ++i)
 52 |   {
 53 |     if (lambda(i) > 0)
 54 |     {
 55 |       BOOST_CHECK(std::abs(lambda(i)*(Cx(i) - u(i))) <= 1e-12);
 56 |     }
 57 |     else
 58 |     {
 59 |       BOOST_CHECK(std::abs(lambda(i)*(Cx(i) - l(i))) <= 1e-12);
 60 |     }
 61 |   }
 62 |   if (lambda(10) > 0)
 63 |   {
 64 |     BOOST_CHECK(std::abs(lambda(10)*(Cx(10) - 1)) <= 1e-12);
 65 |   }
 66 |   else
 67 |   {
 68 |     BOOST_CHECK(std::abs(lambda(10)*(Cx(10) + 1)) <= 1e-12);
 69 |   }
 70 | }
 71 | 
 72 | BOOST_AUTO_TEST_CASE(LeastSquareTest)
 73 | {
 74 |   VectorXd l = -VectorXd::Random(10).cwiseAbs();
 75 |   VectorXd u = VectorXd::Random(10).cwiseAbs();
 76 |   LinearConstraints lc(l, u, -1, 1);
 77 | 
 78 |   VectorXd j = 100*VectorXd::Random(10);
 79 |   double c = -200;
 80 | 
 81 |   VectorXd delta = VectorXd::LinSpaced(10, 0.01, 0.19);
 82 |   LeastSquareObjective obj(delta);
 83 |   VectorXd Jx0 = VectorXd::Zero(9);
 84 |   MatrixXd J = obj.matrix();
 85 | 
 86 |   LeastSquare ls(10);
 87 | 
 88 |   auto s = ls.solve(obj, Jx0, j, c, lc);
 89 | 
 90 |   auto x = ls.x();
 91 |   auto lambda = ls.lambda();
 92 | 
 93 |   //std::cout << x.transpose() << std::endl;
 94 |   //std::cout << lambda.transpose() << std::endl;
 95 | 
 96 |   double tl = 1e-8*lambda.lpNorm<Infinity>();
 97 |   BOOST_CHECK(s == SolverStatus::Converge);
 98 |   BOOST_CHECK(lc.checkPrimal(x));
 99 |   BOOST_CHECK(lc.checkDual(lambda, tl));
100 |   VectorXd kkt = (c + j.dot(x))*j + J.transpose()*(J*x) + lc.matrix().transpose()*lambda;
101 |   BOOST_CHECK(kkt.isZero(1e-8));
102 |   VectorXd Cx = lc.matrix()*x;
103 |   for (DenseIndex i = 0; i < 10; ++i)
104 |   {
105 |     if (lambda(i) > 0)
106 |     {
107 |       BOOST_CHECK(std::abs(lambda(i)*(Cx(i) - u(i))) <= tl);
108 |     }
109 |     else
110 |     {
111 |       BOOST_CHECK(std::abs(lambda(i)*(Cx(i) - l(i))) <= tl);
112 |     }
113 |   }
114 |   if (lambda(10) > 0)
115 |   {
116 |     BOOST_CHECK(std::abs(lambda(10)*(Cx(10) - 1)) <= tl);
117 |   }
118 |   else
119 |   {
120 |     BOOST_CHECK(std::abs(lambda(10)*(Cx(10) + 1)) <= tl);
121 |   }
122 | }
123 | 


--------------------------------------------------------------------------------
/c++/tests/ObjectiveTest.cpp:
--------------------------------------------------------------------------------
  1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  2 |  *
  3 |  * This file is part of CPS.
  4 |  *
  5 |  * CPS is free software: you can redistribute it and/or modify
  6 |  * it under the terms of the GNU Lesser General Public License as published by
  7 |  * the Free Software Foundation, either version 3 of the License, or
  8 |  * (at your option) any later version.
  9 |  *
 10 |  * CPS is distributed in the hope that it will be useful,
 11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13 |  * GNU Lesser General Public License for more details.
 14 |  *
 15 |  * You should have received a copy of the GNU Lesser General Public License
 16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 17 |  */
 18 | 
 19 | #include <iostream>
 20 | #include <vector>
 21 | 
 22 | #include <cps/LinearConstraints.h>
 23 | #include <cps/QuadraticObjective.h>
 24 | 
 25 | // boost
 26 | #define BOOST_TEST_MODULE ObjectiveTest
 27 | #include <boost/test/unit_test.hpp>
 28 | 
 29 | using namespace Eigen;
 30 | using namespace cps;
 31 | 
 32 | //binary form of i
 33 | std::vector<bool> toVec(int i, int nbits)
 34 | {
 35 |   std::vector<bool> b;
 36 |   for (int j = nbits-1; j >= 0; --j)
 37 |   {
 38 |     int a = static_cast<int>(i / (1 << j));
 39 |     b.push_back(a!=0);
 40 |     i -= a*(1 << j);
 41 |   }
 42 |   return b;
 43 | }
 44 | 
 45 | void setActiveSet(LinearConstraints& lc, const std::vector<bool>& act)
 46 | {
 47 |   for (size_t i = 0; i < act.size(); ++i)
 48 |     lc.activate(i, act[i] ? Activation::Equal : Activation::None);
 49 | }
 50 | 
 51 | int nact(const std::vector<bool>& act)
 52 | {
 53 |   int n = 0;
 54 |   for (size_t i = 0; i < act.size(); ++i)
 55 |     n += act[i];
 56 |   return n;
 57 | }
 58 | 
 59 | 
 60 | /** Check if the matrix is a band matrix with lower bandwidth p and upper bandwidth q*/
 61 | bool isBandMatrix(const MatrixXd& M, DenseIndex p, DenseIndex q, double prec = 1e-15)
 62 | {
 63 |   auto m = M.rows();
 64 |   // auto n = M.cols();
 65 | 
 66 |   bool b = true;
 67 |   for (DenseIndex i = p + 1; i < m && b; ++i)
 68 |     b = M.diagonal(-i).isZero(prec);
 69 |   for (DenseIndex i = q + 1; i < m && b; ++i)
 70 |     b = M.diagonal(i).isZero(prec);
 71 | 
 72 |   return b;
 73 | }
 74 | 
 75 | BOOST_AUTO_TEST_CASE(valueTest)
 76 | {
 77 |   VectorXd x = VectorXd::Random(10);
 78 |   VectorXd delta = VectorXd::LinSpaced(10, 0.01, 0.19);
 79 |   LeastSquareObjective obj(delta);
 80 | 
 81 |   MatrixXd J = obj.matrix();
 82 |   VectorXd Jx1 = J*x;
 83 |   VectorXd Jx2(9);
 84 |   obj.applyJToTheLeft(Jx2, x);
 85 |   double v = obj.value(x);
 86 |   BOOST_CHECK(Jx1.isApprox(Jx2));
 87 |   BOOST_CHECK(std::abs(v - 0.5*Jx1.squaredNorm()) < 1e-15);
 88 | 
 89 |   VectorXd JtJx1 = J.transpose()*Jx1;
 90 |   VectorXd JtJx2(10);
 91 |   obj.applyJTransposeToTheLeft(JtJx2, Jx2);
 92 |   BOOST_CHECK(JtJx1.isApprox(JtJx2));
 93 | }
 94 | 
 95 | BOOST_AUTO_TEST_CASE(projectedMatrixTest)
 96 | {
 97 |   int N = 12;
 98 |   VectorXd delta = VectorXd::LinSpaced(N, 0.01, 0.23);
 99 |   LeastSquareObjective obj(delta);
100 |   LinearConstraints lc(N);
101 | 
102 |   for (int i = 0; i < (1 << (N+1)) - 1; ++i)
103 |   {
104 |     auto act = toVec(i, N+1);
105 |     setActiveSet(lc, act);
106 |     auto na = lc.numberOfActiveConstraints();
107 |     auto J = obj.matrix();
108 |     MatrixXd JN0(N - 1, N - na);
109 |     lc.applyNullSpaceOnTheRight(JN0, J);
110 |     auto JN = obj.projectedMatrix(na, act);
111 | 
112 |     BOOST_CHECK(JN.isApprox(JN0, 1e-15));
113 |   }
114 | }
115 | 
116 | BOOST_AUTO_TEST_CASE(qrTest)
117 | {
118 |   int n = 12;
119 |   VectorXd delta = VectorXd::LinSpaced(n, 0.01, 0.02*n-0.01);
120 |   LeastSquareObjective obj(delta);
121 | 
122 |   for (int i=0; i < (1 << (n + 1)) - 1; ++i)
123 |   {
124 |     auto act = toVec(i, n + 1);
125 |     MatrixXd R(n - 1, n-nact(act));
126 |     CondensedOrthogonalMatrix Q(n-1, n-1, 2*n);
127 |     obj.qr(R, Q, act);
128 | 
129 |     MatrixXd J = obj.projectedMatrix(act);
130 |     Q.applyTo(J);
131 |     BOOST_CHECK(isBandMatrix(R, 0, 2, 1e-14));
132 |     BOOST_CHECK(R.isApprox(J, 1e-15));
133 |   }
134 | }
135 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | CPS: a Capture Problem Solver
 2 | =============================
 3 | 
 4 | A dedicated solver in C++ for the capture problem presented initially in 
 5 | [S. Caron, B. Mallein, "Balance control using both ZMP and COM height variations: A convex boundedness approach", ICRA 2018](https://hal.archives-ouvertes.fr/hal-01590509/document).
 6 | 
 7 | The solver itself is presented in
 8 | [S. Caron, A. Escande, L. Lanari, B. Mallein, "Capturability-based Pattern Generation for Walking with Variable Height", IEEE Transactions on Robotics, 2019](https://hal.archives-ouvertes.fr/hal-01689331/document).
 9 | 
10 | This repository contains three folders:
11 |  - the C++ code of the solver
12 |  - a MATLAB code that was used for prototyping
13 |  - a LaTeX technical document, with some of the math behind the solver (not fully complete)
14 | 
15 | Installation
16 | ------------
17 | 
18 | Compilation has been tested on Linux (gcc/clang) and Windows (Visual Studio).
19 | 
20 | ### Dependencies
21 | 
22 | To compile you will need the following tools:
23 | 
24 |  * [Git](https://git-scm.com/)
25 |  * [CMake](https://cmake.org/) >= 2.8.12
26 |  * [pkg-config](https://www.freedesktop.org/wiki/Software/pkg-config/) (use [pkg-config-lite](https://sourceforge.net/projects/pkgconfiglite/) on Windows)
27 |  * [doxygen](http://www.doxygen.org)
28 |  * A compiler with C++11 support
29 |  
30 | and the following dependencies:
31 |  * [Boost](http://www.boost.org/) >= 1.49 (>= 1.64 for Python bindings)
32 |  * [Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page) >= 3.2
33 | 
34 | This repository also uses [jrl-cmakemodules](https://github.com/jrl-umi3218/jrl-cmakemodules) as a submodule.
35 | 
36 | ### Building from source on Linux
37 | 
38 | Follow the standard CMake build procedure:
39 | 
40 | ```sh
41 | git clone --recursive https://github.com/jrl-umi3218/CaptureProblemSolver
42 | cd CaptureProblemSolver
43 | mkdir build && cd build
44 | cmake [options] ..
45 | make && make install
46 | ```
47 | 
48 | where the main options are:
49 |  * `-DCMAKE_BUILD_TYPE=Release` Build in Release mode
50 |  * `-DCMAKE_INSTALL_PREFIX=some/path/to/install` default is `/usr/local`
51 |  * `-DPYTHON_BINDINGS=ON` Build Python bindings
52 | 
53 | Use
54 | ---
55 | 
56 | ### C++ code
57 | 
58 | The C++ code is mainly intended to work as a library.
59 | 
60 | The main class is `cps::CaptureSolver` which provides the solver and is used through its method `solve` to which a `cps::Problem` instance is passed.
61 | A simple example of use can be found in `main.cpp`.
62 | 
63 | `cps::CaptureSolver` is a thin wrapper around `cps::SQP` where the real work is done. `cps::SQP` is used the same way.
64 | 
65 | As an alternative, the compilation of the project `main` generates an executable that takes as arguments the paths of files describing capture problem.
66 | Example of such files can be found in `c++\tests\data\`.
67 | 
68 | ### Input file format
69 | A problem can be described by a simple text file such as those found in `c++\tests\data\`.
70 | 
71 | The file parser looks for (matlab readable, semi-colum terminated) lines with the `=` character in them, and recognize the following fields (in any order):
72 |  * Delta (![](https://latex.codecogs.com/svg.latex?\boldsymbol{\delta}) in the paper)
73 |  * g
74 |  * lambda_min (![](https://latex.codecogs.com/svg.latex?\lambda_{min}))
75 |  * lambda_max (![](https://latex.codecogs.com/svg.latex?\lambda_{max}))
76 |  * omega_i_min (![](https://latex.codecogs.com/svg.latex?\omega_{\mathrm{i},\text{min}}))
77 |  * omega_i_max (![](https://latex.codecogs.com/svg.latex?\omega_{\mathrm{i},\text{max}}))
78 |  * s 
79 |  * z_bar (![](https://latex.codecogs.com/svg.latex?\bar{z}_{\mathrm{i}}))
80 |  * zd_bar (![](https://latex.codecogs.com/svg.latex?\dot{\bar{z}}_{\mathrm{i}}))
81 |  * z_f (![](https://latex.codecogs.com/svg.latex?\bar{z}_{\mathrm{f}}))
82 |  * (optionally) Phi (![](https://latex.codecogs.com/svg.latex?\left[\varphi_0=0,%20\varphi_1,%20\ldots,%20\varphi_n\right])), the solution computed by any other mean.
83 |  
84 | All other lines are ignored.
85 | 
86 | Thanks
87 | ------
88 | 
89 | - To Vincent Samy for his help with the
90 |   [Boost.Python](http://www.boost.org/doc/libs/1_64_0/libs/python/doc/html/) bindings
91 | 


--------------------------------------------------------------------------------
/c++/bindings/PyCaptureProblemSolver.cpp:
--------------------------------------------------------------------------------
 1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
 2 |  *
 3 |  * This file is part of CPS.
 4 |  *
 5 |  * CPS is free software: you can redistribute it and/or modify
 6 |  * it under the terms of the GNU Lesser General Public License as published by
 7 |  * the Free Software Foundation, either version 3 of the License, or
 8 |  * (at your option) any later version.
 9 |  *
10 |  * CPS is distributed in the hope that it will be useful,
11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 |  * GNU Lesser General Public License for more details.
14 |  *
15 |  * You should have received a copy of the GNU Lesser General Public License
16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
17 |  */
18 | 
19 | #include <boost/python.hpp>
20 | #include <boost/python/numpy.hpp>
21 | 
22 | #include <cps/LeastSquare.h>
23 | #include <cps/Problem.h>
24 | #include <cps/QuadraticObjective.h>
25 | #include <cps/SQP.h>
26 | 
27 | #include "converters.h"
28 | 
29 | namespace py = boost::python;
30 | namespace np = boost::python::numpy;
31 | 
32 | BOOST_PYTHON_MODULE(PyCaptureProblemSolver)
33 | {
34 |   Py_Initialize();
35 |   np::initialize();
36 |   pygen::convert<double>(pygen::Converters::Vector);
37 | 
38 |   using namespace cps;
39 | 
40 |   py::class_<LeastSquare>("LeastSquare", py::init<int>());
41 | 
42 |   py::class_<LeastSquareObjective>("LeastSquareObjective", py::init<const Eigen::VectorXd&>());
43 | 
44 |   LeastSquareObjective& (Problem::*pyObjective)() = &Problem::objective;
45 |   BoundenessConstraint& (Problem::*pyNonLinearConstraint)() = &Problem::nonLinearConstraint;
46 |   LinearConstraints& (Problem::*pyLinearConstraints)() = &Problem::linearConstraints;
47 |   py::class_<Problem>("Problem", py::init<RawProblem>())
48 |     .add_property("size", &Problem::size)
49 |     .def("linear_constraints", pyLinearConstraints, py::return_internal_reference<>())
50 |     .def("nonlinear_constraint", pyNonLinearConstraint, py::return_internal_reference<>())
51 |     .def("objective", pyObjective, py::return_internal_reference<>())
52 |     .def("precompute", &Problem::precompute)
53 |     .def("set_init_omega", &Problem::set_init_omega)
54 |     .def("set_init_omega_max", &Problem::set_init_omega_max)
55 |     .def("set_init_omega_min", &Problem::set_init_omega_min)
56 |     .def("set_init_zbar", &Problem::set_init_zbar)
57 |     .def("set_init_zbar_deriv", &Problem::set_init_zbar_deriv)
58 |     .def("set_lambda_max", &Problem::set_lambda_max)
59 |     .def("set_lambda_min", &Problem::set_lambda_min)
60 |     .def("set_lambdas", &Problem::set_lambdas)
61 |     .def("set_target_height", &Problem::set_target_height);
62 | 
63 |   py::class_<RawProblem>("RawProblem")
64 |     .def("read", &RawProblem::read)
65 |     .add_property("delta", 
66 |         py::make_getter(&RawProblem::delta, 
67 |           py::return_value_policy<py::copy_non_const_reference>()),
68 |         py::make_setter(&RawProblem::delta))
69 |     .add_property("Phi_", 
70 |         py::make_getter(&RawProblem::Phi_, 
71 |           py::return_value_policy<py::copy_non_const_reference>()),
72 |         py::make_setter(&RawProblem::Phi_))
73 |     .def_readwrite("g", &RawProblem::g)
74 |     .def_readwrite("lambda_min", &RawProblem::lambda_min)
75 |     .def_readwrite("lambda_max", &RawProblem::lambda_max)
76 |     .def_readwrite("init_omega_min", &RawProblem::init_omega_min)
77 |     .def_readwrite("init_omega_max", &RawProblem::init_omega_max)
78 |     .def_readwrite("init_zbar", &RawProblem::init_zbar)
79 |     .def_readwrite("init_zbar_deriv", &RawProblem::init_zbar_deriv)
80 |     .def_readwrite("target_height", &RawProblem::target_height);
81 | 
82 |   py::enum_<SolverStatus>("SolverStatus")
83 |     .value("Converge", SolverStatus::Converge)
84 |     .value("MaxIteration", SolverStatus::MaxIteration)
85 |     .value("LineSearchFailed", SolverStatus::LineSearchFailed)
86 |     .value("NoLinearlyFeasiblePoint", SolverStatus::NoLinearlyFeasiblePoint)
87 |     .value("NumericallyEquivalentIterates", SolverStatus::NumericallyEquivalentIterates)
88 |     .value("Fail", SolverStatus::Fail);
89 | 
90 |   py::class_<SQP>("SQP", py::init<int>())
91 |     .add_property("nb_iter", &SQP::numberOfIterations)
92 |     .def("lambda_", &SQP::lambda, py::return_value_policy<py::copy_const_reference>())
93 |     .def("numberOfIterations", &SQP::numberOfIterations)
94 |     .def("solve", &SQP::solve)
95 |     .def("x", &SQP::x, py::return_value_policy<py::copy_const_reference>());
96 | }
97 | 


--------------------------------------------------------------------------------
/c++/include/cps/Givens.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  3 |  *
  4 |  * This file is part of CPS.
  5 |  *
  6 |  * CPS is free software: you can redistribute it and/or modify
  7 |  * it under the terms of the GNU Lesser General Public License as published by
  8 |  * the Free Software Foundation, either version 3 of the License, or
  9 |  * (at your option) any later version.
 10 |  *
 11 |  * CPS is distributed in the hope that it will be useful,
 12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 14 |  * GNU Lesser General Public License for more details.
 15 |  *
 16 |  * You should have received a copy of the GNU Lesser General Public License
 17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 18 |  */
 19 | 
 20 | #include <Eigen/Jacobi>
 21 | 
 22 | #include <cps/cps_api.h>
 23 | #include <cps/defs.h>
 24 | 
 25 | namespace cps
 26 | {
 27 |   /** A dummy structure for dinstinguishing overloads.*/
 28 |   struct condensed_t {};
 29 | 
 30 |   /** Represent a Givens matrix G.*/
 31 |   class CPS_DLLAPI Givens
 32 |   {
 33 |   public:
 34 |     using Index = Eigen::DenseIndex;
 35 | 
 36 |     /** Default value: identity matrix*/
 37 |     Givens();
 38 | 
 39 |     /** Build a Givens rotation G with submatrix G([i,j],[i,j]) = [c s; -s c]
 40 |       * (Matlab notations)
 41 |       */
 42 |     Givens(Index i, Index j, double c, double s);
 43 |     /** Version with j = i+1. */
 44 |     Givens(Index i, double c, double s);
 45 | 
 46 |     /** Build a Givens rotation G such that 
 47 |     *                 | M(i,k) |   | x |
 48 |     * G([i,j][i,j])^T | M(j,k) | = | 0 |
 49 |     */
 50 |     template<typename Derived>
 51 |     Givens(const Eigen::MatrixBase<Derived>& M, Index i, Index j, Index k);
 52 |     /** Version with j=i+1. */
 53 |     template<typename Derived>
 54 |     Givens(const Eigen::MatrixBase<Derived>& M, Index i, Index k);
 55 | 
 56 |     /** Performs M = G^T M
 57 |       *
 58 |       * Don't let the const ref on M fool you: it is a (recommended) trick to
 59 |       * accept temporaries such as blocks. Internally, the const is cast away.
 60 |       *
 61 |       * We don't use Eigen::Ref here because this function will be used with
 62 |       * many small fixed-size matrices, and we want the compiler to take 
 63 |       * advantage of that.
 64 |       */
 65 |     template<typename Derived>
 66 |     void applyTo(const Eigen::MatrixBase<Derived>& M) const;
 67 | 
 68 |     /** Same as above for M with row size 2, and we ignore i and j.*/
 69 |     template<typename Derived>
 70 |     void applyTo(const Eigen::MatrixBase<Derived>& M, condensed_t) const;
 71 | 
 72 |     /** Computes M = M * G*/
 73 |     template<typename Derived>
 74 |     void applyOnTheRightTo(const Eigen::MatrixBase<Derived>& M) const;
 75 | 
 76 |     /** Add incr to i and j.
 77 |       * This is in particular useful if G was computed for submatrix S starting
 78 |       * at row incr of a matrix M. In this case with G' the result of G.extend(incr)
 79 |       * G^T S and G'^T M have the same rows changed.
 80 |       */
 81 |     void extend(Index incr);
 82 | 
 83 |   private:
 84 |     Index i_;
 85 |     Index j_;
 86 |     /** We store directily the transpose matrix*/
 87 |     Eigen::JacobiRotation<double> Jt_;
 88 |   };
 89 | 
 90 | 
 91 |   template<typename Derived>
 92 |   inline Givens::Givens(const Eigen::MatrixBase<Derived>& M, Index i, Index j, Index k)
 93 |     : i_(i), j_(j)
 94 |   {
 95 |     Eigen::JacobiRotation<double> G;
 96 |     G.makeGivens(M(i, k), M(j, k));
 97 |     Jt_ = G.transpose();
 98 |   }
 99 | 
100 |   template<typename Derived>
101 |   inline Givens::Givens(const Eigen::MatrixBase<Derived>& M, Index i, Index k)
102 |     : Givens(M, i, i + 1, k)
103 |   {
104 |   }
105 | 
106 |   template<typename Derived>
107 |   inline void Givens::applyTo(const Eigen::MatrixBase<Derived>& M) const
108 |   {
109 |     //recall that Jt_ stores the transpose of the rotation, so we don't need to transpose again.
110 |     const_cast<Eigen::MatrixBase<Derived>&>(M).applyOnTheLeft(i_, j_, Jt_);
111 |   }
112 | 
113 |   template<typename Derived>
114 |   inline void Givens::applyTo(const Eigen::MatrixBase<Derived>& M, condensed_t) const
115 |   {
116 |     static_assert(Eigen::internal::traits<Derived>::RowsAtCompileTime == 2, "This methods works only for matrices with row size 2");
117 |     //FIXME: shall we specialized for fixed size matrix?
118 |     const_cast<Eigen::MatrixBase<Derived>&>(M).applyOnTheLeft(0, 1, Jt_);
119 |   }
120 | 
121 |   template<typename Derived>
122 |   inline void Givens::applyOnTheRightTo(const Eigen::MatrixBase<Derived>& M) const
123 |   {
124 |     const_cast<Eigen::MatrixBase<Derived>&>(M).applyOnTheRight(i_, j_, Jt_.transpose());
125 |   }
126 | }
127 | 


--------------------------------------------------------------------------------
/c++/include/cps/Problem.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  3 |  *
  4 |  * This file is part of CPS.
  5 |  *
  6 |  * CPS is free software: you can redistribute it and/or modify
  7 |  * it under the terms of the GNU Lesser General Public License as published by
  8 |  * the Free Software Foundation, either version 3 of the License, or
  9 |  * (at your option) any later version.
 10 |  *
 11 |  * CPS is distributed in the hope that it will be useful,
 12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 14 |  * GNU Lesser General Public License for more details.
 15 |  *
 16 |  * You should have received a copy of the GNU Lesser General Public License
 17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 18 |  */
 19 | 
 20 | #include <string>
 21 | 
 22 | #include <Eigen/Core>
 23 | 
 24 | #include <cps/cps_api.h>
 25 | #include <cps/BoundenessConstraint.h>
 26 | #include <cps/LinearConstraints.h>
 27 | #include <cps/QuadraticObjective.h>
 28 | 
 29 | 
 30 | namespace cps
 31 | {
 32 |   /** Description of a capture problem parameters.
 33 |     * Fields name correspond to the notations in the paper.
 34 |     */
 35 |   struct CPS_DLLAPI RawProblem
 36 |   {
 37 |     /** Read from file (see examples in test/data/.*/
 38 |     void read(const std::string& filepath);
 39 | 
 40 |     double g = 9.80665; // ISO 80000-3
 41 |     double lambda_min;
 42 |     double lambda_max;
 43 |     Eigen::VectorXd delta;
 44 |     double init_omega_min;
 45 |     double init_omega_max;
 46 |     double init_zbar;         //z_i
 47 |     double init_zbar_deriv;   //dz_i/dt
 48 |     double target_height;     //z_f
 49 | 
 50 |     //solution (optional)
 51 |     Eigen::VectorXd Phi_;
 52 |   };
 53 | 
 54 | 
 55 |   /** A class representing a capture problem to be passed to the SQP solver.
 56 |     */
 57 |   class CPS_DLLAPI Problem
 58 |   {
 59 |   public:
 60 |     /** Build from a RawProblem.*/
 61 |     Problem(const RawProblem& pb);
 62 | 
 63 |     /** Get the underlying LeastSquareObjective instance.*/
 64 |     const LeastSquareObjective& objective() const;
 65 |     /** Get the underlying LeastSquareObjective instance.*/
 66 |     LeastSquareObjective& objective();
 67 |     /** Get the underlying BoundenessConstraint instance.*/
 68 |     const BoundenessConstraint& nonLinearConstraint() const;
 69 |     /** Get the underlying BoundenessConstraint instance.*/
 70 |     BoundenessConstraint& nonLinearConstraint();
 71 |     /** Get the underlying LinearConstraints instance.*/
 72 |     const LinearConstraints& linearConstraints() const;
 73 |     /** Get the underlying LinearConstraints instance.*/
 74 |     LinearConstraints& linearConstraints();
 75 |     /** Get the size n of the problem.*/
 76 |     Eigen::VectorXd::Index size() const;
 77 | 
 78 |     /** Precompute QR decompositions of the projected-nullspace jacobians of
 79 |       * the least-square objective for all possible active set value.
 80 |       *
 81 |       * Note that these pre-computations depend only on the `delta` vector of
 82 |       * the internal RawProblem. All other parameters (`init_zbar`,
 83 |       * `target_height`, etc.) can be changed with no need to call this function
 84 |       * again.
 85 |       */
 86 |     void precompute();
 87 | 
 88 |     /** Change omega_i_min and omega_i_max.*/
 89 |     void set_init_omega(double init_omega_min, double init_omega_max);
 90 |     /** Change omega_i_max.*/
 91 |     void set_init_omega_max(double init_omega_max);
 92 |     /** Change omega_i_min.*/
 93 |     void set_init_omega_min(double init_omega_min);
 94 |     /** Change z_i.*/
 95 |     void set_init_zbar(double init_zbar);
 96 |     /** Change dz_i/dt.*/
 97 |     void set_init_zbar_deriv(double init_zbar_deriv);
 98 |     /** Change lambda_max.*/
 99 |     void set_lambda_max(double lambda_max);
100 |     /** Change lambda_min.*/
101 |     void set_lambda_min(double lambda_min);
102 |     /** Change lambda_min and lambda_max.*/
103 |     void set_lambdas(double lambda_min, double lambda_max);
104 |     /** Change z_f*/
105 |     void set_target_height(double target_height);
106 | 
107 |     /** Get delta.*/
108 |     const Eigen::VectorXd& delta() const { return raw_.delta; }
109 |     /** Get omega_i_max.*/
110 |     double init_omega_max() const { return raw_.init_omega_max; }
111 |     /** Get omega_i_min.*/
112 |     double init_omega_min() const { return raw_.init_omega_min; }
113 |     /** Get z_i.*/
114 |     double init_zbar() const { return raw_.init_zbar; }
115 |     /** Get dz_i/dt.*/
116 |     double init_zbar_deriv() const { return raw_.init_zbar_deriv; }
117 |     /** Get lambda_max.*/
118 |     double lambda_max() const { return raw_.lambda_max; }
119 |     /** Get lambda_min.*/
120 |     double lambda_min() const { return raw_.lambda_min; }
121 |     /** Get z_f.*/
122 |     double target_height() const { return raw_.target_height; }
123 | 
124 |   private:
125 |     void computeAndSetBounds0();
126 |     void computeAndSetZonotopeBounds();
127 |     void computeAndSetBoundsN();
128 |     void computeAndSetAlpha();
129 |     void computeAndSetb();
130 | 
131 |     LeastSquareObjective lso_;
132 |     LinearConstraints lc_;
133 |     BoundenessConstraint bc_;
134 | 
135 |     RawProblem raw_;
136 |   };
137 | }
138 | 


--------------------------------------------------------------------------------
/c++/include/cps/LeastSquare.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  3 |  *
  4 |  * This file is part of CPS.
  5 |  *
  6 |  * CPS is free software: you can redistribute it and/or modify
  7 |  * it under the terms of the GNU Lesser General Public License as published by
  8 |  * the Free Software Foundation, either version 3 of the License, or
  9 |  * (at your option) any later version.
 10 |  *
 11 |  * CPS is distributed in the hope that it will be useful,
 12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 14 |  * GNU Lesser General Public License for more details.
 15 |  *
 16 |  * You should have received a copy of the GNU Lesser General Public License
 17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 18 |  */
 19 | 
 20 | #include <Eigen/Core>
 21 | 
 22 | #include <cps/cps_api.h>
 23 | #include <cps/defs.h>
 24 | #include <cps/LinearConstraints.h>
 25 | #include <cps/QuadraticObjective.h>
 26 | #include <cps/Statistics.h>
 27 | 
 28 | namespace cps
 29 | {
 30 |   /** Information returned by the solvers upon completion.*/
 31 |   enum class SolverStatus
 32 |   {
 33 |     Converge,                       /** A solution was found*/
 34 |     MaxIteration,                   /** The maximum number of iteration was reached*/
 35 |     LineSearchFailed,               /** Step in the line search became too small*/
 36 |     NoLinearlyFeasiblePoint,        /** Linear inequality constraints are incompatible*/
 37 |     Unfeasible,                     /** The constraints (linear and non-linear) are not compatible*/
 38 |     NumericallyEquivalentIterates,  /** Previous and new iterates are equal*/
 39 |     Fail
 40 |   };
 41 | 
 42 |   /** A solver dedicated solver for the problem for the least-square subproblem
 43 |     * occuring in cps::SQP.
 44 |     *
 45 |     * The class in itself handle the memory and statistics useful for its two
 46 |     * main methods:
 47 |     *  - solve
 48 |     *  - solveFeasibility
 49 |     */
 50 |   class CPS_DLLAPI LeastSquare
 51 |   {
 52 |   public:
 53 |     /** A set of parameters for controlling the execution of the resolution.*/
 54 |     class Parameters
 55 |     {
 56 |     public:
 57 |       int    maxIter()       const { return maxIter_; }
 58 |       double minNorm_p()     const { return minNorm_p_; }
 59 |       double dualEps()       const { return dualEps_; }
 60 |       double rankThreshold() const { return rankThreshold_; }
 61 | 
 62 |       Parameters& maxIter      (int i)    { maxIter_ = i;       return *this; }
 63 |       Parameters& minNorm_p    (double d) { minNorm_p_ = d;     return *this; }
 64 |       Parameters& dualEps      (double d) { dualEps_ = d;       return *this; }
 65 |       Parameters& rankThreshold(double d) { rankThreshold_ = d; return *this; }
 66 | 
 67 |     private:
 68 |       int maxIter_ = -1;              //maximum number of active set iterations. If <= 0, defaulted to 10*n
 69 |       double minNorm_p_ = 1e-10;      //bound on the L-inf norm of p under which p is treated as 0
 70 |       double dualEps_ = 1e-15;        //margin on the 0 for the Lagrange multipliers
 71 |       double rankThreshold_ = 1e-12;  //threshold on the last diagonal value in the QR of A, to detect rank loss
 72 |     };
 73 | 
 74 |     /** Create a solver for a problem of size n.*/
 75 |     LeastSquare(int n);
 76 | 
 77 |     /** Change parameters.*/
 78 |     void parameters(const Parameters& param);
 79 |     /** Get the current parameters.*/
 80 |     const Parameters& parameters() const;
 81 | 
 82 |     /** Solve the following problem:
 83 |       *
 84 |       * min. 1/2 ||J x + Jx0||^2 + mu^2/2 ||j^T x + c||^2
 85 |       * s.t. l_i <= x_i-x_{i-1} <= u_i for i=0..n-1  (x_{-1} = 0)
 86 |       *      xln <= x_{n-1} <= xun
 87 |       *
 88 |       * where J is described by obj, and the constraints by lc
 89 |       */
 90 |     SolverStatus solve(const LeastSquareObjective& obj, const VectorConstRef& Jx0, const VectorConstRef& j, double c, LinearConstraints& lc);
 91 | 
 92 |     /** Solve the following problem:
 93 |     *
 94 |     * min. 1/2 ||j^T x + c||^2
 95 |     * s.t. l_i <= x_i-x_{i-1} <= u_i for i=0..n-1  (x_{-1} = 0)
 96 |     *      xln <= x_{n-1} <= xun
 97 |     *
 98 |     * where the constraints are described by lc
 99 |     */
100 |     SolverStatus solveFeasibility(const VectorConstRef& j, double c, LinearConstraints& lc);
101 | 
102 |     /** Retrieve the solution (after call to solve() or solveFeasibility())*/
103 |     const Eigen::VectorXd& x() const;
104 |     /** Retrieve the Lagrange multipliers (after call to solve() or solveFeasibility())*/
105 |     const Eigen::VectorXd& lambda() const;
106 | 
107 |     /** Get stats on the last run. Only meaningful if USE_STATS is defined*/
108 |     const stats::LSStats& statistics() const;
109 | 
110 |   private:
111 |     // size of the problem
112 |     Eigen::DenseIndex n_;
113 | 
114 |     //solver parameters;
115 |     Parameters params_;
116 | 
117 |     //computation data
118 |     Eigen::VectorXd x_;
119 |     Eigen::VectorXd p_;
120 |     Eigen::VectorXd z_;
121 |     Eigen::VectorXd lambda_;
122 |     Eigen::VectorXd lambdaAct_;
123 |     Eigen::VectorXd jN_;
124 |     Eigen::MatrixXd A_;
125 |     Eigen::VectorXd b_;
126 |     Eigen::VectorXd Jx_;
127 |     Eigen::VectorXd JtJx_;
128 |     Eigen::VectorXd tmp_;
129 |     GivensSequence Qg_;
130 |     CondensedOrthogonalMatrix Q_;
131 | 
132 |     //stats
133 |     stats::LSStats stats_;
134 |   };
135 | }
136 | 


--------------------------------------------------------------------------------
/c++/include/cps/SQP.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  3 |  *
  4 |  * This file is part of CPS.
  5 |  *
  6 |  * CPS is free software: you can redistribute it and/or modify
  7 |  * it under the terms of the GNU Lesser General Public License as published by
  8 |  * the Free Software Foundation, either version 3 of the License, or
  9 |  * (at your option) any later version.
 10 |  *
 11 |  * CPS is distributed in the hope that it will be useful,
 12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 14 |  * GNU Lesser General Public License for more details.
 15 |  *
 16 |  * You should have received a copy of the GNU Lesser General Public License
 17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 18 |  */
 19 | 
 20 | #include <Eigen/Core>
 21 | 
 22 | #include <cps/cps_api.h>
 23 | #include <cps/LeastSquare.h>
 24 | #include <cps/Problem.h>
 25 | #include <cps/QuadraticObjective.h>
 26 | #include <cps/Statistics.h>
 27 | 
 28 | namespace cps
 29 | {
 30 |   /** A solver for the (modified) capture problem.
 31 |     *
 32 |     * The class in itself handles the memory and statistics useful for its two
 33 |     * main methods:
 34 |     *  - solve
 35 |     *  - solveFeasibility
 36 |     */
 37 |   class CPS_DLLAPI SQP
 38 |   {
 39 |   public:
 40 |     /** A set of parameters for controlling the execution of the resolution.*/
 41 |     class Parameters
 42 |     {
 43 |     public:
 44 |       int maxIter()           const { return maxIter_; }
 45 |       double mu()             const { return mu_; }
 46 |       double beta()           const { return beta_; }
 47 |       double c1()             const { return c1_; }
 48 |       double smallestLSStep() const { return smallestLSStep_; }
 49 |       double tau_p()          const { return tau_p_; }
 50 |       double tau_d()          const { return tau_d_; }
 51 |       double feasibilityEps() const { return feasibilityEps_; }
 52 | 
 53 |       Parameters& maxIter       (int i)    { maxIter_ = i;        return *this; }
 54 |       Parameters& mu            (double d) { mu_ = d;             return *this; }
 55 |       Parameters& beta          (double d) { beta_ = d;           return *this; }
 56 |       Parameters& c1            (double d) { c1_ = d;             return *this; }
 57 |       Parameters& smallestLSStep(double d) { smallestLSStep_ = d; return *this; }
 58 |       Parameters& tau_p         (double d) { tau_p_ = d;          return *this; }
 59 |       Parameters& tau_d         (double d) { tau_d_ = d;          return *this; }
 60 |       Parameters& feasibilityEps(double d) { feasibilityEps_ = d; return *this; }
 61 | 
 62 |     private:
 63 |       int maxIter_ = 100;            //maximum number of iterations
 64 |       double mu_ = 100000;           //penalty parameter
 65 |       double beta_ = 0.9;            //backtracking multiplier in line search
 66 |       double c1_ = 0.01;             //gradient coefficient in line search
 67 |       double smallestLSStep_ = 1e-8;
 68 |       double tau_p_ = 1e-6;          //precision parameter on primal condition
 69 |       double tau_d_ = 1e-6;          //precision parameter on dual condition
 70 |       double feasibilityEps_ = 1e-8; //margin of feasibility for which a potential numerical issue is detected
 71 |     };
 72 | 
 73 |     /** Build a solver for problems of size n.*/
 74 |     SQP(int n);
 75 |     /** Set a new set of parameters for the SQP.*/
 76 |     void SQPParameters(const Parameters& p);
 77 |     /** Set a new set of parameters for the underlying least-square solver.*/
 78 |     void LSParameters(const LeastSquare::Parameters& p);
 79 |     /** Get the current parameters of the SQP.*/
 80 |     const Parameters& SQPParameters() const;
 81 |     /** Get the current parameters of the underlying least-square solver.*/
 82 |     const LeastSquare::Parameters& LSParameters() const;
 83 | 
 84 |     /** Solve the following problem: 
 85 |       *
 86 |       * min. 1/2 ||J x||^2 + mu^2/2 ||b(x)||^2
 87 |       * s.t. l_i <= x_i-x_{i-1} <= u_i for i=0..n-1  (x_{-1} = 0)
 88 |       *      xln <= x_{n-1} <= xun
 89 |       *
 90 |       * where b(x) is the boundedness constraint violation described by pb.nonLinearConstraint(),
 91 |       * J is described by pb.objective() and the constraints by pb.linearConstraints().
 92 |       */
 93 |     SolverStatus solve(const Problem& pb);
 94 |     /** Solve the following problem: 
 95 |       *
 96 |       * min. 1/2 ||b(x)||^2
 97 |       * s.t. l_i <= x_i-x_{i-1} <= u_i for i=0..n-1  (x_{-1} = 0)
 98 |       *      xln <= x_{n-1} <= xun
 99 |       *
100 |       * where b(x) is the boundedness constraint violation described by pb.nonLinearConstraint(),
101 |       * and the constraints are described by pb.linearConstraints().
102 |       */
103 |     SolverStatus solveFeasibility(const Problem& pb);
104 | 
105 |     /** Retrieve the solution (after call to solve() or solveFeasibility())*/
106 |     const Eigen::VectorXd& x() const;
107 |     /** Retrieve the Lagrange multipliers (after call to solve() or solveFeasibility())*/
108 |     const Eigen::VectorXd& lambda() const;
109 |     /** Retrieve the current active set.*/
110 |     const std::vector<Activation>& activeSet() const;
111 |     /** Get the number of iterations of the last run.*/
112 |     int numberOfIterations() const;
113 | 
114 |     /** Get stats on the last run. Only meaningful if USE_STATS is defined*/
115 |     const stats::SQPStats& statistics() const;
116 | 
117 |   private:
118 |     bool checkKKT(const Eigen::VectorXd& x, const Eigen::VectorXd& lambda, double f, const Eigen::VectorXd& g, 
119 |                   const LinearConstraints& lc, const LeastSquareObjective* const obj = nullptr) const;
120 | 
121 |     Eigen::DenseIndex n_;
122 |     LeastSquare ls_;
123 | 
124 |     //optimization parameters
125 |     Parameters params_;
126 | 
127 |     //optimization data
128 |     LinearConstraints shiftedLC_;                 //constraints passed to the LS
129 |     Eigen::VectorXd x_;                           //value of the iterate
130 |     Eigen::VectorXd xa_;                          //value of the current point in the line search
131 |     Eigen::VectorXd j_;                           //gradient of the non-linear function
132 |     Eigen::VectorXd lambda_;                      //Lagrange multipliers
133 |     mutable Eigen::VectorXd Cx_;                  //value of C*x
134 |     mutable Eigen::VectorXd Cl_;                  //value of C^T*lambda
135 |     mutable Eigen::VectorXd Jx_;                  //value of J*x
136 |     mutable Eigen::VectorXd JtJx_;                //value of J^T * J *x
137 |     mutable Eigen::VectorXd Jp_;                  //value of J*p
138 |     mutable Eigen::VectorXd gradL_;               //gradient of the Lagrangian
139 |     std::vector<Activation> currentActiveSet_;    //current active set
140 |     std::vector<Activation> previousActiveSet_;   //active set backup
141 |     int k_;                                       //iteration number
142 | 
143 |     //stats
144 |     stats::SQPStats stats_;
145 |   };
146 | }
147 | 


--------------------------------------------------------------------------------
/c++/include/cps/QuadraticObjective.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  3 |  *
  4 |  * This file is part of CPS.
  5 |  *
  6 |  * CPS is free software: you can redistribute it and/or modify
  7 |  * it under the terms of the GNU Lesser General Public License as published by
  8 |  * the Free Software Foundation, either version 3 of the License, or
  9 |  * (at your option) any later version.
 10 |  *
 11 |  * CPS is distributed in the hope that it will be useful,
 12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 14 |  * GNU Lesser General Public License for more details.
 15 |  *
 16 |  * You should have received a copy of the GNU Lesser General Public License
 17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 18 |  */
 19 | 
 20 | #include <Eigen/Core>
 21 | 
 22 | #include <cps/cps_api.h>
 23 | #include <cps/CondensedOrthogonalMatrix.h>
 24 | #include <cps/defs.h>
 25 | #include <cps/QRAlgorithms.h>
 26 | 
 27 | namespace cps
 28 | {
 29 |   /** A class for the computations related to the objective function ||J x||^2.*/
 30 |   class CPS_DLLAPI LeastSquareObjective
 31 |   {
 32 |   public:
 33 |     using Index = Eigen::DenseIndex;
 34 | 
 35 |     /** Build the objective from the vector delta.*/
 36 |     LeastSquareObjective(const Eigen::VectorXd& delta);
 37 | 
 38 |     /** Size of the problem.*/
 39 |     Index size() const;
 40 | 
 41 |     /** Evaluate the objective at x.*/
 42 |     double value(const VectorConstRef& x) const;
 43 | 
 44 |     /** Y = J X
 45 |       * Note that the constness of Y is a trick to allow the compiler to accept 
 46 |       * temporary views (like block). The constness is not honored.
 47 |       */
 48 |     template<typename Derived1, typename Derived2>
 49 |     void applyJToTheLeft(const Eigen::MatrixBase<Derived1>& Y, const Eigen::MatrixBase<Derived2>& X) const;
 50 |     /** Y = J^T X
 51 |     * Note that the constness of Y is a trick to allow the compiler to accept
 52 |     * temporary views (like block). The constness is not honored.
 53 |     */
 54 |     template<typename Derived1, typename Derived2>
 55 |     void applyJTransposeToTheLeft(const Eigen::MatrixBase<Derived1>& Y, const Eigen::MatrixBase<Derived2>& X) const;
 56 | 
 57 | 
 58 |     /** Returns the QR decomposition of J_A for the given active-set act.
 59 |       * The computation is performed only of no precomputation were carried,
 60 |       * otherwise the precomputed decomposition is returned.
 61 |       *
 62 |       * If J_A is a subset of (continuous) rows in a bigger matrix, shift is
 63 |       * the number of line above J_A.
 64 |       */
 65 |     void qr(MatrixRef R, CondensedOrthogonalMatrix& Q, const std::vector<bool>& act, Index shift = 0) const;
 66 |     /** Same as above, were the number of active constraints in act is already
 67 |       * given by nact, to  avoid re-scanning the vector.
 68 |       */
 69 |     void qr(MatrixRef R, CondensedOrthogonalMatrix& Q, Index nact, const std::vector<bool>& act, Index shift = 0) const;
 70 | 
 71 |     /** Return J. For Debug*/
 72 |     Eigen::MatrixXd matrix() const;
 73 |     /** Return J*Na. For debug*/
 74 |     Eigen::MatrixXd projectedMatrix(const std::vector<bool>& act) const;
 75 |     /** Same as above, where the number of active constraint in act is given by nact.*/
 76 |     Eigen::MatrixXd projectedMatrix(Index nact, const std::vector<bool>& act) const;
 77 | 
 78 |     /** Given e = d_(dstart:dend), build a matrix with main body of the form
 79 |       *  S x
 80 |       *  x + *
 81 |       *    * + *
 82 |       *      ...
 83 |       *       * + y
 84 |       *         y E
 85 |       * where S and E are specified by startType and endType, the [x * * ... * y] 
 86 |       * are a sub-vector of e, and the + are such that a line * + * (or x + * or
 87 |       * y + *) has the form [e_i  -e_i-e_{i+1}  e_{i+1}] .
 88 |       * startType:
 89 |       * -1: S = [e_0; -e_0 - e_1],  x = e_1
 90 |       * -2: S = -e_0 - e_1,         x = e_1
 91 |       * -3: S = -e_0,               x = e_0 
 92 |       * stopType:
 93 |       * -1: E = [same; e_k],        y = e_{k-1}
 94 |       * -2: E = same,               y = e_{k-1}
 95 |       * -3: E = [same, e_k],        y = e_{k-1}
 96 |       * -4: E = same,               y = e_k (except for corner cases, this means E=-e_k)
 97 |       * where same means that the value in place is not changed.
 98 |       */
 99 |     void buildJj(MatrixRef Jj, Index dstart, Index dend, StartType startType, EndType endType) const;
100 |     
101 |     /** Return the QR of the above matrix.
102 |       * The GivensSequence Q is extended by extend.
103 |       */
104 |     void qrJj(MatrixRef R, GivensSequence& Q, Index extend, Index dstart, Index dend, StartType startType, EndType endType) const;
105 | 
106 |     /** Precompute the decompositions for all the possible active set value.*/
107 |     void precompute(Index shift);
108 | 
109 |   private:
110 |     /** Precomputed QR decomposition.*/
111 |     struct Precomputation
112 |     {
113 |       Eigen::MatrixXd R;
114 |       CondensedOrthogonalMatrix Q;
115 |     };
116 | 
117 |     /** Performe the QR decomposition of J_A in qr(R,Q,act,shift) when it was not precomputed. */
118 |     void qrComputation(MatrixRef R, CondensedOrthogonalMatrix& Q, const std::vector<bool>& act, Index shift = 0) const;
119 |     /** Same as above for qr(R,Q,nact,act,shift)*/
120 |     void qrComputation(MatrixRef R, CondensedOrthogonalMatrix& Q, Index nact, const std::vector<bool>& act, Index shift = 0) const;
121 | 
122 |     //data
123 |     Eigen::DenseIndex n_;
124 |     Eigen::VectorXd delta_;
125 |     Eigen::VectorXd d_;     // 1/delta
126 | 
127 |     //computation data
128 |     mutable Eigen::VectorXd e_;
129 |     SpecialQR qr_;
130 |     bool precomputed_;
131 | 
132 |     //precomputations
133 |     std::vector<Precomputation> precomputations_;
134 |   };
135 | 
136 | 
137 |   template<typename Derived1, typename Derived2>
138 |   inline void LeastSquareObjective::applyJToTheLeft(const Eigen::MatrixBase<Derived1>& Y_, const Eigen::MatrixBase<Derived2>& X) const
139 |   {
140 |     Eigen::MatrixBase<Derived1>& Y = const_cast<Eigen::MatrixBase<Derived1>&>(Y_);
141 | 
142 |     assert(X.rows() == n_);
143 |     assert(Y.rows() == n_ - 1 && Y.cols() == X.cols());
144 | 
145 |     Y.row(0) = d_[1] * X.row(1) - (d_[0] + d_[1])*X.row(0);
146 |     for (Index i = 1; i < n_ - 1; ++i)
147 |       Y.row(i) = d_[i] * X.row(i - 1) - (d_[i] + d_[i + 1])*X.row(i) + d_[i + 1] * X.row(i + 1);
148 |   }
149 | 
150 | 
151 |   template<typename Derived1, typename Derived2>
152 |   inline void LeastSquareObjective::applyJTransposeToTheLeft(const Eigen::MatrixBase<Derived1>& Y_, const Eigen::MatrixBase<Derived2>& X) const
153 |   {
154 |     Eigen::MatrixBase<Derived1>& Y = const_cast<Eigen::MatrixBase<Derived1>&>(Y_);
155 | 
156 | 
157 |     assert(X.rows() == n_ - 1);
158 |     assert(Y.rows() == n_ && Y.cols() == X.cols());
159 | 
160 |     Y.row(0) = d_[1] * X.row(1) - (d_[0] + d_[1])*X.row(0);
161 |     for (Index i = 1; i < n_ - 2; ++i)
162 |       Y.row(i) = d_[i] * X.row(i - 1) - (d_[i] + d_[i + 1])*X.row(i) + d_[i + 1] * X.row(i + 1);
163 |     Y.row(n_ - 2) = d_[n_ - 2] * X.row(n_ - 3) - (d_[n_ - 2] + d_[n_ - 1])*X.row(n_ - 2);
164 |     Y.row(n_ - 1) = d_[n_ - 1] * X.row(n_ - 2);
165 |   }
166 | }
167 | 


--------------------------------------------------------------------------------
/c++/include/cps/CondensedOrthogonalMatrix.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  3 |  *
  4 |  * This file is part of CPS.
  5 |  *
  6 |  * CPS is free software: you can redistribute it and/or modify
  7 |  * it under the terms of the GNU Lesser General Public License as published by
  8 |  * the Free Software Foundation, either version 3 of the License, or
  9 |  * (at your option) any later version.
 10 |  *
 11 |  * CPS is distributed in the hope that it will be useful,
 12 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 13 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 14 |  * GNU Lesser General Public License for more details.
 15 |  *
 16 |  * You should have received a copy of the GNU Lesser General Public License
 17 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 18 |  */
 19 | 
 20 | #include <vector>
 21 | 
 22 | #include <cps/cps_api.h>
 23 | #include <cps/GivensSequence.h>
 24 | 
 25 | #include <iostream>
 26 | 
 27 | namespace cps
 28 | {
 29 |   /** A class representing a product Q1 Q2 ... Qk P Qh where the Qi are 
 30 |     * orthogonal matrices and P is a permutation matrix.
 31 |     * The matrices Qi are expressed as a product of Givens rotations G1 ... Gp.
 32 |     *
 33 |     * The goal of this class is twofold:
 34 |     * - providing an abstraction for easier manipulation
 35 |     * - preallocating memory to avoid memory allocation in critical code
 36 |     */
 37 |   class CPS_DLLAPI CondensedOrthogonalMatrix
 38 |   {
 39 |   public:
 40 |     /** Create an instance preallocating kmax Givens sequences with pmax Givens
 41 |       * rotation each. The represented matrix is nxn
 42 |       * If Ptranspose is true, P^T is stored instead of P.
 43 |       */
 44 |     CondensedOrthogonalMatrix(int n, int kmax, int pmax, bool Ptranspose = false);
 45 | 
 46 |     /**Empty version*/
 47 |     CondensedOrthogonalMatrix() {}
 48 | 
 49 |     /** This methods acts exactly as the operator=(), except that it supposes 
 50 |       * (and checks by assert) that other will fit in the memory already allocated
 51 |       * to this object.
 52 |       */
 53 |     CondensedOrthogonalMatrix& copyNoAlloc(const CondensedOrthogonalMatrix& other);
 54 | 
 55 |     /** Return the size n of the represented matrix.*/
 56 |     Eigen::DenseIndex size() const;
 57 |     /** Reset to identity*/
 58 |     void reset(bool Ptranspose = false);
 59 |     /** Resize the allocated memory. This invalidates the previous values.*/
 60 |     void resize(int n, int kmax, int pmax);
 61 | 
 62 |     /** Get the sequence Q1 Q2 ... Qk.*/
 63 |     std::vector<GivensSequence>& Q();
 64 |     /** Get the i+1-th element of the above sequence (0-based indexation).*/
 65 |     GivensSequence& Q(size_t i);
 66 |     /** Get the Qh matrix*/
 67 |     GivensSequence& Qh();
 68 |     /** Get the P matrix*/
 69 |     Eigen::Transpositions<Eigen::Dynamic>& P();
 70 | 
 71 |     /** M = Qh^T P^T Q_k^T Q_{k-1}^T .... Q_1^T M
 72 |       *
 73 |       * Don't let the const ref on M fool you: it is a (recommended) trick to
 74 |       * accept temporaries such as blocks. Internally, the const is cast away.
 75 |       *
 76 |       * We don't use Eigen::Ref here because this function will be used with
 77 |       * many small fixed - size matrices, and we want the compiler to take
 78 |       * advantage of that.
 79 |       */
 80 |     template<typename Derived>
 81 |     void applyTo(const Eigen::MatrixBase<Derived>& M) const;
 82 | 
 83 |     /** Computes M = M Q_1 Q_2 ... G_n P Qh */
 84 |     template<typename Derived>
 85 |     void applyOnTheRightTo(const Eigen::MatrixBase<Derived>& M) const;
 86 | 
 87 |     /** Return the corresponding nxn orthogonal matrix
 88 |     *
 89 |     * Use only for debugging purposes.
 90 |     */
 91 |     Eigen::MatrixXd matrix();
 92 | 
 93 |   private:
 94 |     /** Wether P (false) or P^T (true) is stored in transpositions_.*/
 95 |     bool ptranspose_;
 96 |     /** Size of the represented matrix.*/
 97 |     Eigen::DenseIndex n_;
 98 |     /** The sequence Q1 Q2 ...  Qk*/
 99 |     std::vector<GivensSequence> sequences_;
100 |     /** The matrix Qh*/
101 |     GivensSequence Qh_;
102 |     /** The matrix P*/
103 |     Eigen::Transpositions<Eigen::Dynamic> transpositions_;
104 |     /** Internal vector used to reset transpositions_ to identity.*/
105 |     Eigen::VectorXi identityTransposition_;
106 |   };
107 | 
108 |   inline CondensedOrthogonalMatrix& CondensedOrthogonalMatrix::copyNoAlloc(const CondensedOrthogonalMatrix& other)
109 |   {
110 |     assert(other.sequences_.size() <= sequences_.size());
111 |     assert(other.n_ == n_);
112 |     ptranspose_ = other.ptranspose_;
113 |     Qh_ = other.Qh_;
114 |     transpositions_ = other.transpositions_;
115 |     //we want to keep the preallocated memory, so we just clear the GivensSequences we don't use
116 |     size_t i;
117 |     for (i = 0; i < other.sequences_.size(); ++i)
118 |     {
119 |       assert(other.sequences_[i].size() <= sequences_[i].capacity());
120 |       sequences_[i] = other.sequences_[i];
121 |     }
122 |     size_t kmax = sequences_.size();
123 |     for (; i < kmax; ++i)
124 |       sequences_[i].clear();
125 | 
126 |     return *this;
127 |   }
128 | 
129 |   inline Eigen::DenseIndex CondensedOrthogonalMatrix::size() const
130 |   {
131 |     return n_;
132 |   }
133 | 
134 |   template<typename Derived>
135 |   inline void CondensedOrthogonalMatrix::applyTo(const Eigen::MatrixBase<Derived>& M) const
136 |   {
137 |     for (const auto& Q : sequences_)
138 |       Q.applyTo(M);
139 |     if (ptranspose_)
140 |       const_cast<Eigen::MatrixBase<Derived>&>(M) = transpositions_*M;
141 |     else
142 |     {
143 | #if EIGEN_VERSION_AT_LEAST(3, 2, 90)
144 |       /** Work around https://stackoverflow.com/questions/49637702/applying-inverse-transposition-in-eigen */
145 |       const auto & indices = transpositions_.indices();
146 |       auto & M_ = const_cast<Eigen::MatrixBase<Derived>&>(M);
147 |       for(Eigen::Index i = indices.size(); i > 0; --i)
148 |       {
149 |         M_.row(i-1).swap(M_.row(indices(i-1)));
150 |       }
151 | #else
152 |       const_cast<Eigen::MatrixBase<Derived>&>(M) = transpositions_.transpose()*M;
153 | #endif
154 |     }
155 |     Qh_.applyTo(M);
156 |   }
157 | 
158 |   template<typename Derived>
159 |   inline void CondensedOrthogonalMatrix::applyOnTheRightTo(const Eigen::MatrixBase<Derived>& M) const
160 |   {
161 |     for (const auto& Q : sequences_)
162 |       Q.applyOnTheRightTo(M);
163 |     if (ptranspose_)
164 |     {
165 | #if EIGEN_VERSION_AT_LEAST(3, 2, 90)
166 |       /** Work around https://stackoverflow.com/questions/49637702/applying-inverse-transposition-in-eigen */
167 |       const auto & indices = transpositions_.indices();
168 |       auto & M_ = const_cast<Eigen::MatrixBase<Derived>&>(M);
169 |       for(Eigen::Index i = indices.size(); i > 0; --i)
170 |       {
171 |         M_.col(i-1).swap(M_.col(indices(i-1)));
172 |       }
173 | #else
174 |       const_cast<Eigen::MatrixBase<Derived>&>(M) = M*transpositions_.transpose();
175 | #endif
176 |     }
177 |     else
178 |       const_cast<Eigen::MatrixBase<Derived>&>(M) = M*transpositions_;
179 |     Qh_.applyOnTheRightTo(M);
180 |   }
181 | 
182 |   inline std::vector<GivensSequence>& CondensedOrthogonalMatrix::Q()
183 |   {
184 |     return sequences_;
185 |   }
186 | 
187 |   inline GivensSequence& CondensedOrthogonalMatrix::Q(size_t i)
188 |   {
189 |     return sequences_[i];
190 |   }
191 | 
192 |   inline GivensSequence& CondensedOrthogonalMatrix::Qh()
193 |   {
194 |     return Qh_;
195 |   }
196 | 
197 |   inline Eigen::Transpositions<Eigen::Dynamic>& CondensedOrthogonalMatrix::P()
198 |   {
199 |     return transpositions_;
200 |   }
201 | }
202 | 


--------------------------------------------------------------------------------
/c++/src/LeastSquare.cpp:
--------------------------------------------------------------------------------
  1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  2 |  *
  3 |  * This file is part of CPS.
  4 |  *
  5 |  * CPS is free software: you can redistribute it and/or modify
  6 |  * it under the terms of the GNU Lesser General Public License as published by
  7 |  * the Free Software Foundation, either version 3 of the License, or
  8 |  * (at your option) any later version.
  9 |  *
 10 |  * CPS is distributed in the hope that it will be useful,
 11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13 |  * GNU Lesser General Public License for more details.
 14 |  *
 15 |  * You should have received a copy of the GNU Lesser General Public License
 16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 17 |  */
 18 | 
 19 | #include <Eigen/SVD>
 20 | 
 21 | #include <cps/LeastSquare.h>
 22 | #include <cps/QRAlgorithms.h>
 23 | 
 24 | using namespace Eigen;
 25 | 
 26 | namespace cps
 27 | {
 28 |   LeastSquare::LeastSquare(int n)
 29 |     : n_(n)
 30 |     , x_(n)
 31 |     , p_(n)
 32 |     , z_(n)
 33 |     , lambda_(n + 1)
 34 |     , lambdaAct_(n + 1)
 35 |     , jN_(n)
 36 |     , A_(n,n)
 37 |     , b_(n)
 38 |     , Jx_(n-1)
 39 |     , JtJx_(n)
 40 |     , tmp_(n)
 41 |     , Q_(n,n,2*n,true)
 42 |   {
 43 |     Qg_.reserve(n);
 44 |   }
 45 | 
 46 |   void LeastSquare::parameters(const LeastSquare::Parameters& param)
 47 |   {
 48 |     params_ = param;
 49 |   }
 50 | 
 51 |   const LeastSquare::Parameters& LeastSquare::parameters() const
 52 |   {
 53 |     return params_;
 54 |   }
 55 | 
 56 |   SolverStatus LeastSquare::solve(const LeastSquareObjective& obj, const VectorConstRef& Jx0, const VectorConstRef& j, double c, LinearConstraints& lc)
 57 |   {
 58 |     assert(j.size() == n_);
 59 |     assert(obj.size() == n_);
 60 |     assert(Jx0.size() == n_ - 1);
 61 |     assert(lc.size() == n_);
 62 | 
 63 |     x_.setZero();
 64 |     assert(lc.checkPrimal(x_) && "This solver supposes that 0 is a feasible point");
 65 | 
 66 |     STATISTICS(stats_.reset());
 67 | 
 68 |     //main loop
 69 |     int k = 0;
 70 |     if (params_.maxIter() <= 0) { params_.maxIter(10 * static_cast<int>(n_)); } //default value for maxIter if need be.
 71 |     bool skip = false;
 72 |     for (k = 0; k < params_.maxIter(); ++k)
 73 |     {
 74 |       auto nz = lc.nullSpaceSize();
 75 |       if (nz > 0 && !skip)
 76 |       {
 77 |         auto A = A_.leftCols(nz);
 78 |         auto jN = jN_.head(nz);
 79 |         auto z = z_.head(nz);
 80 |         //We form A = [j^T * N; R_A] where R_A is the obtain by the qr of J_A
 81 |         lc.applyNullSpaceOnTheRight(jN.transpose(), j.transpose());   //jN^T = j^T N
 82 |         A.row(0) = jN.transpose();
 83 |         obj.qr(A.bottomRows(n_ - 1), Q_, lc.numberOfActiveConstraints(), lc.activeSet(), 1);
 84 |         //Compute b
 85 |         obj.applyJToTheLeft(b_.tail(n_ - 1), x_);
 86 |         b_.tail(n_ - 1) += Jx0;
 87 |         b_[0] = c + j.dot(x_);
 88 |         //QR of A (A is upper Hessenberg
 89 |         bool fullRank = hessenbergQR(A.topLeftCorner(std::min(n_, nz + 1), nz), Qg_, false, params_.rankThreshold());
 90 |         //std::cout << "A= \n" << A << std::endl;
 91 |         //z = A^-1 b
 92 |         Q_.applyTo(b_);
 93 |         Qg_.applyTo(b_);
 94 |         //std::cout << "b = " << b_.transpose() << std::endl; 
 95 |         if (fullRank)
 96 |         {
 97 |           z = -b_.head(nz);
 98 |           A.topLeftCorner(nz, nz).triangularView<Upper>().solveInPlace(z);
 99 |         }
100 |         else
101 |         {
102 |           z.head(nz - 1) = -b_.head(nz - 1);
103 |           A.topLeftCorner(nz - 1, nz - 1).triangularView<Upper>().solveInPlace(z.head(nz - 1));
104 |           z[nz - 1] = 0;
105 |           STATISTICS(++stats_.rankLoss);
106 |         }
107 |         lc.applyNullSpaceOnTheLeft(p_, z);                            //p = Nz
108 |       }
109 |       else
110 |         p_.setZero();
111 | 
112 |       if (p_.lpNorm<Infinity>() < params_.minNorm_p())  //FIXME: hard coded threshold?
113 |       {
114 |         //compute Lagrange multipliers for the active constraints: -C_A^+T(A^T(Ax+[c;Jx0]))
115 |         obj.applyJToTheLeft(Jx_, x_);
116 |         Jx_ += Jx0;
117 |         obj.applyJTransposeToTheLeft(JtJx_, Jx_);
118 |         tmp_ = -JtJx_ - (c + j.dot(x_))*j;
119 |         auto lambdaAct = lambdaAct_.head(lc.numberOfActiveConstraints());
120 |         lc.pinvTransposeMultAct(lambdaAct, tmp_);
121 |         //std::cout << "lambda_act = " << lambdaAct.transpose() << std::endl;
122 |         //std::cout << "lambda_pinv = " << lc.matrixAct().transpose().jacobiSvd(ComputeThinU | ComputeThinV).solve(tmp_) << std::endl;
123 |         //std::cout << "err = " << (lc.matrixAct().transpose()*lambdaAct - tmp_).transpose() << std::endl;
124 |         //std::cout << "optim = " << ((c + j.dot(x_))*j + obj.matrix().transpose()*(obj.matrix()*x_) + lc.matrixAct().transpose()*lambdaAct).transpose() << std::endl;
125 |         lc.expandActive(lambda_, lambdaAct);
126 |         if (lc.checkDual(lambda_, params_.dualEps()))
127 |         {
128 |           STATISTICS(stats_.iter = k + 1);
129 |           STATISTICS(stats_.activeConstraints = (int)lc.numberOfActiveConstraints());
130 |           return SolverStatus::Converge;
131 |         }
132 |         else
133 |         {
134 |           lc.deactivateMaxLambda(lambda_);
135 |           STATISTICS(++stats_.deactivation);
136 |         }
137 |         skip = false;
138 |       }
139 |       else
140 |       {
141 |         lc.performQPstep(x_, p_, &skip);
142 |         STATISTICS(if (!skip) { ++stats_.activation; });
143 |       }
144 |     }
145 | 
146 |     STATISTICS(stats_.iter = k);
147 |     STATISTICS(stats_.activeConstraints = (int)lc.numberOfActiveConstraints());
148 |     return SolverStatus::MaxIteration;
149 |   }
150 | 
151 |   SolverStatus LeastSquare::solveFeasibility(const VectorConstRef& j, double c, LinearConstraints& lc)
152 |   {
153 |     assert(j.size() == n_);
154 |     assert(lc.size() == n_);
155 | 
156 |     x_.setZero();
157 |     assert(lc.checkPrimal(x_) && "This solver supposes that 0 is a feasible point");
158 | 
159 |     //main loop
160 |     int k = 0;
161 |     if (params_.maxIter() <= 0) { params_.maxIter(10 * static_cast<int>(n_)); } //default value for maxIter if need be.
162 |     for (k = 0; k < params_.maxIter(); ++k)
163 |     {
164 |       auto jN = jN_.head(lc.nullSpaceSize());
165 |       auto z = z_.head(lc.nullSpaceSize());
166 |       lc.applyNullSpaceOnTheRight(jN.transpose(), j.transpose());   //jN^T = j^T N
167 |       z = -(c+j.dot(x_))/jN.squaredNorm() * jN;                     //-pinv(jN^T*(c+j^T*x)
168 |       lc.applyNullSpaceOnTheLeft(p_, z);                            //p = Nz
169 | 
170 |       if (p_.lpNorm<Infinity>() < params_.minNorm_p())  //FIXME: hard coded threshold?
171 |       {
172 |         //compute Lagrange multipliers
173 |         auto lambdaAct = lambdaAct_.head(lc.numberOfActiveConstraints());
174 |         lc.pinvTransposeMultAct(lambdaAct, j);
175 |         lambdaAct *= -(c + j.dot(x_));
176 |         lc.expandActive(lambda_, lambdaAct);
177 |         if (lc.checkDual(lambda_, params_.dualEps()))
178 |           return SolverStatus::Converge;
179 |         else
180 |           lc.deactivateMaxLambda(lambda_);
181 |       }
182 |       else
183 |       {
184 |         lc.performQPstep(x_, p_);
185 |       }
186 |     }
187 | 
188 |     return SolverStatus::MaxIteration;
189 |   }
190 | 
191 |   const Eigen::VectorXd & LeastSquare::x() const
192 |   {
193 |     return x_;
194 |   }
195 | 
196 |   const Eigen::VectorXd & LeastSquare::lambda() const
197 |   {
198 |     return lambda_;
199 |   }
200 | 
201 |   const stats::LSStats & LeastSquare::statistics() const
202 |   {
203 |     return stats_;
204 |   }
205 | }
206 | 


--------------------------------------------------------------------------------
/COPYING.LESSER:
--------------------------------------------------------------------------------
  1 |                    GNU LESSER GENERAL PUBLIC LICENSE
  2 |                        Version 3, 29 June 2007
  3 | 
  4 |  Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
  5 |  Everyone is permitted to copy and distribute verbatim copies
  6 |  of this license document, but changing it is not allowed.
  7 | 
  8 | 
  9 |   This version of the GNU Lesser General Public License incorporates
 10 | the terms and conditions of version 3 of the GNU General Public
 11 | License, supplemented by the additional permissions listed below.
 12 | 
 13 |   0. Additional Definitions.
 14 | 
 15 |   As used herein, "this License" refers to version 3 of the GNU Lesser
 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU
 17 | General Public License.
 18 | 
 19 |   "The Library" refers to a covered work governed by this License,
 20 | other than an Application or a Combined Work as defined below.
 21 | 
 22 |   An "Application" is any work that makes use of an interface provided
 23 | by the Library, but which is not otherwise based on the Library.
 24 | Defining a subclass of a class defined by the Library is deemed a mode
 25 | of using an interface provided by the Library.
 26 | 
 27 |   A "Combined Work" is a work produced by combining or linking an
 28 | Application with the Library.  The particular version of the Library
 29 | with which the Combined Work was made is also called the "Linked
 30 | Version".
 31 | 
 32 |   The "Minimal Corresponding Source" for a Combined Work means the
 33 | Corresponding Source for the Combined Work, excluding any source code
 34 | for portions of the Combined Work that, considered in isolation, are
 35 | based on the Application, and not on the Linked Version.
 36 | 
 37 |   The "Corresponding Application Code" for a Combined Work means the
 38 | object code and/or source code for the Application, including any data
 39 | and utility programs needed for reproducing the Combined Work from the
 40 | Application, but excluding the System Libraries of the Combined Work.
 41 | 
 42 |   1. Exception to Section 3 of the GNU GPL.
 43 | 
 44 |   You may convey a covered work under sections 3 and 4 of this License
 45 | without being bound by section 3 of the GNU GPL.
 46 | 
 47 |   2. Conveying Modified Versions.
 48 | 
 49 |   If you modify a copy of the Library, and, in your modifications, a
 50 | facility refers to a function or data to be supplied by an Application
 51 | that uses the facility (other than as an argument passed when the
 52 | facility is invoked), then you may convey a copy of the modified
 53 | version:
 54 | 
 55 |    a) under this License, provided that you make a good faith effort to
 56 |    ensure that, in the event an Application does not supply the
 57 |    function or data, the facility still operates, and performs
 58 |    whatever part of its purpose remains meaningful, or
 59 | 
 60 |    b) under the GNU GPL, with none of the additional permissions of
 61 |    this License applicable to that copy.
 62 | 
 63 |   3. Object Code Incorporating Material from Library Header Files.
 64 | 
 65 |   The object code form of an Application may incorporate material from
 66 | a header file that is part of the Library.  You may convey such object
 67 | code under terms of your choice, provided that, if the incorporated
 68 | material is not limited to numerical parameters, data structure
 69 | layouts and accessors, or small macros, inline functions and templates
 70 | (ten or fewer lines in length), you do both of the following:
 71 | 
 72 |    a) Give prominent notice with each copy of the object code that the
 73 |    Library is used in it and that the Library and its use are
 74 |    covered by this License.
 75 | 
 76 |    b) Accompany the object code with a copy of the GNU GPL and this license
 77 |    document.
 78 | 
 79 |   4. Combined Works.
 80 | 
 81 |   You may convey a Combined Work under terms of your choice that,
 82 | taken together, effectively do not restrict modification of the
 83 | portions of the Library contained in the Combined Work and reverse
 84 | engineering for debugging such modifications, if you also do each of
 85 | the following:
 86 | 
 87 |    a) Give prominent notice with each copy of the Combined Work that
 88 |    the Library is used in it and that the Library and its use are
 89 |    covered by this License.
 90 | 
 91 |    b) Accompany the Combined Work with a copy of the GNU GPL and this license
 92 |    document.
 93 | 
 94 |    c) For a Combined Work that displays copyright notices during
 95 |    execution, include the copyright notice for the Library among
 96 |    these notices, as well as a reference directing the user to the
 97 |    copies of the GNU GPL and this license document.
 98 | 
 99 |    d) Do one of the following:
100 | 
101 |        0) Convey the Minimal Corresponding Source under the terms of this
102 |        License, and the Corresponding Application Code in a form
103 |        suitable for, and under terms that permit, the user to
104 |        recombine or relink the Application with a modified version of
105 |        the Linked Version to produce a modified Combined Work, in the
106 |        manner specified by section 6 of the GNU GPL for conveying
107 |        Corresponding Source.
108 | 
109 |        1) Use a suitable shared library mechanism for linking with the
110 |        Library.  A suitable mechanism is one that (a) uses at run time
111 |        a copy of the Library already present on the user's computer
112 |        system, and (b) will operate properly with a modified version
113 |        of the Library that is interface-compatible with the Linked
114 |        Version.
115 | 
116 |    e) Provide Installation Information, but only if you would otherwise
117 |    be required to provide such information under section 6 of the
118 |    GNU GPL, and only to the extent that such information is
119 |    necessary to install and execute a modified version of the
120 |    Combined Work produced by recombining or relinking the
121 |    Application with a modified version of the Linked Version. (If
122 |    you use option 4d0, the Installation Information must accompany
123 |    the Minimal Corresponding Source and Corresponding Application
124 |    Code. If you use option 4d1, you must provide the Installation
125 |    Information in the manner specified by section 6 of the GNU GPL
126 |    for conveying Corresponding Source.)
127 | 
128 |   5. Combined Libraries.
129 | 
130 |   You may place library facilities that are a work based on the
131 | Library side by side in a single library together with other library
132 | facilities that are not Applications and are not covered by this
133 | License, and convey such a combined library under terms of your
134 | choice, if you do both of the following:
135 | 
136 |    a) Accompany the combined library with a copy of the same work based
137 |    on the Library, uncombined with any other library facilities,
138 |    conveyed under the terms of this License.
139 | 
140 |    b) Give prominent notice with the combined library that part of it
141 |    is a work based on the Library, and explaining where to find the
142 |    accompanying uncombined form of the same work.
143 | 
144 |   6. Revised Versions of the GNU Lesser General Public License.
145 | 
146 |   The Free Software Foundation may publish revised and/or new versions
147 | of the GNU Lesser General Public License from time to time. Such new
148 | versions will be similar in spirit to the present version, but may
149 | differ in detail to address new problems or concerns.
150 | 
151 |   Each version is given a distinguishing version number. If the
152 | Library as you received it specifies that a certain numbered version
153 | of the GNU Lesser General Public License "or any later version"
154 | applies to it, you have the option of following the terms and
155 | conditions either of that published version or of any later version
156 | published by the Free Software Foundation. If the Library as you
157 | received it does not specify a version number of the GNU Lesser
158 | General Public License, you may choose any version of the GNU Lesser
159 | General Public License ever published by the Free Software Foundation.
160 | 
161 |   If the Library as you received it specifies that a proxy can decide
162 | whether future versions of the GNU Lesser General Public License shall
163 | apply, that proxy's public statement of acceptance of any version is
164 | permanent authorization for you to choose that version for the
165 | Library.
166 | 


--------------------------------------------------------------------------------
/c++/src/Problem.cpp:
--------------------------------------------------------------------------------
  1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  2 |  *
  3 |  * This file is part of CPS.
  4 |  *
  5 |  * CPS is free software: you can redistribute it and/or modify
  6 |  * it under the terms of the GNU Lesser General Public License as published by
  7 |  * the Free Software Foundation, either version 3 of the License, or
  8 |  * (at your option) any later version.
  9 |  *
 10 |  * CPS is distributed in the hope that it will be useful,
 11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13 |  * GNU Lesser General Public License for more details.
 14 |  *
 15 |  * You should have received a copy of the GNU Lesser General Public License
 16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 17 |  */
 18 | 
 19 | #include <fstream>
 20 | #include <map>
 21 | #include <sstream>
 22 | #include <vector>
 23 | 
 24 | #include <cps/Problem.h>
 25 | 
 26 | namespace
 27 | {
 28 |   // trim from left
 29 |   inline std::string& ltrim(std::string& s, const char* t = " \t\n\r\f\v")
 30 |   {
 31 |     s.erase(0, s.find_first_not_of(t));
 32 |     return s;
 33 |   }
 34 | 
 35 |   // trim from right
 36 |   inline std::string& rtrim(std::string& s, const char* t = " \t\n\r\f\v")
 37 |   {
 38 |     s.erase(s.find_last_not_of(t) + 1);
 39 |     return s;
 40 |   }
 41 | 
 42 |   // trim from left & right
 43 |   inline std::string& trim(std::string& s, const char* t = " \t\n\r\f\v")
 44 |   {
 45 |     return ltrim(rtrim(s, t), t);
 46 |   }
 47 | 
 48 |   double parseDouble_(const std::string& s)
 49 |   {
 50 |     std::stringstream ss(s);
 51 |     double d;
 52 |     ss >> d;
 53 |     if (ss.fail())
 54 |       throw std::runtime_error("Failed to read double value");
 55 | 
 56 |     return d;
 57 |   }
 58 | 
 59 |   Eigen::VectorXd parseVector_(const std::string& s)
 60 |   {
 61 |     std::stringstream ss(s);
 62 |     double d;
 63 |     char c;
 64 |     std::vector<double> v;
 65 |     
 66 |     ss >> c;
 67 |     while (c != ']')
 68 |     {
 69 |       ss >> d;
 70 |       ss >> c;
 71 |       if (ss.fail())
 72 |         throw std::runtime_error("Failed to read vector value");
 73 | 
 74 |       v.push_back(d);
 75 |     }
 76 | 
 77 |     return Eigen::Map<Eigen::VectorXd>(&v[0], static_cast<Eigen::DenseIndex>(v.size()));
 78 |   }
 79 | 
 80 |   double parseDouble(const std::map<std::string, std::string>& table, const std::string& key, bool optional = false, double defaultValue = 0)
 81 |   {
 82 |     if (table.count(key))
 83 |       return parseDouble_(table.at(key));
 84 |     else
 85 |     {
 86 |       if (optional)
 87 |         return defaultValue;
 88 |       else
 89 |         throw std::runtime_error("No element " + key + " found in the file");
 90 |     }
 91 |   }
 92 | 
 93 |   Eigen::VectorXd parseVector(const std::map<std::string, std::string>& table, const std::string& key, bool optional = false)
 94 |   {
 95 |     if (table.count(key))
 96 |       return parseVector_(table.at(key));
 97 |     else
 98 |     {
 99 |       if (optional)
100 |         return Eigen::VectorXd();
101 |       else
102 |         throw std::runtime_error("No element " + key + " found in the file");
103 |     }
104 |   }
105 | }
106 | 
107 | namespace cps
108 | {
109 |   void RawProblem::read(const std::string& filepath)
110 |   {
111 |     std::ifstream aif(filepath);
112 |     if (aif.is_open())
113 |     {
114 |       std::map<std::string, std::string> table;
115 |       for (std::string line; std::getline(aif, line); )
116 |       {
117 |         auto i = line.find("=");
118 |         if (i != std::string::npos && i>0)
119 |         {
120 |           std::string substr = line.substr(0, i);
121 |           std::string token = trim(substr);
122 |           auto j = line.find(";", i + 1);
123 |           if (j != std::string::npos)
124 |           {
125 |             substr = line.substr(i + 1, j - i - 1);
126 |             std::string value = trim(substr);
127 |             table[token] = value;
128 |           }
129 |           else
130 |             throw std::runtime_error("error in reading line\n" + line);
131 |         }
132 |       }
133 | 
134 |       g = parseDouble(table, "g");
135 |       lambda_min = parseDouble(table, "lambda_min");
136 |       lambda_max = parseDouble(table, "lambda_max");
137 |       delta = parseVector(table, "Delta");
138 |       init_omega_min = parseDouble(table, "omega_i_min");
139 |       init_omega_max = parseDouble(table, "omega_i_max");
140 |       init_zbar = parseDouble(table, "z_bar");
141 |       init_zbar_deriv = parseDouble(table, "zd_bar");
142 |       target_height = parseDouble(table, "z_f");
143 |       Phi_ = parseVector(table, "Phi", true);
144 |     }
145 |     else
146 |     {
147 |       throw std::runtime_error("Unable to open" + filepath);
148 |     }
149 |   }
150 | 
151 |   Problem::Problem(const RawProblem& pb)
152 |     : lso_(pb.delta)
153 |     , lc_(pb.lambda_min*pb.delta, pb.lambda_max*pb.delta, pb.init_omega_min*pb.init_omega_min, pb.init_omega_max*pb.init_omega_max)
154 |     , bc_(pb.delta, pb.init_zbar / pb.g, pb.init_zbar_deriv / pb.g)
155 |     , raw_(pb)
156 |   {
157 |     double d = pb.delta[0] * pb.g / pb.target_height;
158 |     lc_.changeBounds(0, d, d);
159 |   }
160 | 
161 |   const LeastSquareObjective& Problem::objective() const
162 |   {
163 |     return lso_;
164 |   }
165 | 
166 |   LeastSquareObjective& Problem::objective()
167 |   {
168 |     return lso_;
169 |   }
170 | 
171 |   const BoundenessConstraint& Problem::nonLinearConstraint() const
172 |   {
173 |     return bc_;
174 |   }
175 | 
176 |   BoundenessConstraint& Problem::nonLinearConstraint()
177 |   {
178 |     return bc_;
179 |   }
180 | 
181 |   const LinearConstraints& Problem::linearConstraints() const
182 |   {
183 |     return lc_;
184 |   }
185 | 
186 |   LinearConstraints& Problem::linearConstraints()
187 |   {
188 |     return lc_;
189 |   }
190 | 
191 |   Eigen::VectorXd::Index Problem::size() const
192 |   {
193 |     return raw_.delta.size();
194 |   }
195 | 
196 |   void Problem::set_target_height(double target_height)
197 |   {
198 |     raw_.target_height = target_height;
199 |     computeAndSetBounds0();
200 |   }
201 | 
202 |   void Problem::set_init_zbar(double init_zbar)
203 |   {
204 |     raw_.init_zbar = init_zbar;
205 |     computeAndSetAlpha();
206 |   }
207 | 
208 |   void Problem::set_init_zbar_deriv(double init_zbar_deriv)
209 |   {
210 |     raw_.init_zbar_deriv = init_zbar_deriv;
211 |     computeAndSetb();
212 |   }
213 | 
214 |   void Problem::set_lambda_min(double lambda_min)
215 |   {
216 |     raw_.lambda_min = lambda_min;
217 |     computeAndSetZonotopeBounds();
218 |   }
219 | 
220 |   void Problem::set_lambda_max(double lambda_max)
221 |   {
222 |     raw_.lambda_max = lambda_max;
223 |     computeAndSetZonotopeBounds();
224 |   }
225 | 
226 |   void Problem::set_lambdas(double lambda_min, double lambda_max)
227 |   {
228 |     raw_.lambda_min = lambda_min;
229 |     raw_.lambda_max = lambda_max;
230 |     computeAndSetZonotopeBounds();
231 |   }
232 | 
233 |   void Problem::set_init_omega_min(double init_omega_min)
234 |   {
235 |     raw_.init_omega_min = init_omega_min;
236 |     computeAndSetBoundsN();
237 |   }
238 | 
239 |   void Problem::set_init_omega_max(double init_omega_max)
240 |   {
241 |     raw_.init_omega_max = init_omega_max;
242 |     computeAndSetBoundsN();
243 |   }
244 | 
245 |   void Problem::set_init_omega(double init_omega_min, double init_omega_max)
246 |   {
247 |     raw_.init_omega_min = init_omega_min;
248 |     raw_.init_omega_max = init_omega_max;
249 |     computeAndSetBoundsN();
250 |   }
251 | 
252 |   void Problem::computeAndSetBounds0()
253 |   {
254 |     double d = raw_.delta[0] * raw_.g / raw_.target_height;
255 |     lc_.changeBounds(0, d, d);
256 |   }
257 | 
258 |   void Problem::computeAndSetZonotopeBounds()
259 |   {
260 |     lc_.changeBounds(raw_.lambda_min*raw_.delta, raw_.lambda_max*raw_.delta);
261 |     computeAndSetBounds0();
262 |   }
263 | 
264 |   void Problem::computeAndSetBoundsN()
265 |   {
266 |     lc_.changeBounds(raw_.delta.size(), raw_.init_omega_min*raw_.init_omega_min, raw_.init_omega_max*raw_.init_omega_max);
267 |   }
268 | 
269 |   void Problem::computeAndSetAlpha()
270 |   {
271 |     bc_.setAlpha(raw_.init_zbar / raw_.g);
272 |   }
273 | 
274 |   void Problem::computeAndSetb()
275 |   {
276 |     bc_.setb(raw_.init_zbar_deriv / raw_.g);
277 |   }
278 | 
279 |   void Problem::precompute()
280 |   {
281 |     objective().precompute(1);
282 |   }
283 | }
284 | 


--------------------------------------------------------------------------------
/c++/tests/MatrixComputationsTest.cpp:
--------------------------------------------------------------------------------
  1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  2 |  *
  3 |  * This file is part of CPS.
  4 |  *
  5 |  * CPS is free software: you can redistribute it and/or modify
  6 |  * it under the terms of the GNU Lesser General Public License as published by
  7 |  * the Free Software Foundation, either version 3 of the License, or
  8 |  * (at your option) any later version.
  9 |  *
 10 |  * CPS is distributed in the hope that it will be useful,
 11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13 |  * GNU Lesser General Public License for more details.
 14 |  *
 15 |  * You should have received a copy of the GNU Lesser General Public License
 16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 17 |  */
 18 | 
 19 | #include <iostream>
 20 | 
 21 | #include <cps/CondensedOrthogonalMatrix.h>
 22 | #include <cps/Givens.h>
 23 | #include <cps/GivensSequence.h>
 24 | #include <cps/ProblemMatrices.h>
 25 | #include <cps/QRAlgorithms.h>
 26 | #include <cps/QuadraticObjective.h>
 27 | 
 28 | // boost
 29 | #define BOOST_TEST_MODULE MatrixComputationsTest
 30 | #include <boost/test/unit_test.hpp>
 31 | 
 32 | using namespace Eigen;
 33 | using namespace cps;
 34 | 
 35 | BOOST_AUTO_TEST_CASE(GivensTest)
 36 | {
 37 |   MatrixXd M = MatrixXd::Random(5, 5);
 38 |   
 39 |   //zeroing M(4,0) with row 3
 40 |   Givens G1(M, 3, 4, 0);
 41 |   G1.applyTo(M);
 42 |   BOOST_CHECK(std::abs(M(4,0)) <= 1e-15);
 43 | 
 44 |   //zeroing M(3,0) with row 2
 45 |   Givens G2(M, 2, 0);
 46 |   G2.applyTo(M);
 47 |   BOOST_CHECK(std::abs(M(3, 0)) <= 1e-15);
 48 |   BOOST_CHECK(std::abs(M(4, 0)) <= 1e-15);
 49 | 
 50 |   //hand computed value of c and s to zero M(2,0), using row 0
 51 |   double a = M(0, 0);
 52 |   double b = M(2, 0);
 53 |   double c = a / sqrt(a*a + b*b);
 54 |   double s = -b / sqrt(a*a + b*b);
 55 |   Givens G3(0, 2, c, s);
 56 |   G3.applyTo(M);
 57 |   BOOST_CHECK(std::abs(M(2, 0)) <= 1e-15);
 58 |   BOOST_CHECK(std::abs(M(3, 0)) <= 1e-15);
 59 |   BOOST_CHECK(std::abs(M(4, 0)) <= 1e-15);
 60 | 
 61 |   //hand computed value of c and s to zero M(1,0)
 62 |   a = M(0, 0);
 63 |   b = M(1, 0);
 64 |   c = a / sqrt(a*a + b*b);
 65 |   s = -b / sqrt(a*a + b*b);
 66 |   Givens G4(0, 1, c, s);
 67 |   G4.applyTo(M);
 68 |   BOOST_CHECK(std::abs(M(1, 0)) <= 1e-15);
 69 |   BOOST_CHECK(std::abs(M(2, 0)) <= 1e-15);
 70 |   BOOST_CHECK(std::abs(M(3, 0)) <= 1e-15);
 71 |   BOOST_CHECK(std::abs(M(4, 0)) <= 1e-15);
 72 | 
 73 |   //test on a block
 74 |   Givens G5(M.bottomRightCorner(2,4), 0, 0);
 75 |   G5.extend(3);
 76 |   G5.applyTo(M.rightCols(4));
 77 |   BOOST_CHECK(std::abs(M(4, 1)) <= 1e-15);
 78 | }
 79 | 
 80 | 
 81 | BOOST_AUTO_TEST_CASE(GivensSequenceTest)
 82 | {
 83 |   MatrixXd M = MatrixXd::Random(5, 5);
 84 |   MatrixXd M2 = M;
 85 | 
 86 |   Givens G1(M, 3, 0);
 87 |   G1.applyTo(M);
 88 |   Givens G2(M, 2, 0);
 89 |   G2.applyTo(M);
 90 |   Givens G3(M, 1, 0);
 91 |   G3.applyTo(M);
 92 |   Givens G4(M, 0, 0);
 93 |   G4.applyTo(M);
 94 | 
 95 |   GivensSequence seq = { G1, G2, G3 };
 96 |   seq.push_back(G4);
 97 |   seq.applyTo(M2);
 98 | 
 99 |   BOOST_CHECK(M.isApprox(M2, 1e-15));
100 | }
101 | 
102 | BOOST_AUTO_TEST_CASE(CondensedOrthogonalMatrixTest)
103 | {
104 |   MatrixXd M = MatrixXd::Random(5, 5);
105 |   MatrixXd M2 = M;
106 | 
107 |   //Let's make a weird Givens QR algorithm
108 |   Givens G1(M, 0, 3, 0);
109 |   G1.applyTo(M);
110 |   Givens G2(M, 4, 0, 0);
111 |   G2.applyTo(M);
112 |   Givens G3(M, 1, 4, 0);
113 |   G3.applyTo(M);
114 |   Givens G4(M, 2, 1, 0);
115 |   G4.applyTo(M);
116 |   Givens G5(M, 0, 3, 1);
117 |   G5.applyTo(M);
118 |   Givens G6(M, 4, 0, 1);
119 |   G6.applyTo(M);
120 |   Givens G7(M, 1, 4, 1);
121 |   G7.applyTo(M);
122 |   Givens G8(M, 0, 3, 2);
123 |   G8.applyTo(M);
124 |   Givens G9(M, 4, 0, 2);
125 |   G9.applyTo(M);
126 |   Givens G10(M, 0, 3, 3);
127 |   G10.applyTo(M);
128 |   VectorXi idx(5); idx << 0, 1, 0, 0, 3;
129 |   Transpositions<Dynamic> P(idx);
130 |   M = P.transpose()*M;
131 | 
132 |   CondensedOrthogonalMatrix Q(5, 4, 4);
133 |   Q.Q(0).push_back(G1);
134 |   Q.Q(0).push_back(G2);
135 |   Q.Q(0).push_back(G3);
136 |   Q.Q(0).push_back(G4);
137 |   Q.Q(1).push_back(G5);
138 |   Q.Q(1).push_back(G6);
139 |   Q.Q(1).push_back(G7);
140 |   Q.Q(2).push_back(G8);
141 |   Q.Q(2).push_back(G9);
142 |   Q.Q(3).push_back(G10);
143 |   Q.P().indices() = idx;
144 |   Q.applyTo(M2);
145 | 
146 |   BOOST_CHECK(M.isApprox(M2, 1e-14));
147 | }
148 | 
149 | bool isUpperTriangular(const MatrixXd& M)
150 | {
151 |   return M.triangularView<Eigen::StrictlyLower>().toDenseMatrix().isZero(1e-15);
152 | }
153 | 
154 | /** Check if the matrix is a band matrix with lower bandwidth p and upper bandwidth q*/
155 | bool isBandMatrix(const MatrixXd& M, DenseIndex p, DenseIndex q)
156 | {
157 |   auto m = M.rows();
158 |   auto n = M.cols();
159 | 
160 |   bool b = true;
161 |   for (DenseIndex i = p+1; i < m && b; ++i)
162 |     b = M.diagonal(-i).isZero(1e-15);
163 |   for (DenseIndex i = q+1; i < m && b; ++i)
164 |     b = M.diagonal(i).isZero(1e-15);
165 | 
166 |   return b;
167 | }
168 | 
169 | 
170 | BOOST_AUTO_TEST_CASE(HessenbergQRTest)
171 | {
172 |   MatrixXd M = MatrixXd::Random(6, 7);
173 |   M.bottomLeftCorner(5, 5).triangularView<Eigen::StrictlyLower>().setZero();  //make the matrix Hessenberg
174 | 
175 |   MatrixXd M2 = M;
176 |   GivensSequence seq;
177 | 
178 |   bool b = hessenbergQR(M, seq, false, 1e-7);
179 |   BOOST_CHECK(isUpperTriangular(M));
180 |   BOOST_CHECK(b);
181 | 
182 |   MatrixXd Q = seq.matrix(6);
183 |   MatrixXd QR = Q*M;
184 |   BOOST_CHECK(QR.isApprox(M2, 1e-15));
185 | }
186 | 
187 | BOOST_AUTO_TEST_CASE(TridiagonalQRTest)
188 | {
189 |   MatrixXd M = MatrixXd::Random(6, 6);
190 |   M.bottomLeftCorner(5, 5).triangularView<Eigen::StrictlyLower>().setZero();  //make the matrix tridiagonal (set lower part to 0)
191 |   M.topRightCorner(5, 5).triangularView<Eigen::StrictlyUpper>().setZero();  //make the matrix tridiagonal (set upper part to 0)
192 | 
193 |   MatrixXd M2 = M;
194 |   GivensSequence seq;
195 | 
196 |   bool b = tridiagonalQR(M, seq, false, 1e-7);
197 |   BOOST_CHECK(isBandMatrix(M,0,2));
198 |   BOOST_CHECK(b);
199 | 
200 |   MatrixXd Q = seq.matrix(6);
201 |   MatrixXd QR = Q*M;
202 |   BOOST_CHECK(QR.isApprox(M2, 1e-15));
203 | }
204 | 
205 | BOOST_AUTO_TEST_CASE(SpecialQRTest)
206 | {
207 |   SpecialQR qr(10);
208 | 
209 |   VectorXd e = VectorXd::Random(8);
210 |   MatrixXd J1 = buildJj(e);
211 |   MatrixXd J1back = J1;
212 | 
213 |   GivensSequence Q;
214 |   bool b = qr.compute(J1, Q, false);
215 | 
216 |   BOOST_CHECK(isBandMatrix(J1, 0, 2));
217 |   BOOST_CHECK(!b);
218 |   BOOST_CHECK((Q.matrix(9)*J1).isApprox(J1back, 1e-15));
219 | 
220 | 
221 |   //check reentry
222 |   e = VectorXd::Random(9);
223 |   MatrixXd J2 = buildJj(e);
224 |   MatrixXd J2back = J2;
225 |   b = qr.compute(J2, Q, false);
226 |   BOOST_CHECK(isBandMatrix(J2, 0, 2));
227 |   BOOST_CHECK(!b);
228 |   BOOST_CHECK((Q.matrix(10)*J2).isApprox(J2back, 1e-15));
229 | 
230 |   //variant case
231 |   MatrixXd J3 = buildJpm1(e);
232 |   MatrixXd J3back = J3;
233 |   b = qr.compute(J3, Q, true);
234 |   BOOST_CHECK(isBandMatrix(J3, 0, 2));
235 |   BOOST_CHECK(b);
236 |   BOOST_CHECK((Q.matrix(9)*J3).isApprox(J3back, 1e-15));
237 | }
238 | 
239 | BOOST_AUTO_TEST_CASE(SpecialQRTestExtended)
240 | {
241 |   int n = 8;
242 |   VectorXd d = VectorXd::Random(n);
243 |   VectorXd e = d.cwiseInverse();
244 | 
245 |   SpecialQR qr(n+1);
246 |   LeastSquareObjective ls(d);
247 | 
248 |   MatrixXd J1(n + 1, n);
249 |   MatrixXd R1(n + 1, n);
250 |   GivensSequence Q1;
251 |   Q1.reserve(n);
252 |   ls.buildJj(J1, 0, n-1, StartType::Case3, EndType::Case1);
253 |   bool b1 = qr.compute(R1, e, Q1, EndType::Case1);
254 |   BOOST_CHECK(isBandMatrix(R1, 0, 2));
255 |   BOOST_CHECK(b1);
256 |   BOOST_CHECK((Q1.matrix(n+1)*R1).isApprox(J1, 1e-15));
257 | 
258 |   MatrixXd J2(n, n);
259 |   MatrixXd R2(n, n);
260 |   GivensSequence Q2;
261 |   Q2.reserve(n-1);
262 |   ls.buildJj(J2, 0, n - 1, StartType::Case3, EndType::Case2);
263 |   bool b2 = qr.compute(R2, e, Q2, EndType::Case2);
264 |   BOOST_CHECK(isBandMatrix(R2, 0, 2));
265 |   BOOST_CHECK(b2);
266 |   BOOST_CHECK((Q2.matrix(n)*R2).isApprox(J2, 1e-15));
267 | 
268 |   MatrixXd J3(n, n + 1);
269 |   MatrixXd R3(n, n + 1);
270 |   GivensSequence Q3;
271 |   Q3.reserve(n-1);
272 |   ls.buildJj(J3, 0, n - 1, StartType::Case3, EndType::Case3);
273 |   bool b3 = qr.compute(R3, e, Q3, EndType::Case3);
274 |   BOOST_CHECK(isBandMatrix(R3, 0, 2));
275 |   BOOST_CHECK(b3);
276 |   BOOST_CHECK((Q3.matrix(n)*R3).isApprox(J3, 1e-15));
277 | 
278 |   MatrixXd J4(n + 1, n + 1);
279 |   MatrixXd R4(n + 1, n + 1);
280 |   GivensSequence Q4;
281 |   Q4.reserve(n);
282 |   ls.buildJj(J4, 0, n - 1, StartType::Case3, EndType::Case4);
283 |   bool b4 = qr.compute(R4, e, Q4, EndType::Case4);
284 |   BOOST_CHECK(isBandMatrix(R4, 0, 2));
285 |   BOOST_CHECK(!b4);
286 |   BOOST_CHECK((Q4.matrix(n + 1)*R4).isApprox(J4, 1e-15));
287 | }
288 | 


--------------------------------------------------------------------------------
/c++/src/SQP.cpp:
--------------------------------------------------------------------------------
  1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  2 |  *
  3 |  * This file is part of CPS.
  4 |  *
  5 |  * CPS is free software: you can redistribute it and/or modify
  6 |  * it under the terms of the GNU Lesser General Public License as published by
  7 |  * the Free Software Foundation, either version 3 of the License, or
  8 |  * (at your option) any later version.
  9 |  *
 10 |  * CPS is distributed in the hope that it will be useful,
 11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13 |  * GNU Lesser General Public License for more details.
 14 |  *
 15 |  * You should have received a copy of the GNU Lesser General Public License
 16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 17 |  */
 18 | 
 19 | #include <cps/SQP.h>
 20 | 
 21 | using namespace Eigen;
 22 | 
 23 | namespace cps
 24 | {
 25 |   SQP::SQP(int n)
 26 |     : n_(n)
 27 |     , ls_(n)
 28 |     , shiftedLC_(n)
 29 |     , x_(n)
 30 |     , xa_(n)
 31 |     , j_(n)
 32 |     , lambda_(n + 1)
 33 |     , Cx_(n + 1)
 34 |     , Cl_(n)
 35 |     , Jx_(n - 1)
 36 |     , JtJx_(n)
 37 |     , Jp_(n - 1)
 38 |     , gradL_(n)
 39 |     , currentActiveSet_(n + 1, Activation::None)
 40 |     , previousActiveSet_(n + 1, Activation::None)
 41 |   {
 42 |   }
 43 | 
 44 |   void SQP::SQPParameters(const Parameters & p)
 45 |   {
 46 |     params_ = p;
 47 |   }
 48 | 
 49 |   void SQP::LSParameters(const LeastSquare::Parameters & p)
 50 |   {
 51 |     ls_.parameters(p);
 52 |   }
 53 | 
 54 |   const SQP::Parameters& SQP::SQPParameters() const
 55 |   {
 56 |     return params_;
 57 |   }
 58 | 
 59 |   const LeastSquare::Parameters & SQP::LSParameters() const
 60 |   {
 61 |     return ls_.parameters();
 62 |   }
 63 | 
 64 | 
 65 |   SolverStatus SQP::solve(const Problem& pb)
 66 |   {
 67 |     k_ = 0;  // reset iteration number
 68 |     STATISTICS(stats_.reset());
 69 | 
 70 |     //shortcuts
 71 |     const auto& lc = pb.linearConstraints();
 72 |     const auto& nlc = pb.nonLinearConstraint();
 73 |     const auto& obj = pb.objective();
 74 | 
 75 |     //active sets
 76 |     currentActiveSet_ = lc.activationStatus();
 77 |     previousActiveSet_ = lc.activationStatus();
 78 | 
 79 |     //penality parameter
 80 |     double mu = params_.mu();
 81 |     double mu2 = mu*mu;
 82 | 
 83 |     //feasible (x,lambda)
 84 |     FeasiblePointInfo fpi;
 85 |     std::tie(fpi, x_) = lc.initialPoint(true, params_.feasibilityEps());
 86 |     if (fpi != FeasiblePointInfo::Found) //FIXME: we can be more precise here
 87 |     {
 88 |       //attempt to find a point without the active set
 89 |       std::tie(fpi, x_) = lc.initialPoint(false, params_.feasibilityEps());
 90 |       if (fpi != FeasiblePointInfo::Found)
 91 |         return SolverStatus::NoLinearlyFeasiblePoint;
 92 |       else
 93 |       {
 94 |         //reset the active sets
 95 |         for (size_t i = 0; i < currentActiveSet_.size(); ++i)
 96 |         {
 97 |           if (lc.activationStatus(i) != Activation::Equal)
 98 |           {
 99 |             currentActiveSet_[i] = Activation::None;
100 |             previousActiveSet_[i] = Activation::None;
101 |           }
102 |         }
103 | 
104 |       }
105 |     }
106 |     lambda_.setZero();
107 | 
108 |     //main loop
109 |     for (k_ = 0; k_ < params_.maxIter(); ++k_)
110 |     {
111 |       double f;
112 |       nlc.compute(f, j_, x_);
113 |       f *= mu;
114 |       j_ *= mu;
115 |       obj.applyJToTheLeft(Jx_, x_);
116 |       double v = 0.5* Jx_.squaredNorm();
117 | 
118 |       if (checkKKT(x_, lambda_, f, j_, lc, &obj))
119 |         return SolverStatus::Converge;
120 | 
121 |       //solve the least square
122 |       shiftedLC_ = lc.shift(x_, true);
123 |       previousActiveSet_ = currentActiveSet_;
124 |       shiftedLC_.setActivationStatus(currentActiveSet_);
125 |       if (ls_.solve(obj, Jx_, j_, f, shiftedLC_) != SolverStatus::Converge)
126 |       {
127 |         STATISTICS(stats_.lsStats.push_back(ls_.statistics()));
128 |         return SolverStatus::Fail;
129 |       }
130 |       STATISTICS(stats_.lsStats.push_back(ls_.statistics()));
131 |       currentActiveSet_ = shiftedLC_.activationStatus();
132 |       const auto& p = ls_.x();
133 |       const auto& pl = ls_.lambda();
134 | 
135 |       //line search
136 |       double a = 1;
137 |       obj.applyJToTheLeft(Jx_, x_);
138 |       obj.applyJToTheLeft(Jp_, p);
139 |       double cgp = params_.c1()*(f*j_.dot(p) + Jx_.dot(Jp_));
140 | 
141 |       xa_ = x_ + a*p;
142 |       double fa;
143 |       nlc.compute(fa, xa_);
144 |       double va = obj.value(xa_);
145 |       double val0 = 0.5*f*f + v;
146 |       STATISTICS(int kls = 0);
147 |       while (0.5*mu2*fa*fa + va > val0 + a*cgp)
148 |       {
149 |         STATISTICS(++kls);
150 |         a = params_.beta() * a;
151 |         if (a < params_.smallestLSStep())
152 |         {
153 |           STATISTICS(stats_.lineSearchSteps.push_back(kls));
154 |           return SolverStatus::LineSearchFailed;
155 |         }
156 |         xa_ = x_ + a*p;
157 |         nlc.compute(fa, xa_);
158 |         va = obj.value(xa_);
159 |       }
160 |       STATISTICS(stats_.lineSearchSteps.push_back(kls));
161 |       if (x_.isApprox(xa_))
162 |       {
163 |         ++k_; //we did full iteration, we need to count it.
164 |         //x_ didn't change so we just need to update lambda but not the objective-related values
165 |         lambda_ += a*(pl - lambda_);
166 |         if (checkKKT(x_, lambda_, f, j_, lc, &obj))
167 |           return SolverStatus::Converge;
168 |         else
169 |           return SolverStatus::NumericallyEquivalentIterates;
170 |       }
171 |       x_ = xa_;
172 |       lambda_ += a*(pl - lambda_);
173 |       if (a < 1)
174 |         currentActiveSet_ = previousActiveSet_;
175 |     }
176 | 
177 |     return SolverStatus::MaxIteration;
178 |   }
179 | 
180 | 
181 | 
182 |   SolverStatus SQP::solveFeasibility(const Problem& pb)
183 |   {
184 |     k_ = 0;  // reset iteration number
185 | 
186 |     //shortcuts
187 |     const auto& lc = pb.linearConstraints();
188 |     const auto& nlc = pb.nonLinearConstraint();
189 | 
190 |     //feasible (x,lambda)
191 |     FeasiblePointInfo fpi;
192 |     std::tie(fpi, x_) = lc.initialPoint(false, params_.feasibilityEps());
193 |     if (fpi != FeasiblePointInfo::Found) //FIXME: we can be more precise here
194 |       return SolverStatus::NoLinearlyFeasiblePoint; 
195 |     lambda_.setZero();
196 | 
197 |     //main loop
198 |     for (int k = 0; k < params_.maxIter(); ++k)
199 |     {
200 |       double f;
201 |       nlc.compute(f, j_, x_);
202 |       //std::cout << "||c||^2 = " << f*f << std::endl;
203 | 
204 |       if (checkKKT(x_, lambda_, f, j_, lc))
205 |         return SolverStatus::Converge;
206 | 
207 |       //solve the least square
208 |       shiftedLC_ = lc.shift(x_, true);
209 |       ls_.solveFeasibility(j_, f, shiftedLC_);
210 |       const auto& p = ls_.x();
211 |       const auto& pl = ls_.lambda();
212 | 
213 |       //line search
214 |       double a = 1;
215 |       double cgp = params_.c1()*f*j_.dot(p);
216 | 
217 |       xa_ = x_ + a*p;
218 |       double fa;
219 |       nlc.compute(fa, xa_);
220 |       while (0.5*fa*fa > 0.5*f*f + a*cgp)
221 |       {
222 |         a = params_.beta() * a;
223 |         if (a < params_.smallestLSStep())
224 |           return SolverStatus::LineSearchFailed;
225 |         xa_ = x_ + a*p;
226 |         nlc.compute(fa, xa_);
227 |       }
228 |       if (x_.isApprox(xa_))
229 |       {
230 |         //x_ didn't change so we just need to update lambda but not the objective-related values
231 |         lambda_ += a*(pl - lambda_);
232 |         if (checkKKT(x_, lambda_, f, j_, lc))
233 |           return SolverStatus::Converge;
234 |         else
235 |           return SolverStatus::NumericallyEquivalentIterates;
236 |       }
237 |       x_ = xa_;
238 |       lambda_ += a*(pl - lambda_);
239 |     }
240 | 
241 |     return SolverStatus::MaxIteration;
242 |   }
243 | 
244 |   const Eigen::VectorXd & SQP::x() const
245 |   {
246 |     return x_;
247 |   }
248 |   const Eigen::VectorXd & SQP::lambda() const
249 |   {
250 |     return lambda_;
251 |   }
252 | 
253 |   const std::vector<Activation>& SQP::activeSet() const
254 |   {
255 |     return currentActiveSet_;
256 |   }
257 | 
258 |   int SQP::numberOfIterations() const
259 |   {
260 |     return k_;
261 |   }
262 | 
263 |   const stats::SQPStats & SQP::statistics() const
264 |   {
265 |     return stats_;
266 |   }
267 | 
268 |   bool SQP::checkKKT(const Eigen::VectorXd& x, const Eigen::VectorXd& lambda, double f, const Eigen::VectorXd& g, 
269 |                      const LinearConstraints& lc, const LeastSquareObjective* const obj) const
270 |   {
271 |     //see Stan's thesis $4.3.5
272 |     double tx = params_.tau_p()*(1 + x.lpNorm<Infinity>());
273 |     double tl = params_.tau_d()*(1 + lambda.lpNorm<Infinity>());
274 | 
275 |     lc.transposeMult(Cl_, lambda);
276 |     gradL_ = f*g + Cl_;
277 |     if (obj)
278 |     {
279 |       obj->applyJToTheLeft(Jx_, x);
280 |       obj->applyJTransposeToTheLeft(JtJx_, Jx_);
281 |       gradL_ += JtJx_;
282 |     }
283 | 
284 |     if (gradL_.lpNorm<Infinity>() <= tl)
285 |     {
286 |       lc.mult(Cx_, x);
287 |       bool b = true;
288 |       const auto& l = lc.l();
289 |       const auto& u = lc.u();
290 |       for (DenseIndex i = 0; b && i <= n_; ++i)
291 |       {
292 |         b = (std::abs(Cx_[i] - l[i]) <= tx && lambda[i] <= tl)
293 |           || (l[i] - tx <= Cx_[i] && Cx_[i] <= u[i] + tx && std::abs(lambda[i]) <= tl)
294 |           || (std::abs(Cx_[i] - u[i]) <= tx && lambda[i] >= -tl);
295 |       }
296 |       return b;
297 |     }
298 |     else
299 |       return false;
300 |   }
301 | }
302 | 


--------------------------------------------------------------------------------
/c++/tests/LinearConstraintsTest.cpp:
--------------------------------------------------------------------------------
  1 | /* Copyright 2018 CNRS-AIST JRL, CNRS-UM LIRMM
  2 |  *
  3 |  * This file is part of CPS.
  4 |  *
  5 |  * CPS is free software: you can redistribute it and/or modify
  6 |  * it under the terms of the GNU Lesser General Public License as published by
  7 |  * the Free Software Foundation, either version 3 of the License, or
  8 |  * (at your option) any later version.
  9 |  *
 10 |  * CPS is distributed in the hope that it will be useful,
 11 |  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12 |  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13 |  * GNU Lesser General Public License for more details.
 14 |  *
 15 |  * You should have received a copy of the GNU Lesser General Public License
 16 |  * along with CPS.  If not, see <http://www.gnu.org/licenses/>.
 17 |  */
 18 | 
 19 | #include <iostream>
 20 | 
 21 | #include <Eigen/SVD>
 22 | 
 23 | #include <cps/LinearConstraints.h>
 24 | #include <cps/ProblemMatrices.h>
 25 | 
 26 | // boost
 27 | #define BOOST_TEST_MODULE LinearConstraintsTest
 28 | #include <boost/test/unit_test.hpp>
 29 | 
 30 | using namespace Eigen;
 31 | using namespace cps;
 32 | 
 33 | BOOST_AUTO_TEST_CASE(ActivationTest)
 34 | {
 35 |   VectorXd l = -VectorXd::Random(10).cwiseAbs();
 36 |   VectorXd u = VectorXd::Random(10).cwiseAbs();
 37 |   u[3] = l[3];
 38 |   u[6] = l[6];
 39 |   u[7] = l[7];
 40 | 
 41 |   LinearConstraints lc(l, u, -1, 1);
 42 | 
 43 |   BOOST_CHECK(lc.numberOfActiveConstraints() == 3);
 44 |   BOOST_CHECK(lc.activationStatus(0) == Activation::None);
 45 |   BOOST_CHECK(lc.activationStatus(1) == Activation::None);
 46 |   BOOST_CHECK(lc.activationStatus(2) == Activation::None);
 47 |   BOOST_CHECK(lc.activationStatus(3) == Activation::Equal);
 48 |   BOOST_CHECK(lc.activationStatus(4) == Activation::None);
 49 |   BOOST_CHECK(lc.activationStatus(5) == Activation::None);
 50 |   BOOST_CHECK(lc.activationStatus(6) == Activation::Equal);
 51 |   BOOST_CHECK(lc.activationStatus(7) == Activation::Equal);
 52 |   BOOST_CHECK(lc.activationStatus(8) == Activation::None);
 53 |   BOOST_CHECK(lc.activationStatus(9) == Activation::None);
 54 |   BOOST_CHECK(lc.activationStatus(10) == Activation::None);
 55 |   
 56 |   auto act = lc.activeSet();
 57 |   BOOST_CHECK(act.size() == 11);
 58 |   BOOST_CHECK(!act[0]);
 59 |   BOOST_CHECK(!act[1]);
 60 |   BOOST_CHECK(!act[2]);
 61 |   BOOST_CHECK(act[3]);
 62 |   BOOST_CHECK(!act[4]);
 63 |   BOOST_CHECK(!act[5]);
 64 |   BOOST_CHECK(act[6]);
 65 |   BOOST_CHECK(act[7]);
 66 |   BOOST_CHECK(!act[8]);
 67 |   BOOST_CHECK(!act[9]);
 68 |   BOOST_CHECK(!act[10]);
 69 | 
 70 |   auto actIdx = lc.activeSetIdx();
 71 |   BOOST_CHECK(actIdx[0] == 3);
 72 |   BOOST_CHECK(actIdx[1] == 6);
 73 |   BOOST_CHECK(actIdx[2] == 7);
 74 | 
 75 |   //activating a non activated constraint
 76 |   lc.activate(1, Activation::Lower);
 77 |   BOOST_CHECK(lc.activationStatus(1) == Activation::Lower);
 78 |   BOOST_CHECK(lc.activeSet()[1]);
 79 |   BOOST_CHECK(lc.numberOfActiveConstraints() == 4);
 80 | 
 81 |   //activating an activated constraint
 82 |   lc.activate(1, Activation::Upper);
 83 |   BOOST_CHECK(lc.activationStatus(1) == Activation::Upper);
 84 |   BOOST_CHECK(lc.activeSet()[1]);
 85 |   BOOST_CHECK(lc.numberOfActiveConstraints() == 4);
 86 | 
 87 |   //deactivating a constraint with activate
 88 |   lc.activate(3, Activation::None);
 89 |   BOOST_CHECK(lc.activationStatus(3) == Activation::None);
 90 |   BOOST_CHECK(!lc.activeSet()[3]);
 91 |   BOOST_CHECK(lc.numberOfActiveConstraints() == 3);
 92 | 
 93 |   //deactivating an activated constraint
 94 |   lc.deactivate(7);
 95 |   BOOST_CHECK(lc.activationStatus(7) == Activation::None);
 96 |   BOOST_CHECK(!lc.activeSet()[7]);
 97 |   BOOST_CHECK(lc.numberOfActiveConstraints() == 2);
 98 | 
 99 |   //deactivating a deactivated constraint
100 |   lc.deactivate(8);
101 |   BOOST_CHECK(lc.activationStatus(8) == Activation::None);
102 |   BOOST_CHECK(!lc.activeSet()[8]);
103 |   BOOST_CHECK(lc.numberOfActiveConstraints() == 2);
104 | 
105 |   //check that applyNullSpaceOnTheRight correctly computes the active set index
106 |   MatrixXd Y(1, 8);
107 |   lc.applyNullSpaceOnTheRight(Y, MatrixXd(1, 10));
108 |   actIdx = lc.activeSetIdx();
109 |   BOOST_CHECK(actIdx[0] == 1);
110 |   BOOST_CHECK(actIdx[1] == 6);
111 | 
112 |   VectorXd y(11);
113 |   VectorXd x = VectorXd::Random(2);
114 |   lc.expandActive(y, x);
115 |   BOOST_CHECK(y[1] == x[0]);
116 |   BOOST_CHECK(y[6] == x[1]);
117 |   BOOST_CHECK(y[0] == 0);
118 |   BOOST_CHECK((y.array().segment(2, 4) == 0).all());
119 |   BOOST_CHECK((y.array().tail(4) == 0).all());
120 | }
121 | 
122 | BOOST_AUTO_TEST_CASE(NullspaceTest)
123 | {
124 |   VectorXd l = -VectorXd::Random(20).cwiseAbs();
125 |   VectorXd u =  VectorXd::Random(20).cwiseAbs();
126 |   LinearConstraints lc(l,u,-1,1);
127 | 
128 |   for (size_t i = 0; i < 21; ++i)
129 |   {
130 |     if (rand() > RAND_MAX / 2)
131 |       lc.activate(i, Activation::Lower);
132 |   }
133 | 
134 |   MatrixXd Ca = lc.matrixAct();
135 |   MatrixXd N(20,20-lc.numberOfActiveConstraints());
136 |   lc.applyNullSpaceOnTheRight(N, MatrixXd::Identity(20, 20));
137 | 
138 |   MatrixXd CaN = Ca*N;
139 |   MatrixXd NtN = N.transpose()*N;
140 |   BOOST_CHECK(CaN.isZero(1e-15));
141 |   BOOST_CHECK(NtN.isDiagonal());
142 | 
143 |   VectorXd z = VectorXd::Random(20 - lc.numberOfActiveConstraints());
144 |   VectorXd y(20);
145 |   lc.applyNullSpaceOnTheLeft(y, z);
146 |   VectorXd y2 = N*z;
147 |   BOOST_CHECK(y.isApprox(y2, 1e-15));
148 | }
149 | 
150 | BOOST_AUTO_TEST_CASE(MultiplicationTest)
151 | {
152 |   VectorXd l = -VectorXd::Random(20).cwiseAbs();
153 |   VectorXd u = VectorXd::Random(20).cwiseAbs();
154 |   LinearConstraints lc(l, u, -1, 1);
155 | 
156 |   MatrixXd C = lc.matrix();
157 |   VectorXd x = VectorXd::Random(20);
158 |   VectorXd y(21);
159 |   lc.mult(y, x);
160 |   VectorXd z = C*x;
161 |   BOOST_CHECK(y.isApprox(z, 1e-15));
162 | 
163 |   VectorXd q = VectorXd::Random(21);
164 |   VectorXd r(20);
165 |   lc.transposeMult(r, q);
166 |   VectorXd s = C.transpose()*q;
167 |   BOOST_CHECK(r.isApprox(s, 1e-15));
168 | }
169 | 
170 | BOOST_AUTO_TEST_CASE(PseudoInverseTest)
171 | {
172 |   VectorXd l = -VectorXd::Random(20).cwiseAbs();
173 |   VectorXd u = VectorXd::Random(20).cwiseAbs();
174 |   LinearConstraints lc(l, u, -1, 1);
175 | 
176 |   for (size_t i = 1; i < 20; ++i)
177 |   {
178 |     if (rand() > RAND_MAX / 2)
179 |       lc.activate(i, Activation::Lower);
180 |   }
181 | 
182 |   {
183 |     MatrixXd Ca = lc.matrixAct();
184 |     VectorXd b = VectorXd::Random(Ca.cols());
185 |     VectorXd x(Ca.rows());
186 | 
187 |     lc.pinvTransposeMultAct(x, b);
188 |     VectorXd y = Ca.transpose().jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
189 |     BOOST_CHECK(x.isApprox(y, 1e-15));
190 |   }
191 | 
192 |   {
193 |     lc.activate(0, Activation::Lower);
194 |     MatrixXd Ca = lc.matrixAct();
195 |     VectorXd b = VectorXd::Random(Ca.cols());
196 |     VectorXd x(Ca.rows());
197 | 
198 |     lc.pinvTransposeMultAct(x, b);
199 |     VectorXd y = Ca.transpose().jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
200 |     BOOST_CHECK(x.isApprox(y, 1e-15));
201 |   }
202 | 
203 |   {
204 |     lc.activate(20, Activation::Lower);
205 |     MatrixXd Ca = lc.matrixAct();
206 |     VectorXd b = VectorXd::Random(Ca.cols());
207 |     VectorXd x(Ca.rows());
208 | 
209 |     lc.pinvTransposeMultAct(x, b);
210 |     VectorXd y = Ca.transpose().jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
211 |     BOOST_CHECK(x.isApprox(y, 1e-15));
212 |   }
213 | 
214 |   {
215 |     for (size_t i = 0; i < 20; ++i)
216 |       lc.activate(i, Activation::Lower);
217 |     lc.deactivate(20);
218 | 
219 |     MatrixXd Ca = lc.matrixAct();
220 |     VectorXd b = VectorXd::Random(Ca.cols());
221 |     VectorXd x(Ca.rows());
222 | 
223 |     lc.pinvTransposeMultAct(x, b);
224 |     VectorXd y = Ca.transpose().jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
225 |     BOOST_CHECK(x.isApprox(y, 1e-14));
226 |   }
227 | 
228 |   {
229 |     for (size_t i = 1; i < 21; ++i)
230 |       lc.activate(i, Activation::Lower);
231 |     lc.deactivate(0);
232 | 
233 |     MatrixXd Ca = lc.matrixAct();
234 |     VectorXd b = VectorXd::Random(Ca.cols());
235 |     VectorXd x(Ca.rows());
236 | 
237 |     lc.pinvTransposeMultAct(x, b);
238 |     VectorXd y = Ca.transpose().jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
239 |     BOOST_CHECK(x.isApprox(y, 1e-14));
240 |   }
241 | }
242 | 
243 | BOOST_AUTO_TEST_CASE(InitialPoint)
244 | {
245 |   // A point exists (0 is feasible)
246 |   {
247 |     VectorXd l = -VectorXd::Random(5).cwiseAbs();
248 |     VectorXd u = VectorXd::Random(5).cwiseAbs();
249 |     LinearConstraints lc(l, u, -1, 1);
250 | 
251 |     auto init = lc.initialPoint();
252 |     BOOST_CHECK(init.first == FeasiblePointInfo::Found);
253 |     BOOST_CHECK(lc.checkPrimal(init.second));
254 |   }
255 | 
256 |   // No feasible point
257 |   {
258 |     VectorXd l = -VectorXd::Random(5).cwiseAbs();
259 |     VectorXd u = VectorXd::Random(5).cwiseAbs();
260 |     LinearConstraints lc(l, u, 1000, 1010);
261 | 
262 |     auto init = lc.initialPoint();
263 |     BOOST_CHECK(init.first == FeasiblePointInfo::Infeasible);
264 |   }
265 | 
266 |   // numerical issue with lower bound
267 |   {
268 |     VectorXd l = -VectorXd::Random(5).cwiseAbs();
269 |     VectorXd u = VectorXd::Random(5).cwiseAbs();
270 |     LinearConstraints lc(l, u, l.sum(), l.sum()+1);
271 | 
272 |     auto init = lc.initialPoint();
273 |     BOOST_CHECK(init.first == FeasiblePointInfo::NumericalWarningLower);
274 |     BOOST_CHECK(lc.checkPrimal(init.second));
275 |   }
276 | 
277 |   // numerical issue with upper bound
278 |   {
279 |     VectorXd l = -VectorXd::Random(5).cwiseAbs();
280 |     VectorXd u = VectorXd::Random(5).cwiseAbs();
281 |     LinearConstraints lc(l, u, u.sum() - 1, u.sum()-1e-12);
282 | 
283 |     auto init = lc.initialPoint();
284 |     BOOST_CHECK(init.first == FeasiblePointInfo::NumericalWarningUpper);
285 |     BOOST_CHECK(lc.checkPrimal(init.second));
286 |   }
287 | 
288 |   // numerical issue with both bounds
289 |   {
290 |     VectorXd l = -VectorXd::Random(5).cwiseAbs();
291 |     VectorXd u = VectorXd::Random(5).cwiseAbs();
292 |     LinearConstraints lc(l, u, l.sum(), u.sum() - 1e-12);
293 | 
294 |     auto init = lc.initialPoint();
295 |     BOOST_CHECK(init.first == FeasiblePointInfo::NumericalWarningBoth);
296 |     BOOST_CHECK(lc.checkPrimal(init.second));
297 |   }
298 | 
299 |   // numerical issue with zonotope
300 |   {
301 |     VectorXd l = -1e-12* VectorXd::Random(5).cwiseAbs();
302 |     VectorXd u = 1e-12*VectorXd::Random(5).cwiseAbs();
303 |     LinearConstraints lc(l, u, -1, 1);
304 | 
305 |     auto init = lc.initialPoint();
306 |     BOOST_CHECK(init.first == FeasiblePointInfo::TooSmallZonotope);
307 |     BOOST_CHECK(lc.checkPrimal(init.second));
308 |   }
309 | }
310 | 


--------------------------------------------------------------------------------