Skip to content

Commit 3fba74e

Browse files
mayatakafarbod-farshidian
mayataka
authored and
farbod-farshidian
committed
Merged in feature/interior_point_example (pull request #686)
add IPM legged robot example Approved-by: Farbod Farshidian
2 parents 922c649 + 1eac5b6 commit 3fba74e

File tree

13 files changed

+548
-17
lines changed

13 files changed

+548
-17
lines changed

‎ocs2_ipm/src/IpmHelpers.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -110,7 +110,7 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala
110110

111111
const vector_t invFractionToBoundary = (-1.0 / marginRate) * dv.cwiseQuotient(v);
112112
const auto alpha = invFractionToBoundary.maxCoeff();
113-
return alpha > 0.0? std::min(1.0 / alpha, 1.0): 1.0;
113+
return alpha > 0.0 ? std::min(1.0 / alpha, 1.0) : 1.0;
114114
}
115115

116116
} // namespace ipm

‎ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES
1010
ocs2_ddp
1111
ocs2_mpc
1212
ocs2_sqp
13+
ocs2_ipm
1314
ocs2_robotic_tools
1415
ocs2_pinocchio_interface
1516
ocs2_centroidal_model

‎ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,39 @@ sqp
4646
threadPriority 50
4747
}
4848

49+
; Multiple_Shooting IPM settings
50+
ipm
51+
{
52+
nThreads 3
53+
dt 0.015
54+
ipmIteration 1
55+
deltaTol 1e-4
56+
g_max 10.0
57+
g_min 1e-6
58+
computeLagrangeMultipliers true
59+
printSolverStatistics true
60+
printSolverStatus false
61+
printLinesearch false
62+
useFeedbackPolicy true
63+
integratorType RK2
64+
threadPriority 50
65+
66+
initialBarrierParameter 1e-4
67+
targetBarrierParameter 1e-4
68+
barrierLinearDecreaseFactor 0.2
69+
barrierSuperlinearDecreasePower 1.5
70+
barrierReductionCostTol 1e-3
71+
barrierReductionConstraintTol 1e-3
72+
73+
fractionToBoundaryMargin 0.995
74+
usePrimalStepSizeForDual false
75+
76+
initialSlackLowerBound 1e-4
77+
initialDualLowerBound 1e-4
78+
initialSlackMarginRate 1e-2
79+
initialDualMarginRate 1e-2
80+
}
81+
4982
; DDP settings
5083
ddp
5184
{

‎ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml

Lines changed: 312 additions & 0 deletions
Large diffs are not rendered by default.

‎ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/mpc_metrics.xml renamed to ‎ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/zero_velocity.xml

File renamed without changes.

‎ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h

Lines changed: 10 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
3434
#include <ocs2_core/Types.h>
3535
#include <ocs2_core/penalties/Penalties.h>
3636
#include <ocs2_ddp/DDP_Settings.h>
37+
#include <ocs2_ipm/IpmSettings.h>
3738
#include <ocs2_mpc/MPC_Settings.h>
3839
#include <ocs2_oc/rollout/TimeTriggeredRollout.h>
3940
#include <ocs2_pinocchio_interface/PinocchioInterface.h>
@@ -62,8 +63,10 @@ class LeggedRobotInterface final : public RobotInterface {
6263
* @param [in] taskFile: The absolute path to the configuration file for the MPC.
6364
* @param [in] urdfFile: The absolute path to the URDF file for the robot.
6465
* @param [in] referenceFile: The absolute path to the reference configuration file.
66+
* @param [in] useHardFrictionConeConstraint: Which to use hard or soft friction cone constraints.
6567
*/
66-
LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile);
68+
LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile,
69+
bool useHardFrictionConeConstraint = false);
6770

6871
~LeggedRobotInterface() override = default;
6972

