diff --git a/CHANGELOG.md b/CHANGELOG.md index 195c7d724..23ac0b7d4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - Update finite difference helper to support explicit dynamics, replacing the obsolete implicit-only implementation. (https://github.com/Simple-Robotics/aligator/pull/392) - Fix `SolverProxDDP::tryLinearStep()` temporary control update buffer allocation to use `nu_max` instead of `ndx_max`. (https://github.com/Simple-Robotics/aligator/pull/397) +- Tentative support building with the upcoming Pinocchio 4 (https://github.com/Simple-Robotics/aligator/pull/390) ## [0.18.0] - 2026-01-27 diff --git a/CMakeLists.txt b/CMakeLists.txt index 62afb071a..d548c8783 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ # -# Copyright (C) 2022-2024 LAAS-CNRS, 2022-2025 INRIA +# Copyright (C) 2022-2024 LAAS-CNRS, 2022-2026 INRIA # cmake_minimum_required(VERSION 3.22) diff --git a/bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp b/bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp index 9ef88a25a..7741faa01 100644 --- a/bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp +++ b/bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp @@ -197,16 +197,14 @@ auto underactuatedConstraintInvDyn_proxy(const PinModel &model, PinData &data, const RCMVector &constraint_models, RCDVector &constraint_datas) { long nu = actMatrix.cols(); - int d = 0; - for (size_t k = 0; k < constraint_models.size(); ++k) { - d += (int)constraint_models[k].size(); - } + const int d = details::computeRigidConstraintsTotalSize(constraint_models, + constraint_datas); + context::VectorXs out(nu + d); underactuatedConstrainedInverseDynamics( model, data, q, v, actMatrix, constraint_models, constraint_datas, out); - return bp::make_tuple((context::VectorXs)out.head(nu), - (context::VectorXs)out.tail(d)); + return bp::make_tuple(out.head(nu).eval(), out.tail(d).eval()); } void exposePinocchioFunctions() { diff --git a/examples/talos-walk-utils.cpp b/examples/talos-walk-utils.cpp index f80009207..be2a379fe 100644 --- a/examples/talos-walk-utils.cpp +++ b/examples/talos-walk-utils.cpp @@ -9,7 +9,7 @@ #include void makeTalosReduced(Model &model_complete, Model &model, - Eigen::VectorXd &q0) { + Eigen::Ref q0) { const std::string talos_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_reduced.urdf"; const std::string srdf_path = @@ -57,16 +57,17 @@ void makeTalosReduced(Model &model_complete, Model &model, q0 = model.referenceConfigurations["half_sitting"]; } -void foot_traj(Eigen::Vector3d &translation_init, const int &t_ss, - const int &ts) { - double swing_apex = 0.05; - translation_init[2] += swing_apex * sin(ts * M_PI / (double)t_ss); +void getFootTraj(std::size_t t_ss, std::size_t ts, + Eigen::Ref translation_init) { + constexpr double swing_apex = 0.05; + translation_init[2] += swing_apex * std::sin(ts * M_PI / (double)t_ss); } IntegratorSemiImplEuler -create_dynamics(MultibodyPhaseSpace &stage_space, Support &support, - MatrixXd &actuation_matrix, ProximalSettings &proximal_settings, - std::vector &constraint_models) { +createDynamics(MultibodyPhaseSpace &stage_space, Support &support, + MatrixXd &actuation_matrix, ProximalSettings &proximal_settings, + PINOCCHIO_ALIGNED_STD_VECTOR(pin::RigidConstraintModel) & + constraint_models) { pinocchio::context::RigidConstraintModelVector cms; switch (support) { case LEFT: @@ -144,18 +145,25 @@ TrajOptProblem defineLocomotionProblem(const std::size_t T_ss, foot_joint_ids.push_back(rmodel.frames[foot_frame_ids[0]].parentJoint); foot_joint_ids.push_back(rmodel.frames[foot_frame_ids[1]].parentJoint); - std::vector constraint_models; + PINOCCHIO_ALIGNED_STD_VECTOR(pin::RigidConstraintModel) constraint_models; for (std::size_t i = 0; i < 2; i++) { pin::SE3 pl1 = rmodel.frames[foot_frame_ids[i]].placement; pin::SE3 pl2 = rdata.oMf[foot_frame_ids[i]]; - pin::RigidConstraintModel constraint_model = pin::RigidConstraintModel( + pin::RigidConstraintModel constraint_model( pin::ContactType::CONTACT_6D, rmodel, foot_joint_ids[i], pl1, 0, pl2, pin::LOCAL_WORLD_ALIGNED); - constraint_model.corrector.Kp << 0, 0, 100, 0, 0, 0; - constraint_model.corrector.Kd << 50, 50, 50, 50, 50, 50; +#ifdef ALIGATOR_PINOCCHIO_V4 + auto &corrector = constraint_model.baumgarte_corrector_parameters(); + corrector.Kp = 100; + corrector.Kd = 50; +#else + auto &corrector = constraint_model.corrector; + corrector.Kp << 0, 0, 100, 0, 0, 0; + corrector.Kd << 50, 50, 50, 50, 50, 50; +#endif constraint_model.name = foot_frame_name[i]; - constraint_models.push_back(constraint_model); + constraint_models.push_back(std::move(constraint_model)); } std::vector double_phase; @@ -195,14 +203,14 @@ TrajOptProblem defineLocomotionProblem(const std::size_t T_ss, std::shared_ptr frame_fn_LF; switch (ph) { case LEFT: - foot_traj(RF_placement.translation(), T_ss, ts); + getFootTraj(T_ss, ts, RF_placement.translation()); frame_fn_RF = std::make_shared( stage_space.ndx(), nu, rmodel, RF_placement, foot_frame_ids[1]); rcost.addCost("frame_fn_RF", QuadraticResidualCost(stage_space, *frame_fn_RF, w_LFRF)); break; case RIGHT: - foot_traj(LF_placement.translation(), T_ss, ts); + getFootTraj(T_ss, ts, LF_placement.translation()); frame_fn_LF = std::make_shared( stage_space.ndx(), nu, rmodel, LF_placement, foot_frame_ids[0]); rcost.addCost("frame_fn_LF", @@ -213,8 +221,8 @@ TrajOptProblem defineLocomotionProblem(const std::size_t T_ss, break; } stage_models.push_back( - StageModel(rcost, create_dynamics(stage_space, ph, actuation_matrix, - prox_settings, constraint_models))); + StageModel(rcost, createDynamics(stage_space, ph, actuation_matrix, + prox_settings, constraint_models))); } auto ter_space = MultibodyPhaseSpace(rmodel); auto term_cost = CostStack(ter_space, nu); diff --git a/examples/talos-walk-utils.hpp b/examples/talos-walk-utils.hpp index ab773b219..ceceacc92 100644 --- a/examples/talos-walk-utils.hpp +++ b/examples/talos-walk-utils.hpp @@ -38,7 +38,8 @@ using TrajOptProblem = aligator::TrajOptProblemTpl; enum Support { LEFT, RIGHT, DOUBLE }; -void makeTalosReduced(Model &model_complete, Model &model, Eigen::VectorXd &q0); +void makeTalosReduced(Model &model_complete, Model &model, + Eigen::Ref q0); TrajOptProblem defineLocomotionProblem(const std::size_t T_ss, const std::size_t T_ds); diff --git a/include/aligator/fwd.hpp b/include/aligator/fwd.hpp index 7142fb275..90901ce84 100644 --- a/include/aligator/fwd.hpp +++ b/include/aligator/fwd.hpp @@ -164,11 +164,4 @@ template struct ResultsTpl; // fwd FilterTpl template struct FilterTpl; -/// @brief Typedef for vector with Eigen::aligned_allocator allocator type. -/// @deprecated This is deprecated. Use the PINOCCHIO_ALIGNED_STD_VECTOR macro -/// or underlying typedef when relevant, or write the full std::vector template. -template -using StdVectorEigenAligned ALIGATOR_DEPRECATED = - std::vector>; - } // namespace aligator diff --git a/include/aligator/modelling/contact-map.hpp b/include/aligator/modelling/contact-map.hpp index 23c8852a7..907854d31 100644 --- a/include/aligator/modelling/contact-map.hpp +++ b/include/aligator/modelling/contact-map.hpp @@ -8,7 +8,7 @@ namespace aligator { template struct ContactMapTpl { using Scalar = _Scalar; ALIGATOR_DYNAMIC_TYPEDEFS(Scalar); - using PoseVec = std::vector>; + using PoseVec = std::vector; ContactMapTpl(const std::vector &contact_names, const std::vector &contact_states, diff --git a/include/aligator/modelling/dynamics/linear-ode.hpp b/include/aligator/modelling/dynamics/linear-ode.hpp index 9986041c4..349960393 100644 --- a/include/aligator/modelling/dynamics/linear-ode.hpp +++ b/include/aligator/modelling/dynamics/linear-ode.hpp @@ -1,6 +1,7 @@ -/// @copyright Copyright (C) 2022 LAAS-CNRS, INRIA +/// @copyright Copyright (C) 2022-2023 LAAS-CNRS, 2022-2026 INRIA #pragma once +#include "aligator/context.hpp" #include "aligator/modelling/dynamics/ode-abstract.hpp" #include "aligator/core/vector-space.hpp" @@ -63,11 +64,11 @@ template struct LinearODETpl : ODEAbstractTpl<_Scalar> { } }; +#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION +extern template struct LinearODETpl; +#endif + } // namespace dynamics } // namespace aligator #include "aligator/modelling/dynamics/linear-ode.hxx" - -#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION -#include "aligator/modelling/dynamics/linear-ode.txx" -#endif diff --git a/include/aligator/modelling/dynamics/linear-ode.txx b/include/aligator/modelling/dynamics/linear-ode.txx deleted file mode 100644 index 5f39a4e40..000000000 --- a/include/aligator/modelling/dynamics/linear-ode.txx +++ /dev/null @@ -1,13 +0,0 @@ -/// @copyright Copyright (C) 2024 LAAS-CNRS, INRIA -#pragma once - -#include "aligator/context.hpp" -#include "aligator/modelling/dynamics/linear-ode.hpp" - -namespace aligator { -namespace dynamics { - -extern template struct LinearODETpl; - -} // namespace dynamics -} // namespace aligator diff --git a/include/aligator/modelling/dynamics/multibody-constraint-fwd.hpp b/include/aligator/modelling/dynamics/multibody-constraint-fwd.hpp index 22bc4b2c8..f2f2c35e2 100644 --- a/include/aligator/modelling/dynamics/multibody-constraint-fwd.hpp +++ b/include/aligator/modelling/dynamics/multibody-constraint-fwd.hpp @@ -1,4 +1,4 @@ -/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2025 INRIA +/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2026 INRIA #pragma once #include "aligator/modelling/dynamics/ode-abstract.hpp" @@ -78,15 +78,15 @@ struct MultibodyConstraintFwdDataTpl : ContinuousDynamicsDataTpl { MatrixXs dtau_du_; RigidConstraintDataVector constraint_datas_; pinocchio::ProximalSettingsTpl settings; - /// shared_ptr to the underlying pinocchio::DataTpl object. PinDataType pin_data_; - MultibodyConstraintFwdDataTpl( + explicit MultibodyConstraintFwdDataTpl( const MultibodyConstraintFwdDynamicsTpl &cont_dyn); }; -} // namespace dynamics -} // namespace aligator - #ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION -#include "aligator/modelling/dynamics/multibody-constraint-fwd.txx" +extern template struct MultibodyConstraintFwdDynamicsTpl; +extern template struct MultibodyConstraintFwdDataTpl; #endif + +} // namespace dynamics +} // namespace aligator diff --git a/include/aligator/modelling/dynamics/multibody-constraint-fwd.txx b/include/aligator/modelling/dynamics/multibody-constraint-fwd.txx deleted file mode 100644 index ccaba6537..000000000 --- a/include/aligator/modelling/dynamics/multibody-constraint-fwd.txx +++ /dev/null @@ -1,13 +0,0 @@ -/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, INRIA -#pragma once - -#include "./multibody-constraint-fwd.hpp" - -namespace aligator { -namespace dynamics { - -extern template struct MultibodyConstraintFwdDynamicsTpl; -extern template struct MultibodyConstraintFwdDataTpl; - -} // namespace dynamics -} // namespace aligator diff --git a/include/aligator/modelling/dynamics/multibody-free-fwd.hpp b/include/aligator/modelling/dynamics/multibody-free-fwd.hpp index 3519b1c9e..8064ec9ab 100644 --- a/include/aligator/modelling/dynamics/multibody-free-fwd.hpp +++ b/include/aligator/modelling/dynamics/multibody-free-fwd.hpp @@ -1,4 +1,4 @@ -/// @copyright Copyright (C) 2022 LAAS-CNRS, INRIA +/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2026 INRIA #pragma once #include "aligator/modelling/dynamics/ode-abstract.hpp" @@ -11,16 +11,14 @@ namespace aligator { namespace dynamics { template struct MultibodyFreeFwdDataTpl; -/** - * @brief Free-space multibody forward dynamics, using Pinocchio. - * - * @details This is described in state-space \f$\mathcal{X} = T\mathcal{Q}\f$ - * (the phase space \f$x = (q,v)\f$) using the differential equation \f[ \dot{x} - * = f(x, u) = \begin{bmatrix} v \\ a(q, v, \tau(u)) \end{bmatrix} \f] where - * \f$\tau(u) = Bu\f$, \f$B\f$ is a given actuation matrix, and - * \f$a(q,v,\tau)\f$ is the acceleration computed from the ABA algorithm. - * - */ +/// @brief Free-space multibody forward dynamics, using Pinocchio. +/// +/// @details This is described in state-space \f$\mathcal{X} = T\mathcal{Q}\f$ +/// (the phase space \f$x = (q,v)\f$) using the differential equation \f[ +/// \dot{x} = f(x, u) = \begin{bmatrix} v \\ a(q, v, \tau(u)) \end{bmatrix} \f] +/// where +/// \f$\tau(u) = Bu\f$, \f$B\f$ is a given actuation matrix, and +/// \f$a(q,v,\tau)\f$ is the acceleration computed from the ABA algorithm. template struct MultibodyFreeFwdDynamicsTpl : ODEAbstractTpl<_Scalar> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -81,10 +79,11 @@ struct MultibodyFreeFwdDataTpl : ContinuousDynamicsDataTpl { MultibodyFreeFwdDataTpl(const MultibodyFreeFwdDynamicsTpl *cont_dyn); }; -} // namespace dynamics -} // namespace aligator - #ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION -#include "aligator/modelling/dynamics/multibody-free-fwd.txx" +extern template struct MultibodyFreeFwdDynamicsTpl; +extern template struct MultibodyFreeFwdDataTpl; #endif + +} // namespace dynamics +} // namespace aligator #endif diff --git a/include/aligator/modelling/dynamics/multibody-free-fwd.txx b/include/aligator/modelling/dynamics/multibody-free-fwd.txx deleted file mode 100644 index 2cb8ed6c0..000000000 --- a/include/aligator/modelling/dynamics/multibody-free-fwd.txx +++ /dev/null @@ -1,14 +0,0 @@ -/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, INRIA -#pragma once - -#include "aligator/context.hpp" -#include "aligator/modelling/dynamics/multibody-free-fwd.hpp" - -namespace aligator { -namespace dynamics { - -extern template struct MultibodyFreeFwdDynamicsTpl; -extern template struct MultibodyFreeFwdDataTpl; - -} // namespace dynamics -} // namespace aligator diff --git a/include/aligator/modelling/dynamics/ode-abstract.hpp b/include/aligator/modelling/dynamics/ode-abstract.hpp index 04a3e85a5..4a8f2e5f6 100644 --- a/include/aligator/modelling/dynamics/ode-abstract.hpp +++ b/include/aligator/modelling/dynamics/ode-abstract.hpp @@ -1,16 +1,15 @@ #pragma once /// @file ode-abstract.hpp /// @brief Defines a class representing ODEs. -/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, INRIA +/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2026 INRIA #include "aligator/modelling/dynamics/continuous-dynamics-abstract.hpp" namespace aligator { namespace dynamics { -/** @brief Base class for ODE dynamics \f$ \dot{x} = f(x, u) \f$. - * @details Formulated as a DAE (for ContinuousDynamicsAbstractTpl), this class - * models \f[ f(x, u) - \dot{x}. \f] - */ +/// @brief Base class for ODE dynamics \f$ \dot{x} = f(x, u) \f$. +/// @details Formulated as a DAE (for ContinuousDynamicsAbstractTpl), this +/// class models \f[ f(x, u) - \dot{x}. \f] template struct ODEAbstractTpl : ContinuousDynamicsAbstractTpl<_Scalar> { using Scalar = _Scalar; @@ -41,9 +40,9 @@ struct ODEAbstractTpl : ContinuousDynamicsAbstractTpl<_Scalar> { const ConstVectorRef &xdot, Data &data) const override; }; -} // namespace dynamics -} // namespace aligator - #ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION -#include "aligator/modelling/dynamics/ode-abstract.txx" +extern template struct ODEAbstractTpl; #endif + +} // namespace dynamics +} // namespace aligator diff --git a/include/aligator/modelling/dynamics/ode-abstract.txx b/include/aligator/modelling/dynamics/ode-abstract.txx deleted file mode 100644 index 79776d9a1..000000000 --- a/include/aligator/modelling/dynamics/ode-abstract.txx +++ /dev/null @@ -1,13 +0,0 @@ -/// @copyright Copyright (C) 2024 LAAS-CNRS, INRIA -#pragma once - -#include "aligator/context.hpp" -#include "aligator/modelling/dynamics/ode-abstract.hpp" - -namespace aligator { -namespace dynamics { - -extern template struct ODEAbstractTpl; - -} // namespace dynamics -} // namespace aligator diff --git a/include/aligator/modelling/multibody/constrained-rnea.hpp b/include/aligator/modelling/multibody/constrained-rnea.hpp index ba3e1a200..60c3dbeac 100644 --- a/include/aligator/modelling/multibody/constrained-rnea.hpp +++ b/include/aligator/modelling/multibody/constrained-rnea.hpp @@ -16,6 +16,28 @@ using pinocchio::RigidConstraintDataTpl; using pinocchio::RigidConstraintModelTpl; } // namespace +namespace details { +template +int computeRigidConstraintsTotalSize( + const pinocchio::container::aligned_vector< + RigidConstraintModelTpl> &constraint_models, + [[maybe_unused]] const pinocchio::container::aligned_vector< + RigidConstraintDataTpl> &constraint_datas) { + + int d = 0; + for (size_t k = 0; k < constraint_models.size(); ++k) { + const int constraint_size = +#ifdef ALIGATOR_PINOCCHIO_V4 + constraint_models[k].residualSize(); +#else + static_cast(constraint_models[k].size()); +#endif + d += constraint_size; + } + return d; +} +} // namespace details + template void underactuatedConstrainedInverseDynamics( @@ -34,17 +56,15 @@ void underactuatedConstrainedInverseDynamics( OutType &res = res_.const_cast_derived(); - long nu = actMatrix.cols(); - long nv = model.nv; + const long nu = actMatrix.cols(); + const long nv = model.nv; assert(nv == actMatrix.rows() && "Actuation matrix dimension inconsistent."); pin::computeAllTerms(model, data, q, v); const auto &nle = data.nle; - int d = 0; - for (size_t k = 0; k < constraint_models.size(); ++k) { - d += (int)constraint_models[k].size(); - } + const int d = details::computeRigidConstraintsTotalSize(constraint_models, + constraint_datas); assert(res.size() == nu + d); MatrixXs work(nv, nu + d); @@ -53,10 +73,9 @@ void underactuatedConstrainedInverseDynamics( auto JacT = work.rightCols(d); pin::getConstraintsJacobian(model, data, constraint_models, constraint_datas, JacT.transpose()); - JacT = -JacT; - - Eigen::ColPivHouseholderQR qr(work); + JacT *= -1; + Eigen::ColPivHouseholderQR> qr(work); res = qr.solve(nle); } diff --git a/include/aligator/modelling/multibody/contact-force.hpp b/include/aligator/modelling/multibody/contact-force.hpp index 5552939c8..505eecbca 100644 --- a/include/aligator/modelling/multibody/contact-force.hpp +++ b/include/aligator/modelling/multibody/contact-force.hpp @@ -1,12 +1,11 @@ /// @file -/// @copyright Copyright (C) 2024 LAAS-CNRS, 2024-2025 INRIA +/// @copyright Copyright (C) 2024 LAAS-CNRS, 2024-2026 INRIA #pragma once -#include "./fwd.hpp" +#include "fwd.hpp" #include "aligator/core/function-abstract.hpp" - -#include #include "aligator/modelling/spaces/multibody.hpp" + #include namespace aligator { diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 338e728f8..24f748235 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -54,7 +54,6 @@ set( lqr problem manifolds - solver-storage utils ) diff --git a/tests/continuous.cpp b/tests/continuous.cpp index 45d12771b..8e03d874b 100644 --- a/tests/continuous.cpp +++ b/tests/continuous.cpp @@ -2,7 +2,7 @@ #ifdef ALIGATOR_WITH_PINOCCHIO #include "aligator/modelling/dynamics/multibody-free-fwd.hpp" -#include +#include #endif #ifdef ALIGATOR_WITH_PINOCCHIO diff --git a/tests/forces.cpp b/tests/forces.cpp index 5dfdbd15f..28acf0371 100644 --- a/tests/forces.cpp +++ b/tests/forces.cpp @@ -1,9 +1,8 @@ -#include -#include -#include +/// @file +/// @copyright Copyright (C) 2024-2026 INRIA +#include #include #include -#include "aligator/modelling/spaces/multibody.hpp" #include @@ -11,6 +10,8 @@ #include #include +#include "test_util/pinocchio.hpp" + TEST_CASE("contact_forces_6d", "[forces]") { using namespace Eigen; using namespace pinocchio; @@ -45,20 +46,18 @@ TEST_CASE("contact_forces_6d", "[forces]") { RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL); ci_LF.joint1_placement.setRandom(); - ci_LF.corrector.Kp.array() = 10; - ci_LF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_LF, 10.0); ci_LF.name = "LF_foot"; RigidConstraintModel ci_RF(CONTACT_6D, model, RF_id, LOCAL); ci_RF.joint1_placement.setRandom(); - ci_RF.corrector.Kp.array() = 10; - ci_RF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_RF, 10.0); ci_RF.name = "RF_foot"; constraint_models.push_back(ci_LF); - constraint_data.push_back(RigidConstraintData(ci_LF)); + constraint_data.emplace_back(ci_LF); constraint_models.push_back(ci_RF); - constraint_data.push_back(RigidConstraintData(ci_RF)); + constraint_data.emplace_back(ci_RF); const double mu0 = 0.; ProximalSettings prox_settings(1e-12, mu0, 1); @@ -159,20 +158,18 @@ TEST_CASE("contact_forces_3d", "[forces]") { RigidConstraintModel ci_LF(CONTACT_3D, model, LF_id, LOCAL); ci_LF.joint1_placement.setRandom(); - ci_LF.corrector.Kp.array() = 10; - ci_LF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_LF, 10.); ci_LF.name = "LF_foot"; RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL); ci_RF.joint1_placement.setRandom(); - ci_RF.corrector.Kp.array() = 10; - ci_RF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_RF, 10.); ci_RF.name = "RF_foot"; constraint_models.push_back(ci_LF); - constraint_data.push_back(RigidConstraintData(ci_LF)); + constraint_data.emplace_back(ci_LF); constraint_models.push_back(ci_RF); - constraint_data.push_back(RigidConstraintData(ci_RF)); + constraint_data.emplace_back(ci_RF); const double mu0 = 0.; ProximalSettings prox_settings(1e-12, mu0, 1); @@ -273,20 +270,18 @@ TEST_CASE("wrench_cone", "[forces]") { RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL); ci_LF.joint1_placement.setRandom(); - ci_LF.corrector.Kp.array() = 10; - ci_LF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_LF, 10.); ci_LF.name = "LF_foot"; RigidConstraintModel ci_RF(CONTACT_6D, model, RF_id, LOCAL); ci_RF.joint1_placement.setRandom(); - ci_RF.corrector.Kp.array() = 10; - ci_RF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_RF, 10.); ci_RF.name = "RF_foot"; constraint_models.push_back(ci_LF); - constraint_data.push_back(RigidConstraintData(ci_LF)); + constraint_data.emplace_back(ci_LF); // constraint_models.push_back(ci_RF); - // constraint_data.push_back(RigidConstraintData(ci_RF)); + // constraint_data.emplace_back(ci_RF); const double mu0 = 0.; ProximalSettings prox_settings(1e-12, mu0, 1); @@ -385,20 +380,18 @@ TEST_CASE("friction_cone", "[forces]") { RigidConstraintModel ci_LF(CONTACT_3D, model, LF_id, LOCAL); ci_LF.joint1_placement.setRandom(); - ci_LF.corrector.Kp.array() = 10; - ci_LF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_LF, 10.); ci_LF.name = "LF_foot"; RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL); ci_RF.joint1_placement.setRandom(); - ci_RF.corrector.Kp.array() = 10; - ci_RF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_RF, 10.); ci_RF.name = "RF_foot"; constraint_models.push_back(ci_LF); - constraint_data.push_back(RigidConstraintData(ci_LF)); + constraint_data.emplace_back(ci_LF); // constraint_models.push_back(ci_RF); - // constraint_data.push_back(RigidConstraintData(ci_RF)); + // constraint_data.emplace_back(ci_RF); const double mu0 = 0.; ProximalSettings prox_settings(1e-12, mu0, 1); diff --git a/tests/mpc-cycle.cpp b/tests/mpc-cycle.cpp index 3e52bbe3c..da9362dff 100644 --- a/tests/mpc-cycle.cpp +++ b/tests/mpc-cycle.cpp @@ -12,9 +12,10 @@ #include #include -#include #include +#include "test_util/pinocchio.hpp" + using namespace aligator; using context::SolverProxDDP; @@ -99,8 +100,7 @@ auto makeConstraint(pinocchio::Model const &model) { auto constraint = pinocchio::RigidConstraintModel( pinocchio::ContactType::CONTACT_6D, model, frame.parentJoint, frame.placement, pinocchio::LOCAL_WORLD_ALIGNED); - constraint.corrector.Kp << 0, 0, 100, 0, 0, 0; - constraint.corrector.Kd << 50, 50, 50, 50, 50, 50; + set_baumgarte_gains(constraint, 100, 50); constraint.name = "contact"; return constraint; diff --git a/tests/solver-storage.cpp b/tests/solver-storage.cpp deleted file mode 100644 index 858ef7624..000000000 --- a/tests/solver-storage.cpp +++ /dev/null @@ -1,31 +0,0 @@ -#include "aligator/solvers/value-function.hpp" - -#include - -#include - -TEST_CASE("prox_storage", "[solver_workspace]") { - using VParams = aligator::ValueFunctionTpl; - using QParams = aligator::QFunctionTpl; - const int NX = 3; - const int NU = 2; - VParams vstore(NX); - QParams qstore(NX, NU, NX); - - REQUIRE(vstore.Vx_.cols() == 1); - REQUIRE(vstore.Vx_.rows() == NX); - REQUIRE(vstore.Vxx_.rows() == NX); - REQUIRE(vstore.Vxx_.cols() == NX); - fmt::print("{} < Vx\n", vstore.Vx_); - fmt::print("{} < Vxx\n", vstore.Vxx_); - - fmt::print("{} < Qx\n", qstore.Qx); - fmt::print("{} < Qu\n", qstore.Qu); - fmt::print("{} < Qxx\n", qstore.Qxx); - fmt::print("{} < Qxu\n", qstore.Qxu); - fmt::print("{} < Qxy\n", qstore.Qxy); - fmt::print("{} < Quu\n", qstore.Quu); - fmt::print("{} < Qyy\n", qstore.Qyy); -} - -TEST_CASE("fddp_storage", "[solver_workspace]") {} diff --git a/tests/test_util/pinocchio.hpp b/tests/test_util/pinocchio.hpp new file mode 100644 index 000000000..4c16f6e93 --- /dev/null +++ b/tests/test_util/pinocchio.hpp @@ -0,0 +1,26 @@ + +#include +#include + +inline auto & +get_baumgarte_corrector_params(pinocchio::RigidConstraintModel &rcm) { +#ifdef ALIGATOR_PINOCCHIO_V4 + return rcm.baumgarte_corrector_parameters(); +#else + return rcm.corrector; +#endif +} + +inline void set_baumgarte_gains(pinocchio::RigidConstraintModel &rcm, + const double Kp, + std::optional Kd_ = std::nullopt) { + double Kd = Kd_.value_or(Kp); + auto &corr = get_baumgarte_corrector_params(rcm); +#ifdef ALIGATOR_PINOCCHIO_V4 + corr.Kp = Kp; + corr.Kd = Kd; +#else + corr.Kp.array() = Kp; + corr.Kd.array() = Kd; +#endif +}