From 49deeca6997a236d04cf8718e00137357d75bb8d Mon Sep 17 00:00:00 2001 From: earlaud Date: Wed, 7 Feb 2024 16:50:12 +0100 Subject: [PATCH 01/14] Create test (but compile in strange dir) --- CMakeLists.txt | 5 ++++ config/walk_parameters.yaml | 2 +- include/qrw/ResidualFlyHigh.hxx | 18 ++++-------- tests/CMakeLists.txt | 44 +++++++++++++++++++++++++++++ tests/test-costs.cpp | 50 +++++++++++++++++++++++++++++++++ 5 files changed, 105 insertions(+), 14 deletions(-) create mode 100644 tests/CMakeLists.txt create mode 100644 tests/test-costs.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a700369c..85928ef3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,6 +40,7 @@ if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) endif() # Project options +option(BUILD_TESTING "Build the tests" ON) option(BUILD_PYTHON_INTERFACE "Build the python binding" ON) option(BUILD_JOYSTICK "Build with Gamepad support." ON) cmake_dependent_option(INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python bindings" @@ -126,6 +127,10 @@ target_link_libraries(${PROJECT_NAME} PUBLIC crocoddyl::crocoddyl) # Link master board library # target_link_libraries(${PROJECT_NAME} PUBLIC master_board_sdk::master_board_sdk) +if(BUILD_TESTING) + add_subdirectory(unittest) +endif() + add_project_dependency(yaml-cpp CONFIG REQUIRED) if(TARGET yaml-cpp) target_link_libraries(${PROJECT_NAME} PUBLIC yaml-cpp) diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml index d7e225dc..d2ce31f6 100644 --- a/config/walk_parameters.yaml +++ b/config/walk_parameters.yaml @@ -6,7 +6,7 @@ robot: PLOTTING: true # Enable/disable automatic plotting at the end of the experiment DEMONSTRATION: false # Enable/disable demonstration functionalities SIMULATION: true # Enable/disable PyBullet simulation or running on real robot - enable_pyb_GUI: true # Enable/disable PyBullet GUI + enable_pyb_GUI: false # Enable/disable PyBullet GUI env_id: 0 # Identifier of the environment to choose in which one the simulation will happen use_flat_plane: true # If True the ground is flat, otherwise it has bumps predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False) diff --git a/include/qrw/ResidualFlyHigh.hxx b/include/qrw/ResidualFlyHigh.hxx index 3396fe14..5df3c653 100644 --- a/include/qrw/ResidualFlyHigh.hxx +++ b/include/qrw/ResidualFlyHigh.hxx @@ -47,13 +47,9 @@ void ResidualModelFlyHighTpl::calc( Data* d = static_cast(data.get()); pinocchio::updateFramePlacement(pin_model_, *d->pinocchio, frame_id); - data->r = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio, frame_id, - pinocchio::LOCAL_WORLD_ALIGNED) - .linear() - .head(2); + data->r = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio, frame_id, pinocchio::LOCAL_WORLD_ALIGNED).linear().head(2); Scalar z = d->pinocchio->oMf[frame_id].translation()[2]; - // d->ez = exp(-z* z * slope / 2); - d->ez = exp(-z * z * slope / 2); + d->ez = exp(-z*z / (2 * slope*slope)); data->r *= d->ez; } @@ -75,12 +71,8 @@ void ResidualModelFlyHighTpl::calcDiff( * Then r' = v'/e - r/2 z' = R l_v'/e - l_v x Jr/e - r/2 z' */ - pinocchio::getFrameVelocityDerivatives(pin_model_, *d->pinocchio, frame_id, - pinocchio::LOCAL, d->l_dnu_dq, - d->l_dnu_dv); - const Vector3s& v = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio, - frame_id, pinocchio::LOCAL) - .linear(); + pinocchio::getFrameVelocityDerivatives(pin_model_, *d->pinocchio, frame_id, pinocchio::LOCAL, d->l_dnu_dq, d->l_dnu_dv); + const Vector3s& v = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio, frame_id, pinocchio::LOCAL).linear(); const Matrix3s& R = d->pinocchio->oMf[frame_id].rotation(); Scalar z = d->pinocchio->oMf[frame_id].translation()[2]; @@ -98,7 +90,7 @@ void ResidualModelFlyHighTpl::calcDiff( // Second term with derivative of z data->Rx.leftCols(nv).row(0) -= data->r[0] * slope * d->o_dv_dv.row(2); data->Rx.leftCols(nv).row(1) -= data->r[1] * slope * d->o_dv_dv.row(2); - data->Rx.leftCols(nv).topRows(2) *= z; + data->Rx.leftCols(nv).topRows(2) *= z/(2*slope*slope); } template diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt new file mode 100644 index 00000000..b99de303 --- /dev/null +++ b/tests/CMakeLists.txt @@ -0,0 +1,44 @@ +# +# Copyright (C) 2022-2023 LAAS-CNRS, INRIA +# + +find_package(Boost REQUIRED COMPONENTS unit_test_framework) + +macro(ADD_TEST_CFLAGS test_name flag) + set_property( + TARGET ${test_name} + APPEND_STRING + PROPERTY COMPILE_FLAGS " ${flag}") +endmacro() + +file(GLOB TEST_HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/*.hpp) +message(STATUS "Test headers: ${TEST_HEADERS}") + +function(_add_test_prototype name prefix dependencies) + set(test_name "test-cpp-${prefix}${name}") + set(test_file ${name}.cpp) + message(STATUS "Adding C++ test: ${test_file} (${test_name})") + + add_unit_test(${test_name} ${test_file} ${TEST_HEADERS}) + set_target_properties(${test_name} PROPERTIES LINKER_LANGUAGE CXX) + target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + + # define macros required by boost_test + # see: https://www.boost.org/doc/libs/1_78_0/libs/test/doc/html/boost_test/usage_variants.html + # define module name, replace '-' by '_' + set(MODULE_NAME "${name}Test") + string(REGEX REPLACE "-" "_" MODULE_NAME ${MODULE_NAME}) + + add_test_cflags(${test_name} "-DBOOST_TEST_DYN_LINK") + add_test_cflags(${test_name} "-DBOOST_TEST_MODULE=${MODULE_NAME}") + + target_link_libraries(${test_name} PUBLIC ${dependencies}) + target_link_libraries(${test_name} PRIVATE Boost::unit_test_framework) +endfunction(_add_test_prototype) + +set(TEST_NAMES + test-costs) + +foreach(test_name ${TEST_NAMES}) + _add_test_prototype(${test_name} "" ${PROJECT_NAME}) +endforeach(test_name) diff --git a/tests/test-costs.cpp b/tests/test-costs.cpp new file mode 100644 index 00000000..37f34df8 --- /dev/null +++ b/tests/test-costs.cpp @@ -0,0 +1,50 @@ +#include "pinocchio/parsers/urdf.hpp" + +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include "qrw/ResidualFlyHigh.hpp" +#include "crocoddyl/core/residuals/control.hpp" +#include "crocoddyl/multibody/data/multibody.hpp" +#include "crocoddyl/multibody/residuals/state.hpp" +#include "crocoddyl/multibody/actuations/floating-base.hpp" + +#include + +#include + +BOOST_AUTO_TEST_SUITE(costs) + +BOOST_AUTO_TEST_CASE(test_partial_derivatives_against_numdiff) { + // Pinocchio model and data + const std::string urdf_filename = std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf"); + pinocchio::Model pin_model; + pinocchio::urdf::buildModel(urdf_filename, pin_model); + pinocchio::Data pin_data(pin_model); + + pinocchio::FrameIndex id = pin_model.getFrameId("FL_FOOT"); + + // Crocoddyl state and cost + boost::shared_ptr state = boost::make_shared((boost::make_shared(pin_model))); + qrw::ResidualModelFlyHigh flyhigh_cost_model(state, id, 0.1, state->get_nv()); + + // create the residual data + boost::shared_ptr actuation_model = boost::make_shared(state); + const boost::shared_ptr& actuation_data = actuation_model->createData(); + crocoddyl::DataCollectorActMultibody shared_data(&pin_data, actuation_data); + + const boost::shared_ptr& data = flyhigh_cost_model.createData(&shared_data); + + // Generating random values for the state and control + const Eigen::VectorXd x = state->rand(); + const Eigen::VectorXd u = Eigen::VectorXd::Random(actuation_model->get_nu()); + + // // Compute all the pinocchio function needed for the models. + // crocoddyl::unittest::updateAllPinocchio(&pin_model, &pin_data, x); + // crocoddyl::unittest::updateActuation(actuation_model, actuation_data, x, u); + + // Getting the residual value computed by calc() + data->r *= nan(""); + flyhigh_cost_model.calc(data, x, u); +} + +BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file From 912b4f09531619a68c971caf21b9f98001c00ffa Mon Sep 17 00:00:00 2001 From: earlaud Date: Wed, 7 Feb 2024 17:07:54 +0100 Subject: [PATCH 02/14] Test does not produce errors --- tests/test-costs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test-costs.cpp b/tests/test-costs.cpp index 37f34df8..e6ad32ed 100644 --- a/tests/test-costs.cpp +++ b/tests/test-costs.cpp @@ -16,7 +16,7 @@ BOOST_AUTO_TEST_SUITE(costs) BOOST_AUTO_TEST_CASE(test_partial_derivatives_against_numdiff) { // Pinocchio model and data - const std::string urdf_filename = std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf"); + const std::string urdf_filename = std::string(EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf"); pinocchio::Model pin_model; pinocchio::urdf::buildModel(urdf_filename, pin_model); pinocchio::Data pin_data(pin_model); From 73730fb30fef7c474eeff42e6248e31a88a217bc Mon Sep 17 00:00:00 2001 From: earlaud Date: Wed, 7 Feb 2024 17:29:00 +0100 Subject: [PATCH 03/14] Update pinocchio quantites before test --- tests/test-costs.cpp | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/tests/test-costs.cpp b/tests/test-costs.cpp index e6ad32ed..94e566c2 100644 --- a/tests/test-costs.cpp +++ b/tests/test-costs.cpp @@ -2,6 +2,9 @@ #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/algorithm/kinematics.hpp" +#include +#include +#include #include "qrw/ResidualFlyHigh.hpp" #include "crocoddyl/core/residuals/control.hpp" #include "crocoddyl/multibody/data/multibody.hpp" @@ -39,12 +42,26 @@ BOOST_AUTO_TEST_CASE(test_partial_derivatives_against_numdiff) { const Eigen::VectorXd u = Eigen::VectorXd::Random(actuation_model->get_nu()); // // Compute all the pinocchio function needed for the models. - // crocoddyl::unittest::updateAllPinocchio(&pin_model, &pin_data, x); - // crocoddyl::unittest::updateActuation(actuation_model, actuation_data, x, u); + const Eigen::VectorXd& q = x.segment(0, pin_model.nq); + const Eigen::VectorXd& v = x.segment(pin_model.nq, pin_model.nv); + Eigen::VectorXd a = Eigen::VectorXd::Zero(pin_model.nv); + + pinocchio::forwardKinematics(pin_model, pin_data, q, v, a); + pinocchio::computeForwardKinematicsDerivatives(pin_model, pin_data, q, v, a); + pinocchio::computeJointJacobians(pin_model, pin_data, q); + pinocchio::updateFramePlacements(pin_model, pin_data); + pinocchio::computeRNEADerivatives(pin_model, pin_data, q, v, a); + + actuation_model->calc(actuation_data, x, u); + + std::cout << "x: "<< x << std::endl; + std::cout << "u: "<< u << std::endl; // Getting the residual value computed by calc() data->r *= nan(""); flyhigh_cost_model.calc(data, x, u); + + flyhigh_cost_model.calcDiff(data, x, u); } BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file From ad0220a0cbce4c5e25b923cfe311268f6f162482 Mon Sep 17 00:00:00 2001 From: earlaud Date: Fri, 9 Feb 2024 11:13:12 +0100 Subject: [PATCH 04/14] Test compile --- tests/test-costs.cpp | 81 ++++++++++++++++++++++++++++++-------------- 1 file changed, 55 insertions(+), 26 deletions(-) diff --git a/tests/test-costs.cpp b/tests/test-costs.cpp index 94e566c2..e6c0ad49 100644 --- a/tests/test-costs.cpp +++ b/tests/test-costs.cpp @@ -6,10 +6,13 @@ #include #include #include "qrw/ResidualFlyHigh.hpp" +#include "crocoddyl/core/numdiff/residual.hpp" #include "crocoddyl/core/residuals/control.hpp" #include "crocoddyl/multibody/data/multibody.hpp" #include "crocoddyl/multibody/residuals/state.hpp" #include "crocoddyl/multibody/actuations/floating-base.hpp" +#include + #include @@ -17,6 +20,31 @@ BOOST_AUTO_TEST_SUITE(costs) +void updateAllPinocchio(pinocchio::Model* const model, pinocchio::Data* data, const Eigen::VectorXd& x, const Eigen::VectorXd&) { + const Eigen::VectorXd& q = x.segment(0, model->nq); + const Eigen::VectorXd& v = x.segment(model->nq, model->nv); + Eigen::VectorXd a = Eigen::VectorXd::Zero(model->nv); +// Eigen::Matrix tmp; +// tmp.resize(6, model->nv); + pinocchio::forwardKinematics(*model, *data, q, v, a); + pinocchio::computeForwardKinematicsDerivatives(*model, *data, q, v, a); + pinocchio::computeJointJacobians(*model, *data, q); + pinocchio::updateFramePlacements(*model, *data); +// pinocchio::centerOfMass(*model, *data, q, v, a); +// pinocchio::jacobianCenterOfMass(*model, *data, q); +// pinocchio::computeCentroidalMomentum(*model, *data, q, v); +// pinocchio::computeCentroidalDynamicsDerivatives(*model, *data, q, v, a, tmp, + // tmp, tmp, tmp); + pinocchio::computeRNEADerivatives(*model, *data, q, v, a); +} + +void updateActuation( + const boost::shared_ptr& model, + const boost::shared_ptr& data, + const Eigen::VectorXd& x, const Eigen::VectorXd& u) { + model->calc(data, x, u); +} + BOOST_AUTO_TEST_CASE(test_partial_derivatives_against_numdiff) { // Pinocchio model and data const std::string urdf_filename = std::string(EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf"); @@ -26,42 +54,43 @@ BOOST_AUTO_TEST_CASE(test_partial_derivatives_against_numdiff) { pinocchio::FrameIndex id = pin_model.getFrameId("FL_FOOT"); - // Crocoddyl state and cost - boost::shared_ptr state = boost::make_shared((boost::make_shared(pin_model))); - qrw::ResidualModelFlyHigh flyhigh_cost_model(state, id, 0.1, state->get_nv()); - - // create the residual data + // Crocoddyl state, actuation and residual models + boost::shared_ptr state = boost::make_shared(boost::make_shared(pin_model)); boost::shared_ptr actuation_model = boost::make_shared(state); + boost::shared_ptr flyhigh_cost_model = boost::make_shared(state, id, 0.1, actuation_model->get_nu()); + + // create corresponding datas const boost::shared_ptr& actuation_data = actuation_model->createData(); crocoddyl::DataCollectorActMultibody shared_data(&pin_data, actuation_data); - - const boost::shared_ptr& data = flyhigh_cost_model.createData(&shared_data); + const boost::shared_ptr& flyhigh_cost_data = flyhigh_cost_model->createData(&shared_data); // Generating random values for the state and control const Eigen::VectorXd x = state->rand(); const Eigen::VectorXd u = Eigen::VectorXd::Random(actuation_model->get_nu()); - // // Compute all the pinocchio function needed for the models. - const Eigen::VectorXd& q = x.segment(0, pin_model.nq); - const Eigen::VectorXd& v = x.segment(pin_model.nq, pin_model.nv); - Eigen::VectorXd a = Eigen::VectorXd::Zero(pin_model.nv); - - pinocchio::forwardKinematics(pin_model, pin_data, q, v, a); - pinocchio::computeForwardKinematicsDerivatives(pin_model, pin_data, q, v, a); - pinocchio::computeJointJacobians(pin_model, pin_data, q); - pinocchio::updateFramePlacements(pin_model, pin_data); - pinocchio::computeRNEADerivatives(pin_model, pin_data, q, v, a); - + updateAllPinocchio(&pin_model, &pin_data,x, u); actuation_model->calc(actuation_data, x, u); - std::cout << "x: "<< x << std::endl; - std::cout << "u: "<< u << std::endl; - - // Getting the residual value computed by calc() - data->r *= nan(""); - flyhigh_cost_model.calc(data, x, u); - - flyhigh_cost_model.calcDiff(data, x, u); + // Create the equivalent num diff model and data. + crocoddyl::ResidualModelNumDiff model_num_diff(flyhigh_cost_model); + const boost::shared_ptr& data_num_diff = model_num_diff.createData(&shared_data); + + // set the function that needs to be called at every step of the numdiff + std::vector reevals; + reevals.push_back(boost::bind(&updateAllPinocchio, &pin_model, &pin_data, boost::placeholders::_1, boost::placeholders::_2)); + reevals.push_back(boost::bind(&updateActuation, actuation_model, actuation_data, boost::placeholders::_1, boost::placeholders::_2)); + model_num_diff.set_reevals(reevals); + + // Computing the cost derivatives + flyhigh_cost_model->calc(flyhigh_cost_data, x, u); + flyhigh_cost_model->calcDiff(flyhigh_cost_data, x, u); + model_num_diff.calc(data_num_diff, x, u); + model_num_diff.calcDiff(data_num_diff, x, u); + // Tolerance defined as in + // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf + double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.); + BOOST_CHECK((flyhigh_cost_data->Rx - data_num_diff->Rx).isZero(tol)); + BOOST_CHECK((flyhigh_cost_data->Ru - data_num_diff->Ru).isZero(tol)); } BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file From cf2c24129af362bfa6a7971b04aae2cbd5941710 Mon Sep 17 00:00:00 2001 From: earlaud Date: Fri, 9 Feb 2024 14:18:16 +0100 Subject: [PATCH 05/14] =?UTF-8?q?Fix=20flyhigh=20cost=20derivative=C3=83?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/qrw/ResidualFlyHigh.hxx | 5 ++--- tests/test-costs.cpp | 8 -------- 2 files changed, 2 insertions(+), 11 deletions(-) diff --git a/include/qrw/ResidualFlyHigh.hxx b/include/qrw/ResidualFlyHigh.hxx index 5df3c653..34ab8835 100644 --- a/include/qrw/ResidualFlyHigh.hxx +++ b/include/qrw/ResidualFlyHigh.hxx @@ -88,9 +88,8 @@ void ResidualModelFlyHighTpl::calcDiff( data->Rx *= d->ez; // Second term with derivative of z - data->Rx.leftCols(nv).row(0) -= data->r[0] * slope * d->o_dv_dv.row(2); - data->Rx.leftCols(nv).row(1) -= data->r[1] * slope * d->o_dv_dv.row(2); - data->Rx.leftCols(nv).topRows(2) *= z/(2*slope*slope); + data->Rx.leftCols(nv).row(0) -= z / (slope*slope) * data->r[0] * d->o_dv_dv.row(2); + data->Rx.leftCols(nv).row(1) -= z / (slope*slope) * data->r[1] * d->o_dv_dv.row(2); } template diff --git a/tests/test-costs.cpp b/tests/test-costs.cpp index e6c0ad49..0937d459 100644 --- a/tests/test-costs.cpp +++ b/tests/test-costs.cpp @@ -7,7 +7,6 @@ #include #include "qrw/ResidualFlyHigh.hpp" #include "crocoddyl/core/numdiff/residual.hpp" -#include "crocoddyl/core/residuals/control.hpp" #include "crocoddyl/multibody/data/multibody.hpp" #include "crocoddyl/multibody/residuals/state.hpp" #include "crocoddyl/multibody/actuations/floating-base.hpp" @@ -24,17 +23,10 @@ void updateAllPinocchio(pinocchio::Model* const model, pinocchio::Data* data, co const Eigen::VectorXd& q = x.segment(0, model->nq); const Eigen::VectorXd& v = x.segment(model->nq, model->nv); Eigen::VectorXd a = Eigen::VectorXd::Zero(model->nv); -// Eigen::Matrix tmp; -// tmp.resize(6, model->nv); pinocchio::forwardKinematics(*model, *data, q, v, a); pinocchio::computeForwardKinematicsDerivatives(*model, *data, q, v, a); pinocchio::computeJointJacobians(*model, *data, q); pinocchio::updateFramePlacements(*model, *data); -// pinocchio::centerOfMass(*model, *data, q, v, a); -// pinocchio::jacobianCenterOfMass(*model, *data, q); -// pinocchio::computeCentroidalMomentum(*model, *data, q, v); -// pinocchio::computeCentroidalDynamicsDerivatives(*model, *data, q, v, a, tmp, - // tmp, tmp, tmp); pinocchio::computeRNEADerivatives(*model, *data, q, v, a); } From f5ce322fdc8f927f85328d7ab84884826e7da1b0 Mon Sep 17 00:00:00 2001 From: earlaud Date: Fri, 9 Feb 2024 14:37:26 +0100 Subject: [PATCH 06/14] rename test --- tests/CMakeLists.txt | 2 +- tests/test-costs.cpp | 88 ------------------------------- tests/test-residual-models.cpp | 94 ++++++++++++++++++++++++++++++++++ 3 files changed, 95 insertions(+), 89 deletions(-) create mode 100644 tests/test-residual-models.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index b99de303..4859733f 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -37,7 +37,7 @@ function(_add_test_prototype name prefix dependencies) endfunction(_add_test_prototype) set(TEST_NAMES - test-costs) + test-residual-models) foreach(test_name ${TEST_NAMES}) _add_test_prototype(${test_name} "" ${PROJECT_NAME}) diff --git a/tests/test-costs.cpp b/tests/test-costs.cpp index 0937d459..e69de29b 100644 --- a/tests/test-costs.cpp +++ b/tests/test-costs.cpp @@ -1,88 +0,0 @@ -#include "pinocchio/parsers/urdf.hpp" - -#include "pinocchio/algorithm/joint-configuration.hpp" -#include "pinocchio/algorithm/kinematics.hpp" -#include -#include -#include -#include "qrw/ResidualFlyHigh.hpp" -#include "crocoddyl/core/numdiff/residual.hpp" -#include "crocoddyl/multibody/data/multibody.hpp" -#include "crocoddyl/multibody/residuals/state.hpp" -#include "crocoddyl/multibody/actuations/floating-base.hpp" -#include - - -#include - -#include - -BOOST_AUTO_TEST_SUITE(costs) - -void updateAllPinocchio(pinocchio::Model* const model, pinocchio::Data* data, const Eigen::VectorXd& x, const Eigen::VectorXd&) { - const Eigen::VectorXd& q = x.segment(0, model->nq); - const Eigen::VectorXd& v = x.segment(model->nq, model->nv); - Eigen::VectorXd a = Eigen::VectorXd::Zero(model->nv); - pinocchio::forwardKinematics(*model, *data, q, v, a); - pinocchio::computeForwardKinematicsDerivatives(*model, *data, q, v, a); - pinocchio::computeJointJacobians(*model, *data, q); - pinocchio::updateFramePlacements(*model, *data); - pinocchio::computeRNEADerivatives(*model, *data, q, v, a); -} - -void updateActuation( - const boost::shared_ptr& model, - const boost::shared_ptr& data, - const Eigen::VectorXd& x, const Eigen::VectorXd& u) { - model->calc(data, x, u); -} - -BOOST_AUTO_TEST_CASE(test_partial_derivatives_against_numdiff) { - // Pinocchio model and data - const std::string urdf_filename = std::string(EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf"); - pinocchio::Model pin_model; - pinocchio::urdf::buildModel(urdf_filename, pin_model); - pinocchio::Data pin_data(pin_model); - - pinocchio::FrameIndex id = pin_model.getFrameId("FL_FOOT"); - - // Crocoddyl state, actuation and residual models - boost::shared_ptr state = boost::make_shared(boost::make_shared(pin_model)); - boost::shared_ptr actuation_model = boost::make_shared(state); - boost::shared_ptr flyhigh_cost_model = boost::make_shared(state, id, 0.1, actuation_model->get_nu()); - - // create corresponding datas - const boost::shared_ptr& actuation_data = actuation_model->createData(); - crocoddyl::DataCollectorActMultibody shared_data(&pin_data, actuation_data); - const boost::shared_ptr& flyhigh_cost_data = flyhigh_cost_model->createData(&shared_data); - - // Generating random values for the state and control - const Eigen::VectorXd x = state->rand(); - const Eigen::VectorXd u = Eigen::VectorXd::Random(actuation_model->get_nu()); - - updateAllPinocchio(&pin_model, &pin_data,x, u); - actuation_model->calc(actuation_data, x, u); - - // Create the equivalent num diff model and data. - crocoddyl::ResidualModelNumDiff model_num_diff(flyhigh_cost_model); - const boost::shared_ptr& data_num_diff = model_num_diff.createData(&shared_data); - - // set the function that needs to be called at every step of the numdiff - std::vector reevals; - reevals.push_back(boost::bind(&updateAllPinocchio, &pin_model, &pin_data, boost::placeholders::_1, boost::placeholders::_2)); - reevals.push_back(boost::bind(&updateActuation, actuation_model, actuation_data, boost::placeholders::_1, boost::placeholders::_2)); - model_num_diff.set_reevals(reevals); - - // Computing the cost derivatives - flyhigh_cost_model->calc(flyhigh_cost_data, x, u); - flyhigh_cost_model->calcDiff(flyhigh_cost_data, x, u); - model_num_diff.calc(data_num_diff, x, u); - model_num_diff.calcDiff(data_num_diff, x, u); - // Tolerance defined as in - // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf - double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.); - BOOST_CHECK((flyhigh_cost_data->Rx - data_num_diff->Rx).isZero(tol)); - BOOST_CHECK((flyhigh_cost_data->Ru - data_num_diff->Ru).isZero(tol)); -} - -BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file diff --git a/tests/test-residual-models.cpp b/tests/test-residual-models.cpp new file mode 100644 index 00000000..1d9bff0d --- /dev/null +++ b/tests/test-residual-models.cpp @@ -0,0 +1,94 @@ +#include "pinocchio/parsers/urdf.hpp" + +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include +#include +#include +#include "qrw/ResidualFlyHigh.hpp" +#include "crocoddyl/core/numdiff/residual.hpp" +#include "crocoddyl/multibody/data/multibody.hpp" +#include "crocoddyl/multibody/residuals/state.hpp" +#include "crocoddyl/multibody/actuations/floating-base.hpp" +#include + +#include + +#include + + +void updatePinocchio(pinocchio::Model* const model, pinocchio::Data* data, const Eigen::VectorXd& x, const Eigen::VectorXd&) { + const Eigen::VectorXd& q = x.segment(0, model->nq); + const Eigen::VectorXd& v = x.segment(model->nq, model->nv); + Eigen::VectorXd a = Eigen::VectorXd::Zero(model->nv); + pinocchio::forwardKinematics(*model, *data, q, v, a); + pinocchio::computeForwardKinematicsDerivatives(*model, *data, q, v, a); + pinocchio::computeJointJacobians(*model, *data, q); + pinocchio::updateFramePlacements(*model, *data); + pinocchio::computeRNEADerivatives(*model, *data, q, v, a); +} + +void updateActuation( + const boost::shared_ptr& model, + const boost::shared_ptr& data, + const Eigen::VectorXd& x, const Eigen::VectorXd& u) { + model->calc(data, x, u); +} + +BOOST_AUTO_TEST_SUITE(residual_models) + +BOOST_AUTO_TEST_CASE(test_partial_derivatives_against_numdiff) { + // Pinocchio model and data + const std::string urdf_filename = std::string(EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf"); + pinocchio::Model pin_model; + pinocchio::urdf::buildModel(urdf_filename, pin_model); + pinocchio::Data pin_data(pin_model); + + pinocchio::FrameIndex id = pin_model.getFrameId("FL_FOOT"); + + // Crocoddyl state, actuation and residual models + boost::shared_ptr state = boost::make_shared(boost::make_shared(pin_model)); + boost::shared_ptr actuation_model = boost::make_shared(state); + boost::shared_ptr flyhigh_cost_model = boost::make_shared(state, id, 0.1, actuation_model->get_nu()); + + // create corresponding datas + const boost::shared_ptr& actuation_data = actuation_model->createData(); + crocoddyl::DataCollectorActMultibody shared_data(&pin_data, actuation_data); + const boost::shared_ptr& flyhigh_cost_data = flyhigh_cost_model->createData(&shared_data); + + // Starting with null state and control + Eigen::VectorXd x = state->zero(); + Eigen::VectorXd u = Eigen::VectorXd::Zero(actuation_model->get_nu()); + + for(int i=0;i<2;i++) { + updatePinocchio(&pin_model, &pin_data,x, u); + actuation_model->calc(actuation_data, x, u); + + // Create the equivalent num diff model and data. + crocoddyl::ResidualModelNumDiff model_num_diff(flyhigh_cost_model); + const boost::shared_ptr& data_num_diff = model_num_diff.createData(&shared_data); + + // set the function that needs to be called at every step of the numdiff + std::vector reevals; + reevals.push_back(boost::bind(&updatePinocchio, &pin_model, &pin_data, boost::placeholders::_1, boost::placeholders::_2)); + reevals.push_back(boost::bind(&updateActuation, actuation_model, actuation_data, boost::placeholders::_1, boost::placeholders::_2)); + model_num_diff.set_reevals(reevals); + + // Computing the cost derivatives + flyhigh_cost_model->calc(flyhigh_cost_data, x, u); + flyhigh_cost_model->calcDiff(flyhigh_cost_data, x, u); + model_num_diff.calc(data_num_diff, x, u); + model_num_diff.calcDiff(data_num_diff, x, u); + // Tolerance defined as in + // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf + double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.); + BOOST_CHECK((flyhigh_cost_data->Rx - data_num_diff->Rx).isZero(tol)); + BOOST_CHECK((flyhigh_cost_data->Ru - data_num_diff->Ru).isZero(tol)); + + // Pick new random state and control + x = state->rand(); + u = Eigen::VectorXd::Random(actuation_model->get_nu()); + } +} + +BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file From 7c8e3eade37dac0edfd205f8e748007520a47970 Mon Sep 17 00:00:00 2001 From: earlaud Date: Fri, 9 Feb 2024 15:39:23 +0100 Subject: [PATCH 07/14] rename unittest in tests --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 85928ef3..67402432 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -128,7 +128,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC crocoddyl::crocoddyl) # target_link_libraries(${PROJECT_NAME} PUBLIC master_board_sdk::master_board_sdk) if(BUILD_TESTING) - add_subdirectory(unittest) + add_subdirectory(tests) endif() add_project_dependency(yaml-cpp CONFIG REQUIRED) From 58a0d2731138f1f3680d094c5b4f05cab8e44f97 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Fri, 9 Feb 2024 15:46:02 +0100 Subject: [PATCH 08/14] simplify tests/CMakeLists.txt update cmake-format.yaml --- .cmake-format.yaml | 1 + tests/CMakeLists.txt | 19 +++++++------------ 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/.cmake-format.yaml b/.cmake-format.yaml index d23d4a2b..0c3b2a27 100644 --- a/.cmake-format.yaml +++ b/.cmake-format.yaml @@ -3,3 +3,4 @@ first_comment_is_literal: true line_width: 90 disabled_codes: - C0103 + - C0111 diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 4859733f..ea0233e7 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -4,27 +4,23 @@ find_package(Boost REQUIRED COMPONENTS unit_test_framework) -macro(ADD_TEST_CFLAGS test_name flag) +macro(add_test_cflags test_name flag) set_property( TARGET ${test_name} APPEND_STRING PROPERTY COMPILE_FLAGS " ${flag}") endmacro() -file(GLOB TEST_HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/*.hpp) -message(STATUS "Test headers: ${TEST_HEADERS}") - -function(_add_test_prototype name prefix dependencies) - set(test_name "test-cpp-${prefix}${name}") +function(add_qrw_test name dependencies) + set(test_name "test-cpp-${name}") set(test_file ${name}.cpp) message(STATUS "Adding C++ test: ${test_file} (${test_name})") - add_unit_test(${test_name} ${test_file} ${TEST_HEADERS}) + add_unit_test(${test_name} ${test_file}) set_target_properties(${test_name} PROPERTIES LINKER_LANGUAGE CXX) target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) # define macros required by boost_test - # see: https://www.boost.org/doc/libs/1_78_0/libs/test/doc/html/boost_test/usage_variants.html # define module name, replace '-' by '_' set(MODULE_NAME "${name}Test") string(REGEX REPLACE "-" "_" MODULE_NAME ${MODULE_NAME}) @@ -34,11 +30,10 @@ function(_add_test_prototype name prefix dependencies) target_link_libraries(${test_name} PUBLIC ${dependencies}) target_link_libraries(${test_name} PRIVATE Boost::unit_test_framework) -endfunction(_add_test_prototype) +endfunction() -set(TEST_NAMES - test-residual-models) +set(TEST_NAMES test-residual-models) foreach(test_name ${TEST_NAMES}) - _add_test_prototype(${test_name} "" ${PROJECT_NAME}) + add_qrw_test(${test_name} ${PROJECT_NAME}) endforeach(test_name) From e6bf7bad926e408e5d79ad89417ad4d05841215b Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Fri, 9 Feb 2024 16:17:32 +0100 Subject: [PATCH 09/14] run pre-commit --- include/qrw/ResidualFlyHigh.hxx | 21 ++++-- tests/test-residual-models.cpp | 123 ++++++++++++++++++-------------- 2 files changed, 83 insertions(+), 61 deletions(-) diff --git a/include/qrw/ResidualFlyHigh.hxx b/include/qrw/ResidualFlyHigh.hxx index 34ab8835..e5b89aa8 100644 --- a/include/qrw/ResidualFlyHigh.hxx +++ b/include/qrw/ResidualFlyHigh.hxx @@ -47,9 +47,12 @@ void ResidualModelFlyHighTpl::calc( Data* d = static_cast(data.get()); pinocchio::updateFramePlacement(pin_model_, *d->pinocchio, frame_id); - data->r = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio, frame_id, pinocchio::LOCAL_WORLD_ALIGNED).linear().head(2); + data->r = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio, frame_id, + pinocchio::LOCAL_WORLD_ALIGNED) + .linear() + .head(2); Scalar z = d->pinocchio->oMf[frame_id].translation()[2]; - d->ez = exp(-z*z / (2 * slope*slope)); + d->ez = exp(-z * z / (2 * slope * slope)); data->r *= d->ez; } @@ -71,8 +74,12 @@ void ResidualModelFlyHighTpl::calcDiff( * Then r' = v'/e - r/2 z' = R l_v'/e - l_v x Jr/e - r/2 z' */ - pinocchio::getFrameVelocityDerivatives(pin_model_, *d->pinocchio, frame_id, pinocchio::LOCAL, d->l_dnu_dq, d->l_dnu_dv); - const Vector3s& v = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio, frame_id, pinocchio::LOCAL).linear(); + pinocchio::getFrameVelocityDerivatives(pin_model_, *d->pinocchio, frame_id, + pinocchio::LOCAL, d->l_dnu_dq, + d->l_dnu_dv); + const Vector3s& v = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio, + frame_id, pinocchio::LOCAL) + .linear(); const Matrix3s& R = d->pinocchio->oMf[frame_id].rotation(); Scalar z = d->pinocchio->oMf[frame_id].translation()[2]; @@ -88,8 +95,10 @@ void ResidualModelFlyHighTpl::calcDiff( data->Rx *= d->ez; // Second term with derivative of z - data->Rx.leftCols(nv).row(0) -= z / (slope*slope) * data->r[0] * d->o_dv_dv.row(2); - data->Rx.leftCols(nv).row(1) -= z / (slope*slope) * data->r[1] * d->o_dv_dv.row(2); + data->Rx.leftCols(nv).row(0) -= + z / (slope * slope) * data->r[0] * d->o_dv_dv.row(2); + data->Rx.leftCols(nv).row(1) -= + z / (slope * slope) * data->r[1] * d->o_dv_dv.row(2); } template diff --git a/tests/test-residual-models.cpp b/tests/test-residual-models.cpp index 1d9bff0d..5298276e 100644 --- a/tests/test-residual-models.cpp +++ b/tests/test-residual-models.cpp @@ -1,5 +1,5 @@ #include "pinocchio/parsers/urdf.hpp" - + #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/algorithm/kinematics.hpp" #include @@ -16,8 +16,8 @@ #include - -void updatePinocchio(pinocchio::Model* const model, pinocchio::Data* data, const Eigen::VectorXd& x, const Eigen::VectorXd&) { +void updatePinocchio(pinocchio::Model* const model, pinocchio::Data* data, + const Eigen::VectorXd& x, const Eigen::VectorXd&) { const Eigen::VectorXd& q = x.segment(0, model->nq); const Eigen::VectorXd& v = x.segment(model->nq, model->nv); Eigen::VectorXd a = Eigen::VectorXd::Zero(model->nv); @@ -38,57 +38,70 @@ void updateActuation( BOOST_AUTO_TEST_SUITE(residual_models) BOOST_AUTO_TEST_CASE(test_partial_derivatives_against_numdiff) { - // Pinocchio model and data - const std::string urdf_filename = std::string(EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf"); - pinocchio::Model pin_model; - pinocchio::urdf::buildModel(urdf_filename, pin_model); - pinocchio::Data pin_data(pin_model); - - pinocchio::FrameIndex id = pin_model.getFrameId("FL_FOOT"); - - // Crocoddyl state, actuation and residual models - boost::shared_ptr state = boost::make_shared(boost::make_shared(pin_model)); - boost::shared_ptr actuation_model = boost::make_shared(state); - boost::shared_ptr flyhigh_cost_model = boost::make_shared(state, id, 0.1, actuation_model->get_nu()); - - // create corresponding datas - const boost::shared_ptr& actuation_data = actuation_model->createData(); - crocoddyl::DataCollectorActMultibody shared_data(&pin_data, actuation_data); - const boost::shared_ptr& flyhigh_cost_data = flyhigh_cost_model->createData(&shared_data); - - // Starting with null state and control - Eigen::VectorXd x = state->zero(); - Eigen::VectorXd u = Eigen::VectorXd::Zero(actuation_model->get_nu()); - - for(int i=0;i<2;i++) { - updatePinocchio(&pin_model, &pin_data,x, u); - actuation_model->calc(actuation_data, x, u); - - // Create the equivalent num diff model and data. - crocoddyl::ResidualModelNumDiff model_num_diff(flyhigh_cost_model); - const boost::shared_ptr& data_num_diff = model_num_diff.createData(&shared_data); - - // set the function that needs to be called at every step of the numdiff - std::vector reevals; - reevals.push_back(boost::bind(&updatePinocchio, &pin_model, &pin_data, boost::placeholders::_1, boost::placeholders::_2)); - reevals.push_back(boost::bind(&updateActuation, actuation_model, actuation_data, boost::placeholders::_1, boost::placeholders::_2)); - model_num_diff.set_reevals(reevals); - - // Computing the cost derivatives - flyhigh_cost_model->calc(flyhigh_cost_data, x, u); - flyhigh_cost_model->calcDiff(flyhigh_cost_data, x, u); - model_num_diff.calc(data_num_diff, x, u); - model_num_diff.calcDiff(data_num_diff, x, u); - // Tolerance defined as in - // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf - double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.); - BOOST_CHECK((flyhigh_cost_data->Rx - data_num_diff->Rx).isZero(tol)); - BOOST_CHECK((flyhigh_cost_data->Ru - data_num_diff->Ru).isZero(tol)); - - // Pick new random state and control - x = state->rand(); - u = Eigen::VectorXd::Random(actuation_model->get_nu()); - } + // Pinocchio model and data + const std::string urdf_filename = std::string( + EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf"); + pinocchio::Model pin_model; + pinocchio::urdf::buildModel(urdf_filename, pin_model); + pinocchio::Data pin_data(pin_model); + + pinocchio::FrameIndex id = pin_model.getFrameId("FL_FOOT"); + + // Crocoddyl state, actuation and residual models + boost::shared_ptr state = + boost::make_shared( + boost::make_shared(pin_model)); + boost::shared_ptr actuation_model = + boost::make_shared(state); + boost::shared_ptr flyhigh_cost_model = + boost::make_shared(state, id, 0.1, + actuation_model->get_nu()); + + // create corresponding datas + const boost::shared_ptr& actuation_data = + actuation_model->createData(); + crocoddyl::DataCollectorActMultibody shared_data(&pin_data, actuation_data); + const boost::shared_ptr& flyhigh_cost_data = + flyhigh_cost_model->createData(&shared_data); + + // Starting with null state and control + Eigen::VectorXd x = state->zero(); + Eigen::VectorXd u = Eigen::VectorXd::Zero(actuation_model->get_nu()); + + for (int i = 0; i < 2; i++) { + updatePinocchio(&pin_model, &pin_data, x, u); + actuation_model->calc(actuation_data, x, u); + + // Create the equivalent num diff model and data. + crocoddyl::ResidualModelNumDiff model_num_diff(flyhigh_cost_model); + const boost::shared_ptr& data_num_diff = + model_num_diff.createData(&shared_data); + + // set the function that needs to be called at every step of the numdiff + std::vector reevals; + reevals.push_back(boost::bind(&updatePinocchio, &pin_model, &pin_data, + boost::placeholders::_1, + boost::placeholders::_2)); + reevals.push_back(boost::bind(&updateActuation, actuation_model, + actuation_data, boost::placeholders::_1, + boost::placeholders::_2)); + model_num_diff.set_reevals(reevals); + + // Computing the cost derivatives + flyhigh_cost_model->calc(flyhigh_cost_data, x, u); + flyhigh_cost_model->calcDiff(flyhigh_cost_data, x, u); + model_num_diff.calc(data_num_diff, x, u); + model_num_diff.calcDiff(data_num_diff, x, u); + // Tolerance defined as in + // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf + double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.); + BOOST_CHECK((flyhigh_cost_data->Rx - data_num_diff->Rx).isZero(tol)); + BOOST_CHECK((flyhigh_cost_data->Ru - data_num_diff->Ru).isZero(tol)); + + // Pick new random state and control + x = state->rand(); + u = Eigen::VectorXd::Random(actuation_model->get_nu()); + } } -BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file +BOOST_AUTO_TEST_SUITE_END() From 0f7ce491c15742ea21763d145e3ea5f7815b9c34 Mon Sep 17 00:00:00 2001 From: earlaud Date: Fri, 9 Feb 2024 17:29:46 +0100 Subject: [PATCH 10/14] Rename/re-arrange flyhigh slop into sigma_height for physical sense --- config/walk_parameters.yaml | 2 +- include/qrw/ResidualFlyHigh.hpp | 22 ++++++++++++------- include/qrw/ResidualFlyHigh.hxx | 16 ++++++-------- python/expose-residual-fly-high.cpp | 10 ++++----- .../ocp_defs/walking.py | 2 +- .../wb_mpc/task_spec.py | 2 +- 6 files changed, 29 insertions(+), 25 deletions(-) diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml index d2ce31f6..186468c5 100644 --- a/config/walk_parameters.yaml +++ b/config/walk_parameters.yaml @@ -64,7 +64,7 @@ robot: task: walk: &base_task_pms friction_mu: 0.4 - fly_high_slope: 2000. + fly_high_sigma_height: 0.03 # m fly_high_w: 50000. ground_collision_w: 1000. vertical_velocity_reg_w: 1. diff --git a/include/qrw/ResidualFlyHigh.hpp b/include/qrw/ResidualFlyHigh.hpp index 5750ea45..b6fba80d 100644 --- a/include/qrw/ResidualFlyHigh.hpp +++ b/include/qrw/ResidualFlyHigh.hpp @@ -28,11 +28,17 @@ using namespace crocoddyl; /** * @brief Cost penalizing high horizontal velocity near zero altitude. * - * The cost is r(q,v) = v_foot[:2] / exp(slope*z_foot) + * The cost is r(q,v) = v_foot[:2] * exp(- z_foot^2/(2*sigma_height^2)) * with v_foot = J_foot(q) vq the local-world-aligned linear velocity of the * considered frame velocity and z_foot(q) the altitude * oMfoot[frameId].translation[2] of the considered frame wrt world. * + * Because the cost follows a gaussian in z, the rest of the cost is multiplied by + * is multiplied a factor: + * * ~0.30 for z=sigma_height + * * ~0.07 for z=3*sigma_height + * * ~0.006 for z=3*sigma_height + * * \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()` */ template @@ -58,12 +64,12 @@ class ResidualModelFlyHighTpl : public ResidualModelAbstractTpl<_Scalar> { * @param[in] state State of the multibody system * @param[in] frame_id ID of the frame that should be considered for altitude * and velocity - * @param[in] slope Slope value, ie altitude multiplier. + * @param[in] sigma_height Height reference value. * @param[in] nu Dimension of the control vector */ ResidualModelFlyHighTpl(boost::shared_ptr state, const pinocchio::FrameIndex frame_id, - const Scalar slope, const std::size_t nu); + const Scalar sigma_height, const std::size_t nu); /** * @brief Initialize the residual model @@ -73,11 +79,11 @@ class ResidualModelFlyHighTpl : public ResidualModelAbstractTpl<_Scalar> { * @param[in] state State of the multibody system * @param[in] frame_id ID of the frame that should be considered for altitude * and velocity - * @param[in] slope Slope value, ie altitude multiplier. + * @param[in] sigma_height Height reference value. */ ResidualModelFlyHighTpl(boost::shared_ptr state, const pinocchio::FrameIndex frame_id, - const Scalar slope); + const Scalar sigma_height); virtual ~ResidualModelFlyHighTpl(); /** @@ -114,8 +120,8 @@ class ResidualModelFlyHighTpl : public ResidualModelAbstractTpl<_Scalar> { */ void set_frame_id(const pinocchio::FrameIndex& fid); - const Scalar getSlope() const { return slope; } - void setSlope(const Scalar s) { slope = s; } + const Scalar getSigmaHeight() const { return sigma_height; } + void setSigmaHeight(const Scalar s) { sigma_height = s; } protected: using Base::nu_; @@ -126,7 +132,7 @@ class ResidualModelFlyHighTpl : public ResidualModelAbstractTpl<_Scalar> { private: pinocchio::FrameIndex frame_id; - Scalar slope; // multiplication in front of the altitude in the cost + Scalar sigma_height; // multiplication in front of the altitude in the cost typename StateMultibody::PinocchioModel pin_model_; //!< Pinocchio model used for internal computations }; diff --git a/include/qrw/ResidualFlyHigh.hxx b/include/qrw/ResidualFlyHigh.hxx index e5b89aa8..8e6ff5cc 100644 --- a/include/qrw/ResidualFlyHigh.hxx +++ b/include/qrw/ResidualFlyHigh.hxx @@ -18,20 +18,20 @@ using namespace crocoddyl; template ResidualModelFlyHighTpl::ResidualModelFlyHighTpl( boost::shared_ptr state, - const pinocchio::FrameIndex frame_id, const Scalar slope, + const pinocchio::FrameIndex frame_id, const Scalar sigma_height, const std::size_t nu) : Base(state, 2, nu, true, true, false), frame_id(frame_id), - slope(slope), + sigma_height(sigma_height), pin_model_(*state->get_pinocchio()) {} template ResidualModelFlyHighTpl::ResidualModelFlyHighTpl( boost::shared_ptr state, - const pinocchio::FrameIndex frame_id, const Scalar slope) + const pinocchio::FrameIndex frame_id, const Scalar sigma_height) : Base(state, 2, true, true, false), frame_id(frame_id), - slope(slope), + sigma_height(sigma_height), pin_model_(*state->get_pinocchio()) {} template @@ -52,7 +52,7 @@ void ResidualModelFlyHighTpl::calc( .linear() .head(2); Scalar z = d->pinocchio->oMf[frame_id].translation()[2]; - d->ez = exp(-z * z / (2 * slope * slope)); + d->ez = exp(-z*z / (2 * sigma_height*sigma_height)); data->r *= d->ez; } @@ -95,10 +95,8 @@ void ResidualModelFlyHighTpl::calcDiff( data->Rx *= d->ez; // Second term with derivative of z - data->Rx.leftCols(nv).row(0) -= - z / (slope * slope) * data->r[0] * d->o_dv_dv.row(2); - data->Rx.leftCols(nv).row(1) -= - z / (slope * slope) * data->r[1] * d->o_dv_dv.row(2); + data->Rx.leftCols(nv).row(0) -= z / (sigma_height*sigma_height) * data->r[0] * d->o_dv_dv.row(2); + data->Rx.leftCols(nv).row(1) -= z / (sigma_height*sigma_height) * data->r[1] * d->o_dv_dv.row(2); } template diff --git a/python/expose-residual-fly-high.cpp b/python/expose-residual-fly-high.cpp index 18da1e09..1b0dce1b 100644 --- a/python/expose-residual-fly-high.cpp +++ b/python/expose-residual-fly-high.cpp @@ -25,11 +25,11 @@ void exposeResidualFlyHigh() { "ResidualModelFlyHigh", bp::init, pinocchio::FrameIndex, double, std::size_t>( - bp::args("self", "state", "frame_id", "slope", "nu"), + bp::args("self", "state", "frame_id", "sigma_height", "nu"), "Initialize the residual model.\n\n" ":param state: state of the multibody system\n" ":param frame_id: reference colliding frame\n" - ":param slope: slope ie altitude multiplier." + ":param sigma_height: height reference value." ":param nu: dimension of control vector")) .def&, @@ -75,9 +75,9 @@ void exposeResidualFlyHigh() { // &ResidualModelFlyHigh::get_frame_id, // &ResidualModelFlyHigh::set_frame_id, // "Frame ID") - .add_property("slope", &ResidualModelFlyHigh::getSlope, - &ResidualModelFlyHigh::setSlope, - "Set slope (ie altitude multiplicator)"); + .add_property("sigma_height", &ResidualModelFlyHigh::getSigmaHeight, + &ResidualModelFlyHigh::setSigmaHeight, + "Set sigma_height (ie the height reference value)"); bp::register_ptr_to_python >(); diff --git a/python/quadruped_reactive_walking/ocp_defs/walking.py b/python/quadruped_reactive_walking/ocp_defs/walking.py index 778fad85..2b4bf07c 100644 --- a/python/quadruped_reactive_walking/ocp_defs/walking.py +++ b/python/quadruped_reactive_walking/ocp_defs/walking.py @@ -314,7 +314,7 @@ def _add_fly_high_cost(self, i: int, costs: CostModelSum): nu = costs.nu fly_high_cost = CostModelResidual( self.state, - ResidualModelFlyHigh(self.state, i, self.task.fly_high_slope / 2.0, nu), + ResidualModelFlyHigh(self.state, i, self.task.fly_high_sigma_height, nu), ) name = "{}_flyHigh".format(self.rmodel.frames[i].name) costs.addCost( diff --git a/python/quadruped_reactive_walking/wb_mpc/task_spec.py b/python/quadruped_reactive_walking/wb_mpc/task_spec.py index 0b4e08ef..cc130009 100644 --- a/python/quadruped_reactive_walking/wb_mpc/task_spec.py +++ b/python/quadruped_reactive_walking/wb_mpc/task_spec.py @@ -76,7 +76,7 @@ def __init__(self, params: Params): # Cost function weights self.friction_mu = task_pms["friction_mu"] - self.fly_high_slope = task_pms["fly_high_slope"] + self.fly_high_sigma_height = task_pms["fly_high_sigma_height"] self.fly_high_w = task_pms["fly_high_w"] self.ground_collision_w = task_pms["ground_collision_w"] self.vertical_velocity_reg_w = task_pms["vertical_velocity_reg_w"] From 2c09adb1a53bd783baeef95f2881f90da0a1e594 Mon Sep 17 00:00:00 2001 From: earlaud Date: Mon, 12 Feb 2024 16:35:21 +0100 Subject: [PATCH 11/14] Workaround for ROS Cmake setting global variable in CMake and installing test in wrong dir --- CMakeLists.txt | 13 ++++++++++++- tests/CMakeLists.txt | 4 +++- ...test-residual-models.cpp => residual-models.cpp} | 0 tests/test-costs.cpp | 0 4 files changed, 15 insertions(+), 2 deletions(-) rename tests/{test-residual-models.cpp => residual-models.cpp} (100%) delete mode 100644 tests/test-costs.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 67402432..dc2166b3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,7 +40,6 @@ if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) endif() # Project options -option(BUILD_TESTING "Build the tests" ON) option(BUILD_PYTHON_INTERFACE "Build the python binding" ON) option(BUILD_JOYSTICK "Build with Gamepad support." ON) cmake_dependent_option(INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python bindings" @@ -58,9 +57,21 @@ if(NOT INSTALL_PYTHON_INTERFACE_ONLY) endif() if(BUILD_WITH_ROS_SUPPORT) + # ROS macros temper with some variables, save one of them (that affect add_executable for tests) + if(DEFINED CMAKE_RUNTIME_OUTPUT_DIRECTORY) + set(REMEMBER_CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}) + endif(DEFINED CMAKE_RUNTIME_OUTPUT_DIRECTORY) + find_package(catkin REQUIRED message_generation std_msgs) add_service_files(DIRECTORY ros_qrw_msgs FILES MPCInit.srv MPCSolve.srv MPCStop.srv) generate_messages(DEPENDENCIES std_msgs) + + # Restore saved variable to previous state + if(DEFINED REMEMBER_CMAKE_RUNTIME_OUTPUT_DIRECTORY) + set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${REMEMBER_CMAKE_RUNTIME_OUTPUT_DIRECTORY}) + else(DEFINED REMEMBER_CMAKE_RUNTIME_OUTPUT_DIRECTORY) + unset(CMAKE_RUNTIME_OUTPUT_DIRECTORY) + endif(DEFINED REMEMBER_CMAKE_RUNTIME_OUTPUT_DIRECTORY) endif() # Project dependencies diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index ea0233e7..696875fc 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -32,7 +32,9 @@ function(add_qrw_test name dependencies) target_link_libraries(${test_name} PRIVATE Boost::unit_test_framework) endfunction() -set(TEST_NAMES test-residual-models) +set(TEST_NAMES + residual-models +) foreach(test_name ${TEST_NAMES}) add_qrw_test(${test_name} ${PROJECT_NAME}) diff --git a/tests/test-residual-models.cpp b/tests/residual-models.cpp similarity index 100% rename from tests/test-residual-models.cpp rename to tests/residual-models.cpp diff --git a/tests/test-costs.cpp b/tests/test-costs.cpp deleted file mode 100644 index e69de29b..00000000 From bea1194ef20204452275dce8cc0fb64a0bec9034 Mon Sep 17 00:00:00 2001 From: earlaud Date: Mon, 12 Feb 2024 16:42:14 +0100 Subject: [PATCH 12/14] reset default config value --- config/walk_parameters.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml index 186468c5..50fe51db 100644 --- a/config/walk_parameters.yaml +++ b/config/walk_parameters.yaml @@ -6,7 +6,7 @@ robot: PLOTTING: true # Enable/disable automatic plotting at the end of the experiment DEMONSTRATION: false # Enable/disable demonstration functionalities SIMULATION: true # Enable/disable PyBullet simulation or running on real robot - enable_pyb_GUI: false # Enable/disable PyBullet GUI + enable_pyb_GUI: true # Enable/disable PyBullet GUI env_id: 0 # Identifier of the environment to choose in which one the simulation will happen use_flat_plane: true # If True the ground is flat, otherwise it has bumps predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False) From fab348498640c81984c80a255bed6d377e0ef9b8 Mon Sep 17 00:00:00 2001 From: earlaud Date: Mon, 12 Feb 2024 17:02:29 +0100 Subject: [PATCH 13/14] Run pre-commit --- CMakeLists.txt | 2 +- include/qrw/ResidualFlyHigh.hpp | 4 ++-- include/qrw/ResidualFlyHigh.hxx | 8 +++++--- tests/CMakeLists.txt | 4 +--- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dc2166b3..7bc44ecf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,7 +57,7 @@ if(NOT INSTALL_PYTHON_INTERFACE_ONLY) endif() if(BUILD_WITH_ROS_SUPPORT) - # ROS macros temper with some variables, save one of them (that affect add_executable for tests) + # ROS macros temper with some variables (that affect add_executable for tests), save it if(DEFINED CMAKE_RUNTIME_OUTPUT_DIRECTORY) set(REMEMBER_CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}) endif(DEFINED CMAKE_RUNTIME_OUTPUT_DIRECTORY) diff --git a/include/qrw/ResidualFlyHigh.hpp b/include/qrw/ResidualFlyHigh.hpp index b6fba80d..ce054880 100644 --- a/include/qrw/ResidualFlyHigh.hpp +++ b/include/qrw/ResidualFlyHigh.hpp @@ -33,8 +33,8 @@ using namespace crocoddyl; * considered frame velocity and z_foot(q) the altitude * oMfoot[frameId].translation[2] of the considered frame wrt world. * - * Because the cost follows a gaussian in z, the rest of the cost is multiplied by - * is multiplied a factor: + * Because the cost follows a gaussian in z, the rest of the cost is multiplied + * by is multiplied a factor: * * ~0.30 for z=sigma_height * * ~0.07 for z=3*sigma_height * * ~0.006 for z=3*sigma_height diff --git a/include/qrw/ResidualFlyHigh.hxx b/include/qrw/ResidualFlyHigh.hxx index 8e6ff5cc..2ba5c8d4 100644 --- a/include/qrw/ResidualFlyHigh.hxx +++ b/include/qrw/ResidualFlyHigh.hxx @@ -52,7 +52,7 @@ void ResidualModelFlyHighTpl::calc( .linear() .head(2); Scalar z = d->pinocchio->oMf[frame_id].translation()[2]; - d->ez = exp(-z*z / (2 * sigma_height*sigma_height)); + d->ez = exp(-z * z / (2 * sigma_height * sigma_height)); data->r *= d->ez; } @@ -95,8 +95,10 @@ void ResidualModelFlyHighTpl::calcDiff( data->Rx *= d->ez; // Second term with derivative of z - data->Rx.leftCols(nv).row(0) -= z / (sigma_height*sigma_height) * data->r[0] * d->o_dv_dv.row(2); - data->Rx.leftCols(nv).row(1) -= z / (sigma_height*sigma_height) * data->r[1] * d->o_dv_dv.row(2); + data->Rx.leftCols(nv).row(0) -= + z / (sigma_height * sigma_height) * data->r[0] * d->o_dv_dv.row(2); + data->Rx.leftCols(nv).row(1) -= + z / (sigma_height * sigma_height) * data->r[1] * d->o_dv_dv.row(2); } template diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 696875fc..5c68fee9 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -32,9 +32,7 @@ function(add_qrw_test name dependencies) target_link_libraries(${test_name} PRIVATE Boost::unit_test_framework) endfunction() -set(TEST_NAMES - residual-models -) +set(TEST_NAMES residual-models) foreach(test_name ${TEST_NAMES}) add_qrw_test(${test_name} ${PROJECT_NAME}) From e82743fbe24b57000244df9c389b7c55f2dfa74b Mon Sep 17 00:00:00 2001 From: earlaud Date: Mon, 12 Feb 2024 17:04:14 +0100 Subject: [PATCH 14/14] Run pre-commit --- include/qrw/ResidualFlyHigh.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/qrw/ResidualFlyHigh.hpp b/include/qrw/ResidualFlyHigh.hpp index ce054880..518b84dc 100644 --- a/include/qrw/ResidualFlyHigh.hpp +++ b/include/qrw/ResidualFlyHigh.hpp @@ -33,8 +33,8 @@ using namespace crocoddyl; * considered frame velocity and z_foot(q) the altitude * oMfoot[frameId].translation[2] of the considered frame wrt world. * - * Because the cost follows a gaussian in z, the rest of the cost is multiplied - * by is multiplied a factor: + * Because the cost follows a gaussian in z, the rest of the cost is + * multiplied by a factor: * * ~0.30 for z=sigma_height * * ~0.07 for z=3*sigma_height * * ~0.006 for z=3*sigma_height