@@ -74,6 +77,7 @@ class LeggedRobotInterface final : public RobotInterface {
7477
const mpc::Settings& mpcSettings() const { return mpcSettings_; }
7578
const rollout::Settings& rolloutSettings() const { return rolloutSettings_; }
7679
const sqp::Settings& sqpSettings() { return sqpSettings_; }
80+
const ipm::Settings& ipmSettings() { return ipmSettings_; }
7781

7882
const vector_t& getInitialState() const { return initialState_; }
7983
const RolloutBase& getRollout() const { return *rolloutPtr_; }
@@ -93,8 +97,9 @@ class LeggedRobotInterface final : public RobotInterface {
9397
matrix_t initializeInputCostWeight(const std::string& taskFile, const CentroidalModelInfo& info);
9498

9599
std::pair<scalar_t, RelaxedBarrierPenalty::Config> loadFrictionConeSettings(const std::string& taskFile, bool verbose) const;
96-
std::unique_ptr<StateInputCost> getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient,
97-
const RelaxedBarrierPenalty::Config& barrierPenaltyConfig);
100+
std::unique_ptr<StateInputConstraint> getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient);
101+
std::unique_ptr<StateInputCost> getFrictionConeSoftConstraint(size_t contactPointIndex, scalar_t frictionCoefficient,
102+
const RelaxedBarrierPenalty::Config& barrierPenaltyConfig);
98103
std::unique_ptr<StateInputConstraint> getZeroForceConstraint(size_t contactPointIndex);
99104
std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint(const EndEffectorKinematics<scalar_t>& eeKinematics,
100105
size_t contactPointIndex, bool useAnalyticalGradients);
@@ -105,6 +110,8 @@ class LeggedRobotInterface final : public RobotInterface {
105110
ddp::Settings ddpSettings_;
106111
mpc::Settings mpcSettings_;
107112
sqp::Settings sqpSettings_;
113+
ipm::Settings ipmSettings_;
114+
const bool useHardFrictionConeConstraint_;
108115

109116
std::unique_ptr<PinocchioInterface> pinocchioInterfacePtr_;
110117
CentroidalModelInfo centroidalModelInfo_;

‎ocs2_robotic_examples/ocs2_legged_robot/package.xml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
<depend>ocs2_ddp</depend>
1818
<depend>ocs2_mpc</depend>
1919
<depend>ocs2_sqp</depend>
20+
<depend>ocs2_ipm</depend>
2021
<depend>ocs2_robotic_assets</depend>
2122
<depend>ocs2_robotic_tools</depend>
2223
<depend>ocs2_pinocchio_interface</depend>

‎ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp

Lines changed: 24 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -64,7 +64,9 @@ namespace legged_robot {
6464
/******************************************************************************************************/
6565
/******************************************************************************************************/
6666
/******************************************************************************************************/
67-
LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile) {
67+
LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile,
68+
bool useHardFrictionConeConstraint)
69+
: useHardFrictionConeConstraint_(useHardFrictionConeConstraint) {
6870
// check that task file exists
6971
boost::filesystem::path taskFilePath(taskFile);
7072
if (boost::filesystem::exists(taskFilePath)) {
@@ -92,10 +94,11 @@ LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const st
9294

9395
// load setting from loading file
9496
modelSettings_ = loadModelSettings(taskFile, "model_settings", verbose);
95-
ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose);
9697
mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose);
97-
rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose);
98+
ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose);
9899
sqpSettings_ = sqp::loadSettings(taskFile, "sqp", verbose);
100+
ipmSettings_ = ipm::loadSettings(taskFile, "ipm", verbose);
101+
rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose);
99102

100103
// OptimalConrolProblem
101104
setupOptimalConrolProblem(taskFile, urdfFile, referenceFile, verbose);
@@ -174,8 +177,12 @@ void LeggedRobotInterface::setupOptimalConrolProblem(const std::string& taskFile
174177
modelSettings_.recompileLibrariesCppAd, modelSettings_.verboseCppAd));
175178
}
176179

177-
problemPtr_->softConstraintPtr->add(footName + "_frictionCone",
178-
getFrictionConeConstraint(i, frictionCoefficient, barrierPenaltyConfig));
180+
if (useHardFrictionConeConstraint_) {
181+
problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone", getFrictionConeConstraint(i, frictionCoefficient));
182+
} else {
183+
problemPtr_->softConstraintPtr->add(footName + "_frictionCone",
184+
getFrictionConeSoftConstraint(i, frictionCoefficient, barrierPenaltyConfig));
185+
}
179186
problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", getZeroForceConstraint(i));
180187
problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity",
181188
getZeroVelocityConstraint(*eeKinematicsPtr, i, useAnalyticalGradientsConstraints));
@@ -310,15 +317,20 @@ std::pair<scalar_t, RelaxedBarrierPenalty::Config> LeggedRobotInterface::loadFri
310317
/******************************************************************************************************/
311318
/******************************************************************************************************/
312319
/******************************************************************************************************/
313-
std::unique_ptr<StateInputCost> LeggedRobotInterface::getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient,
314-
const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) {
320+
std::unique_ptr<StateInputConstraint> LeggedRobotInterface::getFrictionConeConstraint(size_t contactPointIndex,
321+
scalar_t frictionCoefficient) {
315322
FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient);
316-
auto frictionConeConstraintPtr = std::make_unique<FrictionConeConstraint>(*referenceManagerPtr_, std::move(frictionConeConConfig),
317-
contactPointIndex, centroidalModelInfo_);
318-
319-
auto penalty = std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig);
323+
return std::make_unique<FrictionConeConstraint>(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex,
324+
centroidalModelInfo_);
325+
}
320326

321-
return std::make_unique<StateInputSoftConstraint>(std::move(frictionConeConstraintPtr), std::move(penalty));
327+
/******************************************************************************************************/
328+
/******************************************************************************************************/
329+
/******************************************************************************************************/
330+
std::unique_ptr<StateInputCost> LeggedRobotInterface::getFrictionConeSoftConstraint(
331+
size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) {
332+
return std::make_unique<StateInputSoftConstraint>(getFrictionConeConstraint(contactPointIndex, frictionCoefficient),
333+
std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig));
322334
}
323335

