Skip to content

Commit d9aeebc

Browse files
markusgftmarkusgft
committed
resolving a bug which prevented box constraints from being set correctly using the new HPIPM version
1 parent 7543100 commit d9aeebc

11 files changed

+170
-173
lines changed

‎.clang-format

Lines changed: 1 addition & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -35,8 +35,7 @@ BreakBeforeBinaryOperators: None
3535
BreakBeforeBraces: Custom
3636
BreakBeforeTernaryOperators: false
3737
BreakConstructorInitializersBeforeComma: false
38-
BreakAfterJavaFieldAnnotations: false
39-
BreakStringLiterals: true
38+
#BreakStringLiterals: true
4039
ColumnLimit: 120
4140
CommentPragmas: '^ IWYU pragma:'
4241
ConstructorInitializerAllOnOneLineOrOnePerLine: true
@@ -54,12 +53,9 @@ IncludeCategories:
5453
Priority: 2
5554
- Regex: '.*'
5655
Priority: 3
57-
IncludeIsMainRegex: '([-_](test|unittest))?$'
5856
IndentCaseLabels: true
5957
IndentWidth: 4
6058
IndentWrappedFunctionNames: false
61-
JavaScriptQuotes: Leave
62-
JavaScriptWrapImports: true
6359
KeepEmptyLinesAtTheStartOfBlocks: false
6460
MacroBlockBegin: ''
6561
MacroBlockEnd: ''

‎ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp

Lines changed: 11 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -69,12 +69,18 @@ NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::NLOCB
6969

7070
// if the optimal problem has constraints, change
7171
if (systemInterface_->getOptConProblem().getInputBoxConstraints())
72+
{
7273
changeInputBoxConstraints(systemInterface_->getOptConProblem().getInputBoxConstraints());
74+
}
7375
if (systemInterface_->getOptConProblem().getStateBoxConstraints())
76+
{
7477
changeStateBoxConstraints(systemInterface_->getOptConProblem().getStateBoxConstraints());
78+
}
7579

7680
if (systemInterface_->getOptConProblem().getGeneralConstraints())
81+
{
7782
changeGeneralConstraints(systemInterface_->getOptConProblem().getGeneralConstraints());
83+
}
7884
}
7985

8086
template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
@@ -287,8 +293,6 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
287293
computeBoxConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_box_norm_);
288294

289295
setInputBoxConstraintsForLQOCProblem();
290-
lqocSolver_->configureInputBoxConstraints(lqocProblem_);
291-
lqocSolver_->initializeAndAllocate(); // todo - this is not nice
292296
}
293297

294298
template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
@@ -311,8 +315,6 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
311315
computeBoxConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_box_norm_);
312316

313317
setStateBoxConstraintsForLQOCProblem();
314-
lqocSolver_->configureStateBoxConstraints(lqocProblem_);
315-
lqocSolver_->initializeAndAllocate(); // todo - this is not nice
316318
}
317319

318320

@@ -352,8 +354,6 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
352354
lqocProblem_->d_ub_[K_].resize(lqocProblem_->ng_[K_], 1);
353355

354356
lqocSolver_->setProblem(lqocProblem_);
355-
lqocSolver_->configureGeneralConstraints(lqocProblem_);
356-
lqocSolver_->initializeAndAllocate();
357357

