Skip to content

Commit 6895abe

Browse files
farbod-farshidianfarbod-farshidian
committed
Merged in feature/sqp_with_metrics (pull request #652)
Feature/sqp with metrics Approved-by: Jean-Pierre Sleiman Approved-by: Sotaro Katayama
2 parents 0725c23 + ea9ac8b commit 6895abe

32 files changed

+531
-149
lines changed

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

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -49,10 +49,13 @@ class StateConstraintCollection : public Collection<StateConstraint> {
4949
~StateConstraintCollection() override = default;
5050
StateConstraintCollection* clone() const override;
5151

52-
/** Returns the number of active constraints at given time. */
53-
virtual size_t getNumConstraints(scalar_t time) const;
52+
/** Returns the number of active constraints at a given time. */
53+
size_t getNumConstraints(scalar_t time) const;
5454

55-
/** Get an array of all constraints */
55+
/** Returns the number of active constraints at a given time for each term. If a term is inactive, its size is zero. */
56+
size_array_t getTermsSize(scalar_t time) const;
57+
58+
/** Get an array of all constraints. If a term is inactive, the corresponding element is a vector of size zero. */
5659
virtual vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const;
5760

5861
/** Get the constraint linear approximation */

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

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -49,10 +49,13 @@ class StateInputConstraintCollection : public Collection<StateInputConstraint> {
4949
~StateInputConstraintCollection() override = default;
5050
StateInputConstraintCollection* clone() const override;
5151

52-
/** Returns the number of active constraints at given time. */
53-
virtual size_t getNumConstraints(scalar_t time) const;
52+
/** Returns the number of active constraints at a given time. */
53+
size_t getNumConstraints(scalar_t time) const;
5454

55-
/** Get an array of all constraints */
55+
/** Returns the number of active constraints at a given time for each term. If a term is inactive, its size is zero. */
56+
size_array_t getTermsSize(scalar_t time) const;
57+
58+
/** Get an array of all constraints. If a term is inactive, the corresponding element is a vector of size zero. */
5659
virtual vector_array_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const;
5760

5861
/** Get the constraint linear approximation */

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,7 @@ class LoopshapingStateConstraint final : public StateConstraintCollection {
5050
vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override;
5151
VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state,
5252
const PreComputation& preComp) const override;
53+
5354
VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state,
5455
const PreComputation& preComp) const override;
5556

‎ocs2_core/src/constraint/StateConstraintCollection.cpp

Lines changed: 13 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -48,17 +48,27 @@ StateConstraintCollection* StateConstraintCollection::clone() const {
4848
/******************************************************************************************************/
4949
size_t StateConstraintCollection::getNumConstraints(scalar_t time) const {
5050
size_t numConstraints = 0;
51-
52-
// accumulate number of constraints for each constraintTerm
5351
for (const auto& constraintTerm : this->terms_) {
5452
if (constraintTerm->isActive(time)) {
5553
numConstraints += constraintTerm->getNumConstraints(time);
5654
}
5755
}
58-
5956
return numConstraints;
6057
}
6158

59+
/******************************************************************************************************/
60+
/******************************************************************************************************/
61+
/******************************************************************************************************/
62+
size_array_t StateConstraintCollection::getTermsSize(scalar_t time) const {
63+
size_array_t termsSize(this->terms_.size(), 0);
64+
for (size_t i = 0; i < this->terms_.size(); ++i) {
65+
if (this->terms_[i]->isActive(time)) {
66+
termsSize[i] = this->terms_[i]->getNumConstraints(time);
67+
}
68+
}
69+
return termsSize;
70+
}
71+
6272
/******************************************************************************************************/
6373
/******************************************************************************************************/
6474
/******************************************************************************************************/

‎ocs2_core/src/constraint/StateInputConstraintCollection.cpp

Lines changed: 13 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -49,17 +49,27 @@ StateInputConstraintCollection* StateInputConstraintCollection::clone() const {
4949
/******************************************************************************************************/
5050
size_t StateInputConstraintCollection::getNumConstraints(scalar_t time) const {
5151
size_t numConstraints = 0;
52-
53-
// accumulate number of constraints for each constraintTerm
5452
for (const auto& constraintTerm : this->terms_) {
5553
if (constraintTerm->isActive(time)) {
5654
numConstraints += constraintTerm->getNumConstraints(time);
5755
}
5856
}
59-
6057
return numConstraints;
6158
}
6259

60+
/******************************************************************************************************/
61+
/******************************************************************************************************/
62+
/******************************************************************************************************/
63+
size_array_t StateInputConstraintCollection::getTermsSize(scalar_t time) const {
64+
size_array_t termsSize(this->terms_.size(), 0);
65+
for (size_t i = 0; i < this->terms_.size(); ++i) {
66+
if (this->terms_[i]->isActive(time)) {
67+
termsSize[i] = this->terms_[i]->getNumConstraints(time);
68+
}
69+
}
70+
return termsSize;
71+
}
72+
6373
/******************************************************************************************************/
6474
/******************************************************************************************************/
6575
/******************************************************************************************************/

‎ocs2_core/test/constraint/testConstraintCollection.cpp

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
2727
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
2828
******************************************************************************/
2929

30+
#include <numeric>
31+
3032
#include <gtest/gtest.h>
3133

3234
#include <ocs2_core/constraint/StateConstraintCollection.h>
@@ -56,6 +58,37 @@ TEST(TestConstraintCollection, numberOfConstraints) {
5658
EXPECT_EQ(constraintCollection.getNumConstraints(0.0), addedConstraints);
5759
}
5860

61+
TEST(TestConstraintCollection, termsSize) {
62+
ocs2::StateInputConstraintCollection constraintCollection;
63+
64+
// Initially we have zero constraints for all types
65+
auto termsSize = constraintCollection.getTermsSize(0.0);
66+
EXPECT_EQ(termsSize.size(), 0);
67+
EXPECT_EQ(std::accumulate(termsSize.begin(), termsSize.end(), 0), 0);
68+
69+
// Add 2 Linear inequality constraint term, which has 2 constraints
70+
constraintCollection.add("Constraint1", std::make_unique<TestDummyConstraint>());
71+
constraintCollection.add("Constraint2", std::make_unique<TestDummyConstraint>());
72+
constraintCollection.add("Constraint3", std::make_unique<TestDummyConstraint>());
73+
auto& constraint1 = constraintCollection.get<TestDummyConstraint>("Constraint1");
74+
const size_t constraint1Size = constraint1.getNumConstraints(0.0);
75+
76+
// Check the right constraint size
77+
termsSize = constraintCollection.getTermsSize(0.0);
78+
EXPECT_EQ(termsSize.size(), 3);
79+
if (termsSize.size() == 3) {
80+
EXPECT_EQ(termsSize[0], constraint1Size);
81+
EXPECT_EQ(termsSize[1], constraint1Size);
82+
EXPECT_EQ(termsSize[2], constraint1Size);
83+
}
84+
85+
// Deactivate constraint1
86+
constraint1.setActivity(false);
87+
termsSize = constraintCollection.getTermsSize(0.0);
88+
EXPECT_EQ(termsSize.size(), 3);
89+
EXPECT_EQ(std::accumulate(termsSize.begin(), termsSize.end(), 0), 2 * constraint1Size);
90+
}
91+
5992
TEST(TestConstraintCollection, activatingConstraints) {
6093
ocs2::StateInputConstraintCollection constraintCollection;
6194

‎ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -89,7 +89,7 @@ class GaussNewtonDDP : public SolverBase {
8989

9090
void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const final;
9191

92-
const DualSolution& getDualSolution() const override { return optimizedDualSolution_; }
92+
const DualSolution* getDualSolution() const override { return &optimizedDualSolution_; }
9393

9494
const ProblemMetrics& getSolutionMetrics() const override { return optimizedProblemMetrics_; }
9595

‎ocs2_ddp/src/DDP_HelperFunctions.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,7 @@ void computeRolloutMetrics(OptimalControlProblem& problem, const PrimalSolution&
7373
problemMetrics.intermediates.reserve(tTrajectory.size());
7474

7575
auto nextPostEventIndexItr = postEventIndices.begin();
76-
const auto request = Request::Cost + Request::Constraint + Request::SoftConstraint;
76+
constexpr auto request = Request::Cost + Request::Constraint + Request::SoftConstraint;
7777
for (size_t k = 0; k < tTrajectory.size(); k++) {
7878
// intermediate time cost and constraints
7979
problem.preComputationPtr->request(request, tTrajectory[k], xTrajectory[k], uTrajectory[k]);

‎ocs2_oc/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -132,6 +132,7 @@ install(DIRECTORY test/include/${PROJECT_NAME}/
132132

133133
catkin_add_gtest(test_${PROJECT_NAME}_multiple_shooting
134134
test/multiple_shooting/testProjectionMultiplierCoefficients.cpp
135+
test/multiple_shooting/testTranscriptionMetrics.cpp
135136
test/multiple_shooting/testTranscriptionPerformanceIndex.cpp
136137
)
137138
add_dependencies(test_${PROJECT_NAME}_multiple_shooting

‎ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
3333

3434
#include "ocs2_oc/oc_data/PerformanceIndex.h"
3535
#include "ocs2_oc/oc_data/PrimalSolution.h"
36+
#include "ocs2_oc/oc_data/ProblemMetrics.h"
3637
#include "ocs2_oc/oc_data/TimeDiscretization.h"
3738

3839
namespace ocs2 {
@@ -101,5 +102,14 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche
101102
PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u,
102103
matrix_array_t&& KMatrices);
103104

105+
/**
106+
* Constructs a ProblemMetrics from an array of metrics.
107+
*
108+
* @param [in] time : The annotated time trajectory
109+
* @param [in] metrics: The metrics array.
110+
* @return The ProblemMetrics.
111+
*/
112+
ProblemMetrics toProblemMetrics(const std::vector<AnnotatedTime>& time, std::vector<Metrics>&& metrics);
113+
104114
} // namespace multiple_shooting
105115
} // namespace ocs2

0 commit comments

Comments
 (0)