Skip to content

Commit 835b0c4

Browse files
rubengrandiarubengrandia
committed
Merge branch 'feature/parameters_with_precomputation' into feature/perception
2 parents aa8ea96 + d1cf6c7 commit 835b0c4

File tree

130 files changed

+6712
-869
lines changed

Some content is hidden

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

130 files changed

+6712
-869
lines changed

‎.github/workflows/ros-build-test.yml‎

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
name: Build and Test OCS2
2+
3+
on:
4+
push:
5+
branches: [ main ]
6+
pull_request:
7+
branches: [ main ]
8+
9+
jobs:
10+
build:
11+
12+
# build both Debug and Release mode
13+
strategy:
14+
fail-fast: false
15+
matrix:
16+
build_type: [ Debug, Release ]
17+
18+
# environment: regular Ubuntu with a vanilla ROS container
19+
runs-on: ubuntu-latest
20+
container:
21+
image: ros:noetic
22+
23+
steps:
24+
- name: Environment Info
25+
run: |
26+
pwd
27+
uname -r
28+
lsb_release -a
29+
30+
- name: System deps
31+
run: |
32+
apt-get update
33+
apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev
34+
35+
- uses: actions/checkout@v2
36+
with:
37+
path: src/ocs2
38+
39+
- name: Rosdep
40+
run: |
41+
rosdep update
42+
rosdep install --from-paths src --ignore-src -r -y
43+
44+
- name: Checkout dependencies
45+
run: |
46+
git clone --recurse-submodules https://github.com/leggedrobotics/pinocchio.git src/pinocchio
47+
git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git src/hpp-fcl
48+
git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git src/ocs2_robotic_assets
49+
- name: Build (${{ matrix.build_type }})
50+
shell: bash
51+
run: |
52+
source /opt/ros/noetic/setup.bash
53+
catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --ignore-pkg ocs2_raisim --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build_type }}
54+
55+
- name: Test
56+
shell: bash
57+
run: |
58+
source devel_isolated/setup.bash
59+
catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --catkin-make-args run_tests

‎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

0 commit comments

Comments
 (0)