358358
// TODO can we do this multi-threaded?
359359
if (iteration_ > 0 && (settings_.lineSearchSettings.type != LineSearchSettings::TYPE::NONE))
@@ -803,7 +803,6 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
803803
p.ng_[k] = generalConstraints_[threadId]->getIntermediateConstraintsCount();
804804
if (p.ng_[k] > 0)
805805
{
806-
p.hasGenConstraints_ = true;
807806
p.C_[k] = generalConstraints_[threadId]->jacobianStateIntermediate();
808807
p.D_[k] = generalConstraints_[threadId]->jacobianInputIntermediate();
809808

@@ -835,7 +834,6 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
835834
p.ng_[K_] = generalConstraints_[settings_.nThreads]->getTerminalConstraintsCount();
836835
if (p.ng_[K_] > 0)
837836
{
838-
p.hasGenConstraints_ = true;
839837
p.C_[K_] = generalConstraints_[settings_.nThreads]->jacobianStateTerminal();
840838
p.D_[K_] = generalConstraints_[settings_.nThreads]->jacobianInputTerminal();
841839

@@ -1284,7 +1282,10 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
12841282
lqpCounter_++;
12851283

12861284
// if solver is HPIPM, there's nothing to prepare
1287-
if (settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::HPIPM_SOLVER) {}
1285+
if (settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::HPIPM_SOLVER)
1286+
{
1287+
// do nothing
1288+
}
12881289
// if solver is GNRiccati - we iterate backward up to the first stage
12891290
else if (settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::GNRICCATI_SOLVER)
12901291
{
@@ -1328,28 +1329,6 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
13281329
{
13291330
lqpCounter_++;
13301331

1331-
if (lqocProblem_->isInputBoxConstrained())
1332-
{
1333-
if (settings_.debugPrint)
1334-
std::cout << "LQ Problem has input box constraints. Configuring box constraints now. " << std::endl;
1335-
1336-
lqocSolver_->configureInputBoxConstraints(lqocProblem_);
1337-
}
1338-
if (lqocProblem_->isStateBoxConstrained())
1339-
{
1340-
if (settings_.debugPrint)
1341-
std::cout << "LQ Problem has state box constraints. Configuring box constraints now. " << std::endl;
1342-
1343-
lqocSolver_->configureStateBoxConstraints(lqocProblem_);
1344-
}
1345-
if (lqocProblem_->isGeneralConstrained())
1346-
{
1347-
if (settings_.debugPrint)
1348-
std::cout << "LQ Problem has general constraints. Configuring general constraints now. " << std::endl;
1349-
1350-
lqocSolver_->configureGeneralConstraints(lqocProblem_);
1351-
}
1352-
13531332
lqocSolver_->setProblem(lqocProblem_);
13541333

13551334
lqocSolver_->solve();
@@ -1633,7 +1612,7 @@ std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALA
16331612
ConstraintPtr_t>&
16341613
NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getStateBoxConstraintsInstances()
16351614
{
1636-
return inputBoxConstraints_;
1615+
return stateBoxConstraints_;
16371616
}
16381617

16391618

‎ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp

Lines changed: 17 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,7 @@ namespace optcon {
1010

1111

1212
template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
13-
LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::LQOCProblem(int N)
14-
: hasInputBoxConstraints_(false), hasStateBoxConstraints_(false), hasGenConstraints_(false)
13+
LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::LQOCProblem(int N) : nbu_(N, 0), nbx_(N + 1, 0)
1514
{
1615
changeNumStages(N);
1716
}
@@ -21,23 +20,31 @@ bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isConstrained() const
2120
{
2221
return (isInputBoxConstrained() | isStateBoxConstrained() | isGeneralConstrained());
2322
}
24-
2523
template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
2624
bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isInputBoxConstrained() const
2725
{
28-
return hasInputBoxConstraints_;
26+
if (std::accumulate(nbu_.begin(), nbu_.end(), 0) > 0)
27+
return true;
28+
29+
return false;
2930
}
3031

3132
template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
3233
bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isStateBoxConstrained() const
3334
{
34-
return hasStateBoxConstraints_;
35+
if (std::accumulate(nbx_.begin(), nbx_.end(), 0) > 0)
36+
return true;
37+
38+
return false;
3539
}
3640

3741
template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
3842
bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isGeneralConstrained() const
3943
{
40-
return hasGenConstraints_;
44+
if (std::accumulate(ng_.begin(), ng_.end(), 0) > 0)
45+
return true;
46+
47+
return false;
4148
}
4249

4350
template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
@@ -115,10 +122,6 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setZero(const int& nGenConstr)
115122
D_[i].resize(nGenConstr, CONTROL_DIM);
116123
D_[i].setZero();
117124
}
118-
119-
hasInputBoxConstraints_ = false;
120-
hasStateBoxConstraints_ = false;
121-
hasGenConstraints_ = false;
122125
}
123126

124127

@@ -152,7 +155,6 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setInputBoxConstraint(const in
152155
u_lb_[index](i) = u_lb(i) - u_nom_abs(sp(i)); // substract the corresponding entry in nom-control
153156
u_ub_[index](i) = u_ub(i) - u_nom_abs(sp(i)); // substract the corresponding entry in nom-control
154157
}
155-
hasInputBoxConstraints_ = true;
156158
}
157159

158160
template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
@@ -197,8 +199,6 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setIntermediateStateBoxConstra
197199
x_lb_[index](i) = x_lb(i) - x_nom_abs(sp(i)); // substract the corresponding entry in nom-state
198200
x_ub_[index](i) = x_ub(i) - x_nom_abs(sp(i)); // substract the corresponding entry in nom-state
199201
}
200-
201-
hasStateBoxConstraints_ = true;
202202
}
203203

