Skip to content

Commit 1f684c8

Browse files
farbod-farshidianfarbod-farshidian
committed
removing termsSizePtr from constraint collection
1 parent 85a030c commit 1f684c8

10 files changed

+33
-87
lines changed

‎ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -56,13 +56,12 @@ class StateConstraintCollection : public Collection<StateConstraint> {
5656
virtual vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const;
5757

5858
/** Get the constraint linear approximation */
59-
virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const PreComputation& preComp,
60-
size_array_t* termsSizePtr = nullptr) const;
59+
virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state,
60+
const PreComputation& preComp) const;
6161

6262
/** Get the constraint quadratic approximation */
6363
virtual VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state,
64-
const PreComputation& preComp,
65-
size_array_t* termsSizePtr = nullptr) const;
64+
const PreComputation& preComp) const;
6665

6766
protected:
6867
/** Copy constructor */

‎ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -57,13 +57,11 @@ class StateInputConstraintCollection : public Collection<StateInputConstraint> {
5757

5858
/** Get the constraint linear approximation */
5959
virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input,
60-
const PreComputation& preComp,
61-
size_array_t* termsSizePtr = nullptr) const;
60+
const PreComputation& preComp) const;
6261

6362
/** Get the constraint quadratic approximation */
6463
virtual VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input,
65-
const PreComputation& preComp,
66-
size_array_t* termsSizePtr = nullptr) const;
64+
const PreComputation& preComp) const;
6765

6866
protected:
6967
/** Copy constructor */

‎ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintEliminatePattern.h‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -50,12 +50,10 @@ class LoopshapingConstraintEliminatePattern final : public LoopshapingStateInput
5050
LoopshapingConstraintEliminatePattern* clone() const override { return new LoopshapingConstraintEliminatePattern(*this); };
5151

5252
VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input,
53-
const PreComputation& preComp,
54-
size_array_t* termsSizePtr = nullptr) const override;
53+
const PreComputation& preComp) const override;
5554

5655
VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input,
57-
const PreComputation& preComp,
58-
size_array_t* termsSizePtr = nullptr) const override;
56+
const PreComputation& preComp) const override;
5957

6058
protected:
6159
using BASE::loopshapingDefinition_;

‎ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintOutputPattern.h‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -50,12 +50,10 @@ class LoopshapingConstraintOutputPattern final : public LoopshapingStateInputCon
5050
LoopshapingConstraintOutputPattern* clone() const override { return new LoopshapingConstraintOutputPattern(*this); };
5151

5252
VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input,
53-
const PreComputation& preComp,
54-
size_array_t* termsSizePtr = nullptr) const override;
53+
const PreComputation& preComp) const override;
5554

5655
VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input,
57-
const PreComputation& preComp,
58-
size_array_t* termsSizePtr = nullptr) const override;
56+
const PreComputation& preComp) const override;
5957

6058
protected:
6159
using BASE::loopshapingDefinition_;

‎ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h‎

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -48,12 +48,11 @@ class LoopshapingStateConstraint final : public StateConstraintCollection {
4848
LoopshapingStateConstraint* clone() const override { return new LoopshapingStateConstraint(*this); }
4949

5050
vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override;
51+
VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state,
52+
const PreComputation& preComp) const override;
5153

52-
VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const PreComputation& preComp,
53-
size_array_t* termsSizePtr = nullptr) const override;
54-
55-
VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const PreComputation& preComp,
56-
size_array_t* termsSizePtr = nullptr) const override;
54+
VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state,
55+
const PreComputation& preComp) const override;
5756

5857
private:
5958
LoopshapingStateConstraint(const LoopshapingStateConstraint& other) = default;

‎ocs2_core/src/constraint/StateConstraintCollection.cpp‎

