Skip to content

Commit b24c871

Browse files
farbod-farshidianfarbod-farshidian
committed
Merged in feature/al_definition (pull request #578)
feature/al_definition Approved-by: Jean-Pierre Sleiman Approved-by: Ruben Grandia
2 parents 3566993 + bbc0312 commit b24c871

File tree

109 files changed

+6536
-757
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

109 files changed

+6536
-757
lines changed

‎ocs2_core/CMakeLists.txt‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,11 @@ include_directories(
6767
# Declare a C++ library
6868
add_library(${PROJECT_NAME}
6969
src/Types.cpp
70+
src/augmented_lagrangian/AugmentedLagrangian.cpp
71+
src/augmented_lagrangian/StateAugmentedLagrangian.cpp
72+
src/augmented_lagrangian/StateInputAugmentedLagrangian.cpp
73+
src/augmented_lagrangian/StateAugmentedLagrangianCollection.cpp
74+
src/augmented_lagrangian/StateInputAugmentedLagrangianCollection.cpp
7075
src/automatic_differentation/CppAdInterface.cpp
7176
src/automatic_differentation/CppAdSparsity.cpp
7277
src/automatic_differentation/FiniteDifferenceMethods.cpp
@@ -116,6 +121,11 @@ add_library(${PROJECT_NAME}
116121
src/loopshaping/soft_constraint/LoopshapingStateInputSoftConstraint.cpp
117122
src/loopshaping/soft_constraint/LoopshapingSoftConstraintEliminatePattern.cpp
118123
src/loopshaping/soft_constraint/LoopshapingSoftConstraintOutputPattern.cpp
124+
src/loopshaping/augmented_lagrangian/LoopshapingAugmentedLagrangian.cpp
125+
src/loopshaping/augmented_lagrangian/LoopshapingStateAugmentedLagrangian.cpp
126+
src/loopshaping/augmented_lagrangian/LoopshapingStateInputAugmentedLagrangian.cpp
127+
src/loopshaping/augmented_lagrangian/LoopshapingAugmentedLagrangianEliminatePattern.cpp
128+
src/loopshaping/augmented_lagrangian/LoopshapingAugmentedLagrangianOutputPattern.cpp
119129
src/loopshaping/constraint/LoopshapingConstraint.cpp
120130
src/loopshaping/constraint/LoopshapingStateConstraint.cpp
121131
src/loopshaping/constraint/LoopshapingStateInputConstraint.cpp
@@ -127,12 +137,15 @@ add_library(${PROJECT_NAME}
127137
src/loopshaping/dynamics/LoopshapingFilterDynamics.cpp
128138
src/loopshaping/initialization/LoopshapingInitializer.cpp
129139
src/model_data/ModelData.cpp
140+
src/model_data/Metrics.cpp
141+
src/model_data/Multiplier.cpp
130142
src/misc/LinearAlgebra.cpp
131143
src/misc/Log.cpp
132144
src/soft_constraint/StateSoftConstraint.cpp
133145
src/soft_constraint/StateInputSoftConstraint.cpp
134146
src/soft_constraint/StateInputSoftBoxConstraint.cpp
135147
src/penalties/MultidimensionalPenalty.cpp
148+
src/penalties/Penalties.cpp
136149
src/penalties/penalties/RelaxedBarrierPenalty.cpp
137150
src/penalties/penalties/SquaredHingePenalty.cpp
138151
src/thread_support/ThreadPool.cpp
@@ -263,6 +276,7 @@ target_link_libraries(test_transferfunctionbase
263276

264277
catkin_add_gtest(${PROJECT_NAME}_loopshaping
265278
test/loopshaping/testLoopshapingConfiguration.cpp
279+
test/loopshaping/testLoopshapingAugmentedLagrangian.cpp
266280
test/loopshaping/testLoopshapingConstraint.cpp
267281
test/loopshaping/testLoopshapingCost.cpp
268282
test/loopshaping/testLoopshapingSoftConstraint.cpp
@@ -327,6 +341,24 @@ target_link_libraries(test_constraint
327341
gtest_main
328342
)
329343

344+
catkin_add_gtest(test_metrics
345+
test/model_data/testMetrics.cpp
346+
)
347+
target_link_libraries(test_metrics
348+
${PROJECT_NAME}
349+
${catkin_LIBRARIES}
350+
gtest_main
351+
)
352+
353+
catkin_add_gtest(test_multiplier
354+
test/model_data/testMultiplier.cpp
355+
)
356+
target_link_libraries(test_multiplier
357+
${PROJECT_NAME}
358+
${catkin_LIBRARIES}
359+
gtest_main
360+
)
361+
330362
catkin_add_gtest(test_ModelData
331363
test/model_data/testModelData.cpp
332364
)

‎ocs2_core/include/ocs2_core/augmented_lagrangian/AugmentedLagrangian.h‎

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
/******************************************************************************
2+
Copyright (c) 2020, Farbod Farshidian. All rights reserved.
3+
4+
Redistribution and use in source and binary forms, with or without
5+
modification, are permitted provided that the following conditions are met:
6+
7+
* Redistributions of source code must retain the above copyright notice, this
8+
list of conditions and the following disclaimer.
9+
10+
* Redistributions in binary form must reproduce the above copyright notice,
11+
this list of conditions and the following disclaimer in the documentation
12+
and/or other materials provided with the distribution.
13+
14+
* Neither the name of the copyright holder nor the names of its
15+
contributors may be used to endorse or promote products derived from
16+
this software without specific prior written permission.
17+
18+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22+
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23+
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24+
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25+
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26+
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27+
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28+
******************************************************************************/
29+
30+
#pragma once
31+
32+
#include <memory>
33+
34+
#include <ocs2_core/augmented_lagrangian/StateAugmentedLagrangian.h>
35+
#include <ocs2_core/augmented_lagrangian/StateInputAugmentedLagrangian.h>
36+
37+
/**
38+
* Helper factory functions
39+
*/
40+
namespace ocs2 {
41+
42+
/**
43+
* Factory function for state augmented Lagrangian.
44+
* @param [in] constraintPtr: A pointer to the constraint which will be enforced as soft constraints.
45+
* @param [in] penaltyPtrArray: An array of pointers to the penalty function on the constraint.
46+
*/
47+
std::unique_ptr<StateAugmentedLagrangian> create(std::unique_ptr<StateConstraint> constraintPtr,
48+
std::vector<std::unique_ptr<augmented::AugmentedPenaltyBase>> penaltyPtrArray);
49+
50+
/**
51+
* Factory function for state augmented Lagrangian.
52+
* @param [in] constraintPtr: A pointer to the constraint which will be enforced as soft constraints.
53+
* @param [in] penaltyPtr: A pointer to the penalty function on the constraint.
54+
*/
55+
std::unique_ptr<StateAugmentedLagrangian> create(std::unique_ptr<StateConstraint> constraintPtr,
56+
std::unique_ptr<augmented::AugmentedPenaltyBase> penaltyPtr);
57+
58+
/**
59+
* Factory function for state-input augmented Lagrangian.
60+
* @param [in] constraintPtr: A pointer to the constraint which will be enforced as soft constraints.
61+
* @param [in] penaltyPtrArray: An array of pointers to the penalty function on the constraint.
62+
*/
63+
std::unique_ptr<StateInputAugmentedLagrangian> create(std::unique_ptr<StateInputConstraint> constraintPtr,
64+
std::vector<std::unique_ptr<augmented::AugmentedPenaltyBase>> penaltyPtrArray);
65+
66+
/**
67+
* Factory function for state-input augmented Lagrangian.
68+
* @param [in] constraintPtr: A pointer to the constraint which will be enforced as soft constraints.
69+
* @param [in] penaltyPtr: A pointer to the penalty function on the constraint.
70+
*/
71+
std::unique_ptr<StateInputAugmentedLagrangian> create(std::unique_ptr<StateInputConstraint> constraintPtr,
72+
std::unique_ptr<augmented::AugmentedPenaltyBase> penaltyPtr);
73+
74+
} // namespace ocs2

‎ocs2_core/include/ocs2_core/augmented_lagrangian/StateAugmentedLagrangian.h‎

Lines changed: 87 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
/******************************************************************************
2+
Copyright (c) 2020, Farbod Farshidian. All rights reserved.
3+
4+
Redistribution and use in source and binary forms, with or without
5+
modification, are permitted provided that the following conditions are met:
6+
7+
* Redistributions of source code must retain the above copyright notice, this
8+
list of conditions and the following disclaimer.
9+
10+
* Redistributions in binary form must reproduce the above copyright notice,
11+
this list of conditions and the following disclaimer in the documentation
12+
and/or other materials provided with the distribution.
13+
14+
* Neither the name of the copyright holder nor the names of its
15+
contributors may be used to endorse or promote products derived from
16+
this software without specific prior written permission.
17+
18+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22+
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23+
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24+
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25+
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26+
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27+
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28+
******************************************************************************/
29+
30+
#pragma once
31+
32+
#include <memory>
33+
34+
#include <ocs2_core/augmented_lagrangian/StateAugmentedLagrangianInterface.h>
35+
#include <ocs2_core/penalties/MultidimensionalPenalty.h>
36+
37+
namespace ocs2 {
38+
39+
/** The base class for Augmented Lagrangian penalty of state constraint. */
40+
class StateAugmentedLagrangian final : public StateAugmentedLagrangianInterface {
41+
public:
42+
/**
43+
* Constructor.
44+
* @param [in] constraintPtr: A pointer to the constraint which will be enforced as soft constraints.
45+
* @param [in] penaltyPtrArray: An array of pointers to the penalty function on the constraint.
46+
*/
47+
StateAugmentedLagrangian(std::unique_ptr<StateConstraint> constraintPtr,
48+
std::vector<std::unique_ptr<augmented::AugmentedPenaltyBase>> penaltyPtrArray);
49+
50+
/**
51+
* Constructor.
52+
* @note This allows a varying number of constraints and uses the same penalty function for each constraint.
53+
* @param [in] constraintPtr: A pointer to the constraint which will be enforced as soft constraints.
54+
* @param [in] penaltyPtr: A pointer to the penalty function on the constraint.
55+
*/
56+
StateAugmentedLagrangian(std::unique_ptr<StateConstraint> constraintPtr, std::unique_ptr<augmented::AugmentedPenaltyBase> penaltyPtr);
57+
58+
StateAugmentedLagrangian* clone() const override;
59+
bool isActive(scalar_t time) const override;
60+
size_t getNumConstraints(scalar_t time) const override;
61+
62+
LagrangianMetrics getValue(scalar_t time, const vector_t& state, const Multiplier& multiplier,
63+
const PreComputation& preComp) const override;
64+
65+
ScalarFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const Multiplier& multiplier,
66+
const PreComputation& preComp) const override;
67+
68+
std::pair<Multiplier, scalar_t> updateLagrangian(scalar_t time, const vector_t& state, const vector_t& constraint,
69+
const Multiplier& multiplier) const override;
70+
71+
Multiplier initializeLagrangian(scalar_t time) const override;
72+
73+
/** Gets the wrapped constraint. */
74+
template <typename Derived = StateConstraint>
75+
Derived& get() {
76+
static_assert(std::is_base_of<StateConstraint, Derived>::value, "Template argument must derive from StateConstraint!");
77+
return dynamic_cast<Derived&>(*constraintPtr_);
78+
}
79+
80+
private:
81+
StateAugmentedLagrangian(const StateAugmentedLagrangian& other);
82+
83+
std::unique_ptr<StateConstraint> constraintPtr_;
84+
MultidimensionalPenalty penalty_;
85+
};
86+
87+
} // namespace ocs2

‎ocs2_core/include/ocs2_core/augmented_lagrangian/StateAugmentedLagrangianCollection.h‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
/******************************************************************************
2+
Copyright (c) 2020, Farbod Farshidian. All rights reserved.
3+
4+
Redistribution and use in source and binary forms, with or without
5+
modification, are permitted provided that the following conditions are met:
6+
7+
* Redistributions of source code must retain the above copyright notice, this
8+
list of conditions and the following disclaimer.
9+
10+
* Redistributions in binary form must reproduce the above copyright notice,
11+
this list of conditions and the following disclaimer in the documentation
12+
and/or other materials provided with the distribution.
13+
14+
* Neither the name of the copyright holder nor the names of its
15+
contributors may be used to endorse or promote products derived from
16+
this software without specific prior written permission.
17+
18+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22+
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23+
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24+
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25+
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26+
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27+
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28+
******************************************************************************/
29+
30+
#pragma once
31+
32+
#include <functional>
33+
34+
#include <ocs2_core/Types.h>
35+
#include <ocs2_core/misc/Collection.h>
36+
37+
#include "ocs2_core/augmented_lagrangian/StateAugmentedLagrangianInterface.h"
38+
39+
namespace ocs2 {
40+
41+
/**
42+
* State Augmented Lagrangian penalty class combining a collection of constraint terms.
43+
*
44+
* This class collects a variable number of Augmented Lagrangian penalty terms and provides methods to get the
45+
* summed values and quadratic approximations. Each term can be accessed through its string name.
46+
*/
47+
class StateAugmentedLagrangianCollection : public Collection<StateAugmentedLagrangianInterface> {
48+
public:
49+
StateAugmentedLagrangianCollection() = default;
50+
~StateAugmentedLagrangianCollection() override = default;
51+
StateAugmentedLagrangianCollection* clone() const override;
52+
53+
/** Get total number of active constraints. */
54+
size_t getNumberOfActiveConstraints(scalar_t time) const;
55+
56+
/** Get state constraints and their penalties for each active term */
57+
virtual std::vector<LagrangianMetrics> getValue(scalar_t time, const vector_t& state, const std::vector<Multiplier>& termsMultiplier,
58+
const PreComputation& preComp) const;
59+
60+
/** Get the sum of state Lagrangian penalties quadratic approximation */
61+
virtual ScalarFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state,
62+
const std::vector<Multiplier>& termsMultiplier,
63+
const PreComputation& preComp) const;
64+
65+
/** Update Lagrange/penalty multipliers, and the penalty value for each active term. */
66+
virtual void updateLagrangian(scalar_t time, const vector_t& state, std::vector<LagrangianMetrics>& termsMetrics,
67+
std::vector<Multiplier>& termsMultiplier) const;
68+
69+
/** Initialize Lagrange/penalty multipliers for each active term. */
70+
void initializeLagrangian(scalar_t time, std::vector<Multiplier>& termsMultiplier) const;
71+
72+
protected:
73+
StateAugmentedLagrangianCollection(const StateAugmentedLagrangianCollection& other) = default;
74+
};
75+
76+
} // namespace ocs2

‎ocs2_core/include/ocs2_core/augmented_lagrangian/StateAugmentedLagrangianInterface.h‎

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
/******************************************************************************
2+
Copyright (c) 2020, Farbod Farshidian. All rights reserved.
3+
4+
Redistribution and use in source and binary forms, with or without
5+
modification, are permitted provided that the following conditions are met:
6+
7+
* Redistributions of source code must retain the above copyright notice, this
8+
list of conditions and the following disclaimer.
9+
10+
* Redistributions in binary form must reproduce the above copyright notice,
11+
this list of conditions and the following disclaimer in the documentation
12+
and/or other materials provided with the distribution.
13+
14+
* Neither the name of the copyright holder nor the names of its
15+
contributors may be used to endorse or promote products derived from
16+
this software without specific prior written permission.
17+
18+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22+
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23+
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24+
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25+
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26+
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27+
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28+
******************************************************************************/
29+
30+
#pragma once
31+
32+
#include <ocs2_core/PreComputation.h>
33+
#include <ocs2_core/Types.h>
34+
#include <ocs2_core/constraint/StateConstraint.h>
35+
#include <ocs2_core/model_data/Metrics.h>
36+
#include <ocs2_core/model_data/Multiplier.h>
37+
38+
namespace ocs2 {
39+
40+
/** The base class for Augmented Lagrangian penalty of state constraint. */
41+
class StateAugmentedLagrangianInterface {
42+
public:
43+
StateAugmentedLagrangianInterface() = default;
44+
virtual ~StateAugmentedLagrangianInterface() = default;
45+
virtual StateAugmentedLagrangianInterface* clone() const = 0;
46+
47+
/** Check penalty's activity */
48+
virtual bool isActive(scalar_t time) const = 0;
49+
50+
/** Get the size of the constraint vector at given time */
51+
virtual size_t getNumConstraints(scalar_t time) const = 0;
52+
53+
/** Get the constraint and its penalty value */
54+
virtual LagrangianMetrics getValue(scalar_t time, const vector_t& state, const Multiplier& multiplier,
55+
const PreComputation& preComp) const = 0;
56+
57+
/** Get the constraint's penalty quadratic approximation */
58+
virtual ScalarFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const Multiplier& multiplier,
59+
const PreComputation& preComp) const = 0;
60+
61+
/** Update Lagrange/penalty multipliers and the penalty function value. */
62+
virtual std::pair<Multiplier, scalar_t> updateLagrangian(scalar_t time, const vector_t& state, const vector_t& constraint,
63+
const Multiplier& multiplier) const = 0;
64+
65+
/** Initialize Lagrange/penalty multipliers. */
66+
virtual Multiplier initializeLagrangian(scalar_t time) const = 0;
67+
68+
protected:
69+
StateAugmentedLagrangianInterface(const StateAugmentedLagrangianInterface& rhs) = default;
70+
};
71+
72+
} // namespace ocs2

0 commit comments

Comments
 (0)