204204
template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
@@ -241,7 +241,6 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setTerminalBoxConstraints(cons
241241
x_lb_[K_](i) = x_lb(i) - x_nom_abs(sp(i)); // substract the corresponding entry in nom-state
242242
x_ub_[K_](i) = x_ub(i) - x_nom_abs(sp(i)); // substract the corresponding entry in nom-state
243243
}
244-
hasStateBoxConstraints_ = true;
245244
}
246245
}
247246

@@ -255,8 +254,6 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setGeneralConstraints(const co
255254
d_ub_.setConstant(d_ub);
256255
C_.setConstant(C);
257256
D_.setConstant(D);
258-
259-
hasGenConstraints_ = true;
260257
}
261258

262259

@@ -269,8 +266,10 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setFromTimeInvariantLinearQuad
269266
{
270267
setZero();
271268

272-
core::StateVector<STATE_DIM, SCALAR> x0; x0.setZero(); // by definition
273-
core::ControlVector<CONTROL_DIM, SCALAR> u0; u0.setZero(); // by definition
269+
core::StateVector<STATE_DIM, SCALAR> x0;
270+
x0.setZero(); // by definition
271+
core::ControlVector<CONTROL_DIM, SCALAR> u0;
272+
u0.setZero(); // by definition
274273

275274
core::StateMatrix<STATE_DIM, SCALAR> A;
276275
core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR> B;
@@ -295,10 +294,6 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setFromTimeInvariantLinearQuad
295294
// final stage
296295
Q_[K_] = costFunction.stateSecondDerivativeTerminal();
297296
qv_[K_] = costFunction.stateDerivativeTerminal();
298-
299-
hasInputBoxConstraints_ = false;
300-
hasStateBoxConstraints_ = false;
301-
hasGenConstraints_ = false;
302297
}
303298

304299
} // namespace optcon

‎ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp

Lines changed: 1 addition & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -251,15 +251,9 @@ class LQOCProblem
251251
//! number of general inequality constraints
252252
std::vector<int> ng_;
253253

254-
//! bool indicating if the optimization problem is box-constrained
255-
bool hasInputBoxConstraints_;
256-
bool hasStateBoxConstraints_;
257-
258-
//! bool indicating if the optimization problem hs general inequality constraints
259-
bool hasGenConstraints_;
260254

261255
private:
262-
//! the number of discrete time steps in the LOCP, including terminal stage
256+
//! the number of discrete time steps in the LOCP, not including terminal stage
263257
int K_;
264258
};
265259

‎ct_optcon/include/ct/optcon/problem/OptConProblemBase-impl.h

Lines changed: 0 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -8,17 +8,6 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
88
namespace ct {
99
namespace optcon {
1010

11-
12-
template <size_t STATE_DIM,
13-
size_t CONTROL_DIM,
14-
typename SYSTEM_T,
15-
typename LINEAR_SYSTEM_T,
16-
typename LINEARIZER_T,
17-
typename SCALAR>
18-
OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::OptConProblemBase()
19-
{
20-
}
21-
2211
template <size_t STATE_DIM,
2312
size_t CONTROL_DIM,
2413
typename SYSTEM_T,

‎ct_optcon/include/ct/optcon/problem/OptConProblemBase.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -53,7 +53,7 @@ class OptConProblemBase
5353
typedef std::shared_ptr<optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> ConstraintPtr_t;
5454
typedef typename SYSTEM_T::time_t time_t;
5555

56-
OptConProblemBase();
56+
OptConProblemBase() = default;
5757

5858
/*!
5959
* @brief Construct a simple unconstrained Optimal Control Problem

0 commit comments

Comments
 (0)