From 3526f950e9b80478073c803101888ada7363389b Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Wed, 14 Dec 2022 19:41:57 +0100 Subject: [PATCH 1/9] add IPM legged robot example --- .../ocs2_legged_robot/CMakeLists.txt | 1 + .../ocs2_legged_robot/config/mpc/task.info | 36 +++++++ .../ocs2_legged_robot/LeggedRobotInterface.h | 13 ++- .../ocs2_legged_robot/package.xml | 1 + .../src/LeggedRobotInterface.cpp | 27 +++++- .../ocs2_legged_robot_ros/CMakeLists.txt | 18 ++++ .../launch/legged_robot_ipm.launch | 48 ++++++++++ .../ocs2_legged_robot_ros/package.xml | 1 + .../src/LeggedRobotIpmMpcNode.cpp | 96 +++++++++++++++++++ 9 files changed, 233 insertions(+), 8 deletions(-) create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp diff --git a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt index 8a19765f3..c6d07ed02 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt @@ -10,6 +10,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_ipm ocs2_robotic_tools ocs2_pinocchio_interface ocs2_centroidal_model diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index a6047bd97..40317d367 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -46,6 +46,42 @@ sqp threadPriority 50 } +; Multiple_Shooting IPM settings +ipm +{ + nThreads 3 + dt 0.015 + sqpIteration 1 + deltaTol 1e-4 + g_max 10.0 + g_min 1e-4 + inequalityConstraintMu 0.1 + inequalityConstraintDelta 5.0 + projectStateInputEqualityConstraints true + computeLagrangeMultipliers false + printSolverStatistics true + printSolverStatus false + printLinesearch false + useFeedbackPolicy true + integratorType RK2 + threadPriority 50 + + initialBarrierParameter 1e-4 + targetBarrierParameter 1e-4 + barrierLinearDecreaseFactor 0.2 + barrierSuperlinearDecreasePower 1.5 + barrierReductionCostTol 1e-3 + barrierReductionConstraintTol 1e-3 + + fractionToBoundaryMargin 0.995 + usePrimalStepSizeForDual false + + initialSlackLowerBound 1e-4 + initialDualLowerBound 1e-4 + initialSlackMarginRate 1e-2 + initialDualMarginRate 1e-2 +} + ; DDP settings ddp { diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h index 410d9e2d9..cc9ed6f18 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -62,8 +63,10 @@ class LeggedRobotInterface final : public RobotInterface { * @param [in] taskFile: The absolute path to the configuration file for the MPC. * @param [in] urdfFile: The absolute path to the URDF file for the robot. * @param [in] referenceFile: The absolute path to the reference configuration file. + * @param [in] useHardFrictionConeConstraint: Which to use hard or soft friction cone constraints. */ - LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile); + LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile, + bool useHardFrictionConeConstraint = false); ~LeggedRobotInterface() override = default; @@ -74,6 +77,7 @@ class LeggedRobotInterface final : public RobotInterface { const mpc::Settings& mpcSettings() const { return mpcSettings_; } const rollout::Settings& rolloutSettings() const { return rolloutSettings_; } const sqp::Settings& sqpSettings() { return sqpSettings_; } + const ipm::Settings& ipmSettings() { return ipmSettings_; } const vector_t& getInitialState() const { return initialState_; } const RolloutBase& getRollout() const { return *rolloutPtr_; } @@ -93,8 +97,9 @@ class LeggedRobotInterface final : public RobotInterface { matrix_t initializeInputCostWeight(const std::string& taskFile, const CentroidalModelInfo& info); std::pair loadFrictionConeSettings(const std::string& taskFile, bool verbose) const; - std::unique_ptr getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, - const RelaxedBarrierPenalty::Config& barrierPenaltyConfig); + std::unique_ptr getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient); + std::unique_ptr getFrictionConeSoftConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, + const RelaxedBarrierPenalty::Config& barrierPenaltyConfig); std::unique_ptr getZeroForceConstraint(size_t contactPointIndex); std::unique_ptr getZeroVelocityConstraint(const EndEffectorKinematics& eeKinematics, size_t contactPointIndex, bool useAnalyticalGradients); @@ -105,6 +110,8 @@ class LeggedRobotInterface final : public RobotInterface { ddp::Settings ddpSettings_; mpc::Settings mpcSettings_; sqp::Settings sqpSettings_; + ipm::Settings ipmSettings_; + bool useHardFrictionConeConstraint_; std::unique_ptr pinocchioInterfacePtr_; CentroidalModelInfo centroidalModelInfo_; diff --git a/ocs2_robotic_examples/ocs2_legged_robot/package.xml b/ocs2_robotic_examples/ocs2_legged_robot/package.xml index bb62d1e9d..a9f76f9dd 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot/package.xml @@ -17,6 +17,7 @@ ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_ipm ocs2_robotic_assets ocs2_robotic_tools ocs2_pinocchio_interface diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp index 2e0a6a389..14c3fcc43 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp @@ -64,7 +64,8 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile) { +LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile, + bool useHardFrictionConeConstraint) { // check that task file exists boost::filesystem::path taskFilePath(taskFile); if (boost::filesystem::exists(taskFilePath)) { @@ -96,6 +97,8 @@ LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const st mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose); rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose); sqpSettings_ = sqp::loadSettings(taskFile, "sqp", verbose); + ipmSettings_ = ipm::loadSettings(taskFile, "ipm", verbose); + useHardFrictionConeConstraint_ = useHardFrictionConeConstraint; // OptimalConrolProblem setupOptimalConrolProblem(taskFile, urdfFile, referenceFile, verbose); @@ -174,8 +177,12 @@ void LeggedRobotInterface::setupOptimalConrolProblem(const std::string& taskFile modelSettings_.recompileLibrariesCppAd, modelSettings_.verboseCppAd)); } - problemPtr_->softConstraintPtr->add(footName + "_frictionCone", - getFrictionConeConstraint(i, frictionCoefficient, barrierPenaltyConfig)); + if (useHardFrictionConeConstraint_) { + problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone", getFrictionConeConstraint(i, frictionCoefficient)); + } else { + problemPtr_->softConstraintPtr->add(footName + "_frictionCone", + getFrictionConeSoftConstraint(i, frictionCoefficient, barrierPenaltyConfig)); + } problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", getZeroForceConstraint(i)); problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity", getZeroVelocityConstraint(*eeKinematicsPtr, i, useAnalyticalGradientsConstraints)); @@ -310,8 +317,18 @@ std::pair LeggedRobotInterface::loadFri /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -std::unique_ptr LeggedRobotInterface::getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, - const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) { +std::unique_ptr LeggedRobotInterface::getFrictionConeConstraint(size_t contactPointIndex, + scalar_t frictionCoefficient) { + FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); + return std::make_unique(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex, + centroidalModelInfo_); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::unique_ptr LeggedRobotInterface::getFrictionConeSoftConstraint( + size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) { FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); auto frictionConeConstraintPtr = std::make_unique(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex, centroidalModelInfo_); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt index d5c30446e..a8819c8f8 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt @@ -16,6 +16,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_ipm ocs2_robotic_tools ocs2_pinocchio_interface ocs2_centroidal_model @@ -118,6 +119,21 @@ target_link_libraries(legged_robot_sqp_mpc ) target_compile_options(legged_robot_sqp_mpc PRIVATE ${OCS2_CXX_FLAGS}) +## IPM-MPC node for legged robot +add_executable(legged_robot_ipm_mpc + src/LeggedRobotIpmMpcNode.cpp +) +add_dependencies(legged_robot_ipm_mpc + ${PROJECT_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(legged_robot_ipm_mpc + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +target_compile_options(legged_robot_ipm_mpc PRIVATE ${OCS2_CXX_FLAGS}) + # Dummy node add_executable(legged_robot_dummy src/LeggedRobotDummyNode.cpp @@ -174,6 +190,7 @@ if(cmake_clang_tools_FOUND) ${PROJECT_NAME} legged_robot_ddp_mpc legged_robot_sqp_mpc + legged_robot_ipm_mpc legged_robot_dummy legged_robot_target legged_robot_gait_command @@ -198,6 +215,7 @@ install( TARGETS legged_robot_ddp_mpc legged_robot_sqp_mpc + legged_robot_ipm_mpc legged_robot_dummy legged_robot_target legged_robot_gait_command diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch new file mode 100644 index 000000000..9744aee4f --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml index aa2943272..7bc71c34e 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml @@ -24,6 +24,7 @@ ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_ipm ocs2_robotic_tools ocs2_pinocchio_interface ocs2_centroidal_model diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp new file mode 100644 index 000000000..30a061779 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp @@ -0,0 +1,96 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include + +#include "ocs2_legged_robot_ros/gait/GaitReceiver.h" + +using namespace ocs2; +using namespace legged_robot; + +int main(int argc, char** argv) { + const std::string robotName = "legged_robot"; + + // Initialize ros node + ::ros::init(argc, argv, robotName + "_mpc"); + ::ros::NodeHandle nodeHandle; + // Get node parameters + bool multiplot = false; + std::string taskFile, urdfFile, referenceFile; + nodeHandle.getParam("/multiplot", multiplot); + nodeHandle.getParam("/taskFile", taskFile); + nodeHandle.getParam("/urdfFile", urdfFile); + nodeHandle.getParam("/referenceFile", referenceFile); + + // Robot interface + constexpr bool useHardFrictionConeConstraint = true; + LeggedRobotInterface interface(taskFile, urdfFile, referenceFile, useHardFrictionConeConstraint); + + // Gait receiver + auto gaitReceiverPtr = + std::make_shared(nodeHandle, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), robotName); + + // ROS ReferenceManager + auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(nodeHandle); + + // MPC + IpmMpc mpc(interface.mpcSettings(), interface.ipmSettings(), interface.getOptimalControlProblem(), interface.getInitializer()); + mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); + mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); + + // observer for zero velocity constraints (only add this for debugging as it slows down the solver) + if (multiplot) { + auto createStateInputBoundsObserver = [&](const std::string& termName) { + const ocs2::scalar_array_t observingTimePoints{0.0}; + const std::vector topicNames{"metrics/" + termName + "/0MsLookAhead"}; + auto callback = ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + }; + for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_frictionCone")); + } + } + + // Launch MPC ROS node + MPC_ROS_Interface mpcNode(mpc, robotName); + mpcNode.launchNodes(nodeHandle); + + // Successful exit + return 0; +} From 1a5b44e35dc3e95d9ac13b2e15fb0956635ac1c3 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Wed, 14 Dec 2022 20:41:52 +0100 Subject: [PATCH 2/9] add friction cone metric plotting --- .../config/multiplot/friction_cone.xml | 720 ++++++++++++++++++ .../launch/legged_robot_ipm.launch | 4 +- 2 files changed, 723 insertions(+), 1 deletion(-) create mode 100644 ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml new file mode 100644 index 000000000..e658acd29 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml @@ -0,0 +1,720 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + X + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Y + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Z + + + + true + + 30 + LF_FOOT_frictionCone + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + X + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Y + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Z + + + + true + + 30 + RF_FOOT_frictionCone + + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + X + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Y + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Z + + + + true + + 30 + LH_FOOT_frictionCone + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + X + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Y + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Z + + + + true + + 30 + RH_FOOT_frictionCone + + + + false +
+ diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch index 9744aee4f..085a29175 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch @@ -24,7 +24,9 @@ - + + + From e7fbd444c67ffe5f53b013443301db9ce0886f7d Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Wed, 14 Dec 2022 20:46:45 +0100 Subject: [PATCH 3/9] add friction cone metric plotting --- .../ocs2_cartpole_ros/launch/multiplot.launch | 2 +- .../config/multiplot/friction_cone.xml | 416 +----------------- .../{mpc_metrics.xml => zero_velocity.xml} | 0 .../launch/multiplot.launch | 2 +- 4 files changed, 6 insertions(+), 414 deletions(-) rename ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/{mpc_metrics.xml => zero_velocity.xml} (100%) diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch index 7194cb22b..9a1f5061a 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch @@ -1,6 +1,6 @@ - + 0 100 - X + Constraint value - - - - time - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/LF_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - value/1 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/LF_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - Y - - - - - time - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/LF_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - value/2 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/LF_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - Z - true @@ -248,110 +146,8 @@ 0 100 - X + Constraint value - - - - time - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/RF_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - value/1 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/RF_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - Y - - - - - time - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/RF_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - value/2 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/RF_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - Z - true @@ -426,110 +222,8 @@ 0 100 - X + Constraint value - - - - time - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/LH_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - value/1 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/LH_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - Y - - - - - time - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/LH_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - value/2 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/LH_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - Z - true @@ -602,110 +296,8 @@ 0 100 - X + Constraint value - - - - time - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/RH_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - value/1 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/RH_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - Y - - - - - time - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/RH_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - value/2 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /metrics/RH_FOOT_frictionCone/0MsLookAhead - ocs2_msgs/constraint - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - Z - true diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/mpc_metrics.xml b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/zero_velocity.xml similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/mpc_metrics.xml rename to ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/zero_velocity.xml diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch index 26190f062..e08f5659f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch @@ -1,7 +1,7 @@ - + Date: Thu, 15 Dec 2022 14:26:08 +0100 Subject: [PATCH 4/9] fix task.info --- ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info | 2 -- 1 file changed, 2 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index 970f37fc8..1edc2801f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -55,8 +55,6 @@ ipm deltaTol 1e-4 g_max 10.0 g_min 1e-4 - inequalityConstraintMu 0.1 - inequalityConstraintDelta 5.0 projectStateInputEqualityConstraints true computeLagrangeMultipliers true printSolverStatistics true From 17fdad8569bc4c6ebe0c9b6f578d851727121c33 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Thu, 15 Dec 2022 16:09:01 +0100 Subject: [PATCH 5/9] remove projectStateInputEqualityConstraints --- ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info | 1 - 1 file changed, 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index 1edc2801f..2a130e969 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -55,7 +55,6 @@ ipm deltaTol 1e-4 g_max 10.0 g_min 1e-4 - projectStateInputEqualityConstraints true computeLagrangeMultipliers true printSolverStatistics true printSolverStatus false From 551ef2069ac1b93621f3bb3714a9ad540273ed2c Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Sun, 18 Dec 2022 00:29:48 +0100 Subject: [PATCH 6/9] update pull request --- ocs2_ipm/src/IpmHelpers.cpp | 2 +- .../ocs2_cartpole_ros/launch/multiplot.launch | 2 +- .../ocs2_legged_robot/CMakeLists.txt | 2 +- .../ocs2_legged_robot/config/mpc/task.info | 2 +- .../ocs2_legged_robot/LeggedRobotInterface.h | 2 +- .../src/LeggedRobotInterface.cpp | 16 ++++++---------- .../src/LeggedRobotIpmMpcNode.cpp | 2 +- 7 files changed, 12 insertions(+), 16 deletions(-) diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index 50d1e9564..ea7188006 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -110,7 +110,7 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala const vector_t invFractionToBoundary = (-1.0 / marginRate) * dv.cwiseQuotient(v); const auto alpha = invFractionToBoundary.maxCoeff(); - return alpha > 0.0? std::min(1.0 / alpha, 1.0): 1.0; + return alpha > 0.0 ? std::min(1.0 / alpha, 1.0) : 1.0; } } // namespace ipm diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch index 9a1f5061a..7194cb22b 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch @@ -1,6 +1,6 @@ - + pinocchioInterfacePtr_; CentroidalModelInfo centroidalModelInfo_; diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp index 14c3fcc43..36cb5669f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp @@ -65,7 +65,8 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile, - bool useHardFrictionConeConstraint) { + bool useHardFrictionConeConstraint) + : useHardFrictionConeConstraint_(useHardFrictionConeConstraint) { // check that task file exists boost::filesystem::path taskFilePath(taskFile); if (boost::filesystem::exists(taskFilePath)) { @@ -93,12 +94,11 @@ LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const st // load setting from loading file modelSettings_ = loadModelSettings(taskFile, "model_settings", verbose); - ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose); mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose); - rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose); + ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose); sqpSettings_ = sqp::loadSettings(taskFile, "sqp", verbose); ipmSettings_ = ipm::loadSettings(taskFile, "ipm", verbose); - useHardFrictionConeConstraint_ = useHardFrictionConeConstraint; + rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose); // OptimalConrolProblem setupOptimalConrolProblem(taskFile, urdfFile, referenceFile, verbose); @@ -329,13 +329,9 @@ std::unique_ptr LeggedRobotInterface::getFrictionConeConst /******************************************************************************************************/ std::unique_ptr LeggedRobotInterface::getFrictionConeSoftConstraint( size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) { - FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); - auto frictionConeConstraintPtr = std::make_unique(*referenceManagerPtr_, std::move(frictionConeConConfig), - contactPointIndex, centroidalModelInfo_); - auto penalty = std::make_unique(barrierPenaltyConfig); - - return std::make_unique(std::move(frictionConeConstraintPtr), std::move(penalty)); + return std::make_unique(getFrictionConeConstraint(contactPointIndex, frictionCoefficient), + std::make_unique(barrierPenaltyConfig)); } /******************************************************************************************************/ diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp index 30a061779..e754012b9 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp @@ -72,7 +72,7 @@ int main(int argc, char** argv) { mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); - // observer for zero velocity constraints (only add this for debugging as it slows down the solver) + // observer for friction cone constraints (only add this for debugging as it slows down the solver) if (multiplot) { auto createStateInputBoundsObserver = [&](const std::string& termName) { const ocs2::scalar_array_t observingTimePoints{0.0}; From 0a3f5ad157aada592678393908c0b6f509ff983b Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Sun, 18 Dec 2022 00:30:55 +0100 Subject: [PATCH 7/9] change printLineSearch false --- ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index d936d79e3..017dc40c3 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -58,7 +58,7 @@ ipm computeLagrangeMultipliers true printSolverStatistics true printSolverStatus false - printLinesearch true + printLinesearch false useFeedbackPolicy true integratorType RK2 threadPriority 50 From a6e97e6cd4d3781d8cf70def21d08498f7acdb2a Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Sun, 18 Dec 2022 00:37:05 +0100 Subject: [PATCH 8/9] change printLineSearch false --- .../ocs2_legged_robot/src/LeggedRobotInterface.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp index 36cb5669f..5bb70a26f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp @@ -329,7 +329,6 @@ std::unique_ptr LeggedRobotInterface::getFrictionConeConst /******************************************************************************************************/ std::unique_ptr LeggedRobotInterface::getFrictionConeSoftConstraint( size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) { - auto penalty = std::make_unique(barrierPenaltyConfig); return std::make_unique(getFrictionConeConstraint(contactPointIndex, frictionCoefficient), std::make_unique(barrierPenaltyConfig)); } From 1eac5b65a03e9a31fadfa325a86be8aaf6172b79 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian Date: Sun, 18 Dec 2022 10:35:30 +0000 Subject: [PATCH 9/9] changing to CF_WERROR --- ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt index a27bb3f62..c6d07ed02 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt @@ -122,7 +122,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_FIX + CF_WERROR ) endif(cmake_clang_tools_FOUND)