Lines changed: 4 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -76,29 +76,19 @@ vector_array_t StateConstraintCollection::getValue(scalar_t time, const vector_t
7676
/******************************************************************************************************/
7777
/******************************************************************************************************/
7878
VectorFunctionLinearApproximation StateConstraintCollection::getLinearApproximation(scalar_t time, const vector_t& state,
79-
const PreComputation& preComp,
80-
size_array_t* termsSizePtr) const {
79+
const PreComputation& preComp) const {
8180
VectorFunctionLinearApproximation linearApproximation(getNumConstraints(time), state.rows());
8281

83-
if (termsSizePtr != nullptr) {
84-
termsSizePtr->clear();
85-
termsSizePtr->reserve(this->terms_.size());
86-
}
87-
8882
// append linearApproximation of each constraintTerm
8983
size_t i = 0;
9084
for (const auto& constraintTerm : this->terms_) {
91-
size_t nc = 0;
9285
if (constraintTerm->isActive(time)) {
9386
const auto constraintTermApproximation = constraintTerm->getLinearApproximation(time, state, preComp);
94-
nc = constraintTermApproximation.f.rows();
87+
const size_t nc = constraintTermApproximation.f.rows();
9588
linearApproximation.f.segment(i, nc) = constraintTermApproximation.f;
9689
linearApproximation.dfdx.middleRows(i, nc) = constraintTermApproximation.dfdx;
9790
i += nc;
9891
}
99-
if (termsSizePtr != nullptr) {
100-
termsSizePtr->push_back(nc);
101-
}
10292
}
10393

10494
return linearApproximation;
@@ -108,35 +98,25 @@ VectorFunctionLinearApproximation StateConstraintCollection::getLinearApproximat
10898
/******************************************************************************************************/
10999
/******************************************************************************************************/
110100
VectorFunctionQuadraticApproximation StateConstraintCollection::getQuadraticApproximation(scalar_t time, const vector_t& state,
111-
const PreComputation& preComp,
112-
size_array_t* termsSizePtr) const {
101+
const PreComputation& preComp) const {
113102
const auto numConstraints = getNumConstraints(time);
114103

115104
VectorFunctionQuadraticApproximation quadraticApproximation;
116105
quadraticApproximation.f.resize(numConstraints);
117106
quadraticApproximation.dfdx.resize(numConstraints, state.rows());
118107
quadraticApproximation.dfdxx.reserve(numConstraints); // Use reserve instead of resize to avoid unnecessary allocations.
119108

120-
if (termsSizePtr != nullptr) {
121-
termsSizePtr->clear();
122-
termsSizePtr->reserve(this->terms_.size());
123-
}
124-
125109
// append quadraticApproximation of each constraintTerm
126110
size_t i = 0;
127111
for (const auto& constraintTerm : this->terms_) {
128-
size_t nc = 0;
129112
if (constraintTerm->isActive(time)) {
130113
auto constraintTermApproximation = constraintTerm->getQuadraticApproximation(time, state, preComp);
131-
nc = constraintTermApproximation.f.rows();
114+
const size_t nc = constraintTermApproximation.f.rows();
132115
quadraticApproximation.f.segment(i, nc) = constraintTermApproximation.f;
133116
quadraticApproximation.dfdx.middleRows(i, nc) = constraintTermApproximation.dfdx;
134117
appendVectorToVectorByMoving(quadraticApproximation.dfdxx, std::move(constraintTermApproximation.dfdxx));
135118
i += nc;
136119
}
137-
if (termsSizePtr != nullptr) {
138-
termsSizePtr->push_back(nc);
139-
}
140120
}
141121

142122
return quadraticApproximation;

‎ocs2_core/src/constraint/StateInputConstraintCollection.cpp‎

Lines changed: 4 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -79,30 +79,20 @@ vector_array_t StateInputConstraintCollection::getValue(scalar_t time, const vec
7979
/******************************************************************************************************/
8080
VectorFunctionLinearApproximation StateInputConstraintCollection::getLinearApproximation(scalar_t time, const vector_t& state,
8181
const vector_t& input,
82-
const PreComputation& preComp,
83-
size_array_t* termsSizePtr) const {
82+
const PreComputation& preComp) const {
8483
VectorFunctionLinearApproximation linearApproximation(getNumConstraints(time), state.rows(), input.rows());
8584

86-
if (termsSizePtr != nullptr) {
87-
termsSizePtr->clear();
88-
termsSizePtr->reserve(this->terms_.size());
89-
}
90-
9185
// append linearApproximation of each constraintTerm
9286
size_t i = 0;
9387
for (const auto& constraintTerm : this->terms_) {
94-
size_t nc = 0;
9588
if (constraintTerm->isActive(time)) {
9689
const auto constraintTermApproximation = constraintTerm->getLinearApproximation(time, state, input, preComp);
97-
nc = constraintTermApproximation.f.rows();
90+
const size_t nc = constraintTermApproximation.f.rows();
9891
linearApproximation.f.segment(i, nc) = constraintTermApproximation.f;
9992
linearApproximation.dfdx.middleRows(i, nc) = constraintTermApproximation.dfdx;
10093
linearApproximation.dfdu.middleRows(i, nc) = constraintTermApproximation.dfdu;
10194
i += nc;
10295
}
103-
if (termsSizePtr != nullptr) {
104-
termsSizePtr->push_back(nc);
105-
}
10696
}
10797

10898
return linearApproximation;
@@ -113,8 +103,7 @@ VectorFunctionLinearApproximation StateInputConstraintCollection::getLinearAppro
113103
/******************************************************************************************************/
114104
VectorFunctionQuadraticApproximation StateInputConstraintCollection::getQuadraticApproximation(scalar_t time, const vector_t& state,
115105
const vector_t& input,
116-
const PreComputation& preComp,
117-
size_array_t* termsSizePtr) const {
106+
const PreComputation& preComp) const {
118107
const auto numConstraints = getNumConstraints(time);
119108

120109
VectorFunctionQuadraticApproximation quadraticApproximation;
@@ -125,18 +114,12 @@ VectorFunctionQuadraticApproximation StateInputConstraintCollection::getQuadrati
125114
quadraticApproximation.dfdux.reserve(numConstraints);
126115
quadraticApproximation.dfduu.reserve(numConstraints);
127116

128-
if (termsSizePtr != nullptr) {
129-
termsSizePtr->clear();
130-
termsSizePtr->reserve(this->terms_.size());
131-
}
132-
133117
// append quadraticApproximation of each constraintTerm
134118
size_t i = 0;
135119
for (const auto& constraintTerm : this->terms_) {
136-
size_t nc = 0;
137120
if (constraintTerm->isActive(time)) {
138121
auto constraintTermApproximation = constraintTerm->getQuadraticApproximation(time, state, input, preComp);
139-
nc = constraintTermApproximation.f.rows();
122+
const size_t nc = constraintTermApproximation.f.rows();
140123
quadraticApproximation.f.segment(i, nc) = constraintTermApproximation.f;
141124
quadraticApproximation.dfdx.middleRows(i, nc) = constraintTermApproximation.dfdx;
142125
quadraticApproximation.dfdu.middleRows(i, nc) = constraintTermApproximation.dfdu;
@@ -145,9 +128,6 @@ VectorFunctionQuadraticApproximation StateInputConstraintCollection::getQuadrati
145128
appendVectorToVectorByMoving(quadraticApproximation.dfduu, std::move(constraintTermApproximation.dfduu));
146129
i += nc;
147130
}
148-
if (termsSizePtr != nullptr) {
149-
termsSizePtr->push_back(nc);
150-
}
151131
}
152132

153133
return quadraticApproximation;

‎ocs2_core/src/loopshaping/constraint/LoopshapingConstraintEliminatePattern.cpp‎

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -37,8 +37,7 @@ namespace ocs2 {
3737
/******************************************************************************************************/
3838
VectorFunctionLinearApproximation LoopshapingConstraintEliminatePattern::getLinearApproximation(scalar_t t, const vector_t& x,
3939
const vector_t& u,
40-
const PreComputation& preComp,
41-
size_array_t* termsSizePtr) const {
40+
const PreComputation& preComp) const {
4241
if (this->empty()) {
4342
return VectorFunctionLinearApproximation::Zero(0, x.rows(), u.rows());
4443
}
@@ -54,7 +53,7 @@ VectorFunctionLinearApproximation LoopshapingConstraintEliminatePattern::getLine
5453
const auto filtStateDim = s_filter.getNumStates();
5554

5655
// Not const so we can move
57-
auto g_system = StateInputConstraintCollection::getLinearApproximation(t, x_system, u_system, preComp_system, termsSizePtr);
56+
auto g_system = StateInputConstraintCollection::getLinearApproximation(t, x_system, u_system, preComp_system);
5857
const auto numConstraints = g_system.f.rows();
5958

6059
VectorFunctionLinearApproximation g;
@@ -84,8 +83,7 @@ VectorFunctionLinearApproximation LoopshapingConstraintEliminatePattern::getLine
8483
/******************************************************************************************************/
8584
VectorFunctionQuadraticApproximation LoopshapingConstraintEliminatePattern::getQuadraticApproximation(scalar_t t, const vector_t& x,
8685
const vector_t& u,
87-
const PreComputation& preComp,
88-
size_array_t* termsSizePtr) const {
86+
const PreComputation& preComp) const {
8987
if (this->empty()) {
9088
return VectorFunctionQuadraticApproximation::Zero(0, x.rows(), u.rows());
9189
}
@@ -103,7 +101,7 @@ VectorFunctionQuadraticApproximation LoopshapingConstraintEliminatePattern::getQ
103101
const auto filtStateDim = x_filter.rows();
104102

105103
// Not const so we can move
106-
auto h_system = StateInputConstraintCollection::getQuadraticApproximation(t, x_system, u_system, preComp_system, termsSizePtr);
104+
auto h_system = StateInputConstraintCollection::getQuadraticApproximation(t, x_system, u_system, preComp_system);
107105
const auto numConstraints = h_system.f.rows();
108106

109107
VectorFunctionQuadraticApproximation h;

‎ocs2_core/src/loopshaping/constraint/LoopshapingConstraintOutputPattern.cpp‎

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -37,8 +37,7 @@ namespace ocs2 {
3737
/******************************************************************************************************/
3838
VectorFunctionLinearApproximation LoopshapingConstraintOutputPattern::getLinearApproximation(scalar_t t, const vector_t& x,
3939
const vector_t& u,
40-
const PreComputation& preComp,
41-
size_array_t* termsSizePtr) const {
40+
const PreComputation& preComp) const {
4241
if (this->empty()) {
4342
return VectorFunctionLinearApproximation::Zero(0, x.rows(), u.rows());
4443
}
@@ -52,7 +51,7 @@ VectorFunctionLinearApproximation LoopshapingConstraintOutputPattern::getLinearA
5251
const auto filtStateDim = x.rows() - sysStateDim;
5352

5453
// Not const so we can move
55-
auto g_system = StateInputConstraintCollection::getLinearApproximation(t, x_system, u_system, preComp_system, termsSizePtr);
54+
auto g_system = StateInputConstraintCollection::getLinearApproximation(t, x_system, u_system, preComp_system);
5655
const auto numConstraints = g_system.f.rows();
5756

5857
VectorFunctionLinearApproximation g;
@@ -72,8 +71,7 @@ VectorFunctionLinearApproximation LoopshapingConstraintOutputPattern::getLinearA
7271
/******************************************************************************************************/
7372
VectorFunctionQuadraticApproximation LoopshapingConstraintOutputPattern::getQuadraticApproximation(scalar_t t, const vector_t& x,
7473
const vector_t& u,
75-
const PreComputation& preComp,
76-
size_array_t* termsSizePtr) const {
74+
const PreComputation& preComp) const {
7775
if (this->empty()) {
7876
return VectorFunctionQuadraticApproximation::Zero(0, x.rows(), u.rows());
7977
}
@@ -89,7 +87,7 @@ VectorFunctionQuadraticApproximation LoopshapingConstraintOutputPattern::getQuad
8987
const auto filtStateDim = x_filter.rows();
9088

9189
// Not const so we can move
92-
auto h_system = StateInputConstraintCollection::getQuadraticApproximation(t, x_system, u_system, preComp_system, termsSizePtr);
90+
auto h_system = StateInputConstraintCollection::getQuadraticApproximation(t, x_system, u_system, preComp_system);
9391
const auto numConstraints = h_system.f.rows();
9492

9593
VectorFunctionQuadraticApproximation h;

0 commit comments

Comments
 (0)