324336
/******************************************************************************************************/

‎ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES
1616
ocs2_ddp
1717
ocs2_mpc
1818
ocs2_sqp
19+
ocs2_ipm
1920
ocs2_robotic_tools
2021
ocs2_pinocchio_interface
2122
ocs2_centroidal_model
@@ -118,6 +119,21 @@ target_link_libraries(legged_robot_sqp_mpc
118119
)
119120
target_compile_options(legged_robot_sqp_mpc PRIVATE ${OCS2_CXX_FLAGS})
120121

122+
## IPM-MPC node for legged robot
123+
add_executable(legged_robot_ipm_mpc
124+
src/LeggedRobotIpmMpcNode.cpp
125+
)
126+
add_dependencies(legged_robot_ipm_mpc
127+
${PROJECT_NAME}
128+
${${PROJECT_NAME}_EXPORTED_TARGETS}
129+
${catkin_EXPORTED_TARGETS}
130+
)
131+
target_link_libraries(legged_robot_ipm_mpc
132+
${PROJECT_NAME}
133+
${catkin_LIBRARIES}
134+
)
135+
target_compile_options(legged_robot_ipm_mpc PRIVATE ${OCS2_CXX_FLAGS})
136+
121137
# Dummy node
122138
add_executable(legged_robot_dummy
123139
src/LeggedRobotDummyNode.cpp
@@ -174,6 +190,7 @@ if(cmake_clang_tools_FOUND)
174190
${PROJECT_NAME}
175191
legged_robot_ddp_mpc
176192
legged_robot_sqp_mpc
193+
legged_robot_ipm_mpc
177194
legged_robot_dummy
178195
legged_robot_target
179196
legged_robot_gait_command
@@ -198,6 +215,7 @@ install(
198215
TARGETS
199216
legged_robot_ddp_mpc
200217
legged_robot_sqp_mpc
218+
legged_robot_ipm_mpc
201219
legged_robot_dummy
202220
legged_robot_target
203221
legged_robot_gait_command

‎ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
<?xml version="1.0" ?>
2+
3+
<launch>
4+
<!-- visualization config -->
5+
<arg name="rviz" default="true" />
6+
<arg name="description_name" default="legged_robot_description"/>
7+
<arg name="multiplot" default="false"/>
8+
9+
<!-- The task file for the mpc. -->
10+
<arg name="taskFile" default="$(find ocs2_legged_robot)/config/mpc/task.info"/>
11+
<!-- The reference related config file of the robot -->
12+
<arg name="referenceFile" default="$(find ocs2_legged_robot)/config/command/reference.info"/>
13+
<!-- The URDF model of the robot -->
14+
<arg name="urdfFile" default="$(find ocs2_robotic_assets)/resources/anymal_c/urdf/anymal.urdf"/>
15+
<!-- The file defining gait definition -->
16+
<arg name="gaitCommandFile" default="$(find ocs2_legged_robot)/config/command/gait.info"/>
17+
18+
<!-- rviz -->
19+
<group if="$(arg rviz)">
20+
<param name="$(arg description_name)" textfile="$(arg urdfFile)"/>
21+
<arg name="rvizconfig" default="$(find ocs2_legged_robot_ros)/rviz/legged_robot.rviz" />
22+
<node pkg="rviz" type="rviz" name="rviz" args="-d $(arg rvizconfig)" output="screen" />
23+
</group>
24+
25+
<!-- multiplot -->
26+
<group if="$(arg multiplot)">
27+
<include file="$(find ocs2_legged_robot_ros)/launch/multiplot.launch">
28+
<arg name="metrics_config" value="$(find ocs2_legged_robot)/config/multiplot/friction_cone.xml" />
29+
</include>
30+
</group>
31+
32+
<!-- make the files into global parameters -->
33+
<param name="multiplot" value="$(arg multiplot)"/>
34+
<param name="taskFile" value="$(arg taskFile)" />
35+
<param name="referenceFile" value="$(arg referenceFile)" />
36+
<param name="urdfFile" value="$(arg urdfFile)" />
37+
<param name="gaitCommandFile" value="$(arg gaitCommandFile)"/>
38+
39+
<node pkg="ocs2_legged_robot_ros" type="legged_robot_ipm_mpc" name="legged_robot_ipm_mpc"
40+
output="screen" launch-prefix=""/>
41+
42+
<node pkg="ocs2_legged_robot_ros" type="legged_robot_dummy" name="legged_robot_dummy"
43+
output="screen" launch-prefix="gnome-terminal --"/>
44+
45+
<node pkg="ocs2_legged_robot_ros" type="legged_robot_target" name="legged_robot_target"
46+
output="screen" launch-prefix="gnome-terminal --"/>
47+
48+
<node pkg="ocs2_legged_robot_ros" type="legged_robot_gait_command" name="legged_robot_gait_command"
49+
output="screen" launch-prefix="gnome-terminal --"/>
50+
</launch>

0 commit comments

Comments
 (0)