diff --git a/.gitignore b/.gitignore index b07b91c6..d5b9899e 100644 --- a/.gitignore +++ b/.gitignore @@ -33,4 +33,9 @@ .cache/ build*/ -/__pycache__/ +__pycache__/ + + +.vscode +CMakeFiles/ +CMakeCache.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index c9571c66..e93516b1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -95,8 +95,8 @@ endif() include(dependencies.cmake) # Main Library -file(GLOB mpc_SOURCE CONFIGURE_DEPENDS src/*.cpp) -file(GLOB mpc_HEADER CONFIGURE_DEPENDS include/${PROJECT_NAME}/*.hpp) +file(GLOB_RECURSE mpc_SOURCE CONFIGURE_DEPENDS src/*.cpp) +file(GLOB_RECURSE mpc_HEADER CONFIGURE_DEPENDS include/${PROJECT_NAME}/*.hpp) add_library(${PROJECT_NAME} SHARED ${mpc_HEADER} ${mpc_SOURCE}) target_include_directories( @@ -112,6 +112,7 @@ target_link_libraries( aligator::aligator example-robot-data::example-robot-data ndcurves::ndcurves + tsid::tsid ) target_link_libraries(${PROJECT_NAME} PUBLIC OpenMP::OpenMP_CXX) set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER "${mpc_HEADER}") diff --git a/README.md b/README.md index 41988872..40c8b45a 100644 --- a/README.md +++ b/README.md @@ -81,5 +81,6 @@ source install/setup.bash * [Aligator](https://github.com/edantec/aligator) temporary_fix branch * [example-robot-data](https://github.com/Gepetto/example-robot-data) * [ndcurves](https://github.com/loco-3d/ndcurves) +* [tsid](https://github.com/stack-of-tasks/tsid) * (optional) [eigenpy](https://github.com/stack-of-tasks/eigenpy)>=3.9.0 (Python bindings) * (optional) [bullet](https://github.com/bulletphysics/bullet3) (Simulation examples) diff --git a/benchmark/go2.cpp b/benchmark/go2.cpp index 4dcda296..4e80d3c7 100644 --- a/benchmark/go2.cpp +++ b/benchmark/go2.cpp @@ -40,14 +40,14 @@ int main() ref_FR_foot.translation() = Eigen::Vector3d(0.17, -0.15, 0.0); ref_RL_foot.translation() = Eigen::Vector3d(-0.24, 0.15, 0.0); ref_RR_foot.translation() = Eigen::Vector3d(-0.24, -0.15, 0.0); - model_handler.addFoot("FL_foot", base_joint_name); - model_handler.addFoot("FR_foot", base_joint_name); - model_handler.addFoot("RL_foot", base_joint_name); - model_handler.addFoot("RR_foot", base_joint_name); - model_handler.setFootReferencePlacement("FL_foot", ref_FL_foot); - model_handler.setFootReferencePlacement("FR_foot", ref_FR_foot); - model_handler.setFootReferencePlacement("RL_foot", ref_RL_foot); - model_handler.setFootReferencePlacement("RR_foot", ref_RR_foot); + model_handler.addPointFoot("FL_foot", base_joint_name); + model_handler.addPointFoot("FR_foot", base_joint_name); + model_handler.addPointFoot("RL_foot", base_joint_name); + model_handler.addPointFoot("RR_foot", base_joint_name); + model_handler.setFootReferencePlacement(model_handler.getFootNb("FL_foot"), ref_FL_foot); + model_handler.setFootReferencePlacement(model_handler.getFootNb("FR_foot"), ref_FR_foot); + model_handler.setFootReferencePlacement(model_handler.getFootNb("RL_foot"), ref_RL_foot); + model_handler.setFootReferencePlacement(model_handler.getFootNb("RR_foot"), ref_RR_foot); RobotDataHandler data_handler(model_handler); @@ -109,7 +109,6 @@ int main() int T_fly = 30; int T_contact = 5; simple_mpc::MPCSettings mpc_settings; - mpc_settings.ddpIteration = 1; mpc_settings.support_force = -gravity[2] * model_handler.getMass(); mpc_settings.TOL = 1e-4; mpc_settings.mu_init = 1e-8; diff --git a/benchmark/talos.cpp b/benchmark/talos.cpp index 502db030..c08c67e5 100644 --- a/benchmark/talos.cpp +++ b/benchmark/talos.cpp @@ -72,13 +72,16 @@ int main() RobotModelHandler model_handler(model, "half_sitting", base_joint); // Add feet - model_handler.addFoot("left_sole_link", base_joint); - model_handler.addFoot("right_sole_link", base_joint); + Eigen::Matrix foot_quad{{0.1, 0.075, 0}, {-0.1, 0.075, 0}, {-0.1, -0.075, 0}, {0.1, -0.075, 0}}; + model_handler.addQuadFoot("left_sole_link", base_joint, foot_quad); + model_handler.addQuadFoot("right_sole_link", base_joint, foot_quad); model_handler.setFootReferencePlacement( - "left_sole_link", pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.))); + model_handler.getFootNb("left_sole_link"), + pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.))); model_handler.setFootReferencePlacement( - "right_sole_link", pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., -0.1, 0.))); + model_handler.getFootNb("right_sole_link"), + pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., -0.1, 0.))); RobotDataHandler data_handler(model_handler); @@ -152,36 +155,36 @@ int main() for (std::size_t i = 0; i < 10; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), true}); - contact_state.insert({model_handler.getFootName(1), true}); + contact_state.insert({model_handler.getFootFrameName(0), true}); + contact_state.insert({model_handler.getFootFrameName(1), true}); contact_states.push_back(contact_state); } for (std::size_t i = 0; i < 50; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), true}); - contact_state.insert({model_handler.getFootName(1), false}); + contact_state.insert({model_handler.getFootFrameName(0), true}); + contact_state.insert({model_handler.getFootFrameName(1), false}); contact_states.push_back(contact_state); } for (std::size_t i = 0; i < 10; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), true}); - contact_state.insert({model_handler.getFootName(1), true}); + contact_state.insert({model_handler.getFootFrameName(0), true}); + contact_state.insert({model_handler.getFootFrameName(1), true}); contact_states.push_back(contact_state); } for (std::size_t i = 0; i < 50; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), false}); - contact_state.insert({model_handler.getFootName(1), true}); + contact_state.insert({model_handler.getFootFrameName(0), false}); + contact_state.insert({model_handler.getFootFrameName(1), true}); contact_states.push_back(contact_state); } for (std::size_t i = 0; i < 10; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), true}); - contact_state.insert({model_handler.getFootName(1), true}); + contact_state.insert({model_handler.getFootFrameName(0), true}); + contact_state.insert({model_handler.getFootFrameName(1), true}); contact_states.push_back(contact_state); } diff --git a/bindings/CMakeLists.txt b/bindings/CMakeLists.txt index 137cecfc..7c02ec4b 100644 --- a/bindings/CMakeLists.txt +++ b/bindings/CMakeLists.txt @@ -9,11 +9,11 @@ set( expose-robot-handler.cpp expose-problem.cpp expose-mpc.cpp - expose-qp.cpp expose-interpolate.cpp expose-centroidal.cpp expose-fulldynamics.cpp expose-kinodynamics.cpp + expose-inverse-dynamics.cpp expose-friction-compensation.cpp ) diff --git a/bindings/expose-inverse-dynamics.cpp b/bindings/expose-inverse-dynamics.cpp new file mode 100644 index 00000000..8344dc4a --- /dev/null +++ b/bindings/expose-inverse-dynamics.cpp @@ -0,0 +1,97 @@ +#include + +#include "simple-mpc/inverse-dynamics/centroidal-id.hpp" +#include "simple-mpc/inverse-dynamics/kinodynamics-id.hpp" + +namespace simple_mpc +{ + namespace python + { + namespace bp = boost::python; + + Eigen::VectorXd solveKinoProxy( + KinodynamicsID & self, + const double t, + const Eigen::Ref & q_meas, + const Eigen::Ref & v_meas) + { + Eigen::VectorXd tau_res(self.model_handler_.getModel().nv - 6); + self.solve(t, q_meas, v_meas, tau_res); + return tau_res; + } + + Eigen::VectorXd getAccelerationsKinoProxy(KinodynamicsID & self) + { + Eigen::VectorXd a(self.model_handler_.getModel().nv); + self.getAccelerations(a); + return a; + } + + Eigen::VectorXd solveCentroidalProxy( + CentroidalID & self, + const double t, + const Eigen::Ref & q_meas, + const Eigen::Ref & v_meas) + { + Eigen::VectorXd tau_res(self.model_handler_.getModel().nv - 6); + self.solve(t, q_meas, v_meas, tau_res); + return tau_res; + } + + Eigen::VectorXd getAccelerationsCentroidalProxy(CentroidalID & self) + { + Eigen::VectorXd a(self.model_handler_.getModel().nv); + self.getAccelerations(a); + return a; + } + + void setTarget_CentroidalID( + CentroidalID & self, + const Eigen::Ref> & com_position, + const Eigen::Ref> & com_velocity, + const CentroidalID::FeetPoseVector & feet_pose, + const CentroidalID::FeetVelocityVector & feet_velocity, + const std::vector & contact_state_target, + const std::vector & f_target) + { + self.setTarget(com_position, com_velocity, feet_pose, feet_velocity, contact_state_target, f_target); + } + + void exposeInverseDynamics() + { + bp::class_("KinodynamicsIDSettings", bp::init<>(bp::args("self"))) + .def_readwrite("friction_coefficient", &KinodynamicsID::Settings::friction_coefficient) + .def_readwrite("contact_weight_ratio_max", &KinodynamicsID::Settings::contact_weight_ratio_max) + .def_readwrite("contact_weight_ratio_min", &KinodynamicsID::Settings::contact_weight_ratio_min) + .def_readwrite("kp_base", &KinodynamicsID::Settings::kp_base) + .def_readwrite("kp_posture", &KinodynamicsID::Settings::kp_posture) + .def_readwrite("kp_contact", &KinodynamicsID::Settings::kp_contact) + .def_readwrite("w_base", &KinodynamicsID::Settings::w_base) + .def_readwrite("w_posture", &KinodynamicsID::Settings::w_posture) + .def_readwrite("w_contact_motion", &KinodynamicsID::Settings::w_contact_motion) + .def_readwrite("w_contact_force", &KinodynamicsID::Settings::w_contact_force) + .def_readwrite("contact_motion_equality", &KinodynamicsID::Settings::contact_motion_equality); + + bp::class_( + "KinodynamicsID", bp::init( + bp::args("self", "model_handler", "control_dt", "settings"))) + .def("setTarget", &KinodynamicsID::setTarget) + .def("solve", &solveKinoProxy) + .def("getAccelerations", &getAccelerationsKinoProxy); + + bp::class_>( + "CentroidalIDSettings", bp::init<>(bp::args("self"))) + .def_readwrite("kp_com", &CentroidalID::Settings::kp_com) + .def_readwrite("kp_feet_tracking", &CentroidalID::Settings::kp_feet_tracking) + .def_readwrite("w_com", &CentroidalID::Settings::w_com) + .def_readwrite("w_feet_tracking", &CentroidalID::Settings::w_feet_tracking); + + bp::class_( + "CentroidalID", bp::init( + bp::args("self", "model_handler", "control_dt", "settings"))) + .def("setTarget", &setTarget_CentroidalID) + .def("solve", &solveCentroidalProxy) + .def("getAccelerations", &getAccelerationsCentroidalProxy); + } + } // namespace python +} // namespace simple_mpc diff --git a/bindings/expose-qp.cpp b/bindings/expose-qp.cpp deleted file mode 100644 index e3cc3422..00000000 --- a/bindings/expose-qp.cpp +++ /dev/null @@ -1,127 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// BSD 2-Clause License -// -// Copyright (C) 2024, INRIA -// Copyright note valid unless otherwise stated in individual files. -// All rights reserved. -/////////////////////////////////////////////////////////////////////////////// - -#include "simple-mpc/python.hpp" -#include -#include - -#include "simple-mpc/qp-solvers.hpp" -#include -#include - -namespace simple_mpc -{ - namespace python - { - namespace bp = boost::python; - - template - inline void py_list_to_std_vector(const bp::object & iterable, std::vector & out) - { - out = std::vector(bp::stl_input_iterator(iterable), bp::stl_input_iterator()); - } - - auto * create_idsolver(const bp::dict & settings, const pinocchio::Model & model) - { - IDSettings conf; - - py_list_to_std_vector(settings["contact_ids"], conf.contact_ids); - - conf.mu = bp::extract(settings["mu"]); - conf.Lfoot = bp::extract(settings["Lfoot"]); - conf.Wfoot = bp::extract(settings["Wfoot"]); - - conf.force_size = bp::extract(settings["force_size"]); - conf.kd = bp::extract(settings["kd"]); - conf.w_force = bp::extract(settings["w_force"]); - conf.w_acc = bp::extract(settings["w_acc"]); - conf.w_tau = bp::extract(settings["w_tau"]); - conf.verbose = bp::extract(settings["verbose"]); - - return new IDSolver(conf, model); - } - - auto * create_ikidsolver(const bp::dict & settings, const pinocchio::Model & model) - { - IKIDSettings conf; - - py_list_to_std_vector(settings["Kp_gains"], conf.Kp_gains); - py_list_to_std_vector(settings["Kd_gains"], conf.Kd_gains); - py_list_to_std_vector(settings["contact_ids"], conf.contact_ids); - py_list_to_std_vector(settings["fixed_frame_ids"], conf.fixed_frame_ids); - - conf.x0 = bp::extract(settings["x0"]); - conf.dt = bp::extract(settings["dt"]); - conf.mu = bp::extract(settings["mu"]); - conf.Lfoot = bp::extract(settings["Lfoot"]); - conf.Wfoot = bp::extract(settings["Wfoot"]); - - conf.force_size = bp::extract(settings["force_size"]); - conf.w_qref = bp::extract(settings["w_qref"]); - conf.w_footpose = bp::extract(settings["w_footpose"]); - conf.w_centroidal = bp::extract(settings["w_centroidal"]); - conf.w_baserot = bp::extract(settings["w_baserot"]); - conf.w_force = bp::extract(settings["w_force"]); - conf.verbose = bp::extract(settings["verbose"]); - - return new IKIDSolver(conf, model); - } - - /// \brief Visitor used to expose common elements in a low-level QP object. - struct ll_qp_visitor : bp::def_visitor - { - template - void visit(bp::class_ & cl) const - { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - cl.def("getA", &T::getA, "self"_a) - .def("getH", &T::getH, "self"_a) - .def("getC", &T::getC, "self"_a) - .def("getb", &T::getb, "self"_a) - .def("getg", &T::getg, "self"_a) - .def_readonly("qp", &T::qp_) - .def( - "getQP", &T::getQP, eigenpy::deprecated_member<>("Deprecated getter. Please use the `qp` class attribute."), - "self"_a) - .add_property("solved_acc", &T::solved_acc_) - .add_property("solved_forces", &T::solved_forces_) - .add_property("solved_torque", &T::solved_torque_); -#pragma GCC diagnostic pop - }; - }; - - void exposeIDSolver() - { - eigenpy::StdVectorPythonVisitor, true>::expose("StdVec_SE3"), - eigenpy::details::overload_base_get_item_for_std_vector>(); - bp::class_("IDSolver", bp::no_init) - .def(bp::init(("self"_a, "settings", "model"))) - .def("__init__", bp::make_constructor(&create_idsolver)) - .def("solveQP", &IDSolver::solveQP, ("self"_a, "data", "contact_state", "v", "a", "tau", "forces", "M")) - .def(ll_qp_visitor()); - } - - void exposeIKIDSolver() - { - eigenpy::StdVectorPythonVisitor, true>::expose("StdVec_SE3"), - eigenpy::details::overload_base_get_item_for_std_vector>(); - bp::class_("IKIDSolver", bp::no_init) - .def(bp::init(("self"_a, "settings", "model"))) - .def("__init__", bp::make_constructor(&create_ikidsolver)) - .def( - "solve_qp", &IKIDSolver::solve_qp, - bp::args("self", "data", "contact_state", "x_measured", "forces", "dH", "M")) - .def( - "computeDifferences", &IKIDSolver::computeDifferences, - ("self"_a, "data", "x_measured", "foot_refs", "foot_refs_next")) - .def(ll_qp_visitor()); - } - - } // namespace python -} // namespace simple_mpc diff --git a/bindings/expose-robot-handler.cpp b/bindings/expose-robot-handler.cpp index 2e55ddfa..459dd793 100644 --- a/bindings/expose-robot-handler.cpp +++ b/bindings/expose-robot-handler.cpp @@ -23,17 +23,20 @@ namespace simple_mpc bp::class_( "RobotModelHandler", bp::init( bp::args("self", "model", "reference_configuration_name", "base_frame_name"))) - .def("addFoot", &RobotModelHandler::addFoot) + .def("addPointFoot", &RobotModelHandler::addPointFoot) + .def("addQuadFoot", &RobotModelHandler::addQuadFoot) .def("setFootReferencePlacement", &RobotModelHandler::setFootReferencePlacement) .def("difference", &RobotModelHandler::difference) + .def("getBaseFrameName", &RobotModelHandler::getBaseFrameName) .def("getBaseFrameId", &RobotModelHandler::getBaseFrameId) .def("getReferenceState", &RobotModelHandler::getReferenceState) + .def("getFeetNb", &RobotModelHandler::getFeetNb) .def("getFootNb", &RobotModelHandler::getFootNb) - .def("getFeetIds", &RobotModelHandler::getFeetIds, bp::return_internal_reference<>()) - .def("getFootName", &RobotModelHandler::getFootName, bp::return_internal_reference<>()) - .def("getFeetNames", &RobotModelHandler::getFeetNames, bp::return_internal_reference<>()) - .def("getFootId", &RobotModelHandler::getFootId) - .def("getRefFootId", &RobotModelHandler::getRefFootId) + .def("getFeetFrameIds", &RobotModelHandler::getFeetFrameIds, bp::return_internal_reference<>()) + .def("getFootFrameName", &RobotModelHandler::getFootFrameName, bp::return_internal_reference<>()) + .def("getFeetFrameNames", &RobotModelHandler::getFeetFrameNames, bp::return_internal_reference<>()) + .def("getFootFrameId", &RobotModelHandler::getFootFrameId) + .def("getFootRefFrameId", &RobotModelHandler::getFootRefFrameId) .def("getMass", &RobotModelHandler::getMass) .def("getModel", &RobotModelHandler::getModel, bp::return_internal_reference<>()); @@ -41,9 +44,11 @@ namespace simple_mpc bp::class_( "RobotDataHandler", bp::init(bp::args("self", "model_handler"))) - .def("updateInternalData", &RobotDataHandler::updateInternalData) + .def( + "updateInternalData", static_cast( + &RobotDataHandler::updateInternalData)) .def("updateJacobiansMassMatrix", &RobotDataHandler::updateJacobiansMassMatrix) - .def("getRefFootPose", &RobotDataHandler::getRefFootPose, bp::return_internal_reference<>()) + .def("getFootRefPose", &RobotDataHandler::getFootRefPose, bp::return_internal_reference<>()) .def("getFootPose", &RobotDataHandler::getFootPose, bp::return_internal_reference<>()) .def("getBaseFramePose", &RobotDataHandler::getBaseFramePose, bp::return_internal_reference<>()) .def("getModelHandler", &RobotDataHandler::getModelHandler, bp::return_internal_reference<>()) diff --git a/bindings/module.cpp b/bindings/module.cpp index 1871c79d..c1c7aaa1 100644 --- a/bindings/module.cpp +++ b/bindings/module.cpp @@ -19,6 +19,7 @@ namespace simple_mpc::python void exposeIKIDSolver(); void exposeInterpolator(); void exposeFrictionCompensation(); + void exposeInverseDynamics(); /* PYTHON MODULE */ BOOST_PYTHON_MODULE(simple_mpc_pywrap) @@ -35,9 +36,8 @@ namespace simple_mpc::python exposeCentroidalOcp(); exposeKinodynamicsOcp(); exposeMPC(); - exposeIDSolver(); - exposeIKIDSolver(); exposeInterpolator(); + exposeInverseDynamics(); exposeFrictionCompensation(); } diff --git a/dependencies.cmake b/dependencies.cmake index 9a4b9e04..bca026d7 100644 --- a/dependencies.cmake +++ b/dependencies.cmake @@ -3,6 +3,7 @@ ADD_PROJECT_DEPENDENCY(OpenMP REQUIRED) ADD_PROJECT_DEPENDENCY(proxsuite REQUIRED) ADD_PROJECT_DEPENDENCY(pinocchio REQUIRED) ADD_PROJECT_DEPENDENCY(aligator 0.15.0 REQUIRED) +ADD_PROJECT_DEPENDENCY(tsid REQUIRED) function(get_ndcurves) find_package(ndcurves QUIET) diff --git a/environment-devel.yaml b/environment-devel.yaml index 3bf00855..dd728f5a 100644 --- a/environment-devel.yaml +++ b/environment-devel.yaml @@ -28,4 +28,5 @@ dependencies: - scipy - tracy - urdfdom +- tsid[version='>=1.9.0'] - vcstool diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py index 38fe5c56..94c7109a 100644 --- a/examples/go2_fulldynamics.py +++ b/examples/go2_fulldynamics.py @@ -5,7 +5,6 @@ RobotDataHandler, FullDynamicsOCP, MPC, - IDSolver, Interpolator, FrictionCompensation ) @@ -24,14 +23,10 @@ # Create Model and Data handler model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name) -model_handler.addFoot("FL_foot", base_joint_name) -model_handler.addFoot("FR_foot", base_joint_name) -model_handler.addFoot("RL_foot", base_joint_name) -model_handler.addFoot("RR_foot", base_joint_name) -model_handler.setFootReferencePlacement("FL_foot", pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1]))) -model_handler.setFootReferencePlacement("FR_foot", pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1]))) -model_handler.setFootReferencePlacement("RL_foot", pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1]))) -model_handler.setFootReferencePlacement("RR_foot", pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1]))) +model_handler.addPointFoot("FL_foot", base_joint_name) +model_handler.addPointFoot("FR_foot", base_joint_name) +model_handler.addPointFoot("RL_foot", base_joint_name) +model_handler.addPointFoot("RR_foot", base_joint_name) data_handler = RobotDataHandler(model_handler) @@ -39,7 +34,7 @@ nv = model_handler.getModel().nv nu = nv - 6 force_size = 3 -nk = len(model_handler.getFeetNames()) +nk = model_handler.getFeetNb() nf = force_size gravity = np.array([0, 0, -9.81]) @@ -48,12 +43,12 @@ u0 = np.zeros(model_handler.getModel().nv - 6) w_basepos = [0, 0, 0, 0, 0, 0] -w_legpos = [10, 10, 10] +w_legpos = [1, 1, 1] w_basevel = [10, 10, 10, 10, 10, 10] w_legvel = [0.1, 0.1, 0.1] w_x = np.array(w_basepos + w_legpos * 4 + w_basevel + w_legvel * 4) -w_cent_lin = np.array([0.0, 0.0, 0]) +w_cent_lin = np.array([0.04, 0.04, 0]) w_cent_ang = np.array([0, 0, 0]) w_forces_lin = np.array([0.0001, 0.0001, 0.0001]) w_frame = np.diag(np.array([1000, 1000, 1000])) @@ -140,24 +135,6 @@ mpc.generateCycleHorizon(contact_phases) -""" Initialize whole-body inverse dynamics QP""" -contact_ids = model_handler.getFeetIds() -id_conf = dict( - contact_ids=contact_ids, - x0=model_handler.getReferenceState(), - mu=0.8, - Lfoot=0.01, - Wfoot=0.01, - force_size=3, - kd=0, - w_force=0, - w_acc=0, - w_tau=1, - verbose=False, -) - -qp = IDSolver(id_conf, model_handler.getModel()) - """ Friction """ fcompensation = FrictionCompensation(model_handler.getModel(), True) """ Interpolation """ @@ -183,7 +160,7 @@ x_measured = np.concatenate([q_meas, v_meas]) mpc.getDataHandler().updateInternalData(x_measured, False) -ref_foot_pose = [mpc.getDataHandler().getRefFootPose(mpc.getModelHandler().getFeetNames()[i]) for i in range(4)] +ref_foot_pose = [mpc.getDataHandler().getFootRefPose(i) for i in range(4)] for pose in ref_foot_pose: pose.translation[2] = 0 device.showQuadrupedFeet(*ref_foot_pose) @@ -248,20 +225,20 @@ forces_vec1 = mpc.getContactForces(1) contact_states = mpc.ocp_handler.getContactState(0) - force_FL.append(forces_vec0[:3]) - force_FR.append(forces_vec0[3:6]) - force_RL.append(forces_vec0[6:9]) - force_RR.append(forces_vec0[9:12]) + force_FL.append(forces_vec0[0,:]) + force_FR.append(forces_vec0[1,:]) + force_RL.append(forces_vec0[2,:]) + force_RR.append(forces_vec0[3,:]) - forces = [forces_vec0, forces_vec1] + forces = [forces_vec0.flatten(), forces_vec1.flatten()] # Flattening for interpolation ddqs = [a0, a1] xss = [mpc.xs[0], mpc.xs[1]] uss = [mpc.us[0], mpc.us[1]] - FL_measured.append(mpc.getDataHandler().getFootPose("FL_foot").translation) - FR_measured.append(mpc.getDataHandler().getFootPose("FR_foot").translation) - RL_measured.append(mpc.getDataHandler().getFootPose("RL_foot").translation) - RR_measured.append(mpc.getDataHandler().getFootPose("RR_foot").translation) + FL_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("FL_foot")).translation) + FR_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("FR_foot")).translation) + RL_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("RL_foot")).translation) + RR_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("RR_foot")).translation) FL_references.append(mpc.getReferencePose(0, "FL_foot").translation) FR_references.append(mpc.getReferencePose(0, "FR_foot").translation) RL_references.append(mpc.getReferencePose(0, "RL_foot").translation) @@ -277,7 +254,7 @@ x_interp = interpolator.interpolateState(delay, dt, xss) u_interp = interpolator.interpolateLinear(delay, dt, uss) acc_interp = interpolator.interpolateLinear(delay, dt, ddqs) - force_interp = interpolator.interpolateLinear(delay, dt, forces) + force_interp = interpolator.interpolateLinear(delay, dt, forces).reshape(4,3) q_meas, v_meas = device.measureState() x_measured = np.concatenate([q_meas, v_meas]) @@ -288,19 +265,8 @@ x_measured, x_interp ) - qp.solveQP( - mpc.getDataHandler().getData(), - contact_states, - x_measured[nq:], - acc_interp, - current_torque, - force_interp, - mpc.getDataHandler().getData().M, - ) - - qp_torque = qp.solved_torque.copy() - friction_torque = fcompensation.computeFriction(x_interp[nq + 6:], qp_torque) - device.execute(friction_torque) + friction_torque = fcompensation.computeFriction(x_interp[nq + 6:], current_torque) + device.execute(current_torque) u_multibody.append(copy.deepcopy(friction_torque)) x_multibody.append(x_measured) diff --git a/examples/go2_kinodynamics.py b/examples/go2_kinodynamics.py index 261b335d..1b0145d2 100644 --- a/examples/go2_kinodynamics.py +++ b/examples/go2_kinodynamics.py @@ -1,11 +1,10 @@ import numpy as np from bullet_robot import BulletRobot -from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC, IDSolver, Interpolator +from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC, Interpolator, KinodynamicsID, KinodynamicsIDSettings import example_robot_data as erd import pinocchio as pin import time import copy -from utils import save_trajectory # ####### CONFIGURATION ############ # Load robot @@ -15,14 +14,10 @@ # Create Model and Data handler model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name) -model_handler.addFoot("FL_foot", base_joint_name) -model_handler.addFoot("FR_foot", base_joint_name) -model_handler.addFoot("RL_foot", base_joint_name) -model_handler.addFoot("RR_foot", base_joint_name) -model_handler.setFootReferencePlacement("FL_foot", pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1]))) -model_handler.setFootReferencePlacement("FR_foot", pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1]))) -model_handler.setFootReferencePlacement("RL_foot", pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1]))) -model_handler.setFootReferencePlacement("RR_foot", pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1]))) +model_handler.addPointFoot("FL_foot", base_joint_name) +model_handler.addPointFoot("FR_foot", base_joint_name) +model_handler.addPointFoot("RL_foot", base_joint_name) +model_handler.addPointFoot("RR_foot", base_joint_name) data_handler = RobotDataHandler(model_handler) nq = model_handler.getModel().nq @@ -30,12 +25,12 @@ nu = nv - 6 nf = 12 force_size = 3 -nk = len(model_handler.getFeetNames()) +nk = model_handler.getFeetNb() gravity = np.array([0, 0, -9.81]) fref = np.zeros(force_size) fref[2] = -model_handler.getMass() / nk * gravity[2] u0 = np.concatenate((fref, fref, fref, fref, np.zeros(model_handler.getModel().nv - 6))) -dt = 0.01 +dt_mpc = 0.01 w_basepos = [0, 0, 100, 10, 10, 0] w_legpos = [1, 1, 1] @@ -64,7 +59,7 @@ w_centder = np.diag(np.concatenate((w_centder_lin, w_centder_ang))) problem_conf = dict( - timestep=dt, + timestep=dt_mpc, w_x=w_x, w_u=w_u, w_cent=w_cent, @@ -98,7 +93,7 @@ swing_apex=0.15, T_fly=T_ss, T_contact=T_ds, - timestep=dt, + timestep=dt_mpc, ) mpc = MPC(mpc_conf, dynproblem) @@ -134,33 +129,30 @@ contact_phases += [contact_phase_lift_FR] * T_ss mpc.generateCycleHorizon(contact_phases) -""" Initialize whole-body inverse dynamics QP""" -contact_ids = model_handler.getFeetIds() -id_conf = dict( - contact_ids=contact_ids, - x0=model_handler.getReferenceState(), - mu=0.8, - Lfoot=0.01, - Wfoot=0.01, - force_size=3, - kd=0, - w_force=100, - w_acc=1, - w_tau=0, - verbose=False, -) - -qp = IDSolver(id_conf, model_handler.getModel()) - """ Interpolation """ +N_simu = 10 # Number of substep the simulation does between two MPC computation +dt_simu = dt_mpc/N_simu interpolator = Interpolator(model_handler.getModel()) +""" Inverse Dynamics """ +kino_ID_settings = KinodynamicsIDSettings() +kino_ID_settings.kp_base = 7. +kino_ID_settings.kp_posture = 10. +kino_ID_settings.kp_contact = 10. +kino_ID_settings.w_base = 100. +kino_ID_settings.w_posture = 1. +kino_ID_settings.w_contact_force = 1. +kino_ID_settings.w_contact_motion = 1. + +kino_ID = KinodynamicsID(model_handler, dt_simu, kino_ID_settings) + + """ Initialize simulation""" device = BulletRobot( model_handler.getModel().names, erd.getModelPath(URDF_SUBPATH), URDF_SUBPATH, - 1e-3, + dt_simu, model_handler.getModel(), model_handler.getReferenceState()[:3], ) @@ -172,10 +164,10 @@ x_measured = np.concatenate([q_meas, v_meas]) device.showQuadrupedFeet( - mpc.getDataHandler().getFootPose("FL_foot"), - mpc.getDataHandler().getFootPose("FR_foot"), - mpc.getDataHandler().getFootPose("RL_foot"), - mpc.getDataHandler().getFootPose("RR_foot"), + mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("FL_foot")), + mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("FR_foot")), + mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("RL_foot")), + mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("RR_foot")), ) force_FL = [] @@ -197,12 +189,11 @@ solve_time = [] L_measured = [] -N_simu = 10 v = np.zeros(6) v[0] = 0.2 mpc.velocity_base = v -for t in range(300): - # print("Time " + str(t)) +for step in range(300): + # print("Time " + str(step)) land_LF = mpc.getFootLandCycle("FL_foot") land_RF = mpc.getFootLandCycle("RL_foot") takeoff_LF = mpc.getFootTakeoffCycle("FL_foot") @@ -222,10 +213,10 @@ force_RL.append(mpc.us[0][6:9]) force_RR.append(mpc.us[0][9:12]) - FL_measured.append(mpc.getDataHandler().getFootPose("FL_foot").translation) - FR_measured.append(mpc.getDataHandler().getFootPose("FR_foot").translation) - RL_measured.append(mpc.getDataHandler().getFootPose("RL_foot").translation) - RR_measured.append(mpc.getDataHandler().getFootPose("RR_foot").translation) + FL_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("FL_foot")).translation) + FR_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("FR_foot")).translation) + RL_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("RL_foot")).translation) + RR_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("RR_foot")).translation) FL_references.append(mpc.getReferencePose(0, "FL_foot").translation) FR_references.append(mpc.getReferencePose(0, "FR_foot").translation) RL_references.append(mpc.getReferencePose(0, "RL_foot").translation) @@ -254,30 +245,28 @@ mpc.getReferencePose(0, "RR_foot").translation, ) - for j in range(N_simu): - # time.sleep(0.01) - delay = j / float(N_simu) * dt + for sub_step in range(N_simu): + t = step * dt_mpc + sub_step * dt_simu + + delay = sub_step / float(N_simu) * dt_mpc + xs_interp = interpolator.interpolateState(delay, dt_mpc, xss) + acc_interp = interpolator.interpolateLinear(delay, dt_mpc, ddqs) + force_interp = interpolator.interpolateLinear(delay, dt_mpc, forces).reshape((4,3)) - acc_interp = interpolator.interpolateLinear(delay, dt, ddqs) - force_interp = interpolator.interpolateLinear(delay, dt, forces) + q_interp = xs_interp[:mpc.getModelHandler().getModel().nq] + v_interp = xs_interp[mpc.getModelHandler().getModel().nq:] + force_interp = [force_interp[i, :] for i in range(4)] q_meas, v_meas = device.measureState() x_measured = np.concatenate([q_meas, v_meas]) mpc.getDataHandler().updateInternalData(x_measured, True) - qp.solveQP( - mpc.getDataHandler().getData(), - contact_states, - x_measured[nq:], - acc_interp, - np.zeros(12), - force_interp, - mpc.getDataHandler().getData().M, - ) - - device.execute(qp.solved_torque) - u_multibody.append(copy.deepcopy(qp.solved_torque)) + kino_ID.setTarget(q_interp, v_interp, acc_interp, contact_states, force_interp) + tau_cmd = kino_ID.solve(t, q_meas, v_meas) + + device.execute(tau_cmd) + u_multibody.append(copy.deepcopy(tau_cmd)) x_multibody.append(x_measured) @@ -297,6 +286,6 @@ com_measured = np.array(com_measured) L_measured = np.array(L_measured) -save_trajectory(x_multibody, u_multibody, com_measured, force_FL, force_FR, force_RL, force_RR, solve_time, +""" save_trajectory(x_multibody, u_multibody, com_measured, force_FL, force_FR, force_RL, force_RR, solve_time, FL_measured, FR_measured, RL_measured, RR_measured, - FL_references, FR_references, RL_references, RR_references, L_measured, "kinodynamics") + FL_references, FR_references, RL_references, RR_references, L_measured, "kinodynamics") """ diff --git a/examples/talos_centroidal.py b/examples/talos_centroidal.py index e948e520..975c14a6 100644 --- a/examples/talos_centroidal.py +++ b/examples/talos_centroidal.py @@ -2,9 +2,8 @@ import pinocchio as pin import example_robot_data as erd from bullet_robot import BulletRobot -from simple_mpc import RobotModelHandler, RobotDataHandler, CentroidalOCP, MPC, IKIDSolver +from simple_mpc import RobotModelHandler, RobotDataHandler, CentroidalOCP, MPC, CentroidalID, CentroidalIDSettings, Interpolator from utils import loadTalos -import time # RobotWrapper URDF_SUBPATH = "/talos_data/robots/talos_reduced.urdf" @@ -14,21 +13,27 @@ rmodelComplete, rmodel, qComplete, q0 = loadTalos() # Create Model and Data handler +foot_points = np.array([ + [0.1, 0.075, 0], + [-0.1, 0.075, 0], + [-0.1, -0.075, 0], + [0.1, -0.075, 0], +]) model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name) -model_handler.addFoot("left_sole_link", base_joint_name) -model_handler.addFoot("right_sole_link", base_joint_name) -model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1]))) -model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1]))) - +model_handler.addQuadFoot("left_sole_link", base_joint_name, foot_points) +model_handler.addQuadFoot("right_sole_link", base_joint_name, foot_points) data_handler = RobotDataHandler(model_handler) +nq = model_handler.getModel().nq +nv = model_handler.getModel().nv + x0 = np.zeros(9) x0[:3] = data_handler.getData().com[0] -nu = model_handler.getModel().nv - 6 + len(model_handler.getFeetNames()) * 6 +nu = model_handler.getModel().nv - 6 + model_handler.getFeetNb() * 6 gravity = np.array([0, 0, -9.81]) fref = np.zeros(6) -fref[2] = -model_handler.getMass() / len(model_handler.getFeetNames()) * gravity[2] +fref[2] = -model_handler.getMass() / model_handler.getFeetNb() * gravity[2] u0 = np.concatenate((fref, fref)) w_control_linear = np.ones(3) * 0.001 @@ -44,8 +49,10 @@ w_angular_mom = np.diag(np.array([0.1, 0.1, 1000])) w_angular_acc = 0.01 * np.eye(3) +dt_mpc = 0.01 + problem_conf = dict( - timestep=0.01, + timestep=dt_mpc, w_u=w_u, w_com=w_com, w_linear_mom=w_linear_mom, @@ -101,56 +108,36 @@ mpc.generateCycleHorizon(contact_phases) -""" Initialize inverse dynamics QP """ -g_basepos = [0, 0, 0, 10, 10, 10] -g_legpos = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] -g_torsopos = [1, 1] -g_armpos = [10, 10, 100, 10] - -g_q = np.array(g_basepos + g_legpos * 2 + g_torsopos + g_armpos * 2) * 10 - -g_p = np.array([2000, 2000, 2000, 2000, 2000, 2000]) -g_b = np.array([10, 10, 10]) - -Kp_gains = [g_q, g_p, g_b] -Kd_gains = [2 * np.sqrt(g_q), 2 * np.sqrt(g_p), 2 * np.sqrt(g_b)] -contact_ids = model_handler.getFeetIds() -fixed_frame_ids = [model_handler.getBaseFrameId(), model_handler.getModel().getFrameId("torso_2_link")] -ikid_conf = dict( - Kp_gains=Kp_gains, - Kd_gains=Kd_gains, - contact_ids=contact_ids, - fixed_frame_ids=fixed_frame_ids, - x0=model_handler.getReferenceState(), - dt=0.01, - mu=0.8, - Lfoot=0.1, - Wfoot=0.075, - force_size=6, - w_qref=500, - w_footpose=50000, - w_centroidal=10, - w_baserot=1000, - w_force=100, - verbose=False, -) - -qp = IKIDSolver(ikid_conf, model_handler.getModel()) +""" Interpolation """ +N_simu = 10 # Number of substep the simulation does between two MPC computation +dt_simu = dt_mpc/N_simu +interpolator = Interpolator(model_handler.getModel()) + +""" Inverse Dynamics """ +centroidal_ID_settings = CentroidalIDSettings() +centroidal_ID_settings.kp_base = 7. +centroidal_ID_settings.kp_com = 7. +centroidal_ID_settings.kp_posture = 10. +centroidal_ID_settings.kp_contact = 10. +centroidal_ID_settings.w_base = 50. +centroidal_ID_settings.w_com = 100. +centroidal_ID_settings.w_posture = 1. +centroidal_ID_settings.w_contact_force = 1e-6 +centroidal_ID_settings.w_contact_motion = 1e-3 + +centroidal_ID = CentroidalID(model_handler, dt_simu, centroidal_ID_settings) """ Initialize simulation""" device = BulletRobot( model_handler.getModel().names, erd.getModelPath(URDF_SUBPATH), URDF_SUBPATH, - 1e-3, + dt_simu, model_handler.getModel(), model_handler.getReferenceState()[:3], ) device.initializeJoints(model_handler.getModel().referenceConfigurations[reference_configuration_name]) -device.changeCamera(1.0, 90, -5, [1.5, 0, 1]) - -nq = mpc.getModelHandler().getModel().nq -nv = mpc.getModelHandler().getModel().nv +device.changeCamera(1.0, 50, -15, [1.7, -0.5, 1.2]) q_meas, v_meas = device.measureState() x_measured = np.concatenate([q_meas, v_meas]) @@ -158,19 +145,18 @@ Tmpc = len(contact_phases) nk = 2 force_size = 6 -x_centroidal = mpc.getDataHandler().getCentroidalState() device.showTargetToTrack( - mpc.getDataHandler().getFootPose("left_sole_link"), - mpc.getDataHandler().getFootPose("right_sole_link"), + mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("left_sole_link")), + mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("right_sole_link")), ) v = np.zeros(6) v[0] = 0.2 mpc.velocity_base = v -for t in range(600): - # print("Time " + str(t)) - if t == 400: +for step in range(600): + # print("Time " + str(step)) + if step == 400: print("SWITCH TO STAND") mpc.switchToStand() @@ -193,36 +179,33 @@ ) contact_states = mpc.ocp_handler.getContactState(0) - foot_ref = [mpc.getReferencePose(0, name) for name in model_handler.getFeetNames()] - foot_ref_next = [mpc.getReferencePose(1, name) for name in model_handler.getFeetNames()] - dH = mpc.getStateDerivative(0)[3:9] - qp.computeDifferences( - mpc.getDataHandler().getData(), x_measured, foot_ref, foot_ref_next - ) + feet_ref = [mpc.getReferencePose(0, name) for name in model_handler.getFeetFrameNames()] + feet_ref_next = [mpc.getReferencePose(1, name) for name in model_handler.getFeetFrameNames()] + + pos_com = mpc.xs[0][:3] + pos_com_next = mpc.xs[1][:3] + + forces = mpc.us[0][: nk * force_size] + forces_next = mpc.us[1][: nk * force_size] + + for sub_step in range(N_simu): + t = step * dt_mpc + sub_step * dt_simu - for j in range(10): - time.sleep(0.001) q_meas, v_meas = device.measureState() x_measured = np.concatenate([q_meas, v_meas]) - mpc.getDataHandler().updateInternalData(x_measured, True) - x_centroidal = mpc.getDataHandler().getCentroidalState() - state_diff = mpc.xs[0] - x_centroidal - - forces = ( - mpc.us[0][: nk * force_size] - - 1 * mpc.solver.results.controlFeedbacks()[0] @ state_diff - ) + # Interpolate solution + pos_com_interp = interpolator.interpolateLinear(sub_step, N_simu, [pos_com, pos_com_next]) + v_com = (pos_com_next - pos_com) / dt_simu + feet_ref_interp = [pin.SE3.Interpolate(foot_ref, foot_ref_next, (1.0 * sub_step)/N_simu) for foot_ref, foot_ref_next in zip(feet_ref, feet_ref_next)] + feet_velocity = [ pin.log6(foot_ref.actInv(foot_ref_next)) / dt_simu for foot_ref, foot_ref_next in zip(feet_ref, feet_ref_next)] - qp.solve_qp( - mpc.getDataHandler().getData(), - contact_states, - x_measured[nq:], - forces, - dH, - mpc.getDataHandler().getData().M, - ) + forces_interp = interpolator.interpolateLinear(sub_step, N_simu, [forces, forces_next]) + forces_interp = forces_interp.reshape(2,6) + forces_interp = [forces_interp[i, :] for i in range(2)] + centroidal_ID.setTarget(pos_com_interp, v_com, feet_ref_interp, feet_velocity, contact_states, forces_interp) + tau_cmd = centroidal_ID.solve(t, q_meas, v_meas) - device.execute(qp.solved_torque) + device.execute(tau_cmd) diff --git a/examples/talos_fulldynamics.py b/examples/talos_fulldynamics.py index cee4a643..3c0f0c96 100644 --- a/examples/talos_fulldynamics.py +++ b/examples/talos_fulldynamics.py @@ -21,11 +21,15 @@ rmodelComplete, rmodel, qComplete, q0 = loadTalos() # Create Model and Data handler +foot_points = np.array([ + [0.1, 0.075, 0], + [-0.1, 0.075, 0], + [-0.1, -0.075, 0], + [0.1, -0.075, 0], +]) model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name) -model_handler.addFoot("left_sole_link", base_joint_name) -model_handler.addFoot("right_sole_link", base_joint_name) -model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1]))) -model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1]))) +model_handler.addQuadFoot("left_sole_link", base_joint_name, foot_points) +model_handler.addQuadFoot("right_sole_link", base_joint_name, foot_points) data_handler = RobotDataHandler(model_handler) @@ -152,8 +156,8 @@ takeoff_LF = -1 takeoff_RF = -1 device.showTargetToTrack( - mpc.getDataHandler().getFootPose("left_sole_link"), - mpc.getDataHandler().getFootPose("right_sole_link"), + mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("left_sole_link")), + mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("right_sole_link")), ) v = np.zeros(6) diff --git a/examples/talos_kinodynamics.py b/examples/talos_kinodynamics.py index a47aafe2..6600dd4d 100644 --- a/examples/talos_kinodynamics.py +++ b/examples/talos_kinodynamics.py @@ -4,7 +4,7 @@ from bullet_robot import BulletRobot import time from utils import loadTalos -from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC, IDSolver +from simple_mpc import RobotModelHandler, RobotDataHandler, Interpolator, KinodynamicsOCP, MPC, KinodynamicsID, KinodynamicsIDSettings # RobotWrapper URDF_SUBPATH = "/talos_data/robots/talos_reduced.urdf" @@ -14,11 +14,15 @@ rmodelComplete, rmodel, qComplete, q0 = loadTalos() # Create Model and Data handler +foot_points = np.array([ + [0.1, 0.075, 0], + [-0.1, 0.075, 0], + [-0.1, -0.075, 0], + [0.1, -0.075, 0], +]) model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name) -model_handler.addFoot("left_sole_link", base_joint_name) -model_handler.addFoot("right_sole_link", base_joint_name) -model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1]))) -model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1]))) +model_handler.addQuadFoot("left_sole_link", base_joint_name, foot_points) +model_handler.addQuadFoot("right_sole_link", base_joint_name, foot_points) data_handler = RobotDataHandler(model_handler) nq = model_handler.getModel().nq @@ -30,7 +34,7 @@ """ Define kinodynamics problem """ gravity = np.array([0, 0, -9.81]) fref = np.zeros(6) -fref[2] = -model_handler.getMass() / len(model_handler.getFeetNames()) * gravity[2] +fref[2] = -model_handler.getMass() / model_handler.getFeetNb() * gravity[2] u0 = np.concatenate((fref, fref, np.zeros(nv - 6))) w_basepos = [0, 0, 1000, 1000, 1000, 1000] @@ -73,8 +77,10 @@ w_centder_ang = np.ones(3) * 0.1 w_centder = np.diag(np.concatenate((w_centder_lin, w_centder_ang))) +dt_mpc = 0.01 + problem_conf = dict( - timestep=0.01, + timestep=dt_mpc, w_x=w_x, w_u=w_u, w_cent=w_cent, @@ -136,30 +142,29 @@ mpc.generateCycleHorizon(contact_phases) -""" Initialize whole-body inverse dynamics QP""" -contact_ids = model_handler.getFeetIds() -id_conf = dict( - contact_ids=contact_ids, - x0=model_handler.getReferenceState(), - mu=0.8, - Lfoot=0.1, - Wfoot=0.075, - force_size=6, - kd=0, - w_force=100, - w_acc=1, - w_tau=0, - verbose=False, -) +""" Interpolation """ +N_simu = 10 # Number of substep the simulation does between two MPC computation +dt_simu = dt_mpc/N_simu +interpolator = Interpolator(model_handler.getModel()) -qp = IDSolver(id_conf, model_handler.getModel()) +""" Inverse Dynamics """ +kino_ID_settings = KinodynamicsIDSettings() +kino_ID_settings.kp_base = 7. +kino_ID_settings.kp_posture = 10. +kino_ID_settings.kp_contact = 10. +kino_ID_settings.w_base = 100. +kino_ID_settings.w_posture = 1. +kino_ID_settings.w_contact_force = 0.001 +kino_ID_settings.w_contact_motion = 1. + +kino_ID = KinodynamicsID(model_handler, dt_simu, kino_ID_settings) """ Initialize simulation""" device = BulletRobot( model_handler.getModel().names, erd.getModelPath(URDF_SUBPATH), URDF_SUBPATH, - 1e-3, + dt_simu, model_handler.getModel(), model_handler.getReferenceState()[:3], ) @@ -174,16 +179,16 @@ force_size = 6 device.showTargetToTrack( - mpc.getDataHandler().getFootPose("left_sole_link"), - mpc.getDataHandler().getFootPose("right_sole_link"), + mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("left_sole_link")), + mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("right_sole_link")), ) v = np.zeros(6) v[0] = 0.2 mpc.velocity_base = v -for t in range(600): - # print("Time " + str(t)) - if t == 400: +for step in range(600): + # print("Time " + str(step)) + if step == 400: print("SWITCH TO STAND") mpc.switchToStand() @@ -210,29 +215,35 @@ forces1 = mpc.us[1][: nk * force_size] contact_states = mpc.ocp_handler.getContactState(0) + forces = [forces0, forces1] + ddqs = [a0, a1] + xss = [mpc.xs[0], mpc.xs[1]] + uss = [mpc.us[0], mpc.us[1]] + device.moveMarkers( mpc.getReferencePose(0, "left_sole_link").translation, mpc.getReferencePose(0, "right_sole_link").translation, ) - for j in range(10): + for sub_step in range(N_simu): + t = step * dt_mpc + sub_step * dt_simu + + delay = sub_step / float(N_simu) * dt_mpc + xs_interp = interpolator.interpolateState(delay, dt_mpc, xss) + acc_interp = interpolator.interpolateLinear(delay, dt_mpc, ddqs) + force_interp = interpolator.interpolateLinear(delay, dt_mpc, forces).reshape((2,6)) + force_interp = [force_interp[0, :], force_interp[1, :]] + + q_interp = xs_interp[:mpc.getModelHandler().getModel().nq] + v_interp = xs_interp[mpc.getModelHandler().getModel().nq:] + q_meas, v_meas = device.measureState() x_measured = np.concatenate([q_meas, v_meas]) - state_diff = model_handler.difference(x_measured, mpc.xs[0]) mpc.getDataHandler().updateInternalData(x_measured, True) - a_interp = (10 - j) / 10 * a0 + j / 10 * a1 - f_interp = (10 - j) / 10 * forces0 + j / 10 * forces1 - - qp.solveQP( - mpc.getDataHandler().getData(), - contact_states, - x_measured[nq:], - a_interp, - np.zeros(nu), - f_interp, - mpc.getDataHandler().getData().M, - ) + # TODO: take 6D forces into account + kino_ID.setTarget(q_interp, v_interp, acc_interp, contact_states, force_interp) + tau_cmd = kino_ID.solve(t, q_meas, v_meas) - device.execute(qp.solved_torque) + device.execute(tau_cmd) diff --git a/include/simple-mpc/inverse-dynamics/centroidal-id.hpp b/include/simple-mpc/inverse-dynamics/centroidal-id.hpp new file mode 100644 index 00000000..1b2e194c --- /dev/null +++ b/include/simple-mpc/inverse-dynamics/centroidal-id.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include +#include + +namespace simple_mpc +{ + + class CentroidalID : public KinodynamicsID + { + public: + typedef Eigen::VectorXd TargetContactForce; + // typedef Eigen::Matrix TargetContactForce; + typedef PINOCCHIO_ALIGNED_STD_VECTOR(pinocchio::SE3) FeetPoseVector; + typedef PINOCCHIO_ALIGNED_STD_VECTOR(pinocchio::Motion) FeetVelocityVector; + + struct Settings : public KinodynamicsID::Settings + { + // Tasks gains + double kp_com = 0.; + double kp_feet_tracking = 0.; + + // Tasks weights + double w_com = -1.; // Disabled by default + double w_feet_tracking = -1.; // Disabled by default + }; + + CentroidalID(const RobotModelHandler & model_handler, double control_dt, const Settings settings); + CentroidalID(const CentroidalID &) = delete; + + void setTarget( + const Eigen::Ref> & com_position, + const Eigen::Ref> & com_velocity, + const FeetPoseVector & feet_pose, + const FeetVelocityVector & feet_velocity, + const std::vector & contact_state_target, + const std::vector & f_target); + + using KinodynamicsID::getAccelerations; + using KinodynamicsID::solve; + + private: + using KinodynamicsID::KinodynamicsID; + using KinodynamicsID::setTarget; + + public: + // Order matters to be instantiated in the right order + const Settings settings_; + + private: + std::unique_ptr comTask_; + std::vector trackingTasks_; + tsid::trajectories::TrajectorySample sampleCom_; + std::vector trackingSamples_; + std::vector feet_tracked_; + }; + +} // namespace simple_mpc diff --git a/include/simple-mpc/inverse-dynamics/kinodynamics-id.hpp b/include/simple-mpc/inverse-dynamics/kinodynamics-id.hpp new file mode 100644 index 00000000..c36f9586 --- /dev/null +++ b/include/simple-mpc/inverse-dynamics/kinodynamics-id.hpp @@ -0,0 +1,90 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace simple_mpc +{ + + class KinodynamicsID + { + public: + typedef Eigen::VectorXd TargetContactForce; + + struct Settings + { + + // Physical quantities + double friction_coefficient = 0.6; + + double contact_weight_ratio_max = + 10.0; // Max force for one foot contact (express as a multiple of the robot weight) + + double contact_weight_ratio_min = + 0.01; // Min force for one foot contact (express as a multiple of the robot weight) + + // Tasks gains + double kp_base = 0.; + double kp_posture = 0.; + double kp_contact = 0.; + + // Tasks weights + double w_base = -1.; // Disabled by default + double w_posture = -1.; // Disabled by default + double w_contact_motion = -1.; // Disabled by default + double w_contact_force = -1.; // Disabled by default + + ///< Are the contact motion = 0 handled as a hard contraint (true) or a cost (if false) + bool contact_motion_equality = false; + }; + + KinodynamicsID(const simple_mpc::RobotModelHandler & model_handler, double control_dt, const Settings settings); + KinodynamicsID(const KinodynamicsID &) = delete; + + void setTarget( + const Eigen::Ref & q_target, + const Eigen::Ref & v_target, + const Eigen::Ref & a_target, + const std::vector & contact_state_target, + const std::vector & f_target); + + void solve( + const double t, + const Eigen::Ref & q_meas, + const Eigen::Ref & v_meas, + Eigen::Ref tau_res); + + void getAccelerations(Eigen::Ref a); + + public: + // Order matters to be instantiated in the right order + const Settings settings_; + const simple_mpc::RobotModelHandler & model_handler_; + + protected: + // Order still matters here + simple_mpc::RobotDataHandler data_handler_; + tsid::robots::RobotWrapper robot_; + tsid::InverseDynamicsFormulationAccForce formulation_; + + std::vector active_tsid_contacts_; + std::vector> tsid_contacts; + std::unique_ptr postureTask_; + std::unique_ptr baseTask_; + std::unique_ptr boundsTask_; + std::unique_ptr actuationTask_; + tsid::solvers::SolverProxQP solver_; + tsid::solvers::HQPOutput last_solution_; + tsid::trajectories::TrajectorySample samplePosture_; + tsid::trajectories::TrajectorySample sampleBase_; + }; + +} // namespace simple_mpc diff --git a/include/simple-mpc/mpc.hpp b/include/simple-mpc/mpc.hpp index bb9f1150..f7efccc9 100644 --- a/include/simple-mpc/mpc.hpp +++ b/include/simple-mpc/mpc.hpp @@ -175,7 +175,7 @@ namespace simple_mpc * @brief Return contact forces for a full dynamics MPC problem * @warning Only work with fulldynamics OCP handler */ - const Eigen::VectorXd getContactForces(const std::size_t t); + const Eigen::Matrix getContactForces(const std::size_t t); void switchToWalk(const Vector6d & velocity_base); diff --git a/include/simple-mpc/qp-solvers.hpp b/include/simple-mpc/qp-solvers.hpp deleted file mode 100644 index e27b1b1e..00000000 --- a/include/simple-mpc/qp-solvers.hpp +++ /dev/null @@ -1,243 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// BSD 3-Clause License -// -// Copyright (C) 2024, INRIA -// Copyright note valid unless otherwise stated in individual files. -// All rights reserved. -/////////////////////////////////////////////////////////////////////////////// -/** - * @file qp-solvers.hpp - * @brief QP inverse dynamics for MPC schemes - */ - -#ifndef SIMPLE_MPC_QP_SOLVERS_HPP_ -#define SIMPLE_MPC_QP_SOLVERS_HPP_ - -#include "simple-mpc/deprecated.hpp" -#include "simple-mpc/fwd.hpp" -#include -#include - -namespace simple_mpc -{ - using namespace proxsuite; - - struct IDSettings - { - std::vector contact_ids; //< Index of contacts - double mu; //< Friction parameter - double Lfoot; //< Half-length of foot (if contact 6D) - double Wfoot; //< Half-width of foot (if contact 6D) - long force_size; //< Dimension of contact forces - double kd; //< Baumgarte coefficient - double w_acc; //< Weight for acceleration regularization - double w_tau; //< Weight for torque regularization - double w_force; //< Weight for force regularization - bool verbose; //< Print solver information - }; - - struct IKIDSettings - { - std::vector Kp_gains; //< Proportional gains - std::vector Kd_gains; //< Derivative gains - std::vector contact_ids; //< Index of contacts - std::vector fixed_frame_ids; //< Index of frames kept fixed - Eigen::VectorXd x0; //< Reference state - double dt; //< Integration timestep - double mu; //< Friction parameter - double Lfoot; //< Half-length of foot (if contact 6D) - double Wfoot; //< Half-width of foot (if contact 6D) - long force_size; //< Dimension of contact forces - double w_qref; //< Weight for configuration regularization - double w_footpose; //< Weight for foot placement - double w_centroidal; //< Weight for CoM tracking - double w_baserot; //< Weight for base rotation - double w_force; //< Weight for force regularization - bool verbose; //< Print solver information - }; - - class IDSolver - { - public: - proxqp::dense::QP qp_; - - protected: - IDSettings settings_; - pin::Model model_; - int force_dim_; - int nforcein_; - int nk_; - - Eigen::MatrixXd H_; - Eigen::MatrixXd A_; - Eigen::MatrixXd C_; - Eigen::MatrixXd S_; - Eigen::MatrixXd Cmin_; - Eigen::VectorXd b_; - Eigen::VectorXd g_; - Eigen::VectorXd l_; - Eigen::VectorXd u_; - - Eigen::MatrixXd Jc_; - Eigen::VectorXd gamma_; - Eigen::MatrixXd Jdot_; - - // Internal matrix computation - void computeMatrices( - pin::Data & data, - const std::vector & contact_state, - const ConstVectorRef & v, - const ConstVectorRef & a, - const ConstVectorRef & tau, - const ConstVectorRef & forces, - const ConstMatrixRef & M); - - public: - explicit IDSolver(const IDSettings & settings, const pin::Model & model); - - void solveQP( - pin::Data & data, - const std::vector & contact_state, - const ConstVectorRef & v, - const ConstVectorRef & a, - const ConstVectorRef & tau, - const ConstVectorRef & forces, - const ConstMatrixRef & M); - - SIMPLE_MPC_DEPRECATED - proxqp::dense::Model getQP() - { - return qp_.model; - } - - Eigen::MatrixXd getA() - { - return qp_.model.A; - } - Eigen::MatrixXd getH() - { - return qp_.model.H; - } - Eigen::MatrixXd getC() - { - return qp_.model.C; - } - Eigen::VectorXd getg() - { - return qp_.model.g; - } - Eigen::VectorXd getb() - { - return qp_.model.b; - } - - // QP results - Eigen::VectorXd solved_forces_; - Eigen::VectorXd solved_acc_; - Eigen::VectorXd solved_torque_; - }; - - class IKIDSolver - { - public: - proxqp::dense::QP qp_; - - protected: - IKIDSettings settings_; - pin::Model model_; - int force_dim_; - int nforcein_; - int nk_; - int fs_; - - Eigen::MatrixXd H_; - Eigen::MatrixXd A_; - Eigen::MatrixXd C_; - Eigen::MatrixXd S_; - Eigen::MatrixXd Cmin_; - Eigen::VectorXd b_; - Eigen::VectorXd g_; - Eigen::VectorXd l_; - Eigen::VectorXd u_; - Eigen::VectorXd l_box_; - Eigen::VectorXd u_box_; - pin::Motion Jvel_; - - Eigen::MatrixXd Jfoot_; - Eigen::MatrixXd dJfoot_; - Eigen::MatrixXd Jframe_; - Eigen::MatrixXd dJframe_; - - std::vector foot_diffs_; - std::vector dfoot_diffs_; - std::vector frame_diffs_; - std::vector dframe_diffs_; - Eigen::VectorXd q_diff_; - Eigen::VectorXd dq_diff_; - - // Internal matrix computation - void computeMatrices( - pin::Data & data, - const std::vector & contact_state, - const ConstVectorRef & x_measured, - const ConstVectorRef & forces, - const ConstVectorRef & dH, - const ConstMatrixRef & M); - - public: - explicit IKIDSolver(const IKIDSettings & settings, const pin::Model & model); - - void computeDifferences( - pin::Data & data, - const ConstVectorRef & x_measured, - const std::vector & foot_refs, - const std::vector & foot_refs_next); - - void solve_qp( - pin::Data & data, - const std::vector & contact_state, - const ConstVectorRef & x_measured, - const ConstVectorRef & forces, - const ConstVectorRef & dH, - const ConstMatrixRef & M); - - SIMPLE_MPC_DEPRECATED - proxqp::dense::Model getQP() - { - return qp_.model; - } - - Eigen::MatrixXd getA() - { - return qp_.model.A; - } - Eigen::MatrixXd getH() - { - return qp_.model.H; - } - Eigen::MatrixXd getC() - { - return qp_.model.C; - } - Eigen::VectorXd getg() - { - return qp_.model.g; - } - Eigen::VectorXd getb() - { - return qp_.model.b; - } - - // QP results - Eigen::VectorXd solved_forces_; - Eigen::VectorXd solved_acc_; - Eigen::VectorXd solved_torque_; - }; - -} // namespace simple_mpc - -/* --- Details -------------------------------------------------------------- */ -/* --- Details -------------------------------------------------------------- */ -/* --- Details -------------------------------------------------------------- */ - -#endif // SIMPLE_MPC_QP_SOLVERS_HPP_ diff --git a/include/simple-mpc/robot-handler.hpp b/include/simple-mpc/robot-handler.hpp index 734ae3ec..b9f77463 100644 --- a/include/simple-mpc/robot-handler.hpp +++ b/include/simple-mpc/robot-handler.hpp @@ -27,42 +27,12 @@ namespace simple_mpc */ struct RobotModelHandler { - private: - /** - * @brief Model to be used by ocp - */ - Model model_; - - /** - * @brief Robot total mass - */ - double mass_; - - /** - * @brief Reference configuration and velocity (most probably null velocity) - * to use - */ - Eigen::VectorXd reference_state_; - - /** - * @brief Names of the frames to be in contact with the environment - */ - std::vector feet_names_; - - /** - * @brief Ids of the frames to be in contact with the environment - */ - std::vector feet_ids_; - - /** - * @brief Ids of the frames that are reference position for the feet - */ - std::vector ref_feet_ids_; - - /** - * @brief Base frame id - */ - pinocchio::FrameIndex base_id_; + enum FootType + { + POINT, + QUAD + }; + typedef Eigen::Matrix ContactPointsMatrix; public: /** @@ -77,26 +47,45 @@ namespace simple_mpc const Model & model, const std::string & reference_configuration_name, const std::string & base_frame_name); /** - * @brief + * @brief Create a point foot, that can apply 3D force to the ground. (in comparison to 6D foot) * * @param foot_name Frame name that will be used a a foot - * @param reference_frame_name Frame to which the foot reference + * @param reference_parent_frame_name Frame to which the foot reference * frame will be attached. * + * @return the foot number + * * @note The foot placement will be set by default using the reference configuration of the RobotModelHandler */ - FrameIndex addFoot(const std::string & foot_name, const std::string & reference_frame_name); + size_t addPointFoot(const std::string & foot_name, const std::string & reference_parent_frame_name); /** - * @brief Update the reference placement of the foot wrt to the frame it is attached to + * @brief Create a quad foot, that can apply 6D wrench to the ground. (in comparison to point foot) + * + * @param foot_name Frame name that will be used a a foot + * @param reference_parent_frame_name Frame to which the foot reference + * frame will be attached. + * @param contactPoints 3D positions (in the local frame) of the foot 4 extremum point + * + * @return the foot number + * + * @note The foot placement will be set by default using the reference configuration of the RobotModelHandler + */ + size_t addQuadFoot( + const std::string & foot_name, + const std::string & reference_parent_frame_name, + const ContactPointsMatrix & contactPoints); + + /** + * @brief Update the placement of the foot reference frame wrt to the joint/frame it is attached to * * @param foot_name Foot frame name - * @param refMfoot Placement in the local frame + * @param parentframeMfootref Placement of the foot reference frame wrt the parent frame it is attached to. */ - void setFootReferencePlacement(const std::string & foot_name, const SE3 & refMfoot); + void setFootReferencePlacement(size_t foot_nb, const SE3 & parentframeMfootref); /** - * @brief Perform a finite difference on the sates. + * @brief Perform a difference on the states. * * @param[in] x1 Initial state * @param[in] x2 Desired state @@ -110,39 +99,61 @@ namespace simple_mpc { return reference_state_; } - size_t getFootNb(const std::string & foot_name) const + + const std::string & getFootFrameName(size_t foot_nb) const + { + return feet_frame_names_.at(foot_nb); + } + + size_t getFeetNb() const + { + return size(feet_frame_ids_); + } + + size_t getFootNb(const std::string & foot_frame_name) const { - return size_t(std::find(feet_names_.begin(), feet_names_.end(), foot_name) - feet_names_.begin()); + return size_t( + std::find(feet_frame_names_.begin(), feet_frame_names_.end(), foot_frame_name) - feet_frame_names_.begin()); } - const std::vector & getFeetIds() const + FootType getFootType(size_t foot_nb) const { - return feet_ids_; + return feet_types_[foot_nb]; } - const std::string & getFootName(size_t i) const + const ContactPointsMatrix & getQuadFootContactPoints(size_t foot_nb) const { - return feet_names_.at(i); + return feet_contact_points_.at(foot_nb); } - const std::vector & getFeetNames() const + const std::vector & getFeetFrameIds() const { - return feet_names_; + return feet_frame_ids_; + } + + const std::vector & getFeetFrameNames() const + { + return feet_frame_names_; } FrameIndex getBaseFrameId() const { - return base_id_; + return base_frame_id_; } - FrameIndex getFootId(const std::string & foot_name) const + std::string getBaseFrameName() const { - return feet_ids_.at(getFootNb(foot_name)); + return model_.frames[base_frame_id_].name; } - FrameIndex getRefFootId(const std::string & foot_name) const + FrameIndex getFootFrameId(size_t foot_nb) const { - return ref_feet_ids_.at(getFootNb(foot_name)); + return feet_frame_ids_.at(foot_nb); + } + + FrameIndex getFootRefFrameId(size_t foot_nb) const + { + return feet_ref_frame_ids_.at(foot_nb); } double getMass() const @@ -154,6 +165,63 @@ namespace simple_mpc { return model_; } + + private: + /** + * @brief Common operation to perform to add a foot (of any type) : Create the reference frame and set it default + * pose using the default pose of the robot + * + * @param foot_name Name of the frame foot + * @param reference_parent_frame_name Name of the frame where the reference frame of the foot will be attached + */ + void addFootFrames(const std::string & foot_name, const std::string & reference_parent_frame_name); + + private: + /** + * @brief Model to be used by ocp + */ + Model model_; + + /** + * @brief Robot total mass + */ + double mass_; + + /** + * @brief Reference configuration and velocity (most probably null velocity) + * to use + */ + Eigen::VectorXd reference_state_; + + /** + * @brief Names of the frames to be in contact with the environment + */ + std::vector feet_frame_names_; + + /** + * @brief Is the foot a contact point or a 6D contact + */ + std::vector feet_types_; + + /** + * @brief List of contact points for each 6D feets + */ + std::map> feet_contact_points_; + + /** + * @brief Ids of the frames to be in contact with the environment + */ + std::vector feet_frame_ids_; + + /** + * @brief Ids of the frames that are reference position for the feet + */ + std::vector feet_ref_frame_ids_; + + /** + * @brief Base frame id + */ + pinocchio::FrameIndex base_frame_id_; }; class RobotDataHandler @@ -170,17 +238,18 @@ namespace simple_mpc RobotDataHandler(const RobotModelHandler & model_handler); // Set new robot state + void updateInternalData(const ConstVectorRef & q, const ConstVectorRef & v, const bool updateJacobians); void updateInternalData(const ConstVectorRef & x, const bool updateJacobians); void updateJacobiansMassMatrix(const ConstVectorRef & x); // Const getters - const SE3 & getRefFootPose(const std::string & foot_name) const + const SE3 & getFootRefPose(size_t foot_nb) const { - return data_.oMf[model_handler_.getRefFootId(foot_name)]; + return data_.oMf[model_handler_.getFootRefFrameId(foot_nb)]; }; - const SE3 & getFootPose(const std::string & foot_name) const + const SE3 & getFootPose(size_t foot_nb) const { - return data_.oMf[model_handler_.getFootId(foot_name)]; + return data_.oMf[model_handler_.getFootFrameId(foot_nb)]; }; const SE3 & getBaseFramePose() const { diff --git a/package.xml b/package.xml index 2c4d0611..f9e1d076 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ simple-mpc - 0.1 + 0.1.0 MPC schemes for locomotion Ewen Dantec Ewen Dantec @@ -15,6 +15,7 @@ pinocchio proxsuite aligator + tsid std_msgs cmake diff --git a/pixi.lock b/pixi.lock index b5401983..37c5d742 100644 --- a/pixi.lock +++ b/pixi.lock @@ -8,10 +8,10 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/_libgcc_mutex-0.1-conda_forge.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/linux-64/_openmp_mutex-4.5-2_gnu.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/noarch/_x86_64-microarch-level-1-2_x86_64.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/aligator-0.15.0-py311h57c6026_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/aligator-0.15.0-py311h0c24bc4_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/alsa-lib-1.2.14-hb9d3cd8_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/ampl-asl-1.0.0-h5888daf_2.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/assimp-5.4.3-h8943939_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/assimp-5.4.3-hecf2907_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/benchmark-1.9.4-h5888daf_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/binutils-2.43-h4852527_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/binutils_impl_linux-64-2.43-h4bf12b8_4.conda @@ -27,12 +27,12 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/ccache-4.11.3-h80c52d3_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cli11-2.5.0-h3f2d84a_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cmake-4.0.3-h74e3db0_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-3.0.1-h38be061_1.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-python-3.0.1-py311h48d4333_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-3.0.1-h38be061_4.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-python-3.0.1-py311hc4a192e_4.conda - conda: https://conda.anaconda.org/conda-forge/noarch/colorama-0.4.6-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/console_bridge-1.0.2-h924138e_1.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/linux-64/contourpy-1.3.2-py311hd18a35c_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/crocoddyl-3.0.1-py311h1d1b899_2.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/crocoddyl-3.0.1-py311h5fe6122_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cxx-compiler-1.9.0-h1a2810e_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/cycler-0.12.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cyrus-sasl-2.1.27-h54b06d7_7.conda @@ -40,11 +40,12 @@ environments: - conda: https://conda.anaconda.org/conda-forge/noarch/docstring_parser-0.16-pyhd8ed1ab_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/double-conversion-3.3.1-h5888daf_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/eigen-3.4.0-h00ab1b0_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/eigenpy-3.10.3-np20py311h2672b3f_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/eigenpy-3.12.0-np2py311hdfae693_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/eiquadprog-1.3.0-hb700be7_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/elfutils-0.192-h7f4e02f_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/example-robot-data-4.3.0-pyh6b898a9_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/exceptiongroup-1.3.0-pyhd8ed1ab_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/fmt-11.1.4-h07f6e7f_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/fmt-11.2.0-h07f6e7f_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/font-ttf-dejavu-sans-mono-2.37-hab24e00_0.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/noarch/font-ttf-inconsolata-3.000-h77eed37_0.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/noarch/font-ttf-source-code-pro-2.038-h77eed37_0.tar.bz2 @@ -80,18 +81,18 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/libasprintf-0.24.1-h8e693c7_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libblas-3.9.0-31_h59b9bed_openblas.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libblasfeo-0.1.4.2-h544d10a_100.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-1.86.0-h6c02f8c_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-devel-1.86.0-h1a2810e_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-headers-1.86.0-ha770c72_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-1.86.0-py311h5b7b71f_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-devel-1.86.0-py311hbd00459_3.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-1.88.0-h6c02f8c_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-devel-1.88.0-h1a2810e_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-headers-1.88.0-ha770c72_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-1.88.0-py311h5b7b71f_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-devel-1.88.0-py311hbd00459_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libbrotlicommon-1.1.0-hb9d3cd8_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libbrotlidec-1.1.0-hb9d3cd8_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libbrotlienc-1.1.0-hb9d3cd8_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libcblas-3.9.0-31_he106b2a_openblas.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libclang-cpp20.1-20.1.6-default_h1df26ce_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libclang13-20.1.6-default_he06ed0a_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libcoal-3.0.1-ha55d5d9_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libcoal-3.0.1-h969b668_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libcups-2.3.3-h4637d8d_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libcurl-8.14.1-h332b0f4_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libdeflate-1.24-h86f0d12_0.conda @@ -117,7 +118,8 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/libglx-1.7.0-ha4b6fd6_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgomp-15.1.0-h767d61c_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-cmake3-3.5.3-hac33072_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-math7-7.5.1-h5888daf_2.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-cmake4-4.2.0-h3f2d84a_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-math7-7.5.2-h54a6638_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-tools2-2.0.1-h17585e0_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-utils2-2.2.0-h5888daf_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libhiredis-1.0.2-h2cc385e_0.tar.bz2 @@ -137,12 +139,13 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/libopengl-1.7.0-ha4b6fd6_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libosqp-1.0.0-np122py310h438337b_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libpciaccess-0.18-hd590300_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libpinocchio-3.8.0-h54b0c19_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libpng-1.6.47-h943b412_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libpq-17.5-h27ae623_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libqdldl-0.1.8-h3f2d84a_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libsanitizer-13.3.0-he8ea267_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libscotch-7.0.4-h2fe6a88_5.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libsdformat14-14.5.0-hea37092_2.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libsdformat14-14.8.0-h153a79d_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libspral-2025.03.06-h39c1cf3_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libsqlite-3.50.1-hee588c1_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libssh2-1.11.1-hcf80075_0.conda @@ -182,7 +185,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pcre2-10.45-hc749103_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pillow-11.2.1-py311h1322bbf_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/pinocchio-3.6.0-py311hd79ae85_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/pinocchio-3.7.0-py311h44b0625_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pixman-0.46.2-h29eaf8c_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pkg-config-0.29.2-h4bc722e_1009.conda - conda: https://conda.anaconda.org/conda-forge/noarch/pluggy-1.6.0-pyhd8ed1ab_0.conda @@ -209,6 +212,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/tk-8.6.13-noxft_hd72426e_102.conda - conda: https://conda.anaconda.org/conda-forge/noarch/tomli-2.2.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/tornado-6.5.1-py311h9ecbd09_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/tsid-1.9.0-py311hd572313_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typed-argument-parser-1.10.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_extensions-4.14.0-pyhe01879c_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_inspect-0.9.0-pyhd8ed1ab_1.conda @@ -246,9 +250,9 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/zlib-1.3.1-hb9d3cd8_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/zstd-1.5.7-hb8e6e7a_2.conda osx-arm64: - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/aligator-0.15.0-py311hb38bd0a_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/aligator-0.15.0-py311hd7a0a21_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/ampl-asl-1.0.0-h286801f_2.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/assimp-5.4.3-ha9c0b8d_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/assimp-5.4.3-h31c7375_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/benchmark-1.9.4-h286801f_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/brotli-1.1.0-h5505292_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/brotli-bin-1.1.0-h5505292_3.conda @@ -269,22 +273,23 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/clangxx_osx-arm64-18.1.8-h07b0088_25.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/cli11-2.5.0-ha1acc90_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/cmake-4.0.3-ha25475f_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-3.0.1-ha1ab1f8_1.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-python-3.0.1-py311h9848c70_1.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-3.0.1-ha1ab1f8_4.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-python-3.0.1-py311h244da3a_4.conda - conda: https://conda.anaconda.org/conda-forge/noarch/colorama-0.4.6-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/compiler-rt-18.1.8-h856b3c1_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/compiler-rt_osx-arm64-18.1.8-h832e737_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/console_bridge-1.0.2-h3e96240_1.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/osx-arm64/contourpy-1.3.2-py311h210dab8_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/crocoddyl-3.0.1-py311h2b24f08_2.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/crocoddyl-3.0.1-py311hca5cac9_4.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/cxx-compiler-1.9.0-hba80287_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/cycler-0.12.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/docstring_parser-0.16-pyhd8ed1ab_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/eigen-3.4.0-h1995070_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/eigenpy-3.10.3-np20py311ha624dd0_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/eigenpy-3.12.0-np2py311h972c4b9_1.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/eiquadprog-1.3.0-ha7d2532_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/example-robot-data-4.3.0-pyh6b898a9_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/exceptiongroup-1.3.0-pyhd8ed1ab_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/fmt-11.1.4-h440487c_1.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/fmt-11.2.0-h440487c_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/fonttools-4.58.2-py311h4921393_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/freetype-2.13.3-hce30654_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/gettext-0.24.1-h3dcc1bd_0.conda @@ -304,17 +309,17 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libasprintf-devel-0.24.1-h493aca8_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libblas-3.9.0-31_h10e41b3_openblas.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libblasfeo-0.1.4.2-h37ef02a_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-1.86.0-hc9fb7c5_3.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-devel-1.86.0-hf450f58_3.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-headers-1.86.0-hce30654_3.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-1.86.0-py311h8fc16d6_3.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-devel-1.86.0-py311h35c05fe_3.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-1.88.0-hc9fb7c5_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-devel-1.88.0-hf450f58_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-headers-1.88.0-hce30654_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-1.88.0-py311h8fc16d6_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-devel-1.88.0-py311h35c05fe_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libbrotlicommon-1.1.0-h5505292_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libbrotlidec-1.1.0-h5505292_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libbrotlienc-1.1.0-h5505292_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcblas-3.9.0-31_hb3479ef_openblas.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libclang-cpp18.1-18.1.8-default_hf90f093_10.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcoal-3.0.1-hf5bebb4_1.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcoal-3.0.1-h8660a03_4.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcurl-8.14.1-h73640d1_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcxx-20.1.6-ha82da77_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcxx-devel-18.1.8-h6dc3340_8.conda @@ -332,7 +337,8 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgfortran5-14.2.0-h2c44a93_105.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libglib-2.84.2-hbec27ea_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-cmake3-3.5.3-h00cdb27_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-math7-7.5.1-h5833ebf_2.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-cmake4-4.2.0-ha1acc90_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-math7-7.5.2-h81086ad_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-tools2-2.0.1-hce4b3f6_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-utils2-2.2.0-h286801f_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libhiredis-1.0.2-hbec66e7_0.tar.bz2 @@ -347,10 +353,11 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libnghttp2-1.64.0-h6d7220d_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libopenblas-0.3.29-openmp_hf332438_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libosqp-1.0.0-np123py311h2bed84a_2.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libpinocchio-3.8.0-h3e052fa_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libpng-1.6.47-h3783ad8_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libqdldl-0.1.8-ha1acc90_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libscotch-7.0.6-he56f69b_1.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libsdformat14-14.5.0-h222e674_2.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libsdformat14-14.8.0-hb450cba_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libsqlite-3.50.1-h3f77e49_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libssh2-1.11.1-h1590b86_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libtiff-4.7.0-h2f21f7c_5.conda @@ -359,7 +366,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libxcb-1.17.0-hdb1d25a_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libxml2-2.14.3-h19f518e_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libzlib-1.3.1-h8359307_2.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-openmp-20.1.6-hdb05f8b_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-openmp-21.1.0-hbb9b287_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-tools-18-18.1.8-default_hb458b26_5.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-tools-18.1.8-default_hb458b26_5.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/matplotlib-3.10.3-py311ha1ab1f8_0.conda @@ -378,7 +385,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pcre2-10.45-ha881caa_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pillow-11.2.1-py311hb9ba9e9_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pinocchio-3.6.0-py311ha4cee0f_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pinocchio-3.7.0-py311h128d321_2.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pkg-config-0.29.2-hde07d2e_1009.conda - conda: https://conda.anaconda.org/conda-forge/noarch/pluggy-1.6.0-pyhd8ed1ab_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/proxsuite-0.7.2-py311h5e3411b_1.conda @@ -403,6 +410,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/tk-8.6.13-h892fb3f_2.conda - conda: https://conda.anaconda.org/conda-forge/noarch/tomli-2.2.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/tornado-6.5.1-py311h917b07b_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/tsid-1.9.0-py311h13c97ad_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typed-argument-parser-1.10.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_extensions-4.14.0-pyhe01879c_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_inspect-0.9.0-pyhd8ed1ab_1.conda @@ -426,10 +434,10 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/_libgcc_mutex-0.1-conda_forge.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/linux-64/_openmp_mutex-4.5-2_gnu.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/noarch/_x86_64-microarch-level-1-2_x86_64.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/aligator-0.15.0-py311h57c6026_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/aligator-0.15.0-py311h0c24bc4_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/alsa-lib-1.2.14-hb9d3cd8_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/ampl-asl-1.0.0-h5888daf_2.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/assimp-5.4.3-h8943939_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/assimp-5.4.3-hecf2907_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/benchmark-1.9.4-h5888daf_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/binutils-2.43-h4852527_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/binutils_impl_linux-64-2.43-h4bf12b8_4.conda @@ -448,12 +456,12 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/clangxx-20.1.6-default_h0982aa1_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cli11-2.5.0-h3f2d84a_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cmake-4.0.3-h74e3db0_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-3.0.1-h38be061_1.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-python-3.0.1-py311h48d4333_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-3.0.1-h38be061_4.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-python-3.0.1-py311hc4a192e_4.conda - conda: https://conda.anaconda.org/conda-forge/noarch/colorama-0.4.6-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/console_bridge-1.0.2-h924138e_1.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/linux-64/contourpy-1.3.2-py311hd18a35c_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/crocoddyl-3.0.1-py311h1d1b899_2.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/crocoddyl-3.0.1-py311h5fe6122_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cxx-compiler-1.9.0-h1a2810e_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/cycler-0.12.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cyrus-sasl-2.1.27-h54b06d7_7.conda @@ -461,11 +469,12 @@ environments: - conda: https://conda.anaconda.org/conda-forge/noarch/docstring_parser-0.16-pyhd8ed1ab_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/double-conversion-3.3.1-h5888daf_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/eigen-3.4.0-h00ab1b0_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/eigenpy-3.10.3-np20py311h2672b3f_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/eigenpy-3.12.0-np2py311hdfae693_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/eiquadprog-1.3.0-hb700be7_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/elfutils-0.192-h7f4e02f_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/example-robot-data-4.3.0-pyh6b898a9_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/exceptiongroup-1.3.0-pyhd8ed1ab_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/fmt-11.1.4-h07f6e7f_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/fmt-11.2.0-h07f6e7f_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/font-ttf-dejavu-sans-mono-2.37-hab24e00_0.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/noarch/font-ttf-inconsolata-3.000-h77eed37_0.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/noarch/font-ttf-source-code-pro-2.038-h77eed37_0.tar.bz2 @@ -501,18 +510,18 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/libasprintf-0.24.1-h8e693c7_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libblas-3.9.0-31_h59b9bed_openblas.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libblasfeo-0.1.4.2-h544d10a_100.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-1.86.0-h6c02f8c_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-devel-1.86.0-h1a2810e_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-headers-1.86.0-ha770c72_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-1.86.0-py311h5b7b71f_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-devel-1.86.0-py311hbd00459_3.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-1.88.0-h6c02f8c_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-devel-1.88.0-h1a2810e_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-headers-1.88.0-ha770c72_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-1.88.0-py311h5b7b71f_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-devel-1.88.0-py311hbd00459_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libbrotlicommon-1.1.0-hb9d3cd8_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libbrotlidec-1.1.0-hb9d3cd8_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libbrotlienc-1.1.0-hb9d3cd8_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libcblas-3.9.0-31_he106b2a_openblas.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libclang-cpp20.1-20.1.6-default_h1df26ce_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libclang13-20.1.6-default_he06ed0a_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libcoal-3.0.1-ha55d5d9_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libcoal-3.0.1-h969b668_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libcups-2.3.3-h4637d8d_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libcurl-8.14.1-h332b0f4_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libdeflate-1.24-h86f0d12_0.conda @@ -538,7 +547,8 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/libglx-1.7.0-ha4b6fd6_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgomp-15.1.0-h767d61c_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-cmake3-3.5.3-hac33072_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-math7-7.5.1-h5888daf_2.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-cmake4-4.2.0-h3f2d84a_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-math7-7.5.2-h54a6638_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-tools2-2.0.1-h17585e0_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-utils2-2.2.0-h5888daf_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libhiredis-1.0.2-h2cc385e_0.tar.bz2 @@ -558,12 +568,13 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/libopengl-1.7.0-ha4b6fd6_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libosqp-1.0.0-np122py310h438337b_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libpciaccess-0.18-hd590300_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libpinocchio-3.8.0-h54b0c19_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libpng-1.6.47-h943b412_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libpq-17.5-h27ae623_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libqdldl-0.1.8-h3f2d84a_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libsanitizer-13.3.0-he8ea267_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libscotch-7.0.4-h2fe6a88_5.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libsdformat14-14.5.0-hea37092_2.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libsdformat14-14.8.0-h153a79d_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libspral-2025.03.06-h39c1cf3_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libsqlite-3.50.1-hee588c1_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libssh2-1.11.1-hcf80075_0.conda @@ -603,7 +614,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pcre2-10.45-hc749103_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pillow-11.2.1-py311h1322bbf_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/pinocchio-3.6.0-py311hd79ae85_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/pinocchio-3.7.0-py311h44b0625_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pixman-0.46.2-h29eaf8c_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pkg-config-0.29.2-h4bc722e_1009.conda - conda: https://conda.anaconda.org/conda-forge/noarch/pluggy-1.6.0-pyhd8ed1ab_0.conda @@ -630,6 +641,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/tk-8.6.13-noxft_hd72426e_102.conda - conda: https://conda.anaconda.org/conda-forge/noarch/tomli-2.2.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/tornado-6.5.1-py311h9ecbd09_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/tsid-1.9.0-py311hd572313_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typed-argument-parser-1.10.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_extensions-4.14.0-pyhe01879c_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_inspect-0.9.0-pyhd8ed1ab_1.conda @@ -674,10 +686,10 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/_libgcc_mutex-0.1-conda_forge.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/linux-64/_openmp_mutex-4.5-2_gnu.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/noarch/_x86_64-microarch-level-1-2_x86_64.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/aligator-0.15.0-py311h57c6026_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/aligator-0.15.0-py311h0c24bc4_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/alsa-lib-1.2.14-hb9d3cd8_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/ampl-asl-1.0.0-h5888daf_2.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/assimp-5.4.3-h8943939_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/assimp-5.4.3-hecf2907_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/benchmark-1.9.4-h5888daf_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/binutils-2.43-h4852527_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/binutils_impl_linux-64-2.43-h4bf12b8_4.conda @@ -693,12 +705,12 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/ccache-4.11.3-h80c52d3_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cli11-2.5.0-h3f2d84a_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cmake-4.0.3-h74e3db0_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-3.0.1-h38be061_1.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-python-3.0.1-py311h48d4333_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-3.0.1-h38be061_4.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-python-3.0.1-py311hc4a192e_4.conda - conda: https://conda.anaconda.org/conda-forge/noarch/colorama-0.4.6-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/console_bridge-1.0.2-h924138e_1.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/linux-64/contourpy-1.3.2-py311hd18a35c_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/crocoddyl-3.0.1-py311h1d1b899_2.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/crocoddyl-3.0.1-py311h5fe6122_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cxx-compiler-1.9.0-h1a2810e_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/cycler-0.12.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cyrus-sasl-2.1.27-h54b06d7_7.conda @@ -706,11 +718,12 @@ environments: - conda: https://conda.anaconda.org/conda-forge/noarch/docstring_parser-0.16-pyhd8ed1ab_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/double-conversion-3.3.1-h5888daf_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/eigen-3.4.0-h00ab1b0_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/eigenpy-3.10.3-np20py311h2672b3f_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/eigenpy-3.12.0-np2py311hdfae693_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/eiquadprog-1.3.0-hb700be7_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/elfutils-0.192-h7f4e02f_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/example-robot-data-4.3.0-pyh6b898a9_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/exceptiongroup-1.3.0-pyhd8ed1ab_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/fmt-11.1.4-h07f6e7f_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/fmt-11.2.0-h07f6e7f_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/font-ttf-dejavu-sans-mono-2.37-hab24e00_0.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/noarch/font-ttf-inconsolata-3.000-h77eed37_0.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/noarch/font-ttf-source-code-pro-2.038-h77eed37_0.tar.bz2 @@ -746,18 +759,18 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/libasprintf-0.24.1-h8e693c7_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libblas-3.9.0-31_h59b9bed_openblas.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libblasfeo-0.1.4.2-h544d10a_100.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-1.86.0-h6c02f8c_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-devel-1.86.0-h1a2810e_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-headers-1.86.0-ha770c72_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-1.86.0-py311h5b7b71f_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-devel-1.86.0-py311hbd00459_3.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-1.88.0-h6c02f8c_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-devel-1.88.0-h1a2810e_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-headers-1.88.0-ha770c72_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-1.88.0-py311h5b7b71f_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-devel-1.88.0-py311hbd00459_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libbrotlicommon-1.1.0-hb9d3cd8_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libbrotlidec-1.1.0-hb9d3cd8_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libbrotlienc-1.1.0-hb9d3cd8_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libcblas-3.9.0-31_he106b2a_openblas.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libclang-cpp20.1-20.1.6-default_h1df26ce_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libclang13-20.1.6-default_he06ed0a_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libcoal-3.0.1-ha55d5d9_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libcoal-3.0.1-h969b668_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libcups-2.3.3-h4637d8d_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libcurl-8.14.1-h332b0f4_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libdeflate-1.24-h86f0d12_0.conda @@ -783,7 +796,8 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/libglx-1.7.0-ha4b6fd6_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgomp-15.1.0-h767d61c_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-cmake3-3.5.3-hac33072_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-math7-7.5.1-h5888daf_2.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-cmake4-4.2.0-h3f2d84a_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-math7-7.5.2-h54a6638_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-tools2-2.0.1-h17585e0_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-utils2-2.2.0-h5888daf_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libhiredis-1.0.2-h2cc385e_0.tar.bz2 @@ -803,12 +817,13 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/libopengl-1.7.0-ha4b6fd6_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libosqp-1.0.0-np122py310h438337b_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libpciaccess-0.18-hd590300_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libpinocchio-3.8.0-h54b0c19_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libpng-1.6.47-h943b412_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libpq-17.5-h27ae623_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libqdldl-0.1.8-h3f2d84a_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libsanitizer-13.3.0-he8ea267_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libscotch-7.0.4-h2fe6a88_5.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libsdformat14-14.5.0-hea37092_2.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libsdformat14-14.8.0-h153a79d_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libspral-2025.03.06-h39c1cf3_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libsqlite-3.50.1-hee588c1_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libssh2-1.11.1-hcf80075_0.conda @@ -848,7 +863,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pcre2-10.45-hc749103_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pillow-11.2.1-py311h1322bbf_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/pinocchio-3.6.0-py311hd79ae85_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/pinocchio-3.7.0-py311h44b0625_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pixman-0.46.2-h29eaf8c_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pkg-config-0.29.2-h4bc722e_1009.conda - conda: https://conda.anaconda.org/conda-forge/noarch/pluggy-1.6.0-pyhd8ed1ab_0.conda @@ -875,6 +890,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/tk-8.6.13-noxft_hd72426e_102.conda - conda: https://conda.anaconda.org/conda-forge/noarch/tomli-2.2.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/tornado-6.5.1-py311h9ecbd09_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/tsid-1.9.0-py311hd572313_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typed-argument-parser-1.10.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_extensions-4.14.0-pyhe01879c_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_inspect-0.9.0-pyhd8ed1ab_1.conda @@ -912,9 +928,9 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/zlib-1.3.1-hb9d3cd8_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/zstd-1.5.7-hb8e6e7a_2.conda osx-arm64: - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/aligator-0.15.0-py311hb38bd0a_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/aligator-0.15.0-py311hd7a0a21_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/ampl-asl-1.0.0-h286801f_2.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/assimp-5.4.3-ha9c0b8d_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/assimp-5.4.3-h31c7375_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/benchmark-1.9.4-h286801f_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/brotli-1.1.0-h5505292_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/brotli-bin-1.1.0-h5505292_3.conda @@ -935,22 +951,23 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/clangxx_osx-arm64-18.1.8-h07b0088_25.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/cli11-2.5.0-ha1acc90_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/cmake-4.0.3-ha25475f_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-3.0.1-ha1ab1f8_1.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-python-3.0.1-py311h9848c70_1.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-3.0.1-ha1ab1f8_4.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-python-3.0.1-py311h244da3a_4.conda - conda: https://conda.anaconda.org/conda-forge/noarch/colorama-0.4.6-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/compiler-rt-18.1.8-h856b3c1_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/compiler-rt_osx-arm64-18.1.8-h832e737_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/console_bridge-1.0.2-h3e96240_1.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/osx-arm64/contourpy-1.3.2-py311h210dab8_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/crocoddyl-3.0.1-py311h2b24f08_2.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/crocoddyl-3.0.1-py311hca5cac9_4.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/cxx-compiler-1.9.0-hba80287_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/cycler-0.12.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/docstring_parser-0.16-pyhd8ed1ab_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/eigen-3.4.0-h1995070_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/eigenpy-3.10.3-np20py311ha624dd0_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/eigenpy-3.12.0-np2py311h972c4b9_1.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/eiquadprog-1.3.0-ha7d2532_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/example-robot-data-4.3.0-pyh6b898a9_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/exceptiongroup-1.3.0-pyhd8ed1ab_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/fmt-11.1.4-h440487c_1.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/fmt-11.2.0-h440487c_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/fonttools-4.58.2-py311h4921393_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/freetype-2.13.3-hce30654_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/gettext-0.24.1-h3dcc1bd_0.conda @@ -970,17 +987,17 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libasprintf-devel-0.24.1-h493aca8_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libblas-3.9.0-31_h10e41b3_openblas.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libblasfeo-0.1.4.2-h37ef02a_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-1.86.0-hc9fb7c5_3.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-devel-1.86.0-hf450f58_3.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-headers-1.86.0-hce30654_3.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-1.86.0-py311h8fc16d6_3.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-devel-1.86.0-py311h35c05fe_3.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-1.88.0-hc9fb7c5_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-devel-1.88.0-hf450f58_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-headers-1.88.0-hce30654_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-1.88.0-py311h8fc16d6_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-devel-1.88.0-py311h35c05fe_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libbrotlicommon-1.1.0-h5505292_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libbrotlidec-1.1.0-h5505292_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libbrotlienc-1.1.0-h5505292_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcblas-3.9.0-31_hb3479ef_openblas.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libclang-cpp18.1-18.1.8-default_hf90f093_10.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcoal-3.0.1-hf5bebb4_1.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcoal-3.0.1-h8660a03_4.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcurl-8.14.1-h73640d1_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcxx-20.1.6-ha82da77_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcxx-devel-18.1.8-h6dc3340_8.conda @@ -998,7 +1015,8 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgfortran5-14.2.0-h2c44a93_105.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libglib-2.84.2-hbec27ea_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-cmake3-3.5.3-h00cdb27_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-math7-7.5.1-h5833ebf_2.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-cmake4-4.2.0-ha1acc90_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-math7-7.5.2-h81086ad_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-tools2-2.0.1-hce4b3f6_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-utils2-2.2.0-h286801f_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libhiredis-1.0.2-hbec66e7_0.tar.bz2 @@ -1013,10 +1031,11 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libnghttp2-1.64.0-h6d7220d_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libopenblas-0.3.29-openmp_hf332438_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libosqp-1.0.0-np123py311h2bed84a_2.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libpinocchio-3.8.0-h3e052fa_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libpng-1.6.47-h3783ad8_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libqdldl-0.1.8-ha1acc90_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libscotch-7.0.6-he56f69b_1.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libsdformat14-14.5.0-h222e674_2.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libsdformat14-14.8.0-hb450cba_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libsqlite-3.50.1-h3f77e49_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libssh2-1.11.1-h1590b86_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libtiff-4.7.0-h2f21f7c_5.conda @@ -1025,7 +1044,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libxcb-1.17.0-hdb1d25a_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libxml2-2.14.3-h19f518e_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libzlib-1.3.1-h8359307_2.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-openmp-20.1.6-hdb05f8b_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-openmp-21.1.0-hbb9b287_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-tools-18-18.1.8-default_hb458b26_5.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-tools-18.1.8-default_hb458b26_5.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/matplotlib-3.10.3-py311ha1ab1f8_0.conda @@ -1044,7 +1063,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pcre2-10.45-ha881caa_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pillow-11.2.1-py311hb9ba9e9_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pinocchio-3.6.0-py311ha4cee0f_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pinocchio-3.7.0-py311h128d321_2.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pkg-config-0.29.2-hde07d2e_1009.conda - conda: https://conda.anaconda.org/conda-forge/noarch/pluggy-1.6.0-pyhd8ed1ab_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/proxsuite-0.7.2-py311h5e3411b_1.conda @@ -1069,6 +1088,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/tk-8.6.13-h892fb3f_2.conda - conda: https://conda.anaconda.org/conda-forge/noarch/tomli-2.2.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/tornado-6.5.1-py311h917b07b_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/tsid-1.9.0-py311h13c97ad_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typed-argument-parser-1.10.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_extensions-4.14.0-pyhe01879c_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_inspect-0.9.0-pyhd8ed1ab_1.conda @@ -1092,10 +1112,10 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/_libgcc_mutex-0.1-conda_forge.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/linux-64/_openmp_mutex-4.5-2_gnu.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/noarch/_x86_64-microarch-level-1-2_x86_64.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/aligator-0.15.0-py311h57c6026_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/aligator-0.15.0-py311h0c24bc4_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/alsa-lib-1.2.14-hb9d3cd8_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/ampl-asl-1.0.0-h5888daf_2.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/assimp-5.4.3-h8943939_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/assimp-5.4.3-hecf2907_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/benchmark-1.9.4-h5888daf_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/binutils-2.43-h4852527_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/binutils_impl_linux-64-2.43-h4bf12b8_4.conda @@ -1113,12 +1133,12 @@ environments: - conda: https://conda.anaconda.org/conda-forge/noarch/cfgv-3.3.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cli11-2.5.0-h3f2d84a_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cmake-4.0.3-h74e3db0_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-3.0.1-h38be061_1.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-python-3.0.1-py311h48d4333_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-3.0.1-h38be061_4.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/coal-python-3.0.1-py311hc4a192e_4.conda - conda: https://conda.anaconda.org/conda-forge/noarch/colorama-0.4.6-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/console_bridge-1.0.2-h924138e_1.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/linux-64/contourpy-1.3.2-py311hd18a35c_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/crocoddyl-3.0.1-py311h1d1b899_2.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/crocoddyl-3.0.1-py311h5fe6122_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cxx-compiler-1.9.0-h1a2810e_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/cycler-0.12.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/cyrus-sasl-2.1.27-h54b06d7_7.conda @@ -1127,12 +1147,13 @@ environments: - conda: https://conda.anaconda.org/conda-forge/noarch/docstring_parser-0.16-pyhd8ed1ab_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/double-conversion-3.3.1-h5888daf_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/eigen-3.4.0-h00ab1b0_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/eigenpy-3.10.3-np20py311h2672b3f_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/eigenpy-3.12.0-np2py311hdfae693_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/eiquadprog-1.3.0-hb700be7_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/elfutils-0.192-h7f4e02f_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/example-robot-data-4.3.0-pyh6b898a9_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/exceptiongroup-1.3.0-pyhd8ed1ab_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/filelock-3.18.0-pyhd8ed1ab_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/fmt-11.1.4-h07f6e7f_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/fmt-11.2.0-h07f6e7f_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/font-ttf-dejavu-sans-mono-2.37-hab24e00_0.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/noarch/font-ttf-inconsolata-3.000-h77eed37_0.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/noarch/font-ttf-source-code-pro-2.038-h77eed37_0.tar.bz2 @@ -1169,18 +1190,18 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/libasprintf-0.24.1-h8e693c7_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libblas-3.9.0-31_h59b9bed_openblas.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libblasfeo-0.1.4.2-h544d10a_100.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-1.86.0-h6c02f8c_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-devel-1.86.0-h1a2810e_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-headers-1.86.0-ha770c72_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-1.86.0-py311h5b7b71f_3.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-devel-1.86.0-py311hbd00459_3.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-1.88.0-h6c02f8c_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-devel-1.88.0-h1a2810e_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-headers-1.88.0-ha770c72_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-1.88.0-py311h5b7b71f_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-devel-1.88.0-py311hbd00459_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libbrotlicommon-1.1.0-hb9d3cd8_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libbrotlidec-1.1.0-hb9d3cd8_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libbrotlienc-1.1.0-hb9d3cd8_3.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libcblas-3.9.0-31_he106b2a_openblas.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libclang-cpp20.1-20.1.6-default_h1df26ce_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libclang13-20.1.6-default_he06ed0a_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libcoal-3.0.1-ha55d5d9_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libcoal-3.0.1-h969b668_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libcups-2.3.3-h4637d8d_4.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libcurl-8.14.1-h332b0f4_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libdeflate-1.24-h86f0d12_0.conda @@ -1206,7 +1227,8 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/libglx-1.7.0-ha4b6fd6_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgomp-15.1.0-h767d61c_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-cmake3-3.5.3-hac33072_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-math7-7.5.1-h5888daf_2.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-cmake4-4.2.0-h3f2d84a_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-math7-7.5.2-h54a6638_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-tools2-2.0.1-h17585e0_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-utils2-2.2.0-h5888daf_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libhiredis-1.0.2-h2cc385e_0.tar.bz2 @@ -1226,12 +1248,13 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/libopengl-1.7.0-ha4b6fd6_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libosqp-1.0.0-np122py310h438337b_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libpciaccess-0.18-hd590300_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libpinocchio-3.8.0-h54b0c19_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libpng-1.6.47-h943b412_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libpq-17.5-h27ae623_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libqdldl-0.1.8-h3f2d84a_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libsanitizer-13.3.0-he8ea267_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libscotch-7.0.4-h2fe6a88_5.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/libsdformat14-14.5.0-hea37092_2.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/libsdformat14-14.8.0-h153a79d_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libspral-2025.03.06-h39c1cf3_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libsqlite-3.50.1-hee588c1_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/libssh2-1.11.1-hcf80075_0.conda @@ -1272,7 +1295,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pcre2-10.45-hc749103_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pillow-11.2.1-py311h1322bbf_0.conda - - conda: https://conda.anaconda.org/conda-forge/linux-64/pinocchio-3.6.0-py311hd79ae85_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/pinocchio-3.7.0-py311h44b0625_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pixman-0.46.2-h29eaf8c_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/pkg-config-0.29.2-h4bc722e_1009.conda - conda: https://conda.anaconda.org/conda-forge/noarch/platformdirs-4.3.8-pyhe01879c_0.conda @@ -1304,6 +1327,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/tk-8.6.13-noxft_hd72426e_102.conda - conda: https://conda.anaconda.org/conda-forge/noarch/tomli-2.2.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/tornado-6.5.1-py311h9ecbd09_0.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/tsid-1.9.0-py311hd572313_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typed-argument-parser-1.10.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_extensions-4.14.0-pyhe01879c_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_inspect-0.9.0-pyhd8ed1ab_1.conda @@ -1343,9 +1367,9 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/zlib-1.3.1-hb9d3cd8_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/zstd-1.5.7-hb8e6e7a_2.conda osx-arm64: - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/aligator-0.15.0-py311hb38bd0a_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/aligator-0.15.0-py311hd7a0a21_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/ampl-asl-1.0.0-h286801f_2.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/assimp-5.4.3-ha9c0b8d_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/assimp-5.4.3-h31c7375_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/benchmark-1.9.4-h286801f_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/brotli-1.1.0-h5505292_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/brotli-bin-1.1.0-h5505292_3.conda @@ -1368,24 +1392,25 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/clangxx_osx-arm64-18.1.8-h07b0088_25.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/cli11-2.5.0-ha1acc90_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/cmake-4.0.3-ha25475f_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-3.0.1-ha1ab1f8_1.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-python-3.0.1-py311h9848c70_1.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-3.0.1-ha1ab1f8_4.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-python-3.0.1-py311h244da3a_4.conda - conda: https://conda.anaconda.org/conda-forge/noarch/colorama-0.4.6-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/compiler-rt-18.1.8-h856b3c1_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/compiler-rt_osx-arm64-18.1.8-h832e737_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/console_bridge-1.0.2-h3e96240_1.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/osx-arm64/contourpy-1.3.2-py311h210dab8_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/crocoddyl-3.0.1-py311h2b24f08_2.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/crocoddyl-3.0.1-py311hca5cac9_4.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/cxx-compiler-1.9.0-hba80287_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/cycler-0.12.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/distlib-0.3.9-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/docstring_parser-0.16-pyhd8ed1ab_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/eigen-3.4.0-h1995070_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/eigenpy-3.10.3-np20py311ha624dd0_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/eigenpy-3.12.0-np2py311h972c4b9_1.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/eiquadprog-1.3.0-ha7d2532_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/example-robot-data-4.3.0-pyh6b898a9_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/exceptiongroup-1.3.0-pyhd8ed1ab_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/filelock-3.18.0-pyhd8ed1ab_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/fmt-11.1.4-h440487c_1.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/fmt-11.2.0-h440487c_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/fonttools-4.58.2-py311h4921393_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/freetype-2.13.3-hce30654_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/gettext-0.24.1-h3dcc1bd_0.conda @@ -1406,17 +1431,17 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libasprintf-devel-0.24.1-h493aca8_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libblas-3.9.0-31_h10e41b3_openblas.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libblasfeo-0.1.4.2-h37ef02a_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-1.86.0-hc9fb7c5_3.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-devel-1.86.0-hf450f58_3.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-headers-1.86.0-hce30654_3.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-1.86.0-py311h8fc16d6_3.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-devel-1.86.0-py311h35c05fe_3.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-1.88.0-hc9fb7c5_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-devel-1.88.0-hf450f58_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-headers-1.88.0-hce30654_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-1.88.0-py311h8fc16d6_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-devel-1.88.0-py311h35c05fe_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libbrotlicommon-1.1.0-h5505292_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libbrotlidec-1.1.0-h5505292_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libbrotlienc-1.1.0-h5505292_3.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcblas-3.9.0-31_hb3479ef_openblas.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libclang-cpp18.1-18.1.8-default_hf90f093_10.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcoal-3.0.1-hf5bebb4_1.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcoal-3.0.1-h8660a03_4.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcurl-8.14.1-h73640d1_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcxx-20.1.6-ha82da77_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcxx-devel-18.1.8-h6dc3340_8.conda @@ -1434,7 +1459,8 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgfortran5-14.2.0-h2c44a93_105.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libglib-2.84.2-hbec27ea_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-cmake3-3.5.3-h00cdb27_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-math7-7.5.1-h5833ebf_2.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-cmake4-4.2.0-ha1acc90_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-math7-7.5.2-h81086ad_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-tools2-2.0.1-hce4b3f6_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-utils2-2.2.0-h286801f_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libhiredis-1.0.2-hbec66e7_0.tar.bz2 @@ -1449,10 +1475,11 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libnghttp2-1.64.0-h6d7220d_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libopenblas-0.3.29-openmp_hf332438_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libosqp-1.0.0-np123py311h2bed84a_2.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libpinocchio-3.8.0-h3e052fa_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libpng-1.6.47-h3783ad8_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libqdldl-0.1.8-ha1acc90_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libscotch-7.0.6-he56f69b_1.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libsdformat14-14.5.0-h222e674_2.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libsdformat14-14.8.0-hb450cba_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libsqlite-3.50.1-h3f77e49_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libssh2-1.11.1-h1590b86_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libtiff-4.7.0-h2f21f7c_5.conda @@ -1461,7 +1488,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libxcb-1.17.0-hdb1d25a_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libxml2-2.14.3-h19f518e_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/libzlib-1.3.1-h8359307_2.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-openmp-20.1.6-hdb05f8b_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-openmp-21.1.0-hbb9b287_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-tools-18-18.1.8-default_hb458b26_5.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-tools-18.1.8-default_hb458b26_5.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/matplotlib-3.10.3-py311ha1ab1f8_0.conda @@ -1481,7 +1508,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/noarch/packaging-25.0-pyh29332c3_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pcre2-10.45-ha881caa_0.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pillow-11.2.1-py311hb9ba9e9_0.conda - - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pinocchio-3.6.0-py311ha4cee0f_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pinocchio-3.7.0-py311h128d321_2.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pkg-config-0.29.2-hde07d2e_1009.conda - conda: https://conda.anaconda.org/conda-forge/noarch/platformdirs-4.3.8-pyhe01879c_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/pluggy-1.6.0-pyhd8ed1ab_0.conda @@ -1511,6 +1538,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/osx-arm64/tk-8.6.13-h892fb3f_2.conda - conda: https://conda.anaconda.org/conda-forge/noarch/tomli-2.2.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/osx-arm64/tornado-6.5.1-py311h917b07b_0.conda + - conda: https://conda.anaconda.org/conda-forge/osx-arm64/tsid-1.9.0-py311h13c97ad_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typed-argument-parser-1.10.1-pyhd8ed1ab_1.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_extensions-4.14.0-pyhe01879c_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/typing_inspect-0.9.0-pyhd8ed1ab_1.conda @@ -1558,52 +1586,52 @@ packages: license_family: BSD size: 7773 timestamp: 1717599240447 -- conda: https://conda.anaconda.org/conda-forge/linux-64/aligator-0.15.0-py311h57c6026_0.conda - sha256: dc21eae2f9de05ec39c5b82ed3159b6086f7125b7c4c7ee0cbd56af4a77e2249 - md5: 7956f8b52688d62e3158b159ef58fd96 +- conda: https://conda.anaconda.org/conda-forge/linux-64/aligator-0.15.0-py311h0c24bc4_3.conda + sha256: 917e7d25303e34d332a358be6b0d351bf88b9a359b16b34cd25abb285b760470 + md5: 5fa2672870ee76dacaf2f07106d795d9 depends: - __glibc >=2.17,<3.0.a0 - crocoddyl >=3.0.1,<3.0.2.0a0 - eigen - - eigenpy >=3.10.3,<3.10.4.0a0 - - fmt >=11.1.4,<11.2.0a0 - - libboost >=1.86.0,<1.87.0a0 - - libboost-python >=1.86.0,<1.87.0a0 - - libgcc >=13 - - libstdcxx >=13 + - eigenpy >=3.12.0,<3.12.1.0a0 + - fmt >=11.2.0,<11.3.0a0 + - libboost >=1.88.0,<1.89.0a0 + - libboost-python >=1.88.0,<1.89.0a0 + - libgcc >=14 + - libstdcxx >=14 - matplotlib-base - - numpy >=1.19,<3 - - pinocchio >=3.6.0,<3.6.1.0a0 + - numpy >=1.23,<3 + - pinocchio >=3.7.0,<3.7.1.0a0 - python >=3.11,<3.12.0a0 - python_abi 3.11.* *_cp311 - typed-argument-parser license: BSD-2-Clause license_family: BSD - size: 6059793 - timestamp: 1748007876957 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/aligator-0.15.0-py311hb38bd0a_0.conda - sha256: 14c0435bc3a01c06069735fd56532e89f8274632a24a501fc3b6f0604d37ae5b - md5: 3e7bce593b38bee37918bca6baaf1bf8 + size: 5878194 + timestamp: 1757454142623 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/aligator-0.15.0-py311hd7a0a21_3.conda + sha256: 9082c4aa4bb1600318386bf1112a64f479a622ac631e51b89cbe4a7016a4cc06 + md5: b5e1d90d91df4df2a45c684e2ef46c9d depends: - __osx >=11.0 - crocoddyl >=3.0.1,<3.0.2.0a0 - eigen - - eigenpy >=3.10.3,<3.10.4.0a0 - - fmt >=11.1.4,<11.2.0a0 - - libboost >=1.86.0,<1.87.0a0 - - libboost-python >=1.86.0,<1.87.0a0 - - libcxx >=18 + - eigenpy >=3.12.0,<3.12.1.0a0 + - fmt >=11.2.0,<11.3.0a0 + - libboost >=1.88.0,<1.89.0a0 + - libboost-python >=1.88.0,<1.89.0a0 + - libcxx >=19 - matplotlib-base - - numpy >=1.19,<3 - - pinocchio >=3.6.0,<3.6.1.0a0 + - numpy >=1.23,<3 + - pinocchio >=3.7.0,<3.7.1.0a0 - python >=3.11,<3.12.0a0 - python >=3.11,<3.12.0a0 *_cpython - python_abi 3.11.* *_cp311 - typed-argument-parser license: BSD-2-Clause license_family: BSD - size: 5126882 - timestamp: 1748007631984 + size: 4746185 + timestamp: 1757455673911 - conda: https://conda.anaconda.org/conda-forge/linux-64/alsa-lib-1.2.14-hb9d3cd8_0.conda sha256: b9214bc17e89bf2b691fad50d952b7f029f6148f4ac4fe7c60c08f093efdf745 md5: 76df83c2a9035c54df5d04ff81bcc02d @@ -1637,33 +1665,33 @@ packages: license: BSD-3-Clause AND SMLNJ size: 411989 timestamp: 1732439500161 -- conda: https://conda.anaconda.org/conda-forge/linux-64/assimp-5.4.3-h8943939_0.conda - sha256: c9022ee34f756847f48907472514da3395a8c0549679cfd2a1b4f6833dd6298c - md5: 5546062a59566def2fa6482acf531841 +- conda: https://conda.anaconda.org/conda-forge/linux-64/assimp-5.4.3-hecf2907_1.conda + sha256: c49e992c1a2978f5a8cdf3cdf9aac0c0a444bbddb7316dbfbf16f5a94ff71c10 + md5: 649115e82c2a9620fcf0d3a846ee365f depends: - __glibc >=2.17,<3.0.a0 - - libboost >=1.86.0,<1.87.0a0 - - libgcc >=13 - - libstdcxx >=13 + - libboost >=1.88.0,<1.89.0a0 + - libgcc >=14 + - libstdcxx >=14 - libzlib >=1.3.1,<2.0a0 - zlib license: BSD-3-Clause license_family: BSD - size: 3535704 - timestamp: 1725086969417 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/assimp-5.4.3-ha9c0b8d_0.conda - sha256: d5c348d697abbb824f12bebb8da4de960e0ca2d62350745cfc08da3b984491c4 - md5: 4f6abafa61c3c6d3ff9a1b012fe2e9fa + size: 3645199 + timestamp: 1753274588181 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/assimp-5.4.3-h31c7375_1.conda + sha256: c991724e0ec7e0ab0a0710b0f0a2f5194ba6512365268abbd89b3ad9c46b82ac + md5: 4ac2a2ba9181ce4643dd35fefbf9359c depends: - __osx >=11.0 - - libboost >=1.86.0,<1.87.0a0 - - libcxx >=17 + - libboost >=1.88.0,<1.89.0a0 + - libcxx >=19 - libzlib >=1.3.1,<2.0a0 - zlib license: BSD-3-Clause license_family: BSD - size: 2478543 - timestamp: 1725087172579 + size: 2433041 + timestamp: 1753274954973 - conda: https://conda.anaconda.org/conda-forge/linux-64/benchmark-1.9.4-h5888daf_0.conda sha256: d9a8e7243b4aad9352cc6ad2165396192d23b7b4c69276dee45659111babed47 md5: 8dc41abc97df6e4aae27c4e2e0e83c94 @@ -2163,61 +2191,61 @@ packages: license_family: BSD size: 16532294 timestamp: 1749761424556 -- conda: https://conda.anaconda.org/conda-forge/linux-64/coal-3.0.1-h38be061_1.conda - sha256: 2f3c899a39fd3c835dde7b7b0b1b0fd41c36d8f8f240fa944ce5944ded27b4e6 - md5: be2c588c61e2f14b6597427e2f1c5959 +- conda: https://conda.anaconda.org/conda-forge/linux-64/coal-3.0.1-h38be061_4.conda + sha256: 2f28a07a7c875b05361e279e90dadd7176c3671202706cb807f86957e4e70097 + md5: 842b4b50be44386880c2b7c2ca1b1871 depends: - - coal-python 3.0.1 py311h48d4333_1 - - libcoal 3.0.1 ha55d5d9_1 + - coal-python 3.0.1 py311hc4a192e_4 + - libcoal 3.0.1 h969b668_4 - python_abi 3.11.* *_cp311 license: BSD-3-Clause license_family: BSD - size: 8130 - timestamp: 1739461961126 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-3.0.1-ha1ab1f8_1.conda - sha256: 067cedae188031e38c45cd497ff8692819ad4343df289807b85c934ea948c3e2 - md5: 0b93f625ccfceaf1c3d6f4a0159059e9 - depends: - - coal-python 3.0.1 py311h9848c70_1 - - libcoal 3.0.1 hf5bebb4_1 + size: 8615 + timestamp: 1757330947297 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-3.0.1-ha1ab1f8_4.conda + sha256: 1c8492d375934865c36ce5b634f016d6e8050e1f9f3c4bbe36d127a33dbd19aa + md5: 6d0305a973bd1cc24abb7ebe07101e3b + depends: + - coal-python 3.0.1 py311h244da3a_4 + - libcoal 3.0.1 h8660a03_4 - python_abi 3.11.* *_cp311 license: BSD-3-Clause license_family: BSD - size: 8317 - timestamp: 1739462458406 -- conda: https://conda.anaconda.org/conda-forge/linux-64/coal-python-3.0.1-py311h48d4333_1.conda - sha256: e5e31a51031a3e7f1b2d63257a955a0c5ada4b3c0398c71d7c625795f896a939 - md5: 609427c3290805c2f0e3e98ac7e4eaf1 + size: 8753 + timestamp: 1757331532294 +- conda: https://conda.anaconda.org/conda-forge/linux-64/coal-python-3.0.1-py311hc4a192e_4.conda + sha256: c8364835e389c123337edf990d00088fa97a1f1e6e59e90462585b0d8cdef45d + md5: b95bc6d85b409c8a3c11b79e10b82e6a depends: - __glibc >=2.17,<3.0.a0 - assimp >=5.4.3,<5.4.4.0a0 - - eigenpy >=3.10.3,<3.10.4.0a0 - - libboost >=1.86.0,<1.87.0a0 - - libboost-python >=1.86.0,<1.87.0a0 - - libcoal 3.0.1 ha55d5d9_1 - - libgcc >=13 - - libstdcxx >=13 - - numpy >=1.19,<3 + - eigenpy >=3.12.0,<3.12.1.0a0 + - libboost >=1.88.0,<1.89.0a0 + - libboost-python >=1.88.0,<1.89.0a0 + - libcoal 3.0.1 h969b668_4 + - libgcc >=14 + - libstdcxx >=14 + - numpy >=1.23,<3 - octomap >=1.10.0,<1.11.0a0 - python >=3.11,<3.12.0a0 - python_abi 3.11.* *_cp311 - qhull >=2020.2,<2020.3.0a0 license: BSD-3-Clause license_family: BSD - size: 1391098 - timestamp: 1739461935035 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-python-3.0.1-py311h9848c70_1.conda - sha256: 8794ce5c27601750fd06b82cbdea926b0bc0355bacfdd4a9b58e17a6d3e89758 - md5: 7bd3dfef47641f4d313436a54b71ca67 + size: 1401438 + timestamp: 1757330925922 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/coal-python-3.0.1-py311h244da3a_4.conda + sha256: 6dc0117c266bbb97fcf7740d86ede812c373268c2f72b65911601ca569e6ddb5 + md5: 6f15370802f898a382df07a0e1971de6 depends: - __osx >=11.0 - assimp >=5.4.3,<5.4.4.0a0 - - eigenpy >=3.10.3,<3.10.4.0a0 - - libboost >=1.86.0,<1.87.0a0 - - libboost-python >=1.86.0,<1.87.0a0 - - libcoal 3.0.1 hf5bebb4_1 - - libcxx >=18 - - numpy >=1.19,<3 + - eigenpy >=3.12.0,<3.12.1.0a0 + - libboost >=1.88.0,<1.89.0a0 + - libboost-python >=1.88.0,<1.89.0a0 + - libcoal 3.0.1 h8660a03_4 + - libcxx >=19 + - numpy >=1.23,<3 - octomap >=1.10.0,<1.11.0a0 - python >=3.11,<3.12.0a0 - python >=3.11,<3.12.0a0 *_cpython @@ -2225,8 +2253,8 @@ packages: - qhull >=2020.2,<2020.3.0a0 license: BSD-3-Clause license_family: BSD - size: 1005968 - timestamp: 1739462424485 + size: 1014875 + timestamp: 1757331492883 - conda: https://conda.anaconda.org/conda-forge/noarch/colorama-0.4.6-pyhd8ed1ab_1.conda sha256: ab29d57dc70786c1269633ba3dff20288b81664d3ff8d21af995742e2bb03287 md5: 962b9857ee8e7018c22f2776ffa0b2d7 @@ -2307,46 +2335,46 @@ packages: license_family: BSD size: 248716 timestamp: 1744743514765 -- conda: https://conda.anaconda.org/conda-forge/linux-64/crocoddyl-3.0.1-py311h1d1b899_2.conda - sha256: a4c0619630d7664076329deca86cf7142e5c936c7be69438bda008001dfbab4e - md5: 5e9fa7540b1df9398791a1b3a23d810b +- conda: https://conda.anaconda.org/conda-forge/linux-64/crocoddyl-3.0.1-py311h5fe6122_4.conda + sha256: 8c5bb950796b86ee024824e0338fe98e73893df0683b7e75aef5600caa0784ed + md5: 563f13061a92bcd7b74501999e6871bb depends: - __glibc >=2.17,<3.0.a0 - eigen - - eigenpy >=3.10.3,<3.10.4.0a0 + - eigenpy >=3.12.0,<3.12.1.0a0 - example-robot-data - - libboost >=1.86.0,<1.87.0a0 - - libboost-python >=1.86.0,<1.87.0a0 - - libgcc >=13 - - libstdcxx >=13 - - numpy >=1.19,<3 - - pinocchio >=3.6.0,<3.6.1.0a0 + - libboost >=1.88.0,<1.89.0a0 + - libboost-python >=1.88.0,<1.89.0a0 + - libgcc >=14 + - libstdcxx >=14 + - numpy >=1.23,<3 + - pinocchio >=3.7.0,<3.7.1.0a0 - python >=3.11,<3.12.0a0 - python_abi 3.11.* *_cp311 license: BSD-3-Clause license_family: BSD - size: 4225451 - timestamp: 1746005034526 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/crocoddyl-3.0.1-py311h2b24f08_2.conda - sha256: ab1bd438265cae9134eb1df1c507b7068fcade3d6f1d47e1547fd0b1747dfe9c - md5: 74b1acf0b66fc5692ebfea262f5c3f4d + size: 4196639 + timestamp: 1757429418232 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/crocoddyl-3.0.1-py311hca5cac9_4.conda + sha256: a7f424f38f878d33635e001701d28ddd2de3a3976c0e53f074998ee4dada7210 + md5: 569a146be4a8841f7d5d15b83bad7343 depends: - __osx >=11.0 - eigen - - eigenpy >=3.10.3,<3.10.4.0a0 + - eigenpy >=3.12.0,<3.12.1.0a0 - example-robot-data - - libboost >=1.86.0,<1.87.0a0 - - libboost-python >=1.86.0,<1.87.0a0 - - libcxx >=18 - - numpy >=1.19,<3 - - pinocchio >=3.6.0,<3.6.1.0a0 + - libboost >=1.88.0,<1.89.0a0 + - libboost-python >=1.88.0,<1.89.0a0 + - libcxx >=19 + - numpy >=1.23,<3 + - pinocchio >=3.7.0,<3.7.1.0a0 - python >=3.11,<3.12.0a0 - python >=3.11,<3.12.0a0 *_cpython - python_abi 3.11.* *_cp311 license: BSD-3-Clause license_family: BSD - size: 28494158 - timestamp: 1746005554240 + size: 28602298 + timestamp: 1757430346976 - conda: https://conda.anaconda.org/conda-forge/linux-64/cxx-compiler-1.9.0-h1a2810e_0.conda sha256: 5efc51b8e7d87fc5380f00ace9f9c758142eade520a63d3631d2616d1c1b25f9 md5: 1ce8b218d359d9ed0ab481f2a3f3c512 @@ -2453,42 +2481,65 @@ packages: license_family: MOZILLA size: 1087751 timestamp: 1690275869049 -- conda: https://conda.anaconda.org/conda-forge/linux-64/eigenpy-3.10.3-np20py311h2672b3f_0.conda - sha256: de1dd19f29d5093ed6a3e29e61b9e8b960b2df8fc6e4ea634d1d72c52c0ce378 - md5: 2a18613d5daf3261b0e5547f823823d0 +- conda: https://conda.anaconda.org/conda-forge/linux-64/eigenpy-3.12.0-np2py311hdfae693_1.conda + sha256: 37f35caff4cc471677bb14a094ecf731886461f043ac49ef7338c213eaa140aa + md5: a0012f13618fd94f735617b5b243e1c0 depends: - eigen - libboost-python-devel - python - scipy - __glibc >=2.17,<3.0.a0 - - libstdcxx >=13 - - libgcc >=13 - - libboost-python >=1.86.0,<1.87.0a0 - - numpy >=1.19,<3 + - libstdcxx >=14 + - libgcc >=14 + - numpy >=1.23,<3 + - libboost-python >=1.88.0,<1.89.0a0 - python_abi 3.11.* *_cp311 license: BSD-2-Clause license_family: BSD - size: 3336306 - timestamp: 1739356434768 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/eigenpy-3.10.3-np20py311ha624dd0_0.conda - sha256: 3a38f30971b393a850b66190d20c0c73a5cd7f9c5dc9ff22680d956f23d2f270 - md5: 0cff7696ec26a8874a92a7950d2c615c + size: 3927253 + timestamp: 1757320755129 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/eigenpy-3.12.0-np2py311h972c4b9_1.conda + sha256: 1630aee17596ffbef57b4ed0ae105da4a50f741738b672ce90ef75fa8df400ee + md5: 3055a3e13e9563d22114e3ba87fd242f depends: - eigen - libboost-python-devel - python - scipy - - libcxx >=18 - - __osx >=11.0 - python 3.11.* *_cpython + - libcxx >=19 + - __osx >=11.0 - python_abi 3.11.* *_cp311 - - libboost-python >=1.86.0,<1.87.0a0 - - numpy >=1.19,<3 + - numpy >=1.23,<3 + - libboost-python >=1.88.0,<1.89.0a0 license: BSD-2-Clause license_family: BSD - size: 2539830 - timestamp: 1739356451606 + size: 2954871 + timestamp: 1757320889163 +- conda: https://conda.anaconda.org/conda-forge/linux-64/eiquadprog-1.3.0-hb700be7_0.conda + sha256: c15caa5a967bce16678b37e73a08ed7d2432f4e7efcb0c46c4167c36aa0f6829 + md5: 1528c25fbd415f1cd4700f491c92684d + depends: + - __glibc >=2.17,<3.0.a0 + - eigen + - libgcc >=14 + - libstdcxx >=14 + license: LGPL-3.0-or-later + license_family: LGPL + size: 78564 + timestamp: 1758785461805 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/eiquadprog-1.3.0-ha7d2532_0.conda + sha256: 5224c40d580db650cfbdbd8bdf1a177863ef0fd3273555c0ff05623261c092bb + md5: d2bd8bdb1761a66d47284ba8e5f35ada + depends: + - __osx >=11.0 + - eigen + - libcxx >=19 + license: LGPL-3.0-or-later + license_family: LGPL + size: 66308 + timestamp: 1758785747207 - conda: https://conda.anaconda.org/conda-forge/linux-64/elfutils-0.192-h7f4e02f_1.conda sha256: 16097884784f9eb81625e0e2a566d1a6ec677fe39916b41926629fa723874f45 md5: 369ce48a589a2aac91906c9ed89dd6e8 @@ -2537,27 +2588,27 @@ packages: license: Unlicense size: 17887 timestamp: 1741969612334 -- conda: https://conda.anaconda.org/conda-forge/linux-64/fmt-11.1.4-h07f6e7f_1.conda - sha256: 2db2a6a1629bc2ac649b31fd990712446394ce35930025e960e1765a9249af5d - md5: 288a90e722fd7377448b00b2cddcb90d +- conda: https://conda.anaconda.org/conda-forge/linux-64/fmt-11.2.0-h07f6e7f_0.conda + sha256: e0f53b7801d0bcb5d61a1ddcb873479bfe8365e56fd3722a232fbcc372a9ac52 + md5: 0c2f855a88fab6afa92a7aa41217dc8e depends: - __glibc >=2.17,<3.0.a0 - libgcc >=13 - libstdcxx >=13 license: MIT license_family: MIT - size: 191161 - timestamp: 1742833273257 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/fmt-11.1.4-h440487c_1.conda - sha256: 39249dc4021742f1a126ad0efc39904fe058c89fdf43240f39316d34f948f3f1 - md5: f957ef7cf1dda0c27acdfbeff72ddb84 + size: 192721 + timestamp: 1751277120358 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/fmt-11.2.0-h440487c_0.conda + sha256: 1449ec46468860f6fb77edba87797ce22d4f6bfe8d5587c46fd5374c4f7383ee + md5: 24109723ac700cce5ff96ea3e63a83a3 depends: - __osx >=11.0 - libcxx >=18 license: MIT license_family: MIT - size: 178005 - timestamp: 1742833557859 + size: 177090 + timestamp: 1751277262419 - conda: https://conda.anaconda.org/conda-forge/noarch/font-ttf-dejavu-sans-mono-2.37-hab24e00_0.tar.bz2 sha256: 58d7f40d2940dd0a8aa28651239adbf5613254df0f75789919c4e6762054403b md5: 0c96522c6bdaed4b1566d11387caaf45 @@ -3173,80 +3224,80 @@ packages: license_family: BSD size: 509787 timestamp: 1740067975938 -- conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-1.86.0-h6c02f8c_3.conda - sha256: bad622863b3e4c8f0d107d8efd5b808e52d79cb502a20d700d05357b59a51e8f - md5: eead4e74198698d1c74f06572af753bc +- conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-1.88.0-h6c02f8c_0.conda + sha256: 0f51eada581974dbaab5c763c2f26664d7c41fce441083c87d2204d55cb767da + md5: e0afbff39e5218b5ccdac40ad3cbc5cf depends: - __glibc >=2.17,<3.0.a0 - bzip2 >=1.0.8,<2.0a0 - icu >=75.1,<76.0a0 - libgcc >=13 - - liblzma >=5.6.3,<6.0a0 + - liblzma >=5.8.1,<6.0a0 - libstdcxx >=13 - libzlib >=1.3.1,<2.0a0 - - zstd >=1.5.6,<1.6.0a0 + - zstd >=1.5.7,<1.6.0a0 constrains: - boost-cpp <0.0a0 license: BSL-1.0 - size: 2946990 - timestamp: 1733501899743 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-1.86.0-hc9fb7c5_3.conda - sha256: 793da2d2f7e2e14ed34549e3085771eefcc13ee6e06de2409a681ff0a545e905 - md5: 722715e61d51bcc7bd74f7a2b133f0d7 + size: 3011043 + timestamp: 1744432286447 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-1.88.0-hc9fb7c5_0.conda + sha256: 9e8de5fe4f13e25926225bc63fb71105acb466fc6e962c5c7e47f62dc361fec4 + md5: edf8618e63adf0f2e38f646a16514032 depends: - __osx >=11.0 - bzip2 >=1.0.8,<2.0a0 - icu >=75.1,<76.0a0 - libcxx >=18 - - liblzma >=5.6.3,<6.0a0 + - liblzma >=5.8.1,<6.0a0 - libzlib >=1.3.1,<2.0a0 - - zstd >=1.5.6,<1.6.0a0 + - zstd >=1.5.7,<1.6.0a0 constrains: - boost-cpp <0.0a0 license: BSL-1.0 - size: 1937185 - timestamp: 1733503730683 -- conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-devel-1.86.0-h1a2810e_3.conda - sha256: 1bbc13f4bed720af80e67e5df1e609f4efd801ae27d85107c36416de20ebb84c - md5: ffe09ce10ce1e03e1e762ab5bc006a35 - depends: - - libboost 1.86.0 h6c02f8c_3 - - libboost-headers 1.86.0 ha770c72_3 + size: 1945701 + timestamp: 1744432470394 +- conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-devel-1.88.0-h1a2810e_0.conda + sha256: ac95138550eeec69257d09d2f3d775f80c7389bcad0c9e1c2279db3a9022765a + md5: 5716bcc21136f7815a24b0fa92212582 + depends: + - libboost 1.88.0 h6c02f8c_0 + - libboost-headers 1.88.0 ha770c72_0 constrains: - boost-cpp <0.0a0 license: BSL-1.0 - size: 37554 - timestamp: 1733502001252 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-devel-1.86.0-hf450f58_3.conda - sha256: 785fec14fff95b87b1ef1e947367255cb54e8a580c67a9544ef51cf44399d638 - md5: b5ee687fa1ca8cb36149519a9e14541c - depends: - - libboost 1.86.0 hc9fb7c5_3 - - libboost-headers 1.86.0 hce30654_3 + size: 38071 + timestamp: 1744432439724 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-devel-1.88.0-hf450f58_0.conda + sha256: 35958f53863a8b97379c82ae1b6b1ed99209b46be5131a760d204ff892f38bb2 + md5: caa1898b431a8b820b0384c0717db845 + depends: + - libboost 1.88.0 hc9fb7c5_0 + - libboost-headers 1.88.0 hce30654_0 constrains: - boost-cpp <0.0a0 license: BSL-1.0 - size: 37678 - timestamp: 1733503973845 -- conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-headers-1.86.0-ha770c72_3.conda - sha256: 322be3cc409ee8b7f46a6e237c91cdcf810bc528af5865f6b7c46cc56ad5f070 - md5: be60ca34cfa7a867c2911506cad8f7c3 + size: 37962 + timestamp: 1744432597384 +- conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-headers-1.88.0-ha770c72_0.conda + sha256: 0dd66c472fd6ece3fba754c45a893e8d818ff16635664a3502f60bf457ca6811 + md5: a7cc18340af8dc486c7c3e8bd709ff35 constrains: - boost-cpp <0.0a0 license: BSL-1.0 - size: 13991670 - timestamp: 1733501914699 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-headers-1.86.0-hce30654_3.conda - sha256: b5287d295bb3ee2f074f8bfede7c021f220ecee681da3843d8e537a51aad83f2 - md5: 81b1cfe069c865273f8809ade3e80bf8 + size: 14511954 + timestamp: 1744432304949 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-headers-1.88.0-hce30654_0.conda + sha256: 5c8e805f027257dc53c810d1c94ec36b2af3f9e87e9a63b76e892c29e34441b9 + md5: c095f0350fba0f7f6ec3ef8a446af6d1 constrains: - boost-cpp <0.0a0 license: BSL-1.0 - size: 14139980 - timestamp: 1733503796088 -- conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-1.86.0-py311h5b7b71f_3.conda - sha256: 1ae2533b2b3b38fac2e06f742c0cd2dbadcd899c0b21d49fa6ca4f2f1c5d6d92 - md5: 1f2e5e17b5d0c42dba4c8740b64f5ac3 + size: 14601844 + timestamp: 1744432500701 +- conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-1.88.0-py311h5b7b71f_0.conda + sha256: db8d0c59952d2899a4901955875c7d5b882d062a42de0bcf1621f2dbcabe8a48 + md5: 49f6fa5efb7c8d3b3adda879623650fe depends: - __glibc >=2.17,<3.0.a0 - libgcc >=13 @@ -3258,11 +3309,11 @@ packages: - boost <0.0a0 - py-boost <0.0a0 license: BSL-1.0 - size: 121008 - timestamp: 1733502281031 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-1.86.0-py311h8fc16d6_3.conda - sha256: 030bb19312d66c05b600d92391800d6c77149d531b6de9baccb48a9b7e89b500 - md5: a6a4231c44250975a0f97af3da2510c9 + size: 126120 + timestamp: 1744432806098 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-1.88.0-py311h8fc16d6_0.conda + sha256: acb3a631b527ad2aacc47adf7b90f7723902068eb73f878ac9ce286229eb09bb + md5: b8f320e8c05010a8cc8b23187dc2f5bf depends: - __osx >=11.0 - libcxx >=18 @@ -3274,14 +3325,14 @@ packages: - py-boost <0.0a0 - boost <0.0a0 license: BSL-1.0 - size: 103521 - timestamp: 1733504401140 -- conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-devel-1.86.0-py311hbd00459_3.conda - sha256: 509654d990eb3088cf8db5b1454cfdaaad0d3677d938056ff741acea814b172a - md5: 1e129554c9b2ea37d26d9d3be7b5f399 - depends: - - libboost-devel 1.86.0 h1a2810e_3 - - libboost-python 1.86.0 py311h5b7b71f_3 + size: 105312 + timestamp: 1744432930284 +- conda: https://conda.anaconda.org/conda-forge/linux-64/libboost-python-devel-1.88.0-py311hbd00459_0.conda + sha256: 5144c46befb85c1829d20be9165c6fd5075aa4b450a4f97f02215a34335cf6f9 + md5: 6d43342e68cff37ea2d9b1b7c6f281f1 + depends: + - libboost-devel 1.88.0 h1a2810e_0 + - libboost-python 1.88.0 py311h5b7b71f_0 - numpy >=1.19,<3 - python >=3.11,<3.12.0a0 - python_abi 3.11.* *_cp311 @@ -3289,14 +3340,14 @@ packages: - boost <0.0a0 - py-boost <0.0a0 license: BSL-1.0 - size: 17437 - timestamp: 1733502339095 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-devel-1.86.0-py311h35c05fe_3.conda - sha256: 325a0e75276b0f824b47443aec9ea25a7644211009b3928a288d71b9a4cd8529 - md5: 189eb07b38da124c9f8f39cdfae592ff - depends: - - libboost-devel 1.86.0 hf450f58_3 - - libboost-python 1.86.0 py311h8fc16d6_3 + size: 17140 + timestamp: 1744432972317 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libboost-python-devel-1.88.0-py311h35c05fe_0.conda + sha256: 58176f8f76691b2db0096a392ab76772626d626abacca5933dcdd70fb81dfada + md5: 2f22d33514898440c7880972fb96d11d + depends: + - libboost-devel 1.88.0 hf450f58_0 + - libboost-python 1.88.0 py311h8fc16d6_0 - numpy >=1.19,<3 - python >=3.11,<3.12.0a0 - python_abi 3.11.* *_cp311 @@ -3304,8 +3355,8 @@ packages: - py-boost <0.0a0 - boost <0.0a0 license: BSL-1.0 - size: 17742 - timestamp: 1733505146830 + size: 17354 + timestamp: 1744433264621 - conda: https://conda.anaconda.org/conda-forge/linux-64/libbrotlicommon-1.1.0-hb9d3cd8_3.conda sha256: 462a8ed6a7bb9c5af829ec4b90aab322f8bcd9d8987f793e6986ea873bbd05cf md5: cb98af5db26e3f482bebb80ce9d947d3 @@ -3430,39 +3481,39 @@ packages: license_family: Apache size: 12111337 timestamp: 1748541871797 -- conda: https://conda.anaconda.org/conda-forge/linux-64/libcoal-3.0.1-ha55d5d9_1.conda - sha256: 64b97b9bf28ac5c33d1ce8e842e85de0ca2d74ec1b505fe7a31690b45bc77ca8 - md5: 69969d007c58287f6de16c8e8dca42d3 +- conda: https://conda.anaconda.org/conda-forge/linux-64/libcoal-3.0.1-h969b668_4.conda + sha256: 5bca884bdd5907d1cdc9218dec69e7ab6069ef982bde2ed6d92d5b685e1ef8dc + md5: ff18a44111262c29505df6c62bc61b09 depends: - __glibc >=2.17,<3.0.a0 - assimp >=5.4.3,<5.4.4.0a0 - - libboost >=1.86.0,<1.87.0a0 + - libboost >=1.88.0,<1.89.0a0 - libboost-devel - - libgcc >=13 - - libstdcxx >=13 + - libgcc >=14 + - libstdcxx >=14 - octomap >=1.10.0,<1.11.0a0 - qhull >=2020.2,<2020.3.0a0 - qhull-static license: BSD-3-Clause license_family: BSD - size: 1460301 - timestamp: 1739461489164 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcoal-3.0.1-hf5bebb4_1.conda - sha256: 1ef7ee2118ed8caef9bae98318a7b88b812022472e4646e3ce72e1cc1a1831b2 - md5: e8745e4dc3a8cc0ec625d968640f5de7 + size: 1478103 + timestamp: 1757330471411 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libcoal-3.0.1-h8660a03_4.conda + sha256: bbc34e81aae7e6106c8bc0e7b3b68e7d8645724bbe0f60e39d83805ed93d345a + md5: 03cb2c55367d304bdff8a7ff48f7d8c7 depends: - __osx >=11.0 - assimp >=5.4.3,<5.4.4.0a0 - - libboost >=1.86.0,<1.87.0a0 + - libboost >=1.88.0,<1.89.0a0 - libboost-devel - - libcxx >=18 + - libcxx >=19 - octomap >=1.10.0,<1.11.0a0 - qhull >=2020.2,<2020.3.0a0 - qhull-static license: BSD-3-Clause license_family: BSD - size: 1033019 - timestamp: 1739461543325 + size: 1031409 + timestamp: 1757330560602 - conda: https://conda.anaconda.org/conda-forge/linux-64/libcups-2.3.3-h4637d8d_4.conda sha256: bc67b9b21078c99c6bd8595fe7e1ed6da1f721007726e717f0449de7032798c4 md5: d4529f4dff3057982a7617c7ac58fde3 @@ -3911,33 +3962,55 @@ packages: license_family: APACHE size: 205431 timestamp: 1715098752998 -- conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-math7-7.5.1-h5888daf_2.conda - sha256: 40a013f40aa5f8c60b19068c1187ac0c2aad0ac3d219e0ce88de7f8518af2760 - md5: 2f4fda7179aa9d92cb32bbf84880721f +- conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-cmake4-4.2.0-h3f2d84a_0.conda + sha256: fd2f551f754285a45955478d072f81724d72dc91a3d3d472b03567bbbb413c89 + md5: a5ef8dad9018481b084713687ae6087e depends: + - libstdcxx >=13 + - libgcc >=13 - __glibc >=2.17,<3.0.a0 + license: Apache-2.0 + license_family: APACHE + size: 217822 + timestamp: 1745624503941 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-cmake4-4.2.0-ha1acc90_0.conda + sha256: 019fbfd33cef567989b475afc8176272785d696b6cedc860eca511bba531cd32 + md5: 9db25d7b7577332d28eac789264b97e1 + depends: + - __osx >=11.0 + - libcxx >=18 + license: Apache-2.0 + license_family: APACHE + size: 218401 + timestamp: 1745624519519 +- conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-math7-7.5.2-h54a6638_0.conda + sha256: 5c5d535ab668250b9db47e617ea12a1fb680412b9274f9bfd477c68f268d818b + md5: 535c6f2ab176f226baa4e315d0d77216 + depends: - eigen - - libgcc >=13 - - libgz-cmake3 >=3.5.3,<4.0a0 + - libgcc >=14 + - __glibc >=2.17,<3.0.a0 + - libstdcxx >=14 + - libgcc >=14 - libgz-utils2 >=2.2.0,<3.0a0 - - libstdcxx >=13 + - libgz-cmake4 >=4.2.0,<5.0a0 license: Apache-2.0 license_family: APACHE - size: 272182 - timestamp: 1730305982786 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-math7-7.5.1-h5833ebf_2.conda - sha256: 2d331fe19c829d775178a76817c536daef3803418edcb404539ae25fa374f570 - md5: bf32c3f6fb58c2e135c0196ec161cc10 + size: 295907 + timestamp: 1752689098818 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libgz-math7-7.5.2-h81086ad_0.conda + sha256: f48b61a099adb49db15c8bcbee9d046c70bddc2cb6f2173a749058dedefd8bf9 + md5: ebdce21f34b295598f8a8c7a9d3c0e4a depends: - - __osx >=11.0 - eigen - - libcxx >=17 - - libgz-cmake3 >=3.5.3,<4.0a0 + - libcxx >=19 + - __osx >=11.0 + - libgz-cmake4 >=4.2.0,<5.0a0 - libgz-utils2 >=2.2.0,<3.0a0 license: Apache-2.0 license_family: APACHE - size: 225645 - timestamp: 1730306118572 + size: 241802 + timestamp: 1752689108336 - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-tools2-2.0.1-h17585e0_0.conda sha256: 5acb80f086503978e410ec6f2722e2408de9689cc8576f835e62c7bdc43c6e58 md5: 395c1139239414c65e46e994ead5e997 @@ -4318,6 +4391,48 @@ packages: license_family: MIT size: 28361 timestamp: 1707101388552 +- conda: https://conda.anaconda.org/conda-forge/linux-64/libpinocchio-3.8.0-h54b0c19_1.conda + sha256: d8b54fd0bd99efc4b2f353f3382aa3f65e6198abdabef3839931afda80afd2f2 + md5: ba084c0baa6e1a58facd926acb9aeab9 + depends: + - __glibc >=2.17,<3.0.a0 + - _openmp_mutex >=4.5 + - casadi >=3.7.0,<3.8.0a0 + - console_bridge >=1.0.2,<1.1.0a0 + - hpp-fcl >=3.0.1,<3.0.2.0a0 + - libboost >=1.88.0,<1.89.0a0 + - libboost-devel + - libgcc >=14 + - libsdformat14 >=14.8.0,<15.0a0 + - libstdcxx >=14 + - qhull >=2020.2,<2020.3.0a0 + - qhull-static + - urdfdom >=4.0.1,<4.1.0a0 + license: BSD-2-Clause + license_family: BSD + size: 4428080 + timestamp: 1758543934661 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libpinocchio-3.8.0-h3e052fa_1.conda + sha256: 51e4efa481a70cc0c6d7a844ff24fe905089c251dd267ba709e28ec5f5ed8dd2 + md5: b31ed0fc4ecf795e53ca601024a4a40c + depends: + - __osx >=11.0 + - casadi >=3.7.0,<3.8.0a0 + - console_bridge >=1.0.2,<1.1.0a0 + - hpp-fcl >=3.0.1,<3.0.2.0a0 + - libboost >=1.88.0,<1.89.0a0 + - libboost-devel + - libcxx >=19 + - libsdformat14 >=14.8.0,<15.0a0 + - llvm-openmp >=19.1.7 + - llvm-openmp >=21.1.0 + - qhull >=2020.2,<2020.3.0a0 + - qhull-static + - urdfdom >=4.0.1,<4.1.0a0 + license: BSD-2-Clause + license_family: BSD + size: 3092552 + timestamp: 1758544627854 - conda: https://conda.anaconda.org/conda-forge/linux-64/libpng-1.6.47-h943b412_0.conda sha256: 23367d71da58c9a61c8cbd963fcffb92768d4ae5ffbef9a47cdf1f54f98c5c36 md5: 55199e2ae2c3651f6f9b2a447b47bdc9 @@ -4409,39 +4524,39 @@ packages: license: CECILL-C size: 273604 timestamp: 1737537249207 -- conda: https://conda.anaconda.org/conda-forge/linux-64/libsdformat14-14.5.0-hea37092_2.conda - sha256: b2568f702c963cd6b9c93567e38881f3c22bc5778697ae56d21cb0202999b39f - md5: 8740ef11d696ee72cd7ae7f749adb010 +- conda: https://conda.anaconda.org/conda-forge/linux-64/libsdformat14-14.8.0-h153a79d_0.conda + sha256: 5639666a7318ae9bd2e810795a1a35725f5a42ff8296b68660d760db4a07cd32 + md5: 63d05c39c4ad45fdaecee1bdf7e8213e depends: - __glibc >=2.17,<3.0.a0 - - libgcc >=13 + - libgcc >=14 - libgz-cmake3 >=3.5.3,<4.0a0 - - libgz-math7 >=7.5.1,<8.0a0 + - libgz-math7 >=7.5.2,<8.0a0 - libgz-tools2 >=2.0.1,<3.0a0 - libgz-utils2 >=2.2.0,<3.0a0 - - libstdcxx >=13 + - libstdcxx >=14 - tinyxml2 >=11.0.0,<11.1.0a0 - urdfdom >=4.0.1,<4.1.0a0 license: Apache-2.0 license_family: APACHE - size: 1117571 - timestamp: 1743978928063 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libsdformat14-14.5.0-h222e674_2.conda - sha256: 01025ee80c10ead6c563c0ca04909139c169f9869903b68d1b2deb8862ffe2aa - md5: 260e791dada0e1b4e93da7787252af16 + size: 1145964 + timestamp: 1752753789847 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/libsdformat14-14.8.0-hb450cba_0.conda + sha256: 6b75273f0149923df19dc908d3caea97529da9e2874278729dd0828c7240ee4d + md5: 6bb3d4b5bca803afc244c9b7d0ac126e depends: - __osx >=11.0 - - libcxx >=18 + - libcxx >=19 - libgz-cmake3 >=3.5.3,<4.0a0 - - libgz-math7 >=7.5.1,<8.0a0 + - libgz-math7 >=7.5.2,<8.0a0 - libgz-tools2 >=2.0.1,<3.0a0 - libgz-utils2 >=2.2.0,<3.0a0 - tinyxml2 >=11.0.0,<11.1.0a0 - urdfdom >=4.0.1,<4.1.0a0 license: Apache-2.0 license_family: APACHE - size: 811911 - timestamp: 1743978443138 + size: 818428 + timestamp: 1752753696817 - conda: https://conda.anaconda.org/conda-forge/linux-64/libspral-2025.03.06-h39c1cf3_0.conda sha256: a4a6b0473ce4d7f74ee4ab128fc5acb2745914981a6c61982d19aaf574d0f3d8 md5: 23e84e1dc106ce0e073c0404f2f42a38 @@ -4741,17 +4856,18 @@ packages: license_family: Other size: 46438 timestamp: 1727963202283 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-openmp-20.1.6-hdb05f8b_0.conda - sha256: 99c8aa89a77870d6ee16d62b858be67e30f2ad4fe13555570c7660cc38f9557b - md5: 7a3b28d59940a28e761e0a623241a832 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-openmp-21.1.0-hbb9b287_0.conda + sha256: c6750073a128376a14bedacfa90caab4c17025c9687fcf6f96e863b28d543af4 + md5: e57d95fec6eaa747e583323cba6cfe5c depends: - __osx >=11.0 constrains: - - openmp 20.1.6|20.1.6.* + - intel-openmp <0.0a0 + - openmp 21.1.0|21.1.0.* license: Apache-2.0 WITH LLVM-exception license_family: APACHE - size: 282698 - timestamp: 1748570308073 + size: 286039 + timestamp: 1756673290280 - conda: https://conda.anaconda.org/conda-forge/osx-arm64/llvm-tools-18.1.8-default_hb458b26_5.conda sha256: caa3f3fcc12b84e815a431706634eb850f05eaafc073ca1216e3fd87ec93134c md5: 704b3d78d5cd327f3ce1372d07be01fd @@ -5231,24 +5347,24 @@ packages: license: HPND size: 42632596 timestamp: 1746646647318 -- conda: https://conda.anaconda.org/conda-forge/linux-64/pinocchio-3.6.0-py311hd79ae85_0.conda - sha256: 4e4961ad216374b3acfd344fb3496b2fba0380cfaecdfde26ff033637e4a271b - md5: 22896671ba62fcfbfb8664422682d699 +- conda: https://conda.anaconda.org/conda-forge/linux-64/pinocchio-3.7.0-py311h44b0625_2.conda + sha256: be3d5a8878ca4db1f5821b3b11bba664db91093976f66d5e8de365827d476461 + md5: cee245ec1c79b1eec74cbb2caa84be5b depends: - __glibc >=2.17,<3.0.a0 - _openmp_mutex >=4.5 - casadi >=3.7.0,<3.8.0a0 - console_bridge >=1.0.2,<1.1.0a0 - - eigenpy >=3.10.3,<3.10.4.0a0 + - eigenpy >=3.12.0,<3.12.1.0a0 - hpp-fcl >=3.0.1,<3.0.2.0a0 - - libboost >=1.86.0,<1.87.0a0 + - libboost >=1.88.0,<1.89.0a0 - libboost-devel - - libboost-python >=1.86.0,<1.87.0a0 + - libboost-python >=1.88.0,<1.89.0a0 - libboost-python-devel - - libgcc >=13 - - libsdformat14 >=14.5.0,<15.0a0 - - libstdcxx >=13 - - numpy >=1.19,<3 + - libgcc >=14 + - libsdformat14 >=14.8.0,<15.0a0 + - libstdcxx >=14 + - numpy >=1.23,<3 - python >=3.11,<3.12.0a0 - python_abi 3.11.* *_cp311 - qhull >=2020.2,<2020.3.0a0 @@ -5256,26 +5372,26 @@ packages: - urdfdom >=4.0.1,<4.1.0a0 license: BSD-2-Clause license_family: BSD - size: 14121358 - timestamp: 1745874156847 -- conda: https://conda.anaconda.org/conda-forge/osx-arm64/pinocchio-3.6.0-py311ha4cee0f_0.conda - sha256: c3e985b067dcd1d2bf11aadbfc744b1a5b3c1d3a9883a7be880050b71907d9a4 - md5: 27ed2cdff7d0bfc6a41bfcfbe4424ad1 + size: 14411473 + timestamp: 1757409559002 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/pinocchio-3.7.0-py311h128d321_2.conda + sha256: a07f4d16f105a1f75e00bac3a45239261005ad19f0169de8df8a5c574905401c + md5: 360a920bce5bd6fcdc51d472a3e786ab depends: - __osx >=11.0 - casadi >=3.7.0,<3.8.0a0 - console_bridge >=1.0.2,<1.1.0a0 - - eigenpy >=3.10.3,<3.10.4.0a0 + - eigenpy >=3.12.0,<3.12.1.0a0 - hpp-fcl >=3.0.1,<3.0.2.0a0 - - libboost >=1.86.0,<1.87.0a0 + - libboost >=1.88.0,<1.89.0a0 - libboost-devel - - libboost-python >=1.86.0,<1.87.0a0 + - libboost-python >=1.88.0,<1.89.0a0 - libboost-python-devel - - libcxx >=18 - - libsdformat14 >=14.5.0,<15.0a0 - - llvm-openmp >=18.1.8 - - llvm-openmp >=20.1.3 - - numpy >=1.19,<3 + - libcxx >=19 + - libsdformat14 >=14.8.0,<15.0a0 + - llvm-openmp >=19.1.7 + - llvm-openmp >=21.1.0 + - numpy >=1.23,<3 - python >=3.11,<3.12.0a0 - python >=3.11,<3.12.0a0 *_cpython - python_abi 3.11.* *_cp311 @@ -5284,8 +5400,8 @@ packages: - urdfdom >=4.0.1,<4.1.0a0 license: BSD-2-Clause license_family: BSD - size: 11555416 - timestamp: 1745870873609 + size: 11404204 + timestamp: 1757411924580 - conda: https://conda.anaconda.org/conda-forge/linux-64/pixman-0.46.2-h29eaf8c_0.conda sha256: 6cb261595b5f0ae7306599f2bb55ef6863534b6d4d1bc0dcfdfa5825b0e4e53d md5: 39b4228a867772d610c02e06f939a5b8 @@ -5932,6 +6048,44 @@ packages: license_family: Apache size: 867366 timestamp: 1748003598139 +- conda: https://conda.anaconda.org/conda-forge/linux-64/tsid-1.9.0-py311hd572313_1.conda + sha256: a59b6b12150ab2db011c5fbbf14e1ec90188e98472fd24893e8ced4e21055038 + md5: 660e6bc58d6d809bcce274fbbf8c1ed3 + depends: + - __glibc >=2.17,<3.0.a0 + - eigenpy >=3.12.0,<3.12.1.0a0 + - eiquadprog >=1.3.0,<1.4.0a0 + - libboost-python >=1.88.0,<1.89.0a0 + - libgcc >=14 + - libpinocchio >=3.8.0,<3.8.1.0a0 + - libstdcxx >=14 + - numpy >=1.23,<3 + - python >=3.11,<3.12.0a0 + - python_abi 3.11.* *_cp311 + - urdfdom >=4.0.1,<4.1.0a0 + license: BSD-2-Clause + license_family: BSD + size: 920746 + timestamp: 1759222804337 +- conda: https://conda.anaconda.org/conda-forge/osx-arm64/tsid-1.9.0-py311h13c97ad_1.conda + sha256: 691c6b410f52037e4630aeaad2cac05686ceb8503c5623722a9bcf82d9c55b44 + md5: bc0d5141939a35a5722ef96be236b540 + depends: + - __osx >=11.0 + - eigenpy >=3.12.0,<3.12.1.0a0 + - eiquadprog >=1.3.0,<1.4.0a0 + - libboost-python >=1.88.0,<1.89.0a0 + - libcxx >=19 + - libpinocchio >=3.8.0,<3.8.1.0a0 + - numpy >=1.23,<3 + - python >=3.11,<3.12.0a0 + - python >=3.11,<3.12.0a0 *_cpython + - python_abi 3.11.* *_cp311 + - urdfdom >=4.0.1,<4.1.0a0 + license: BSD-2-Clause + license_family: BSD + size: 698751 + timestamp: 1759223284961 - conda: https://conda.anaconda.org/conda-forge/noarch/typed-argument-parser-1.10.1-pyhd8ed1ab_1.conda sha256: 996c714b106b63bf12be4c26079efba7723a33ef65e2e676594ad1c6c911f3a8 md5: 635f579b7058970d83708c27a2193df1 diff --git a/pixi.toml b/pixi.toml index 568e0187..bd8e076e 100644 --- a/pixi.toml +++ b/pixi.toml @@ -24,6 +24,7 @@ proxsuite = ">=0.7.2" benchmark = ">=1.8.0" pytest = ">=8.3.0" matplotlib = ">=3.9" +tsid = ">=1.9.0,<2" [activation] scripts = ["build_scripts/pixi/activation.sh"] diff --git a/src/centroidal-dynamics.cpp b/src/centroidal-dynamics.cpp index 82a8775c..c2ba2717 100644 --- a/src/centroidal-dynamics.cpp +++ b/src/centroidal-dynamics.cpp @@ -29,7 +29,7 @@ namespace simple_mpc , settings_(settings) { nx_ = 9; - nu_ = (int)model_handler_.getFeetNames().size() * settings_.force_size; + nu_ = (int)model_handler_.getFeetNb() * settings_.force_size; control_ref_.resize(nu_); control_ref_.setZero(); com_ref_.setZero(); @@ -58,7 +58,7 @@ namespace simple_mpc computeControlFromForces(contact_force); - ContactMap contact_map = ContactMap(model_handler_.getFeetNames(), contact_states, contact_poses); + ContactMap contact_map = ContactMap(model_handler_.getFeetFrameNames(), contact_states, contact_poses); auto com_res = CentroidalCoMResidual(nx_, nu_, com_ref_); auto linear_mom = LinearMomentumResidual(nx_, nu_, Eigen::Vector3d::Zero()); @@ -83,7 +83,7 @@ namespace simple_mpc StageModel stm = StageModel(rcost, dyn_model); int i = 0; - for (auto const & name : model_handler_.getFeetNames()) + for (auto const & name : model_handler_.getFeetFrameNames()) { if (contact_phase.at(name)) { @@ -107,14 +107,14 @@ namespace simple_mpc void CentroidalOCP::computeControlFromForces(const std::map & force_refs) { - for (std::size_t i = 0; i < model_handler_.getFeetNames().size(); i++) + for (std::size_t i = 0; i < model_handler_.getFeetNb(); i++) { - if (settings_.force_size != force_refs.at(model_handler_.getFootName(i)).size()) + if (settings_.force_size != force_refs.at(model_handler_.getFootFrameName(i)).size()) { throw std::runtime_error("force size in settings does not match reference force size"); } control_ref_.segment((long)i * settings_.force_size, settings_.force_size) = - force_refs.at(model_handler_.getFootName(i)); + force_refs.at(model_handler_.getFootFrameName(i)); } } @@ -124,7 +124,7 @@ namespace simple_mpc { throw std::runtime_error("Stage index exceeds stage vector size"); } - if (pose_refs.size() != model_handler_.getFeetNames().size()) + if (pose_refs.size() != model_handler_.getFeetNb()) { throw std::runtime_error("pose_refs size does not match number of end effectors"); } @@ -137,7 +137,7 @@ namespace simple_mpc } CostStack * cs = getCostStack(t); - for (auto ee_name : model_handler_.getFeetNames()) + for (auto ee_name : model_handler_.getFeetFrameNames()) { QuadraticResidualCost * qrc1 = cs->getComponent("linear_acc_cost"); QuadraticResidualCost * qrc2 = cs->getComponent("angular_acc_cost"); @@ -192,7 +192,7 @@ namespace simple_mpc void CentroidalOCP::setReferenceForce(const std::size_t t, const std::string & ee_name, const ConstVectorRef & force_ref) { - std::vector hname = model_handler_.getFeetNames(); + std::vector hname = model_handler_.getFeetFrameNames(); std::vector::iterator it = std::find(hname.begin(), hname.end(), ee_name); long id = it - hname.begin(); control_ref_.segment(id * settings_.force_size, settings_.force_size) = force_ref; @@ -201,7 +201,7 @@ namespace simple_mpc const Eigen::VectorXd CentroidalOCP::getReferenceForce(const std::size_t t, const std::string & ee_name) { - std::vector hname = model_handler_.getFeetNames(); + std::vector hname = model_handler_.getFeetFrameNames(); std::vector::iterator it = std::find(hname.begin(), hname.end(), ee_name); long id = it - hname.begin(); @@ -219,8 +219,8 @@ namespace simple_mpc QuadraticResidualCost * qca = cs->getComponent("angular_mom_cost"); AngularMomentumResidual * cfa = qca->getResidual(); - v.head(3) = cfm->getReference() / model_handler_.getMass(); - v.tail(3) = cfa->getReference() / model_handler_.getMass(); + v.head<3>() = cfm->getReference() / model_handler_.getMass(); + v.tail<3>() = cfa->getReference() / model_handler_.getMass(); return v; } @@ -234,8 +234,8 @@ namespace simple_mpc QuadraticResidualCost * qca = cs->getComponent("angular_mom_cost"); AngularMomentumResidual * cfa = qca->getResidual(); - cfm->setReference(velocity_base.head(3) * model_handler_.getMass()); - cfa->setReference(velocity_base.tail(3) * model_handler_.getMass()); + cfm->setReference(velocity_base.head<3>() * model_handler_.getMass()); + cfa->setReference(velocity_base.tail<3>() * model_handler_.getMass()); } const Eigen::VectorXd CentroidalOCP::getPoseBase(const std::size_t t) @@ -252,7 +252,7 @@ namespace simple_mpc CostStack * cs = getCostStack(t); QuadraticResidualCost * qrc = cs->getComponent("com_cost"); CentroidalCoMResidual * cfr = qrc->getResidual(); - com_ref_ = pose_base.head(3); + com_ref_ = pose_base.head<3>(); cfr->setReference(com_ref_); } @@ -267,7 +267,7 @@ namespace simple_mpc problem_->stages_[t]->getDynamics()->getDynamics(); size_t active_contacts = 0; - for (auto name : model_handler_.getFeetNames()) + for (auto name : model_handler_.getFeetFrameNames()) { if (ode->contact_map_.getContactState(name)) { @@ -283,7 +283,7 @@ namespace simple_mpc CentroidalFwdDynamics * ode = problem_->stages_[t]->getDynamics()->getDynamics(); assert(ode != nullptr); - for (auto name : model_handler_.getFeetNames()) + for (auto name : model_handler_.getFeetFrameNames()) contact_state.push_back(ode->contact_map_.getContactState(name)); return contact_state; @@ -292,14 +292,14 @@ namespace simple_mpc void CentroidalOCP::setReferenceState(const std::size_t t, const ConstVectorRef & x_ref) { assert(x_ref.size() == 9 && "x_ref not of the right size"); - setPoseBase(t, x_ref.head(3)); - setVelocityBase(t, x_ref.tail(6)); + setPoseBase(t, x_ref.head<3>()); + setVelocityBase(t, x_ref.tail<6>()); } const ConstVectorRef CentroidalOCP::getReferenceState(const std::size_t t) { - x0_.head(3) = getPoseBase(t); - x0_.tail(6) = getVelocityBase(t); + x0_.head<3>() = getPoseBase(t); + x0_.tail<6>() = getVelocityBase(t); return x0_; } diff --git a/src/fulldynamics.cpp b/src/fulldynamics.cpp index dacc71bc..49db57db 100644 --- a/src/fulldynamics.cpp +++ b/src/fulldynamics.cpp @@ -47,9 +47,9 @@ namespace simple_mpc throw std::runtime_error("Force must be of same size as Kd correction"); } - for (auto const & name : model_handler_.getFeetNames()) + for (size_t foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++) { - auto frame_ids = model_handler_.getFootId(name); + auto frame_ids = model_handler_.getFootFrameId(foot_nb); auto joint_ids = model_handler_.getModel().frames[frame_ids].parentJoint; pinocchio::SE3 pl1 = model_handler_.getModel().frames[frame_ids].placement; pinocchio::SE3 pl2 = pinocchio::SE3::Identity(); @@ -60,7 +60,7 @@ namespace simple_mpc pinocchio::LOCAL_WORLD_ALIGNED); constraint_model.corrector.Kp = settings.Kp_correction; constraint_model.corrector.Kd = settings.Kd_correction; - constraint_model.name = name; + constraint_model.name = model_handler_.getFootFrameName(foot_nb); constraint_models_.push_back(constraint_model); } else @@ -69,7 +69,7 @@ namespace simple_mpc pinocchio::ContactType::CONTACT_3D, model_handler_.getModel(), joint_ids, pl1, 0, pl2, pinocchio::LOCAL); constraint_model.corrector.Kp = settings.Kp_correction; constraint_model.corrector.Kd = settings.Kd_correction; - constraint_model.name = name; + constraint_model.name = model_handler_.getFootFrameName(foot_nb); constraint_models_.push_back(constraint_model); } } @@ -94,12 +94,13 @@ namespace simple_mpc pinocchio::context::RigidConstraintModelVector cms; size_t c_id = 0; - for (auto const & name : model_handler_.getFeetNames()) + for (size_t foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++) { + const std::string & name = model_handler_.getFootFrameName(foot_nb); if (settings_.force_size == 6) { FramePlacementResidual frame_residual = FramePlacementResidual( - space.ndx(), nu_, model_handler_.getModel(), contact_pose.at(name), model_handler_.getFootId(name)); + space.ndx(), nu_, model_handler_.getModel(), contact_pose.at(name), model_handler_.getFootFrameId(foot_nb)); rcost.addCost(name + "_pose_cost", QuadraticResidualCost(space, frame_residual, settings_.w_frame)); } @@ -107,7 +108,7 @@ namespace simple_mpc { FrameTranslationResidual frame_residual = FrameTranslationResidual( space.ndx(), nu_, model_handler_.getModel(), contact_pose.at(name).translation(), - model_handler_.getFootId(name)); + model_handler_.getFootFrameId(foot_nb)); rcost.addCost(name + "_pose_cost", QuadraticResidualCost(space, frame_residual, settings_.w_frame)); } @@ -118,8 +119,9 @@ namespace simple_mpc c_id++; } - for (auto const & name : model_handler_.getFeetNames()) + for (size_t foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++) { + const std::string & name = model_handler_.getFootFrameName(foot_nb); std::shared_ptr frame_force; if (contact_force.at(name).size() != settings_.force_size) { @@ -157,8 +159,9 @@ namespace simple_mpc stm.addConstraint(state_slice, BoxConstraint(settings_.qmin, settings_.qmax)); } - for (auto const & name : model_handler_.getFeetNames()) + for (size_t foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++) { + const std::string & name = model_handler_.getFootFrameName(foot_nb); if (settings_.force_size == 6 and contact_phase.at(name)) { if (settings_.force_cone) @@ -172,7 +175,7 @@ namespace simple_mpc if (settings_.land_cstr and land_constraint.at(name)) { FrameVelocityResidual velocity_residual = FrameVelocityResidual( - space.ndx(), nu_, model_handler_.getModel(), Motion::Zero(), model_handler_.getFootId(name), + space.ndx(), nu_, model_handler_.getModel(), Motion::Zero(), model_handler_.getFootFrameId(foot_nb), pinocchio::LOCAL_WORLD_ALIGNED); stm.addConstraint(velocity_residual, EqualityConstraint()); } @@ -189,7 +192,7 @@ namespace simple_mpc { std::vector vel_id = {0, 1, 2}; FrameVelocityResidual velocity_residual = FrameVelocityResidual( - space.ndx(), nu_, model_handler_.getModel(), Motion::Zero(), model_handler_.getFootId(name), + space.ndx(), nu_, model_handler_.getModel(), Motion::Zero(), model_handler_.getFootFrameId(foot_nb), pinocchio::LOCAL_WORLD_ALIGNED); FunctionSliceXpr vel_slice = FunctionSliceXpr(velocity_residual, vel_id); stm.addConstraint(vel_slice, EqualityConstraint()); @@ -198,7 +201,7 @@ namespace simple_mpc FrameTranslationResidual frame_residual = FrameTranslationResidual( space.ndx(), nu_, model_handler_.getModel(), contact_pose.at(name).translation(), - model_handler_.getFootId(name)); + model_handler_.getFootFrameId(foot_nb)); FunctionSliceXpr frame_slice = FunctionSliceXpr(frame_residual, frame_id); @@ -212,13 +215,13 @@ namespace simple_mpc void FullDynamicsOCP::setReferencePoses(const std::size_t t, const std::map & pose_refs) { - if (pose_refs.size() != model_handler_.getFeetNames().size()) + if (pose_refs.size() != model_handler_.getFeetNb()) { throw std::runtime_error("pose_refs size does not match number of end effectors"); } CostStack * cs = getCostStack(t); - for (auto ee_name : model_handler_.getFeetNames()) + for (auto ee_name : model_handler_.getFeetFrameNames()) { QuadraticResidualCost * qrc = cs->getComponent(ee_name + "_pose_cost"); @@ -272,11 +275,11 @@ namespace simple_mpc FullDynamicsOCP::setReferenceForces(const std::size_t t, const std::map & force_refs) { CostStack * cs = getCostStack(t); - if (force_refs.size() != model_handler_.getFeetNames().size()) + if (force_refs.size() != model_handler_.getFeetNb()) { throw std::runtime_error("force_refs size does not match number of end effectors"); } - for (auto ee_name : model_handler_.getFeetNames()) + for (auto ee_name : model_handler_.getFeetFrameNames()) { QuadraticResidualCost * qrc = cs->getComponent(ee_name + "_force_cost"); ContactForceResidual * cfr = qrc->getResidual(); @@ -343,7 +346,7 @@ namespace simple_mpc { CostStack * cs = getCostStack(t); QuadraticStateCost * qc = cs->getComponent("state_cost"); - return qc->getTarget().head(7); + return qc->getTarget().head<7>(); }; void FullDynamicsOCP::setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) @@ -355,7 +358,7 @@ namespace simple_mpc CostStack * cs = getCostStack(t); QuadraticStateCost * qc = cs->getComponent("state_cost"); x0_ = getReferenceState(t); - x0_.head(7) = pose_base; + x0_.head<7>() = pose_base; qc->setTarget(x0_); } @@ -378,7 +381,7 @@ namespace simple_mpc MultibodyConstraintFwdDynamics * ode = problem_->stages_[t]->getDynamics()->getDynamics(); assert(ode != nullptr); - for (auto name : model_handler_.getFeetNames()) + for (auto name : model_handler_.getFeetFrameNames()) { std::size_t i; for (i = 0; i < ode->constraint_models_.size(); i++) diff --git a/src/inverse-dynamics/centroidal-id.cpp b/src/inverse-dynamics/centroidal-id.cpp new file mode 100644 index 00000000..b661a524 --- /dev/null +++ b/src/inverse-dynamics/centroidal-id.cpp @@ -0,0 +1,147 @@ +#include + +namespace simple_mpc +{ + + CentroidalID::CentroidalID(const RobotModelHandler & model_handler, double control_dt, const Settings settings) + : KinodynamicsID(model_handler, control_dt, settings) + , settings_(settings) + { + // Update base task to be only on the base orientation + const Eigen::Vector orientation_mask{0., 0., 0., 1., 1., 1.}; + baseTask_->Kp(settings_.kp_base * Eigen::VectorXd::Ones(6)); + baseTask_->Kd(2.0 * baseTask_->Kp().cwiseSqrt()); + baseTask_->setMask(orientation_mask); + if (settings_.w_base > 0.) + { + // Task has changed size, need to be removed and added again for new size to be taken into account. + formulation_.removeTask(baseTask_->name(), 0); + formulation_.addMotionTask(*baseTask_, settings_.w_base, 1); + } + + // Add the center of mass task + comTask_ = std::make_unique("task-com", robot_); + comTask_->Kp(settings_.kp_com * Eigen::VectorXd::Ones(3)); + comTask_->Kd(2.0 * comTask_->Kp().cwiseSqrt()); + if (settings_.w_com > 0.) + formulation_.addMotionTask(*comTask_, settings_.w_com, 1); + + sampleCom_.resize(3); + + // Add foot tracking task + const size_t n_contacts = model_handler_.getFeetNb(); + for (size_t i = 0; i < n_contacts; i++) + { + const RobotModelHandler::FootType foot_type = model_handler.getFootType(i); + const std::string frame_name = model_handler.getFootFrameName(i); + Eigen::Vector position_mask = Eigen::Vector::Ones(); + if (foot_type == RobotModelHandler::POINT) + position_mask.tail<3>().setZero(); + else if (foot_type == RobotModelHandler::QUAD) + position_mask.tail<3>().setOnes(); + else + assert(false); + tsid::tasks::TaskSE3Equality & trackingTask = + trackingTasks_.emplace_back("task-tracking-" + frame_name, robot_, frame_name); + trackingTask.Kp(settings_.kp_feet_tracking * Eigen::VectorXd::Ones(6)); + trackingTask.Kd(2.0 * trackingTask.Kp().cwiseSqrt()); + trackingTask.setMask(position_mask); + // Do not add tasks ; will be done in setTarget depending on desired contacts. + feet_tracked_.push_back(false); + trackingSamples_.emplace_back(12, 6); + } + + // By default initialize target in reference state + const int nq = model_handler_.getModel().nq; + const int nv = model_handler_.getModel().nv; + const Eigen::VectorXd q_ref = model_handler.getReferenceState().head(nq); + const Eigen::VectorXd v_ref = model_handler.getReferenceState().tail(nv); + data_handler_.updateInternalData(q_ref, v_ref, false); + const Eigen::Vector3d com_pos{data_handler_.getData().com[0]}; + const Eigen::Vector3d com_vel{0., 0., 0.}; + FeetPoseVector feet_pose(n_contacts); + FeetVelocityVector feet_vel(n_contacts); + std::vector feet_contact(n_contacts); + std::vector feet_force; + for (size_t i = 0; i < n_contacts; i++) + { + // By default initialize all foot in contact with same amount of force + feet_contact[i] = true; + const RobotModelHandler::FootType foot_type = model_handler.getFootType(i); + if (foot_type == RobotModelHandler::POINT) + feet_force.push_back(TargetContactForce::Zero(3)); + else if (foot_type == RobotModelHandler::QUAD) + feet_force.push_back(TargetContactForce::Zero(6)); + else + assert(false); + feet_force[i][2] = 9.81 * model_handler_.getMass() / static_cast(n_contacts); // Weight on Z axis + feet_pose[i] = data_handler_.getFootRefPose(i); + feet_vel[i].setZero(); + } + setTarget(com_pos, com_vel, feet_pose, feet_vel, feet_contact, feet_force); + + // Dry run to initialize solver data & output + const tsid::solvers::HQPData & solver_data = formulation_.computeProblemData(0, q_ref, v_ref); + last_solution_ = solver_.solve(solver_data); + } + + void CentroidalID::setTarget( + const Eigen::Ref> & com_position, + const Eigen::Ref> & com_velocity, + const FeetPoseVector & feet_pose, + const FeetVelocityVector & feet_velocity, + const std::vector & contact_state_target, + const std::vector & f_target) + { + const int nq = model_handler_.getModel().nq; + const int nv = model_handler_.getModel().nv; + + // Set CoM target + sampleCom_.setValue(com_position); + sampleCom_.setDerivative(com_velocity); + sampleCom_.setSecondDerivative(Eigen::Vector3d::Zero()); + comTask_->setReference(sampleCom_); + + // Set feet tracking + if (settings_.w_feet_tracking > 0.) + { + for (std::size_t foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++) + { + const std::string & task_name{"task-tracking-" + model_handler_.getFootFrameName(foot_nb)}; + if (!contact_state_target[foot_nb]) + { + tsid::tasks::TaskSE3Equality & task{trackingTasks_[foot_nb]}; + tsid::trajectories::TrajectorySample & sample{trackingSamples_[foot_nb]}; + + // Add tracking task to tsid if necessary + if (!feet_tracked_[foot_nb]) + { + formulation_.addMotionTask(task, settings_.w_feet_tracking, 1); + feet_tracked_[foot_nb] = true; + } + // Set foot reference + tsid::math::SE3ToVector(feet_pose[foot_nb], sample.pos); + sample.vel.head<3>() = feet_velocity[foot_nb].linear(); + sample.vel.tail<3>() = feet_velocity[foot_nb].angular(); + sample.acc.setZero(); + task.setReference(sample); + } + else + { + // remove task if necessary + if (feet_tracked_[foot_nb]) + { + formulation_.removeTask(task_name, 0); + feet_tracked_[foot_nb] = false; + } + } + } + } + + // Set kinodynamics targets (will resize solver properly) + KinodynamicsID::setTarget( + model_handler_.getReferenceState().head(nq), model_handler_.getReferenceState().tail(nv), + Eigen::VectorXd::Zero(nv), contact_state_target, f_target); + } + +} // namespace simple_mpc diff --git a/src/inverse-dynamics/kinodynamics-id.cpp b/src/inverse-dynamics/kinodynamics-id.cpp new file mode 100644 index 00000000..27ae9786 --- /dev/null +++ b/src/inverse-dynamics/kinodynamics-id.cpp @@ -0,0 +1,226 @@ +#include +#include +#include + +using namespace simple_mpc; + +KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double control_dt, const Settings settings) +: settings_(settings) +, model_handler_(model_handler) +, data_handler_(model_handler_) +, robot_(model_handler_.getModel()) +, formulation_("tsid", robot_) +, solver_("solver-proxqp") +{ + const pinocchio::Model & model = model_handler.getModel(); + const int nq = model.nq; + const int nq_actuated = robot_.nq_actuated(); + const int nv = model.nv; + const int nu = nv - 6; + + // Prepare foot contact tasks + const size_t n_contacts = model_handler_.getFeetNb(); + const Eigen::Vector3d normal{0, 0, 1}; + const double weight = model_handler_.getMass() * 9.81; + const double max_f = settings_.contact_weight_ratio_max * weight; + const double min_f = settings_.contact_weight_ratio_min * weight; + for (size_t i = 0; i < n_contacts; i++) + { + const std::string frame_name = model_handler.getFootFrameName(i); + switch (model_handler.getFootType(i)) + { + case RobotModelHandler::FootType::POINT: { + auto contact_point = new tsid::contacts::ContactPoint( + frame_name, robot_, frame_name, normal, settings_.friction_coefficient, min_f, max_f); + contact_point->Kp(settings_.kp_contact * Eigen::VectorXd::Ones(3)); + contact_point->Kd(2.0 * contact_point->Kp().cwiseSqrt()); + contact_point->useLocalFrame(false); + tsid_contacts.emplace_back(contact_point); + break; + } + case RobotModelHandler::FootType::QUAD: { + auto contact_6D = new tsid::contacts::Contact6d( + frame_name, robot_, frame_name, model_handler_.getQuadFootContactPoints(i).transpose(), normal, + settings_.friction_coefficient, min_f, max_f); + contact_6D->Kp(settings_.kp_contact * Eigen::VectorXd::Ones(6)); + contact_6D->Kd(2.0 * contact_6D->Kp().cwiseSqrt()); + tsid_contacts.emplace_back(contact_6D); + break; + } + default: { + assert(false); + } + } + // By default contact is not active (will be by setTarget) + active_tsid_contacts_.push_back(false); + } + + // Add the posture task + postureTask_ = std::make_unique("task-posture", robot_); + postureTask_->Kp(settings_.kp_posture * Eigen::VectorXd::Ones(nu)); + postureTask_->Kd(2.0 * postureTask_->Kp().cwiseSqrt()); + if (settings_.w_posture > 0.) + formulation_.addMotionTask(*postureTask_, settings_.w_posture, 1); + + samplePosture_.resize(nq_actuated, nu); + + // Add the base task + baseTask_ = std::make_unique("task-base", robot_, model_handler_.getBaseFrameName()); + baseTask_->Kp(settings_.kp_base * Eigen::VectorXd::Ones(6)); + baseTask_->Kd(2.0 * baseTask_->Kp().cwiseSqrt()); + if (settings_.w_base > 0.) + formulation_.addMotionTask(*baseTask_, settings_.w_base, 1); + + sampleBase_.resize(12, 6); + + // Add joint limit task + boundsTask_ = std::make_unique("task-joint-limits", robot_, control_dt); + boundsTask_->setPositionBounds( + model_handler_.getModel().lowerPositionLimit.tail(nq_actuated), + model_handler_.getModel().upperPositionLimit.tail(nq_actuated)); + boundsTask_->setVelocityBounds(model_handler_.getModel().velocityLimit.tail(nu)); + boundsTask_->setImposeBounds( + true, true, true, false); // For now do not impose acceleration bound as it is not provided in URDF + formulation_.addMotionTask(*boundsTask_, 1.0, 0); // No weight needed as it is set as constraint + + // Add actuation limit task + actuationTask_ = std::make_unique("actuation-limits", robot_); + actuationTask_->setBounds( + -model_handler_.getModel().effortLimit.tail(nu), model_handler_.getModel().effortLimit.tail(nu)); + formulation_.addActuationTask(*actuationTask_, 1.0, 0); // No weight needed as it is set as constraint + + // Create an HQP solver + solver_.resize(formulation_.nVar(), formulation_.nEq(), formulation_.nIn()); + + // By default initialize target in reference state + const Eigen::VectorXd q_ref = model_handler.getReferenceState().head(nq); + const Eigen::VectorXd v_ref = model_handler.getReferenceState().tail(nv); + std::vector c_ref(n_contacts); + std::vector f_ref; + for (size_t i = 0; i < n_contacts; i++) + { + // By default initialize all foot in contact with same amount of force + c_ref[i] = true; + const RobotModelHandler::FootType foot_type = model_handler.getFootType(i); + if (foot_type == RobotModelHandler::POINT) + f_ref.push_back(TargetContactForce::Zero(3)); + else if (foot_type == RobotModelHandler::QUAD) + f_ref.push_back(TargetContactForce::Zero(6)); + else + assert(false); + f_ref[i][2] = weight / static_cast(n_contacts); // Weight on Z axis + } + setTarget(q_ref, v_ref, v_ref, c_ref, f_ref); + + // Dry run to initialize solver data & output + const tsid::solvers::HQPData & solver_data = formulation_.computeProblemData(0, q_ref, v_ref); + last_solution_ = solver_.solve(solver_data); +} + +void KinodynamicsID::setTarget( + const Eigen::Ref & q_target, + const Eigen::Ref & v_target, + const Eigen::Ref & a_target, + const std::vector & contact_state_target, + const std::vector & f_target) +{ + data_handler_.updateInternalData(q_target, v_target, false); + + // Posture task + samplePosture_.setValue(q_target.tail(robot_.nq_actuated())); + samplePosture_.setDerivative(v_target.tail(robot_.na())); + samplePosture_.setSecondDerivative(a_target.tail(robot_.na())); + postureTask_->setReference(samplePosture_); + + // Base task + tsid::math::SE3ToVector(data_handler_.getBaseFramePose(), sampleBase_.pos); + sampleBase_.setDerivative(v_target.head<6>()); + sampleBase_.setSecondDerivative(a_target.head<6>()); + baseTask_->setReference(sampleBase_); + + // Foot contacts + for (std::size_t foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++) + { + const std::string & name{model_handler_.getFootFrameName(foot_nb)}; + if (contact_state_target[foot_nb]) + { + // Add contact to tsid if necessary + if (!active_tsid_contacts_[foot_nb]) + { + formulation_.addRigidContact( + *tsid_contacts[foot_nb], settings_.w_contact_force, settings_.w_contact_motion, + settings_.contact_motion_equality ? 0 : 1); + active_tsid_contacts_[foot_nb] = true; + } + // Set contact target force + switch (model_handler_.getFootType(foot_nb)) + { + case RobotModelHandler::FootType::POINT: { + assert(f_target.at(foot_nb).size() == 3); + static_cast(*tsid_contacts[foot_nb]).setForceReference(f_target.at(foot_nb)); + break; + } + case RobotModelHandler::FootType::QUAD: { + assert(f_target.at(foot_nb).size() == 6); + static_cast(*tsid_contacts[foot_nb]).setForceReference(f_target.at(foot_nb)); + break; + } + default: { + assert(false); + } + } + } + else + { + // Remove contact from tsid if necessary + if (active_tsid_contacts_[foot_nb]) + { + formulation_.removeRigidContact(name, 0); + active_tsid_contacts_[foot_nb] = false; + } + } + } + solver_.resize(formulation_.nVar(), formulation_.nEq(), formulation_.nIn()); +} + +void KinodynamicsID::solve( + const double t, + const Eigen::Ref & q_meas, + const Eigen::Ref & v_meas, + Eigen::Ref tau_res) +{ + // Update contact position based on the real robot foot placement + data_handler_.updateInternalData(q_meas, v_meas, false); + for (std::size_t foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++) + { + if (active_tsid_contacts_[foot_nb]) + { + switch (model_handler_.getFootType(foot_nb)) + { + case RobotModelHandler::FootType::POINT: { + static_cast(*tsid_contacts[foot_nb]) + .setReference(data_handler_.getFootPose(foot_nb)); + break; + } + case RobotModelHandler::FootType::QUAD: { + static_cast(*tsid_contacts[foot_nb]) + .setReference(data_handler_.getFootPose(foot_nb)); + break; + } + default: { + assert(false); + } + } + } + } + + const tsid::solvers::HQPData & solver_data = formulation_.computeProblemData(t, q_meas, v_meas); + last_solution_ = solver_.solve(solver_data); + assert(last_solution_.status == tsid::solvers::HQPStatus::HQP_STATUS_OPTIMAL); + tau_res = formulation_.getActuatorForces(last_solution_); +} + +void KinodynamicsID::getAccelerations(Eigen::Ref ddq) +{ + ddq = formulation_.getAccelerations(last_solution_); +} diff --git a/src/kinodynamics.cpp b/src/kinodynamics.cpp index 6c93c0cb..8a4498ec 100644 --- a/src/kinodynamics.cpp +++ b/src/kinodynamics.cpp @@ -31,7 +31,7 @@ namespace simple_mpc , settings_(settings) { - nu_ = nv_ - 6 + settings_.force_size * (int)model_handler_.getFeetNames().size(); + nu_ = nv_ - 6 + settings_.force_size * (int)model_handler_.getFeetNb(); x0_ = model_handler_.getReferenceState(); control_ref_.resize(nu_); control_ref_.setZero(); @@ -55,19 +55,20 @@ namespace simple_mpc auto cent_mom = CentroidalMomentumResidual(space.ndx(), nu_, model_handler_.getModel(), Eigen::VectorXd::Zero(6)); auto centder_mom = CentroidalMomentumDerivativeResidual( - space.ndx(), model_handler_.getModel(), settings_.gravity, contact_states, model_handler_.getFeetIds(), + space.ndx(), model_handler_.getModel(), settings_.gravity, contact_states, model_handler_.getFeetFrameIds(), settings_.force_size); rcost.addCost("state_cost", QuadraticStateCost(space, nu_, model_handler_.getReferenceState(), settings_.w_x)); rcost.addCost("control_cost", QuadraticControlCost(space, control_ref_, settings_.w_u)); rcost.addCost("centroidal_cost", QuadraticResidualCost(space, cent_mom, settings_.w_cent)); rcost.addCost("centroidal_derivative_cost", QuadraticResidualCost(space, centder_mom, settings_.w_centder)); - for (auto const & name : model_handler_.getFeetNames()) + for (size_t foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++) { + const std::string & name = model_handler_.getFootFrameName(foot_nb); if (settings_.force_size == 6) { FramePlacementResidual frame_residual = FramePlacementResidual( - space.ndx(), nu_, model_handler_.getModel(), contact_pose.at(name), model_handler_.getFootId(name)); + space.ndx(), nu_, model_handler_.getModel(), contact_pose.at(name), model_handler_.getFootFrameId(foot_nb)); rcost.addCost(name + "_pose_cost", QuadraticResidualCost(space, frame_residual, settings_.w_frame)); } @@ -75,14 +76,14 @@ namespace simple_mpc { FrameTranslationResidual frame_residual = FrameTranslationResidual( space.ndx(), nu_, model_handler_.getModel(), contact_pose.at(name).translation(), - model_handler_.getFootId(name)); + model_handler_.getFootFrameId(foot_nb)); rcost.addCost(name + "_pose_cost", QuadraticResidualCost(space, frame_residual, settings_.w_frame)); } } KinodynamicsFwdDynamics ode = KinodynamicsFwdDynamics( - space, model_handler_.getModel(), settings_.gravity, contact_states, model_handler_.getFeetIds(), + space, model_handler_.getModel(), settings_.gravity, contact_states, model_handler_.getFeetFrameIds(), settings_.force_size); IntegratorSemiImplEuler dyn_model = IntegratorSemiImplEuler(ode, settings_.timestep); StageModel stm = StageModel(rcost, dyn_model); @@ -101,12 +102,13 @@ namespace simple_mpc Motion v_ref = Motion::Zero(); int i = 0; - for (auto const & name : model_handler_.getFeetNames()) + for (size_t foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++) { + const std::string & name = model_handler_.getFootFrameName(foot_nb); if (contact_phase.at(name)) { FrameVelocityResidual frame_vel = FrameVelocityResidual( - space.ndx(), nu_, model_handler_.getModel(), v_ref, model_handler_.getFootId(name), pinocchio::LOCAL); + space.ndx(), nu_, model_handler_.getModel(), v_ref, model_handler_.getFootFrameId(foot_nb), pinocchio::LOCAL); if (settings_.force_size == 6) { if (settings_.force_cone) @@ -135,7 +137,7 @@ namespace simple_mpc FrameTranslationResidual frame_residual = FrameTranslationResidual( space.ndx(), nu_, model_handler_.getModel(), contact_pose.at(name).translation(), - model_handler_.getFootId(name)); + model_handler_.getFootFrameId(foot_nb)); FunctionSliceXpr frame_slice = FunctionSliceXpr(frame_residual, frame_id); @@ -168,13 +170,13 @@ namespace simple_mpc void KinodynamicsOCP::setReferencePoses(const std::size_t t, const std::map & pose_refs) { - if (pose_refs.size() != model_handler_.getFeetNames().size()) + if (pose_refs.size() != model_handler_.getFeetNb()) { throw std::runtime_error("pose_refs size does not match number of end effectors"); } CostStack * cs = getCostStack(t); - for (auto ee_name : model_handler_.getFeetNames()) + for (auto ee_name : model_handler_.getFeetFrameNames()) { QuadraticResidualCost * qrc = cs->getComponent(ee_name + "_pose_cost"); if (settings_.force_size == 6) @@ -226,14 +228,14 @@ namespace simple_mpc void KinodynamicsOCP::computeControlFromForces(const std::map & force_refs) { - for (std::size_t i = 0; i < model_handler_.getFeetNames().size(); i++) + for (std::size_t i = 0; i < model_handler_.getFeetNb(); i++) { - if (settings_.force_size != force_refs.at(model_handler_.getFootName(i)).size()) + if (settings_.force_size != force_refs.at(model_handler_.getFootFrameName(i)).size()) { throw std::runtime_error("force size in settings does not match reference force size"); } control_ref_.segment((long)i * settings_.force_size, settings_.force_size) = - force_refs.at(model_handler_.getFootName(i)); + force_refs.at(model_handler_.getFootFrameName(i)); } } @@ -247,7 +249,7 @@ namespace simple_mpc void KinodynamicsOCP::setReferenceForce(const std::size_t i, const std::string & ee_name, const ConstVectorRef & force_ref) { - std::vector hname = model_handler_.getFeetNames(); + std::vector hname = model_handler_.getFeetFrameNames(); std::vector::iterator it = std::find(hname.begin(), hname.end(), ee_name); long id = it - hname.begin(); control_ref_.segment(id * settings_.force_size, settings_.force_size) = force_ref; @@ -256,7 +258,7 @@ namespace simple_mpc const Eigen::VectorXd KinodynamicsOCP::getReferenceForce(const std::size_t i, const std::string & ee_name) { - std::vector hname = model_handler_.getFeetNames(); + std::vector hname = model_handler_.getFeetFrameNames(); std::vector::iterator it = std::find(hname.begin(), hname.end(), ee_name); long id = it - hname.begin(); @@ -287,7 +289,7 @@ namespace simple_mpc { CostStack * cs = getCostStack(t); QuadraticStateCost * qc = cs->getComponent("state_cost"); - return qc->getTarget().head(7); + return qc->getTarget().head<7>(); }; void KinodynamicsOCP::setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) @@ -299,7 +301,7 @@ namespace simple_mpc CostStack * cs = getCostStack(t); QuadraticStateCost * qc = cs->getComponent("state_cost"); x0_ = getReferenceState(t); - x0_.head(7) = pose_base; + x0_.head<7>() = pose_base; qc->setTarget(x0_); } diff --git a/src/mpc.cpp b/src/mpc.cpp index f7449212..b68da215 100644 --- a/src/mpc.cpp +++ b/src/mpc.cpp @@ -21,15 +21,17 @@ namespace simple_mpc , ocp_handler_(problem) { - data_handler_ = std::make_shared(ocp_handler_->getModelHandler()); - data_handler_->updateInternalData(ocp_handler_->getModelHandler().getReferenceState(), true); + const RobotModelHandler & model_handler = ocp_handler_->getModelHandler(); + data_handler_ = std::make_shared(model_handler); + data_handler_->updateInternalData(model_handler.getReferenceState(), true); std::map starting_poses; - for (auto const & name : ocp_handler_->getModelHandler().getFeetNames()) + for (size_t foot_nb = 0; foot_nb < model_handler.getFeetNb(); foot_nb++) { - starting_poses.insert({name, data_handler_->getFootPose(name).translation()}); + const std::string & name = model_handler.getFootFrameName(foot_nb); + starting_poses.insert({name, data_handler_->getFootPose(foot_nb).translation()}); relative_feet_poses_.insert( - {name, data_handler_->getBaseFramePose().inverse() * data_handler_->getFootPose(name)}); + {name, data_handler_->getBaseFramePose().inverse() * data_handler_->getFootPose(foot_nb)}); } foot_trajectories_ = FootTrajectory( starting_poses, settings_.swing_apex, settings_.T_fly, settings_.T_contact, ocp_handler_->getSize()); @@ -51,8 +53,8 @@ namespace simple_mpc solver_->force_initial_condition_ = true; // solver_->reg_min = 1e-6; - ee_names_ = ocp_handler_->getModelHandler().getFeetNames(); - Eigen::VectorXd force_ref(ocp_handler_->getReferenceForce(0, ocp_handler_->getModelHandler().getFootName(0))); + ee_names_ = model_handler.getFeetFrameNames(); + Eigen::VectorXd force_ref(ocp_handler_->getReferenceForce(0, model_handler.getFootFrameName(0))); std::map contact_states; std::map land_constraint; @@ -63,7 +65,7 @@ namespace simple_mpc { contact_states.insert({name, true}); land_constraint.insert({name, false}); - contact_poses.insert({name, data_handler_->getFootPose(name)}); + contact_poses.insert({name, data_handler_->getFootPose(model_handler.getFootNb(name))}); force_map.insert({name, force_ref}); } @@ -144,8 +146,10 @@ namespace simple_mpc active_contacts += 1; } - Eigen::VectorXd force_ref(ocp_handler_->getReferenceForce(0, ocp_handler_->getModelHandler().getFootName(0))); - Eigen::VectorXd force_zero(ocp_handler_->getReferenceForce(0, ocp_handler_->getModelHandler().getFootName(0))); + Eigen::VectorXd force_ref( + ocp_handler_->getReferenceForce(0, ocp_handler_->getModelHandler().getFootFrameName(0))); + Eigen::VectorXd force_zero( + ocp_handler_->getReferenceForce(0, ocp_handler_->getModelHandler().getFootFrameName(0))); force_ref.setZero(); force_zero.setZero(); force_ref[2] = settings_.support_force / active_contacts; @@ -155,7 +159,7 @@ namespace simple_mpc for (auto const & name : ee_names_) { - contact_poses.insert({name, data_handler_->getFootPose(name)}); + contact_poses.insert({name, data_handler_->getFootPose(ocp_handler_->getModelHandler().getFootNb(name))}); if (state.at(name)) force_map.insert({name, force_ref}); else @@ -275,6 +279,7 @@ namespace simple_mpc { for (auto const & name : ee_names_) { + const size_t foot_nb = ocp_handler_->getModelHandler().getFootNb(name); int foot_land_time = -1; if (!foot_land_times_.at(name).empty()) foot_land_time = foot_land_times_.at(name)[0]; @@ -285,16 +290,16 @@ namespace simple_mpc // Use the Raibert heuristics to compute the next foot pose twist_vect_[0] = - -(data_handler_->getRefFootPose(name).translation()[1] - data_handler_->getBaseFramePose().translation()[1]); + -(data_handler_->getFootRefPose(foot_nb).translation()[1] - data_handler_->getBaseFramePose().translation()[1]); twist_vect_[1] = - data_handler_->getRefFootPose(name).translation()[0] - data_handler_->getBaseFramePose().translation()[0]; - next_pose_.head(2) = data_handler_->getRefFootPose(name).translation().head(2); - next_pose_.head(2) += (velocity_base_.head(2) + velocity_base_[5] * twist_vect_) - * (settings_.T_fly + settings_.T_contact) * settings_.timestep; - next_pose_[2] = data_handler_->getFootPose(name).translation()[2]; + data_handler_->getFootRefPose(foot_nb).translation()[0] - data_handler_->getBaseFramePose().translation()[0]; + next_pose_.head<2>() = data_handler_->getFootRefPose(foot_nb).translation().head<2>(); + next_pose_.head<2>() += (velocity_base_.head<2>() + velocity_base_[5] * twist_vect_) + * (settings_.T_fly + settings_.T_contact) * settings_.timestep; + next_pose_[2] = data_handler_->getFootPose(foot_nb).translation()[2]; foot_trajectories_.updateTrajectory( - update, foot_land_time, data_handler_->getFootPose(name).translation(), next_pose_, name); + update, foot_land_time, data_handler_->getFootPose(foot_nb).translation(), next_pose_, name); pinocchio::SE3 pose = pinocchio::SE3::Identity(); for (unsigned long time = 0; time < ocp_handler_->getSize(); time++) { @@ -346,10 +351,9 @@ namespace simple_mpc return int_data->continuous_data->xdot_; } - const Eigen::VectorXd MPC::getContactForces(const std::size_t t) + const Eigen::Matrix MPC::getContactForces(const std::size_t t) { - Eigen::VectorXd contact_forces; - contact_forces.resize(3 * (long)ee_names_.size()); + Eigen::Matrix contact_forces(ee_names_.size(), 3); ExplicitIntegratorData * int_data = dynamic_cast(&*solver_->workspace_.problem_data.stage_data[t]->dynamics_data); @@ -364,12 +368,12 @@ namespace simple_mpc { if (contact_state[i]) { - contact_forces.segment((long)i * 3, 3) = mc_data->constraint_datas_[force_id].contact_force.linear(); + contact_forces.row(i) = mc_data->constraint_datas_[force_id].contact_force.linear(); force_id += 1; } else { - contact_forces.segment((long)i * 3, 3).setZero(); + contact_forces.row(i).setZero(); } } return contact_forces; diff --git a/src/ocp-handler.cpp b/src/ocp-handler.cpp index 6ff6692b..881e8d1b 100644 --- a/src/ocp-handler.cpp +++ b/src/ocp-handler.cpp @@ -32,7 +32,7 @@ namespace simple_mpc throw std::runtime_error("Contact phases and forces sequences do not have the same size"); } std::map previous_phases; - for (auto const & name : model_handler_.getFeetNames()) + for (auto const & name : model_handler_.getFeetFrameNames()) { previous_phases.insert({name, true}); } @@ -40,7 +40,7 @@ namespace simple_mpc for (std::size_t i = 0; i < contact_phases.size(); i++) { std::map land_constraint; - for (auto const & name : model_handler_.getFeetNames()) + for (auto const & name : model_handler_.getFeetFrameNames()) { if (!previous_phases.at(name) and contact_phases[i].at(name)) land_constraint.insert({name, true}); @@ -106,12 +106,12 @@ namespace simple_mpc Eigen::VectorXd force_ref(force_size); force_ref.setZero(); - force_ref[2] = -model_handler_.getMass() * gravity / (double)model_handler_.getFeetNames().size(); + force_ref[2] = -model_handler_.getMass() * gravity / (double)model_handler_.getFeetNb(); std::map contact_phase; std::map contact_pose; std::map contact_force; - for (auto & name : model_handler_.getFeetNames()) + for (auto & name : model_handler_.getFeetFrameNames()) { contact_phase.insert({name, true}); contact_pose.insert({name, pinocchio::SE3::Identity()}); @@ -132,7 +132,7 @@ namespace simple_mpc if (terminal_constraint) { - createTerminalConstraint(x0.head(3)); + createTerminalConstraint(x0.head<3>()); } } } // namespace simple_mpc diff --git a/src/qp-solvers.cpp b/src/qp-solvers.cpp deleted file mode 100644 index f5437b2e..00000000 --- a/src/qp-solvers.cpp +++ /dev/null @@ -1,427 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// BSD 2-Clause License -// -// Copyright (C) 2024, INRIA -// Copyright note valid unless otherwise stated in individual files. -// All rights reserved. -/////////////////////////////////////////////////////////////////////////////// -#include "simple-mpc/qp-solvers.hpp" -#include -#include -#include - -namespace simple_mpc -{ - - IDSolver::IDSolver(const IDSettings & settings, const pin::Model & model) - : qp_(1, 1, 1) - , settings_(settings) - , model_(model) - { - - // Set the dimension of the problem - nk_ = (int)settings.contact_ids.size(); - force_dim_ = (int)settings.force_size * nk_; - int n = 2 * model_.nv - 6 + force_dim_; - int neq = model_.nv + force_dim_; - if (settings.force_size == 6) - nforcein_ = 9; - else - nforcein_ = 5; - int nin = nforcein_ * nk_ + model_.nv - 6; - - // Initialize QP matrices - A_ = Eigen::MatrixXd::Zero(neq, n); - b_ = Eigen::VectorXd::Zero(neq); - l_ = Eigen::VectorXd::Zero(nin); - u_ = Eigen::VectorXd::Ones(nin) * 100000; - C_ = Eigen::MatrixXd::Zero(nin, n); - g_ = Eigen::VectorXd::Zero(n); - H_ = Eigen::MatrixXd::Zero(n, n); - H_.topLeftCorner(model_.nv, model_.nv).diagonal() = Eigen::VectorXd::Ones(model_.nv) * settings_.w_acc; - H_.block(model_.nv, model_.nv, force_dim_, force_dim_).diagonal() = - Eigen::VectorXd::Ones(force_dim_) * settings_.w_force; - H_.bottomRightCorner(model_.nv - 6, model_.nv - 6).diagonal() = - Eigen::VectorXd::Ones(model_.nv - 6) * settings_.w_tau; - - // Initialize torque selection matrix - S_ = Eigen::MatrixXd::Zero(model_.nv, model_.nv - 6); - S_.bottomRows(model_.nv - 6).diagonal().setOnes(); - - // Initialize full contact Jacobian - Jc_ = Eigen::MatrixXd::Zero(force_dim_, model_.nv); - - // Initialize derivative of contact Jacobian - Jdot_ = Eigen::MatrixXd::Zero(6, model_.nv); - - // Initialize acceleration drift - gamma_ = Eigen::VectorXd::Zero(force_dim_); - - // Create the block matrix used for contact force cone - Cmin_.resize(nforcein_, settings.force_size); - if (settings.force_size == 3) - { - Cmin_ << -1, 0, settings.mu, 1, 0, settings.mu, 0, -1, settings.mu, 0, 1, settings.mu, 0, 0, 1; - } - else - { - Cmin_ << -1, 0, settings.mu, 0, 0, 0, 1, 0, settings.mu, 0, 0, 0, 0, -1, settings.mu, 0, 0, 0, 0, 1, settings.mu, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, settings.Wfoot, -1, 0, 0, 0, 0, settings.Wfoot, 1, 0, 0, 0, 0, settings.Lfoot, - 0, -1, 0, 0, 0, settings.Lfoot, 0, 1, 0; - } - for (long i = 0; i < nk_; i++) - { - C_.block(i * nforcein_, model_.nv + i * settings_.force_size, nforcein_, settings_.force_size) = Cmin_; - } - - // Set the block matrix for torque limits - // C_.bottomRightCorner(model_.nv - 6, model_.nv - 6).diagonal() = Eigen::VectorXd::Ones(model_.nv - 6); - - // Set size of solutions - solved_forces_.resize(force_dim_); - solved_acc_.resize(model_.nv); - solved_torque_.resize(model_.nv - 6); - - // Create and initialize the QP object - qp_ = - proxqp::dense::QP(n, neq, nin, false, proxqp::HessianType::Dense, proxqp::DenseBackend::PrimalDualLDLT); - qp_.settings.eps_abs = 1e-3; - qp_.settings.eps_rel = 0.0; - qp_.settings.primal_infeasibility_solving = true; - qp_.settings.check_duality_gap = true; - qp_.settings.verbose = settings.verbose; - qp_.settings.max_iter = 10; - qp_.settings.max_iter_in = 10; - - qp_.init(H_, g_, A_, b_, C_, l_, u_); - } - - void IDSolver::computeMatrices( - pinocchio::Data & data, - const std::vector & contact_state, - const ConstVectorRef & v, - const ConstVectorRef & a, - const ConstVectorRef & tau, - const ConstVectorRef & forces, - const ConstMatrixRef & M) - { - // Reset matrices - Jc_.setZero(); - gamma_.setZero(); - l_.head(nforcein_ * nk_).setZero(); - C_.block(0, 0, nforcein_ * nk_, model_.nv + force_dim_).setZero(); - - // Update diff torque lower and upper limits - l_.tail(model_.nv - 6) = -model_.effortLimit.tail(model_.nv - 6) - tau; - u_.tail(model_.nv - 6) = model_.effortLimit.tail(model_.nv - 6) - tau; - - // Update the problem with respect to current set of contacts - for (long i = 0; i < nk_; i++) - { - Jdot_.setZero(); - if (contact_state[(size_t)i]) - { - getFrameJacobianTimeVariation(model_, data, settings_.contact_ids[(size_t)i], pin::LOCAL_WORLD_ALIGNED, Jdot_); - Jc_.middleRows(i * settings_.force_size, settings_.force_size) = - getFrameJacobian(model_, data, settings_.contact_ids[(size_t)i], pin::LOCAL_WORLD_ALIGNED) - .topRows(settings_.force_size); - gamma_.segment(i * settings_.force_size, settings_.force_size) = Jdot_.topRows(settings_.force_size) * v; - - // Friction cone inequality update - l_.segment(i * nforcein_, 5) << forces[i * settings_.force_size] - - forces[i * settings_.force_size + 2] * settings_.mu, - -forces[i * settings_.force_size] - forces[i * settings_.force_size + 2] * settings_.mu, - forces[i * settings_.force_size + 1] - forces[i * settings_.force_size + 2] * settings_.mu, - -forces[i * settings_.force_size + 1] - forces[i * settings_.force_size + 2] * settings_.mu, - -forces[i * settings_.force_size + 2]; - if (nforcein_ == 9) - { - l_.segment(i * nforcein_ + 5, 4) - << forces[i * settings_.force_size + 3] - forces[i * settings_.force_size + 2] * settings_.Wfoot, - -forces[i * settings_.force_size + 3] - forces[i * settings_.force_size + 2] * settings_.Wfoot, - forces[i * settings_.force_size + 4] - forces[i * settings_.force_size + 2] * settings_.Lfoot, - -forces[i * settings_.force_size + 4] - forces[i * settings_.force_size + 2] * settings_.Lfoot; - } - - C_.block(i * nforcein_, model_.nv + i * settings_.force_size, nforcein_, settings_.force_size) = Cmin_; - } - } - - // Update equality matrices - A_.topLeftCorner(model_.nv, model_.nv) = M; - A_.block(0, model_.nv, model_.nv, force_dim_) = -Jc_.transpose(); - A_.topRightCorner(model_.nv, model_.nv - 6) = -S_; - A_.bottomLeftCorner(force_dim_, model_.nv) = Jc_; - - b_.head(model_.nv) = -data.nle - M * a + Jc_.transpose() * forces + S_ * tau; - b_.tail(force_dim_) = -gamma_ - Jc_ * a - settings_.kd * Jc_ * v; - } - - void IDSolver::solveQP( - pinocchio::Data & data, - const std::vector & contact_state, - const ConstVectorRef & v, - const ConstVectorRef & a, - const ConstVectorRef & tau, - const ConstVectorRef & forces, - const ConstMatrixRef & M) - { - - computeMatrices(data, contact_state, v, a, tau, forces, M); - qp_.update(H_, g_, A_, b_, C_, l_, u_, false); - qp_.solve(); - - solved_acc_ = a + qp_.results.x.head(model_.nv); - solved_forces_ = forces + qp_.results.x.segment(model_.nv, force_dim_); - solved_torque_ = tau + qp_.results.x.tail(model_.nv - 6); - } - - IKIDSolver::IKIDSolver(const IKIDSettings & settings, const pinocchio::Model & model) - : qp_(1, 1, 1) - , settings_(settings) - , model_(model) - { - - Jfoot_.resize(6, model_.nv); - Jfoot_.setZero(); - - dJfoot_.resize(6, model_.nv); - dJfoot_.setZero(); - - for (size_t i = 0; i < settings_.contact_ids.size(); i++) - { - Eigen::VectorXd foot_diff(6); - foot_diff.setZero(); - foot_diffs_.push_back(foot_diff); - - Eigen::VectorXd dfoot_diff(6); - dfoot_diff.setZero(); - dfoot_diffs_.push_back(dfoot_diff); - } - - for (size_t i = 0; i < settings_.fixed_frame_ids.size(); i++) - { - Eigen::Vector3d frame_diff; - frame_diff.setZero(); - frame_diffs_.push_back(frame_diff); - - Eigen::Vector3d dframe_diff; - dframe_diff.setZero(); - dframe_diffs_.push_back(dframe_diff); - } - q_diff_.resize(model_.nv); - q_diff_.setZero(); - dq_diff_.resize(model_.nv); - dq_diff_.setZero(); - - fs_ = (int)settings.force_size; - nk_ = (int)settings.contact_ids.size(); - force_dim_ = fs_ * nk_; - - int n = 2 * model_.nv - 6 + force_dim_; - int neq = model_.nv + force_dim_; - if (settings.force_size == 6) - nforcein_ = 9; - else - nforcein_ = 5; - int nin = nforcein_ * nk_; - - A_.resize(neq, n); - A_.setZero(); - b_.resize(neq); - b_.setZero(); - l_.resize(nin); - l_.setZero(); - C_.resize(nin, n); - C_.setZero(); - S_.resize(model_.nv, model_.nv - 6); - S_.setZero(); - S_.bottomRows(model_.nv - 6).diagonal().setOnes(); - l_box_.resize(n); - l_box_.setOnes(); - l_box_ *= -100000; - l_box_.tail(model.nv - 6) = -model.effortLimit.tail(model.nv - 6); - u_box_.resize(n); - u_box_.setOnes(); - u_box_ *= 100000; - u_box_.tail(model.nv - 6) = model.effortLimit.tail(model.nv - 6); - - Cmin_.resize(nforcein_, settings.force_size); - if (settings.force_size == 3) - { - Cmin_ << -1, 0, settings.mu, 1, 0, settings.mu, 0, -1, settings.mu, 0, 1, settings.mu, 0, 0, 1; - } - else - { - Cmin_ << -1, 0, settings.mu, 0, 0, 0, 1, 0, settings.mu, 0, 0, 0, 0, -1, settings.mu, 0, 0, 0, 0, 1, settings.mu, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, settings.Wfoot, -1, 0, 0, 0, 0, settings.Wfoot, 1, 0, 0, 0, 0, settings.Lfoot, - 0, -1, 0, 0, 0, settings.Lfoot, 0, 1, 0; - } - - for (long i = 0; i < nk_; i++) - { - C_.block(i * nforcein_, model_.nv + i * settings_.force_size, nforcein_, settings_.force_size) = Cmin_; - } - Jframe_.resize(3, model_.nv); - Jframe_.setZero(); - dJframe_.resize(6, model_.nv); - dJframe_.setZero(); - - u_ = Eigen::VectorXd::Ones(nin) * 100000; - g_ = Eigen::VectorXd::Zero(n); - H_ = Eigen::MatrixXd::Zero(n, n); - H_.block(model_.nv, model_.nv, force_dim_, force_dim_).diagonal() = - Eigen::VectorXd::Ones(force_dim_) * settings.w_force; - - solved_forces_.resize(force_dim_); - solved_acc_.resize(model_.nv); - solved_torque_.resize(model_.nv - 6); - - qp_ = - proxqp::dense::QP(n, neq, nin, true, proxqp::HessianType::Dense, proxqp::DenseBackend::PrimalDualLDLT); - qp_.settings.eps_abs = 1e-3; - qp_.settings.eps_rel = 0.0; - qp_.settings.primal_infeasibility_solving = true; - qp_.settings.check_duality_gap = true; - qp_.settings.verbose = settings.verbose; - qp_.settings.max_iter = 10; - qp_.settings.max_iter_in = 10; - - qp_.init(H_, g_, A_, b_, C_, l_, u_, l_box_, u_box_); - } - - void IKIDSolver::computeDifferences( - pinocchio::Data & data, - const ConstVectorRef & x_measured, - const std::vector & foot_refs, - const std::vector & foot_refs_next) - { - difference(model_, x_measured.head(model_.nq), settings_.x0.head(model_.nq), q_diff_); - dq_diff_ = settings_.x0.tail(model_.nv) - x_measured.tail(model_.nv); - - for (size_t i = 0; i < settings_.contact_ids.size(); i++) - { - FrameIndex id = settings_.contact_ids[i]; - foot_diffs_[i].head(3) = foot_refs[i].translation() - data.oMf[id].translation(); - foot_diffs_[i].tail(3) = -pin::log3(foot_refs[i].rotation().transpose() * data.oMf[id].rotation()); - - dfoot_diffs_[i].head(3) = (foot_refs_next[i].translation() - foot_refs[i].translation()) / settings_.dt - - pin::getFrameVelocity(model_, data, id, pin::LOCAL).linear(); - dfoot_diffs_[i].tail(3) = - pin::log3(foot_refs[i].rotation().transpose() * foot_refs_next[i].rotation()) / settings_.dt - - pin::getFrameVelocity(model_, data, id, pin::LOCAL).angular(); - } - for (size_t i = 0; i < settings_.fixed_frame_ids.size(); i++) - { - FrameIndex id = settings_.fixed_frame_ids[i]; - frame_diffs_[i] = -pin::log3(data.oMf[id].rotation()); - dframe_diffs_[i] = -pin::getFrameVelocity(model_, data, id, pin::LOCAL).angular(); - } - } - - void IKIDSolver::computeMatrices( - pinocchio::Data & data, - const std::vector & contact_state, - const ConstVectorRef & v_current, - const ConstVectorRef & forces, - const ConstVectorRef & dH, - const ConstMatrixRef & M) - { - - H_.topLeftCorner(model_.nv, model_.nv) = settings_.w_qref * Eigen::MatrixXd::Identity(model_.nv, model_.nv); - - H_.topLeftCorner(model_.nv, model_.nv) += settings_.w_centroidal * data.Ag.transpose() * data.Ag; - - g_.head(model_.nv) = - settings_.w_qref * (-settings_.Kp_gains[0].cwiseProduct(q_diff_) - settings_.Kd_gains[0].cwiseProduct(dq_diff_)); - g_.head(model_.nv) -= settings_.w_centroidal * (dH - data.dAg * v_current).transpose() * data.Ag; - - A_.topLeftCorner(model_.nv, model_.nv) = M; - A_.topRightCorner(model_.nv, model_.nv - 6) = -S_; - - b_.head(model_.nv) = -data.nle; - b_.tail(force_dim_).setZero(); - l_.setZero(); - C_.setZero(); - - for (size_t i = 0; i < settings_.contact_ids.size(); i++) - { - dJfoot_.setZero(); - FrameIndex id = settings_.contact_ids[i]; - Jfoot_ = getFrameJacobian(model_, data, id, pin::LOCAL); - getFrameJacobianTimeVariation(model_, data, id, pin::LOCAL, dJfoot_); - - H_.topLeftCorner(model_.nv, model_.nv) += - settings_.w_footpose * Jfoot_.topRows(fs_).transpose() * Jfoot_.topRows(fs_); - - g_.head(model_.nv) += - settings_.w_footpose - * (dJfoot_.topRows(fs_) * v_current - settings_.Kp_gains[1].cwiseProduct(foot_diffs_[i].topRows(fs_)) - - settings_.Kd_gains[1].cwiseProduct(dfoot_diffs_[i].topRows(fs_))) - .transpose() - * Jfoot_.topRows(fs_); - - long il = (long)i; - if (contact_state[i]) - { - A_.block(0, model_.nv + il * fs_, model_.nv, fs_) = -Jfoot_.topRows(fs_).transpose(); - A_.block(model_.nv + il * fs_, 0, fs_, model_.nv) = Jfoot_.topRows(fs_); - b_.head(model_.nv) += Jfoot_.topRows(fs_).transpose() * forces.segment(il * fs_, fs_); - b_.segment(model_.nv + il * fs_, fs_) = -dJfoot_.topRows(fs_) * v_current; - - l_.segment(il * nforcein_, 5) << forces[il * fs_] - forces[il * fs_ + 2] * settings_.mu, - -forces[il * fs_] - forces[il * fs_ + 2] * settings_.mu, - forces[il * fs_ + 1] - forces[il * fs_ + 2] * settings_.mu, - -forces[il * fs_ + 1] - forces[il * fs_ + 2] * settings_.mu, -forces[il * fs_ + 2]; - if (nforcein_ == 9) - { - l_.segment(il * nforcein_ + 5, 4) << forces[il * fs_ + 3] - forces[il * fs_ + 2] * settings_.Wfoot, - -forces[il * fs_ + 3] - forces[il * fs_ + 2] * settings_.Wfoot, - forces[il * fs_ + 4] - forces[il * fs_ + 2] * settings_.Lfoot, - -forces[il * fs_ + 4] - forces[il * fs_ + 2] * settings_.Lfoot; - } - - C_.block(il * nforcein_, model_.nv + il * fs_, nforcein_, fs_) = Cmin_; - } - else - { - A_.block(0, model_.nv + il * fs_, model_.nv, fs_).setZero(); - A_.block(model_.nv + il * fs_, 0, fs_, model_.nv).setZero(); - } - } - - for (size_t i = 0; i < settings_.fixed_frame_ids.size(); i++) - { - dJframe_.setZero(); - FrameIndex id = settings_.fixed_frame_ids[i]; - Jframe_ = pin::getFrameJacobian(model_, data, id, pin::LOCAL).bottomRows(3); - pin::getFrameJacobianTimeVariation(model_, data, id, pin::LOCAL, dJframe_); - - H_.topLeftCorner(model_.nv, model_.nv) += settings_.w_baserot * Jframe_.transpose() * Jframe_; - g_.head(model_.nv) += settings_.w_baserot - * (dJframe_.bottomRows(3) * v_current - settings_.Kp_gains[2].cwiseProduct(frame_diffs_[i]) - - settings_.Kd_gains[2].cwiseProduct(dframe_diffs_[i])) - .transpose() - * Jframe_; - } - } - - void IKIDSolver::solve_qp( - pinocchio::Data & data, - const std::vector & contact_state, - const ConstVectorRef & v_current, - const ConstVectorRef & forces, - const ConstVectorRef & dH, - const ConstMatrixRef & M) - { - computeMatrices(data, contact_state, v_current, forces, dH, M); - - qp_.update(H_, g_, A_, b_, C_, l_, u_, l_box_, u_box_, false); - qp_.solve(); - - solved_acc_ = qp_.results.x.head(model_.nv); - solved_forces_ = forces + qp_.results.x.segment(model_.nv, force_dim_); - solved_torque_ = qp_.results.x.tail(model_.nv - 6); - } - -} // namespace simple_mpc diff --git a/src/robot-handler.cpp b/src/robot-handler.cpp index f77c4015..1ee0c563 100644 --- a/src/robot-handler.cpp +++ b/src/robot-handler.cpp @@ -14,7 +14,7 @@ namespace simple_mpc : model_(model) { // Root frame id - base_id_ = model_.getFrameId(base_frame_name); + base_frame_id_ = model_.getFrameId(base_frame_name); // Set reference state reference_state_.resize(model_.nq + model_.nv); @@ -24,37 +24,58 @@ namespace simple_mpc mass_ = pinocchio::computeTotalMass(model_); } - FrameIndex RobotModelHandler::addFoot(const std::string & foot_name, const std::string & reference_frame_name) + void RobotModelHandler::addFootFrames(const std::string & foot_name, const std::string & reference_parent_frame_name) { - feet_names_.push_back(foot_name); - feet_ids_.push_back(model_.getFrameId(foot_name)); + const size_t new_foot_index = getFeetNb(); + const FrameIndex foot_frame_id = model_.getFrameId(foot_name); + + feet_frame_names_.push_back(foot_name); + feet_frame_ids_.push_back(foot_frame_id); // Create reference frame - FrameIndex reference_frame_id = model_.getFrameId(reference_frame_name); - JointIndex parent_joint = model_.frames[reference_frame_id].parentJoint; + FrameIndex reference_parent_frame_id = model_.getFrameId(reference_parent_frame_name); + JointIndex parent_joint = model_.frames[reference_parent_frame_id].parentJoint; auto new_frame = pinocchio::Frame( - foot_name + "_ref", parent_joint, reference_frame_id, pinocchio::SE3::Identity(), pinocchio::OP_FRAME); + foot_name + "_ref", parent_joint, reference_parent_frame_id, pinocchio::SE3::Identity(), pinocchio::OP_FRAME); auto frame_id = model_.addFrame(new_frame); // Save foot id - ref_feet_ids_.push_back(frame_id); + feet_ref_frame_ids_.push_back(frame_id); // Set placement to default value pinocchio::Data data(model_); pinocchio::forwardKinematics(model_, data, getReferenceState().head(model_.nq)); pinocchio::updateFramePlacements(model_, data); - const pinocchio::SE3 default_placement = data.oMf[reference_frame_id].actInv(data.oMf[frame_id]); + const pinocchio::SE3 default_placement = data.oMf[reference_parent_frame_id].actInv(data.oMf[foot_frame_id]); + + setFootReferencePlacement(new_foot_index, default_placement); + } - setFootReferencePlacement(foot_name, default_placement); + size_t RobotModelHandler::addPointFoot(const std::string & foot_name, const std::string & reference_parent_frame_name) + { + addFootFrames(foot_name, reference_parent_frame_name); + feet_types_.push_back(FootType::POINT); + const size_t foot_nb = feet_types_.size() - 1; + return foot_nb; + } - return frame_id; + size_t RobotModelHandler::addQuadFoot( + const std::string & foot_name, + const std::string & reference_parent_frame_name, + const ContactPointsMatrix & contactPoints) + { + addFootFrames(foot_name, reference_parent_frame_name); + feet_types_.push_back(FootType::QUAD); + const size_t foot_nb = feet_types_.size() - 1; + feet_contact_points_.insert({foot_nb, contactPoints}); + return foot_nb; } - void RobotModelHandler::setFootReferencePlacement(const std::string & foot_name, const SE3 & refMfoot) + void RobotModelHandler::setFootReferencePlacement(size_t foot_nb, const SE3 & parentframeMfootref) { - model_.frames[model_.getFrameId(foot_name + "_ref", pinocchio::OP_FRAME)].placement = refMfoot; + model_.frames[feet_ref_frame_ids_.at(foot_nb)].placement = parentframeMfootref; } Eigen::VectorXd RobotModelHandler::difference(const ConstVectorRef & x1, const ConstVectorRef & x2) const @@ -77,6 +98,7 @@ namespace simple_mpc RobotDataHandler::RobotDataHandler(const RobotModelHandler & model_handler) : model_handler_(model_handler) , data_(model_handler.getModel()) + , x_(model_handler.getReferenceState().size()) { updateInternalData(model_handler.getReferenceState(), true); } @@ -85,15 +107,22 @@ namespace simple_mpc { const Eigen::Block q = x.head(model_handler_.getModel().nq); const Eigen::Block v = x.tail(model_handler_.getModel().nv); - x_ = x; - forwardKinematics(model_handler_.getModel(), data_, q); + updateInternalData(q, v, updateJacobians); + } + + void + RobotDataHandler::updateInternalData(const ConstVectorRef & q, const ConstVectorRef & v, const bool updateJacobians) + { + x_ << q, v; + + forwardKinematics(model_handler_.getModel(), data_, q, v); updateFramePlacements(model_handler_.getModel(), data_); computeCentroidalMomentum(model_handler_.getModel(), data_, q, v); if (updateJacobians) { - updateJacobiansMassMatrix(x); + updateJacobiansMassMatrix(x_); } } @@ -113,9 +142,9 @@ namespace simple_mpc RobotDataHandler::CentroidalStateVector RobotDataHandler::getCentroidalState() const { RobotDataHandler::CentroidalStateVector x_centroidal; - x_centroidal.head(3) = data_.com[0]; - x_centroidal.segment(3, 3) = data_.hg.linear(); - x_centroidal.tail(3) = data_.hg.angular(); + x_centroidal.head<3>() = data_.com[0]; + x_centroidal.segment<3>(3) = data_.hg.linear(); + x_centroidal.tail<3>() = data_.hg.angular(); return x_centroidal; } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 8540071b..da3bf376 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -25,9 +25,10 @@ function(set_standard_output_directory target) ) endfunction() -function(_add_test_prototype name prefix dependencies) +function(_add_test_prototype filename prefix dependencies) + string(REGEX REPLACE "/" "_" name ${filename}) set(test_name "test-cpp-${prefix}${name}") - set(test_file ${name}.cpp) + set(test_file ${filename}.cpp) message(STATUS "Adding C++ test: ${test_file} (${test_name})") ADD_UNIT_TEST(${test_name} ${test_file} ${TEST_HEADERS}) @@ -45,17 +46,18 @@ function(_add_test_prototype name prefix dependencies) target_link_libraries(${test_name} PRIVATE Boost::unit_test_framework) endfunction() -function(add_aligator_test name) - _add_test_prototype(${name} "" ${PROJECT_NAME}) +function(add_aligator_test filename) + _add_test_prototype(${filename} "" ${PROJECT_NAME}) endfunction() set( TEST_NAMES robot_handler problem + inverse-dynamics/kinodynamics-id + inverse-dynamics/centroidal-id #mpc friction - qpsolvers interpolator ) diff --git a/tests/inverse-dynamics/centroidal-id.cpp b/tests/inverse-dynamics/centroidal-id.cpp new file mode 100644 index 00000000..0449f154 --- /dev/null +++ b/tests/inverse-dynamics/centroidal-id.cpp @@ -0,0 +1,446 @@ + +#include +#include +#include +#include + +#include "simple-mpc/inverse-dynamics/centroidal-id.hpp" +#include "simple-mpc/robot-handler.hpp" +#include "test_utils.cpp" + +BOOST_AUTO_TEST_SUITE(inverse_dynamics_centroidal) + +using namespace simple_mpc; + +Eigen::VectorXd solo_q_start(const RobotModelHandler & model_handler) +{ + Eigen::VectorXd q_start = model_handler.getReferenceState().head(model_handler.getModel().nq); + for (int l = 0; l < 4; l++) + { + q_start[7 + 3 * l + 1] = 0.9; + q_start[7 + 3 * l + 2] = -1.8; + } + q_start[0] = 0.01; + q_start[1] = 0.01; + q_start[2] = 0.21; + + return q_start; +} + +// Helper class to create the problem and run it +class TestCentroidalID +{ +public: + TestCentroidalID(RobotModelHandler model_handler_, CentroidalID::Settings settings_) + : model_handler(model_handler_) + , data_handler(model_handler) + , settings(settings_) + , solver(model_handler, dt, settings) + , q(model_handler.getReferenceState().head(model_handler.getModel().nq)) + , dq(Eigen::VectorXd::Zero(model_handler.getModel().nv)) + , ddq(Eigen::VectorXd::Zero(model_handler.getModel().nv)) + , tau(Eigen::VectorXd::Zero(model_handler.getModel().nv - 6)) + { + } + + void step() + { + // Solve + solver.solve(t, q, dq, tau); + solver.getAccelerations(ddq); + + // Integrate + step_i += 1; + t += dt; + q = pinocchio::integrate(model_handler.getModel(), q, (dq + ddq / 2. * dt) * dt); + dq += ddq * dt; + + // Update data handler + data_handler.updateInternalData(q, dq, true); + + // Check common to all tests + check_joint_limits(); + } + + bool is_error_decreasing(std::string name, double error) + { + if (errors.count(name) == 0) + { + errors.insert({name, error}); + return true; // no further check + } + const bool res{error <= errors.at(name)}; + errors.at(name) = error; // Update value + return res; + } + +protected: + void check_joint_limits() + { + const pinocchio::Model & model = model_handler.getModel(); + for (int i = 0; i < model.nv - 6; i++) + { + BOOST_CHECK_LE(q[7 + i], model.upperPositionLimit[7 + i]); + BOOST_CHECK_GE(q[7 + i], model.lowerPositionLimit[7 + i]); + BOOST_CHECK_LE(dq[6 + i], model.velocityLimit[6 + i]); + // Do not use lower velocity bound as TSID cannot handle it + BOOST_CHECK_GE(dq[6 + i], -model.velocityLimit[6 + i]); + BOOST_CHECK_LE(tau[i], model.effortLimit[6 + i]); + BOOST_CHECK_GE(tau[i], -model.effortLimit[6 + i]); + } + } + +public: + const RobotModelHandler model_handler; + RobotDataHandler data_handler; + CentroidalID::Settings settings; + double dt = 1e-3; + CentroidalID solver; + + double t = 0.; + int step_i = 0; + Eigen::VectorXd q; + Eigen::VectorXd dq; + Eigen::VectorXd ddq; + Eigen::VectorXd tau; + + std::map errors; +}; + +BOOST_AUTO_TEST_CASE(CentroidalID_postureTask) +{ + CentroidalID::Settings settings; + settings.kp_posture = 20.; + settings.w_posture = 1.; + + TestCentroidalID test(getSoloHandler(), settings); + + // Easy access + const RobotModelHandler & model_handler = test.model_handler; + const int nq = model_handler.getModel().nq; + const int nv = model_handler.getModel().nv; + + // Target state + const Eigen::VectorXd q_target = + model_handler.getReferenceState().head(nq); // CentroidalID set posture target to reference configuration + CentroidalID::FeetPoseVector feet_pose_vec; + CentroidalID::FeetVelocityVector feet_vel_vec; + for (size_t foot_nb = 0; foot_nb < test.model_handler.getFeetNb(); foot_nb++) + { + feet_pose_vec.push_back(pinocchio::SE3::Identity()); + feet_vel_vec.push_back(pinocchio::Motion::Zero()); + } + test.solver.setTarget( + Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), feet_pose_vec, feet_vel_vec, {false, false, false, false}, {}); + + // Change initial state + test.q = solo_q_start(model_handler); + for (int i = 0; i < 1000; i++) + { + // Solve + test.step(); + + // compensate for free fall as we did not set any contact (we only care about joint posture) + test.q.head(7) = q_target.head(7); + test.dq.head(6).setZero(); + + // Check error is decreasing + Eigen::VectorXd delta_q = pinocchio::difference(model_handler.getModel(), test.q, q_target); + const double error = delta_q.tail(nv - 6).norm(); // Consider only the posture not the free flyer + BOOST_CHECK(test.is_error_decreasing("posture", error)); + } +} + +void test_contact(TestCentroidalID & test) +{ + // Easy access + const RobotModelHandler & model_handler = test.model_handler; + const RobotDataHandler & data_handler = test.data_handler; + const int nq = model_handler.getModel().nq; + + // No need to set target as CentroidalID sets it by default to reference state + const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq); + + // Let the robot stabilize + const int N_STEP = 500; + while (test.step_i < N_STEP) + { + // Solve + test.step(); + + // Check that contact velocity is null + for (size_t foot_nb = 0; foot_nb < model_handler.getFeetNb(); foot_nb++) + { + const pinocchio::Motion foot_vel = pinocchio::getFrameVelocity( + model_handler.getModel(), data_handler.getData(), model_handler.getFootFrameId(foot_nb), pinocchio::WORLD); + BOOST_CHECK_LE(foot_vel.linear().norm(), 1e-2); + if (model_handler.getFootType(foot_nb) == RobotModelHandler::FootType::QUAD) + { + // Rotation should also be null for quadrilateral contacts + BOOST_CHECK_LE(foot_vel.angular().norm(), 1e-1); + } + } + } +} + +BOOST_AUTO_TEST_CASE(CentroidalID_contactPoint_cost) +{ + CentroidalID::Settings settings; + settings.kp_base = 1.0; + settings.kp_com = 1.0; + settings.kp_contact = 10.0; + settings.w_base = 1.; + settings.w_com = 1.; + settings.w_contact_motion = 10.0; + settings.w_contact_force = 1.0; + + TestCentroidalID simu(getSoloHandler(), settings); + simu.q = solo_q_start(simu.model_handler); // Set initial configuration + test_contact(simu); +} + +BOOST_AUTO_TEST_CASE(CentroidalID_contactQuad_cost) +{ + CentroidalID::Settings settings; + settings.kp_base = 1.0; + settings.kp_posture = 1.; + settings.kp_contact = 10.0; + settings.w_base = 1.; + settings.w_posture = 0.05; + settings.w_contact_motion = 10.0; + settings.w_contact_force = 1.0; + + TestCentroidalID simu(getTalosModelHandler(), settings); + test_contact(simu); +} + +BOOST_AUTO_TEST_CASE(CentroidalID_contactPoint_equality) +{ + CentroidalID::Settings settings; + settings.kp_base = 1.0; + settings.kp_com = 1.0; + settings.kp_contact = 10.0; + settings.w_base = 1.; + settings.w_com = 1.; + settings.w_contact_motion = 10.0; + settings.w_contact_force = 1.0; + settings.contact_motion_equality = true; + TestCentroidalID simu(getSoloHandler(), settings); + simu.q = solo_q_start(simu.model_handler); // Set initial configuration + test_contact(simu); +} + +BOOST_AUTO_TEST_CASE(CentroidalID_contactQuad_equality) +{ + CentroidalID::Settings settings; + settings.kp_base = 1.0; + settings.kp_posture = 1.; + settings.kp_contact = 10.0; + settings.w_base = 1.; + settings.w_posture = 0.05; + settings.w_contact_motion = 10.0; + settings.w_contact_force = 1.0; + settings.contact_motion_equality = true; + + TestCentroidalID simu(getTalosModelHandler(), settings); + test_contact(simu); +} + +BOOST_AUTO_TEST_CASE(CentroidalID_baseTask) +{ + CentroidalID::Settings settings; + settings.kp_base = 7.; + settings.kp_contact = .1; + settings.w_base = 100.0; + settings.w_contact_force = 1.0; + settings.w_contact_motion = 1.0; + + TestCentroidalID test(getSoloHandler(), settings); + + // Easy access + const RobotModelHandler & model_handler = test.model_handler; + const int nq = model_handler.getModel().nq; + + // CentroidalID sets posture task by default to reference state + const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq); + + // Change initial state + test.q.segment<4>(3) << 0.0025, 0.05, 0.05, 0.998; // small orientation perturbation (~6° on pitch and yaw) + test.q.segment<4>(3) /= test.q.segment<4>(3).norm(); + + const int N_STEP = 5000; + for (int i = 0; i < N_STEP; i++) + { + // Solve + test.step(); + + // Compute error + const Eigen::VectorXd delta_orientation = + pinocchio::difference(model_handler.getModel(), test.q, q_target).segment<3>(3); + const double error = delta_orientation.norm(); + + // Checks + if (error > 1e-3) // If haven't converged yet, should be strictly decreasing + BOOST_CHECK(test.is_error_decreasing("base_orientation", error)); + if (i > 9 * N_STEP / 10) // Should have converged by now + BOOST_CHECK(error < 1e-3); + } +} + +BOOST_AUTO_TEST_CASE(CentroidalID_comTask) +{ + CentroidalID::Settings settings; + settings.kp_com = 7.; + settings.kp_contact = .1; + settings.w_com = 100.0; + settings.w_contact_force = 1.0; + settings.w_contact_motion = 1.0; + + TestCentroidalID test(getSoloHandler(), settings); + test.q = solo_q_start(test.model_handler); + + // Easy access + const RobotModelHandler & model_handler = test.model_handler; + RobotDataHandler & data_handler = test.data_handler; + const int feet_nb = test.model_handler.getFeetNb(); + + // Set target + const Eigen::Vector3d com_target{-0.01, -0.01, 0.15}; + const Eigen::Vector3d com_vel{0., 0., 0.}; + data_handler.updateInternalData(test.q, test.dq, false); + CentroidalID::FeetPoseVector feet_pose_vec(feet_nb); + CentroidalID::FeetVelocityVector feet_vel_vec(feet_nb); + std::vector feet_contact(feet_nb); + std::vector feet_force; + for (int i = 0; i < feet_nb; i++) + { + feet_pose_vec[i] = data_handler.getFootPose(i); + feet_vel_vec[i].setZero(); + feet_contact[i] = true; + feet_force.push_back(CentroidalID::TargetContactForce::Zero(3)); + feet_force[i][2] = 9.81 * model_handler.getMass() / feet_nb; + } + test.solver.setTarget(com_target, com_vel, feet_pose_vec, feet_vel_vec, feet_contact, feet_force); + + const int N_STEP = 5000; + for (int i = 0; i < N_STEP; i++) + { + // Solve + test.step(); + + // Compute error + data_handler.updateInternalData(test.q, test.dq, false); + const Eigen::VectorXd delta_position = data_handler.getData().com[0] - com_target; + const double error = delta_position.norm(); + + // Checks + if (error > 1e-3) // If haven't converged yet, should be strictly decreasing + BOOST_CHECK(test.is_error_decreasing("com_position", error)); + if (i > 9 * N_STEP / 10) // Should have converged by now + BOOST_CHECK(error < 1e-3); + } +} + +BOOST_AUTO_TEST_CASE(CentroidalID_footTrackingTask) +{ + CentroidalID::Settings settings; + settings.kp_feet_tracking = 5.; + settings.kp_posture = 0.1; + settings.kp_contact = 1.0; + settings.w_feet_tracking = 1e3; + settings.w_posture = 1.0; + settings.w_contact_force = 0.1; + settings.contact_motion_equality = true; + + TestCentroidalID test(getSoloHandler(), settings); + + // Rotate the robot slightly so that it doesn't fall forward due to gravity + test.q.segment<4>(3) << 0, -0.17, 0, 0.98; + test.q.segment<4>(3) /= test.q.segment<4>(3).norm(); + + // Easy access + const RobotModelHandler & model_handler = test.model_handler; + RobotDataHandler & data_handler = test.data_handler; + const int feet_nb = test.model_handler.getFeetNb(); + + // Set target + data_handler.updateInternalData(test.q, test.dq, false); + const Eigen::Vector3d com_target{data_handler.getData().com[0]}; + const Eigen::Vector3d com_vel{0., 0., 0.}; + CentroidalID::FeetPoseVector feet_pose_vec(feet_nb); + CentroidalID::FeetVelocityVector feet_vel_vec(feet_nb); + std::vector feet_contact(feet_nb); + std::vector feet_force; + for (int i = 0; i < feet_nb; i++) + { + feet_pose_vec[i] = data_handler.getFootPose(i); + feet_vel_vec[i].setZero(); + feet_contact[i] = true; + feet_force.push_back(CentroidalID::TargetContactForce::Zero(3)); + feet_force[i][2] = 9.81 * model_handler.getMass() / (feet_nb - 1); + } + + // Lift first foot (front right) + feet_contact[0] = false; + feet_pose_vec[0].translation()[0] -= 0.05; // Move foot 5 cm backward + feet_pose_vec[0].translation()[1] += 0.05; // Move foot 5cm to the left (inside) + feet_pose_vec[0].translation()[2] += 0.05; // Lift foot 5 cm higher + + test.solver.setTarget(com_target, com_vel, feet_pose_vec, feet_vel_vec, feet_contact, feet_force); + + const int N_STEP = 5000; + for (int i = 0; i < N_STEP; i++) + { + // Solve + test.step(); + + // Compute error + data_handler.updateInternalData(test.q, test.dq, false); + const Eigen::VectorXd delta_foot_pose = + pinocchio::log6(feet_pose_vec[0].actInv(data_handler.getFootPose(0))).toVector().head<3>(); + const double error = delta_foot_pose.norm(); + + // Checks + if (error > 1e-3) // If haven't converged yet, should be strictly decreasing + BOOST_CHECK(test.is_error_decreasing("foot_pose", error)); + if (i > 9 * N_STEP / 10) // Should have converged by now + BOOST_CHECK(error < 1e-3); + } +} + +BOOST_AUTO_TEST_CASE(CentroidalID_allTasks) +{ + CentroidalID::Settings settings; + settings.kp_base = 10.; + settings.kp_posture = 1.; + settings.kp_contact = 10.; + settings.w_base = 10.0; + settings.w_posture = 0.1; + settings.w_contact_force = 1.0; + settings.w_contact_motion = 1.0; + + TestCentroidalID test(getSoloHandler(), settings); + + // Easy access + const RobotModelHandler & model_handler = test.model_handler; + const int nq = model_handler.getModel().nq; + + // No need to set target as CentroidalID sets it by default to reference state + const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq); + + test.q = solo_q_start(model_handler); + const int N_STEP = 1000; + for (int i = 0; i < N_STEP; i++) + { + // Solve + test.step(); + + // Check error is decreasing + const Eigen::VectorXd delta_q = pinocchio::difference(model_handler.getModel(), test.q, q_target); + const double error = delta_q.norm(); + + BOOST_CHECK(test.is_error_decreasing("q", error)); + } +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/tests/inverse-dynamics/kinodynamics-id.cpp b/tests/inverse-dynamics/kinodynamics-id.cpp new file mode 100644 index 00000000..d5806fc5 --- /dev/null +++ b/tests/inverse-dynamics/kinodynamics-id.cpp @@ -0,0 +1,312 @@ + +#include +#include +#include +#include + +#include "simple-mpc/inverse-dynamics/kinodynamics-id.hpp" +#include "simple-mpc/robot-handler.hpp" +#include "test_utils.cpp" + +BOOST_AUTO_TEST_SUITE(inverse_dynamics_kinodynamics) + +using namespace simple_mpc; + +Eigen::VectorXd solo_q_start(const RobotModelHandler & model_handler) +{ + Eigen::VectorXd q_start = model_handler.getReferenceState().head(model_handler.getModel().nq); + for (int l = 0; l < 4; l++) + { + q_start[7 + 3 * l + 1] = 0.9; + q_start[7 + 3 * l + 2] = -1.8; + } + q_start[0] = 0.01; + q_start[1] = 0.01; + q_start[2] = 0.21; + + return q_start; +} + +// Helper class to create the problem and run it +class TestKinoID +{ +public: + TestKinoID(RobotModelHandler model_handler_, KinodynamicsID::Settings settings_) + : model_handler(model_handler_) + , data_handler(model_handler) + , settings(settings_) + , solver(model_handler, dt, settings) + , q(model_handler.getReferenceState().head(model_handler.getModel().nq)) + , dq(Eigen::VectorXd::Zero(model_handler.getModel().nv)) + , ddq(Eigen::VectorXd::Zero(model_handler.getModel().nv)) + , tau(Eigen::VectorXd::Zero(model_handler.getModel().nv - 6)) + { + } + + void step() + { + // Solve + solver.solve(t, q, dq, tau); + solver.getAccelerations(ddq); + + // Integrate + step_i += 1; + t += dt; + q = pinocchio::integrate(model_handler.getModel(), q, (dq + ddq / 2. * dt) * dt); + dq += ddq * dt; + + // Update data handler + data_handler.updateInternalData(q, dq, true); + + // Check common to all tests + check_joint_limits(); + } + + bool is_error_decreasing(std::string name, double error) + { + if (errors.count(name) == 0) + { + errors.insert({name, error}); + return true; // no further check + } + const bool res{error <= errors.at(name)}; + errors.at(name) = error; // Update value + return res; + } + +protected: + void check_joint_limits() + { + const pinocchio::Model & model = model_handler.getModel(); + for (int i = 0; i < model.nv - 6; i++) + { + BOOST_CHECK_LE(q[7 + i], model.upperPositionLimit[7 + i]); + BOOST_CHECK_GE(q[7 + i], model.lowerPositionLimit[7 + i]); + BOOST_CHECK_LE(dq[6 + i], model.velocityLimit[6 + i]); + // Do not use lower velocity bound as TSID cannot handle it + BOOST_CHECK_GE(dq[6 + i], -model.velocityLimit[6 + i]); + BOOST_CHECK_LE(tau[i], model.effortLimit[6 + i]); + BOOST_CHECK_GE(tau[i], -model.effortLimit[6 + i]); + } + } + +public: + const RobotModelHandler model_handler; + RobotDataHandler data_handler; + KinodynamicsID::Settings settings; + double dt = 1e-3; + KinodynamicsID solver; + + double t = 0.; + int step_i = 0; + Eigen::VectorXd q; + Eigen::VectorXd dq; + Eigen::VectorXd ddq; + Eigen::VectorXd tau; + + std::map errors; +}; + +BOOST_AUTO_TEST_CASE(KinodynamicsID_postureTask) +{ + KinodynamicsID::Settings settings; + settings.kp_posture = 20.; + settings.w_posture = 1.; + + TestKinoID test(getSoloHandler(), settings); + + // Easy access + const RobotModelHandler & model_handler = test.model_handler; + const int nq = model_handler.getModel().nq; + const int nv = model_handler.getModel().nv; + + // Target state + const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq); + test.solver.setTarget( + q_target, Eigen::VectorXd::Zero(nv), Eigen::VectorXd::Zero(nv), {false, false, false, false}, {}); + + // Change initial state + test.q = solo_q_start(model_handler); + for (int i = 0; i < 1000; i++) + { + // Solve + test.step(); + + // compensate for free fall as we did not set any contact (we only care about joint posture) + test.q.head(7) = q_target.head(7); + test.dq.head(6).setZero(); + + // Check error is decreasing + Eigen::VectorXd delta_q = pinocchio::difference(model_handler.getModel(), test.q, q_target); + const double error = delta_q.tail(nv - 6).norm(); // Consider only the posture not the free flyer + BOOST_CHECK(test.is_error_decreasing("posture", error)); + } +} + +void test_contact(TestKinoID & test) +{ + // Easy access + const RobotModelHandler & model_handler = test.model_handler; + const RobotDataHandler & data_handler = test.data_handler; + const int nq = model_handler.getModel().nq; + + // No need to set target as KinodynamicsID sets it by default to reference state + const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq); + + // Let the robot stabilize + const int N_STEP = 500; + while (test.step_i < N_STEP) + { + // Solve + test.step(); + + // Check that contact velocity is null + for (size_t foot_nb = 0; foot_nb < model_handler.getFeetNb(); foot_nb++) + { + const pinocchio::Motion foot_vel = pinocchio::getFrameVelocity( + model_handler.getModel(), data_handler.getData(), model_handler.getFootFrameId(foot_nb), pinocchio::WORLD); + BOOST_CHECK_LE(foot_vel.linear().norm(), 1e-2); + if (model_handler.getFootType(foot_nb) == RobotModelHandler::FootType::QUAD) + { + // Rotation should also be null for quadrilateral contacts + BOOST_CHECK_LE(foot_vel.angular().norm(), 1e-1); + } + } + } +} + +BOOST_AUTO_TEST_CASE(KinodynamicsID_contactPoint_cost) +{ + KinodynamicsID::Settings settings; + settings.kp_base = 1.0; + settings.kp_contact = 10.0; + settings.w_base = 1.; + settings.w_contact_motion = 10.0; + settings.w_contact_force = 1.0; + + TestKinoID simu(getSoloHandler(), settings); + simu.q = solo_q_start(simu.model_handler); // Set initial configuration + test_contact(simu); +} + +BOOST_AUTO_TEST_CASE(KinodynamicsID_contactQuad_cost) +{ + KinodynamicsID::Settings settings; + settings.kp_base = 1.0; + settings.kp_posture = 1.; + settings.kp_contact = 10.0; + settings.w_base = 1.; + settings.w_posture = 0.05; + settings.w_contact_motion = 10.0; + settings.w_contact_force = 1.0; + + TestKinoID simu(getTalosModelHandler(), settings); + test_contact(simu); +} + +BOOST_AUTO_TEST_CASE(KinodynamicsID_contactPoint_equality) +{ + KinodynamicsID::Settings settings; + settings.kp_base = 1.0; + settings.kp_contact = 10.0; + settings.w_base = 1.; + settings.w_contact_motion = 10.0; + settings.w_contact_force = 1.0; + settings.contact_motion_equality = true; + + TestKinoID simu(getSoloHandler(), settings); + simu.q = solo_q_start(simu.model_handler); // Set initial configuration + test_contact(simu); +} + +BOOST_AUTO_TEST_CASE(KinodynamicsID_contactQuad_equality) +{ + KinodynamicsID::Settings settings; + settings.kp_base = 1.0; + settings.kp_posture = 1.; + settings.kp_contact = 10.0; + settings.w_base = 1.; + settings.w_posture = 0.05; + settings.w_contact_motion = 10.0; + settings.w_contact_force = 1.0; + settings.contact_motion_equality = true; + + TestKinoID simu(getTalosModelHandler(), settings); + test_contact(simu); +} + +BOOST_AUTO_TEST_CASE(KinodynamicsID_baseTask) +{ + KinodynamicsID::Settings settings; + settings.kp_base = 7.; + settings.kp_contact = .1; + settings.w_base = 100.0; + settings.w_contact_force = 1.0; + settings.w_contact_motion = 1.0; + + TestKinoID test(getSoloHandler(), settings); + + // Easy access + const RobotModelHandler & model_handler = test.model_handler; + const int nq = model_handler.getModel().nq; + + // No need to set target as KinodynamicsID sets it by default to reference state + const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq); + + // Change initial state + test.q = solo_q_start(model_handler); + + const int N_STEP = 10000; + for (int i = 0; i < N_STEP; i++) + { + // Solve + test.step(); + + // Compute error + const Eigen::VectorXd delta_pose = pinocchio::difference(model_handler.getModel(), test.q, q_target).head<6>(); + const double error = delta_pose.norm(); + + // Checks + if (error > 2e-2) // If haven't converged yet, should be strictly decreasing + BOOST_CHECK(test.is_error_decreasing("base", error)); + if (i > 9 * N_STEP / 10) // Should have converged by now + BOOST_CHECK(error < 2e-2); + } +} + +BOOST_AUTO_TEST_CASE(KinodynamicsID_allTasks) +{ + KinodynamicsID::Settings settings; + settings.kp_base = 10.; + settings.kp_posture = 1.; + settings.kp_contact = 10.; + settings.w_base = 10.0; + settings.w_posture = 0.1; + settings.w_contact_force = 1.0; + settings.w_contact_motion = 1.0; + + TestKinoID test(getSoloHandler(), settings); + + // Easy access + const RobotModelHandler & model_handler = test.model_handler; + const int nq = model_handler.getModel().nq; + + // No need to set target as KinodynamicsID sets it by default to reference state + const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq); + + test.q = solo_q_start(model_handler); + const int N_STEP = 1000; + for (int i = 0; i < N_STEP; i++) + { + // Solve + test.step(); + + // Check error is decreasing + const Eigen::VectorXd delta_q = pinocchio::difference(model_handler.getModel(), test.q, q_target); + const double error = delta_q.norm(); + + BOOST_CHECK(test.is_error_decreasing("q", error)); + } +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/tests/mpc.cpp b/tests/mpc.cpp index e021c009..a0a03be3 100644 --- a/tests/mpc.cpp +++ b/tests/mpc.cpp @@ -47,29 +47,29 @@ BOOST_AUTO_TEST_CASE(mpc_fulldynamics) for (std::size_t i = 0; i < 10; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), true}); - contact_state.insert({model_handler.getFootName(1), true}); + contact_state.insert({model_handler.getFootFrameName(0), true}); + contact_state.insert({model_handler.getFootFrameName(1), true}); contact_states.push_back(contact_state); } for (std::size_t i = 0; i < 50; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), true}); - contact_state.insert({model_handler.getFootName(1), false}); + contact_state.insert({model_handler.getFootFrameName(0), true}); + contact_state.insert({model_handler.getFootFrameName(1), false}); contact_states.push_back(contact_state); } for (std::size_t i = 0; i < 10; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), true}); - contact_state.insert({model_handler.getFootName(1), true}); + contact_state.insert({model_handler.getFootFrameName(0), true}); + contact_state.insert({model_handler.getFootFrameName(1), true}); contact_states.push_back(contact_state); } for (std::size_t i = 0; i < 50; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), false}); - contact_state.insert({model_handler.getFootName(1), true}); + contact_state.insert({model_handler.getFootFrameName(0), false}); + contact_state.insert({model_handler.getFootFrameName(1), true}); contact_states.push_back(contact_state); } @@ -91,7 +91,7 @@ BOOST_AUTO_TEST_CASE(mpc_fulldynamics) Eigen::VectorXd xdot = mpc.getStateDerivative(0); - Eigen::VectorXd forces = mpc.getContactForces(0); + Eigen::Matrix forces = mpc.getContactForces(0); } BOOST_AUTO_TEST_CASE(mpc_kinodynamics) @@ -133,29 +133,29 @@ BOOST_AUTO_TEST_CASE(mpc_kinodynamics) for (std::size_t i = 0; i < 10; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), true}); - contact_state.insert({model_handler.getFootName(1), true}); + contact_state.insert({model_handler.getFootFrameName(0), true}); + contact_state.insert({model_handler.getFootFrameName(1), true}); contact_states.push_back(contact_state); } for (std::size_t i = 0; i < 50; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), true}); - contact_state.insert({model_handler.getFootName(1), false}); + contact_state.insert({model_handler.getFootFrameName(0), true}); + contact_state.insert({model_handler.getFootFrameName(1), false}); contact_states.push_back(contact_state); } for (std::size_t i = 0; i < 10; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), true}); - contact_state.insert({model_handler.getFootName(1), true}); + contact_state.insert({model_handler.getFootFrameName(0), true}); + contact_state.insert({model_handler.getFootFrameName(1), true}); contact_states.push_back(contact_state); } for (std::size_t i = 0; i < 50; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), false}); - contact_state.insert({model_handler.getFootName(1), true}); + contact_state.insert({model_handler.getFootFrameName(0), false}); + contact_state.insert({model_handler.getFootFrameName(1), true}); contact_states.push_back(contact_state); } @@ -210,29 +210,29 @@ BOOST_AUTO_TEST_CASE(mpc_centroidal) for (std::size_t i = 0; i < 10; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), true}); - contact_state.insert({model_handler.getFootName(1), true}); + contact_state.insert({model_handler.getFootFrameName(0), true}); + contact_state.insert({model_handler.getFootFrameName(1), true}); contact_states.push_back(contact_state); } for (std::size_t i = 0; i < 50; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), true}); - contact_state.insert({model_handler.getFootName(1), false}); + contact_state.insert({model_handler.getFootFrameName(0), true}); + contact_state.insert({model_handler.getFootFrameName(1), false}); contact_states.push_back(contact_state); } for (std::size_t i = 0; i < 10; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), true}); - contact_state.insert({model_handler.getFootName(1), true}); + contact_state.insert({model_handler.getFootFrameName(0), true}); + contact_state.insert({model_handler.getFootFrameName(1), true}); contact_states.push_back(contact_state); } for (std::size_t i = 0; i < 50; i++) { std::map contact_state; - contact_state.insert({model_handler.getFootName(0), false}); - contact_state.insert({model_handler.getFootName(1), true}); + contact_state.insert({model_handler.getFootFrameName(0), false}); + contact_state.insert({model_handler.getFootFrameName(1), true}); contact_states.push_back(contact_state); } diff --git a/tests/problem.cpp b/tests/problem.cpp index 6cc4cb53..01ab5e76 100644 --- a/tests/problem.cpp +++ b/tests/problem.cpp @@ -27,8 +27,8 @@ BOOST_AUTO_TEST_CASE(fulldynamics) land_constraint.insert({contact_names[0], true}); land_constraint.insert({contact_names[1], false}); - pinocchio::SE3 p1 = data_handler.getFootPose(contact_names[0]); - pinocchio::SE3 p2 = data_handler.getFootPose(contact_names[1]); + pinocchio::SE3 p1 = data_handler.getFootPose(0); + pinocchio::SE3 p2 = data_handler.getFootPose(1); p1.translation() << 0, 0.1, 0; p2.translation() << 0, -0.1, 0; contact_poses.insert({contact_names[0], p1}); @@ -118,8 +118,8 @@ BOOST_AUTO_TEST_CASE(kinodynamics) land_constraint.insert({contact_names[0], true}); land_constraint.insert({contact_names[1], false}); - pinocchio::SE3 p1 = data_handler.getFootPose(contact_names[0]); - pinocchio::SE3 p2 = data_handler.getFootPose(contact_names[1]); + pinocchio::SE3 p1 = data_handler.getFootPose(0); + pinocchio::SE3 p2 = data_handler.getFootPose(1); p1.translation() << 0, 0.1, 0; p2.translation() << 0, -0.1, 0; contact_poses.insert({contact_names[0], p1}); @@ -213,8 +213,8 @@ BOOST_AUTO_TEST_CASE(centroidal) land_constraint.insert({contact_names[0], true}); land_constraint.insert({contact_names[1], false}); - pinocchio::SE3 p1 = data_handler.getFootPose(contact_names[0]); - pinocchio::SE3 p2 = data_handler.getFootPose(contact_names[1]); + pinocchio::SE3 p1 = data_handler.getFootPose(0); + pinocchio::SE3 p2 = data_handler.getFootPose(1); p1.translation() << 0, 0.1, 0; p2.translation() << 0, -0.1, 0; contact_poses.insert({contact_names[0], p1}); @@ -303,10 +303,10 @@ BOOST_AUTO_TEST_CASE(centroidal_solo) contact_states.insert({contact_names[1], true}); contact_states.insert({contact_names[2], true}); contact_states.insert({contact_names[3], false}); - pinocchio::SE3 p1 = data_handler.getFootPose("FR_FOOT"); - pinocchio::SE3 p2 = data_handler.getFootPose("FL_FOOT"); - pinocchio::SE3 p3 = data_handler.getFootPose("HR_FOOT"); - pinocchio::SE3 p4 = data_handler.getFootPose("HL_FOOT"); + pinocchio::SE3 p1 = data_handler.getFootPose(model_handler.getFootNb("FR_FOOT")); + pinocchio::SE3 p2 = data_handler.getFootPose(model_handler.getFootNb("FL_FOOT")); + pinocchio::SE3 p3 = data_handler.getFootPose(model_handler.getFootNb("HR_FOOT")); + pinocchio::SE3 p4 = data_handler.getFootPose(model_handler.getFootNb("HL_FOOT")); contact_poses.insert({contact_names[0], p1}); contact_poses.insert({contact_names[1], p2}); diff --git a/tests/qpsolvers.cpp b/tests/qpsolvers.cpp deleted file mode 100644 index 5bf5b634..00000000 --- a/tests/qpsolvers.cpp +++ /dev/null @@ -1,119 +0,0 @@ - -#include -#include - -#include "simple-mpc/qp-solvers.hpp" -#include "simple-mpc/robot-handler.hpp" -#include "test_utils.cpp" - -BOOST_AUTO_TEST_SUITE(qpsolvers) - -using namespace simple_mpc; - -BOOST_AUTO_TEST_CASE(ID_solver) -{ - RobotModelHandler model_handler = getTalosModelHandler(); - RobotDataHandler data_handler(model_handler); - - IDSettings settings; - settings.contact_ids = model_handler.getFeetIds(); - settings.mu = 0.8; - settings.Lfoot = 0.1; - settings.Wfoot = 0.075; - settings.force_size = 6; - settings.kd = 10; - settings.w_force = 1000; - settings.w_acc = 1; - settings.w_tau = 0; - settings.verbose = false; - - IDSolver ID_solver(settings, model_handler.getModel()); - - std::vector contact_states; - contact_states.push_back(true); - contact_states.push_back(true); - - Eigen::VectorXd v = Eigen::VectorXd::Random(model_handler.getModel().nv); - Eigen::VectorXd a = Eigen::VectorXd::Random(model_handler.getModel().nv); - Eigen::VectorXd tau = Eigen::VectorXd::Zero(model_handler.getModel().nv - 6); - Eigen::VectorXd forces = Eigen::VectorXd::Random(6 * 2); - - Eigen::MatrixXd M = data_handler.getData().M; - pinocchio::Data rdata = data_handler.getData(); - ID_solver.solveQP(rdata, contact_states, v, a, tau, forces, M); -} - -BOOST_AUTO_TEST_CASE(IKID_solver) -{ - RobotModelHandler model_handler = getTalosModelHandler(); - RobotDataHandler data_handler(model_handler); - - std::vector vec_base; - std::vector Kp; - std::vector Kd; - - Eigen::VectorXd g_q(model_handler.getModel().nv); - g_q << 0, 0, 0, 100, 100, 100, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 10, 10, 100, 100, 100, 100, 100, 100, 100, 100; - - double g_p = 400; - double g_b = 10; - - Kp.push_back(g_q); - Kp.push_back(Eigen::VectorXd::Constant(6, g_p)); - Kp.push_back(Eigen::VectorXd::Constant(3, g_b)); - - Kd.push_back(2 * g_q.array().sqrt()); - Kd.push_back(Eigen::VectorXd::Constant(6, 2 * sqrt(g_p))); - Kd.push_back(Eigen::VectorXd::Constant(3, 2 * sqrt(g_b))); - - vec_base.push_back(model_handler.getBaseFrameId()); - IKIDSettings settings; - settings.contact_ids = model_handler.getFeetIds(); - settings.fixed_frame_ids = vec_base; - settings.x0 = model_handler.getReferenceState(); - settings.Kp_gains = Kp, settings.Kd_gains = Kd, settings.dt = 0.01, settings.mu = 0.8; - settings.Lfoot = 0.1; - settings.Wfoot = 0.075; - settings.force_size = 6; - settings.w_qref = 500; - settings.w_footpose = 50000; - settings.w_centroidal = 10; - settings.w_centroidal = 10; - settings.w_baserot = 1000; - settings.w_force = 100; - settings.verbose = false; - - IKIDSolver IKID_solver(settings, model_handler.getModel()); - - std::vector contact_states; - contact_states.push_back(true); - contact_states.push_back(true); - - std::vector foot_refs; - std::vector foot_refs_next; - for (auto const & name : model_handler.getFeetNames()) - { - pinocchio::SE3 foot_ref = data_handler.getFootPose(name); - foot_refs.push_back(foot_ref); - foot_ref.translation()[0] += 0.1; - - foot_refs_next.push_back(foot_ref); - } - - Eigen::VectorXd dq = Eigen::VectorXd::Random(model_handler.getModel().nv); - Eigen::VectorXd dv = Eigen::VectorXd::Random(model_handler.getModel().nv); - Eigen::VectorXd dH = Eigen::VectorXd::Random(6); - Eigen::VectorXd x0 = model_handler.getReferenceState(); - Eigen::VectorXd xm = model_handler.getReferenceState(); - Eigen::VectorXd new_q = pinocchio::integrate(model_handler.getModel(), x0.head(model_handler.getModel().nq), dq); - Eigen::VectorXd forces = Eigen::VectorXd::Random(6 * 2); - xm << new_q, dv; - - Eigen::MatrixXd M = data_handler.getData().M; - pinocchio::Data rdata = data_handler.getData(); - - IKID_solver.computeDifferences(rdata, xm, foot_refs, foot_refs_next); - IKID_solver.solve_qp(rdata, contact_states, dv, forces, dH, M); -} - -BOOST_AUTO_TEST_SUITE_END() diff --git a/tests/robot_handler.cpp b/tests/robot_handler.cpp index 49191566..b9bb3b65 100644 --- a/tests/robot_handler.cpp +++ b/tests/robot_handler.cpp @@ -46,10 +46,10 @@ BOOST_AUTO_TEST_CASE(model_handler) RobotModelHandler model_handler(model, default_conf_name, base_frame); // Add feet - for (size_t i = 0; i < feet_names.size(); i++) + for (size_t foot_nb = 0; foot_nb < feet_names.size(); foot_nb++) { - model_handler.addFoot(feet_names.at(i), base_frame); - model_handler.setFootReferencePlacement(feet_names.at(i), feet_refs.at(i)); + model_handler.addPointFoot(feet_names.at(foot_nb), base_frame); + model_handler.setFootReferencePlacement(foot_nb, feet_refs.at(foot_nb)); } /*********/ @@ -71,12 +71,12 @@ BOOST_AUTO_TEST_CASE(model_handler) for (size_t i = 0; i < feet_names.size(); i++) { const std::string foot_name = feet_names.at(i); - BOOST_CHECK_EQUAL(foot_name, model_handler.getFootName(i)); - BOOST_CHECK_EQUAL(foot_name, model_handler.getFeetNames().at(i)); - BOOST_CHECK_EQUAL(foot_name, model.frames.at(model_handler.getFeetIds().at(i)).name); - BOOST_CHECK_EQUAL(foot_name, model.frames.at(model_handler.getFootId(foot_name)).name); + BOOST_CHECK_EQUAL(foot_name, model_handler.getFootFrameName(i)); + BOOST_CHECK_EQUAL(foot_name, model_handler.getFeetFrameNames().at(i)); + BOOST_CHECK_EQUAL(foot_name, model.frames.at(model_handler.getFeetFrameIds().at(i)).name); + BOOST_CHECK_EQUAL(foot_name, model.frames.at(model_handler.getFootFrameId(i)).name); - const FrameIndex ref_frame = model_handler.getRefFootId(foot_name); + const FrameIndex ref_frame = model_handler.getFootRefFrameId(i); const FrameIndex ref_frame_parent = model_handler.getModel().frames.at(ref_frame).parentFrame; BOOST_CHECK_EQUAL(model_handler.getModel().frames.at(ref_frame_parent).name, base_frame); BOOST_CHECK(model_handler.getModel().frames.at(ref_frame).placement.isApprox(feet_refs.at(i))); @@ -154,7 +154,7 @@ BOOST_AUTO_TEST_CASE(model_handler) // Check that the default foot pose actually match the reference configuration { const std::string foot_name = "FR_calf"; - model_handler.addFoot(foot_name, base_frame); + const size_t foot_nb = model_handler.addPointFoot(foot_name, base_frame); // Do not set foot reference placement to check the default one RobotDataHandler data_handler(model_handler); @@ -162,7 +162,7 @@ BOOST_AUTO_TEST_CASE(model_handler) data_handler.updateInternalData(model_handler.getReferenceState(), false); // Foot and ref should be coincident with reference configuration - data_handler.getFootPose(foot_name).isApprox(data_handler.getRefFootPose(foot_name)); + data_handler.getFootPose(foot_nb).isApprox(data_handler.getFootRefPose(foot_nb)); } } @@ -213,9 +213,10 @@ BOOST_AUTO_TEST_CASE(data_handler) const std::string ref_foot_name = foot_name + "_ref"; const FrameIndex foot_id = model.getFrameId(foot_name); const FrameIndex ref_foot_id = model.getFrameId(ref_foot_name); + const size_t foot_nb = model_handler.getFootNb(foot_name); - BOOST_CHECK(data.oMf[foot_id].isApprox(data_handler.getFootPose(foot_name))); - BOOST_CHECK(data.oMf[ref_foot_id].isApprox(data_handler.getRefFootPose(foot_name))); + BOOST_CHECK(data.oMf[foot_id].isApprox(data_handler.getFootPose(foot_nb))); + BOOST_CHECK(data.oMf[ref_foot_id].isApprox(data_handler.getFootRefPose(foot_nb))); } } diff --git a/tests/test_utils.cpp b/tests/test_utils.cpp index cbf39d35..880cf323 100644 --- a/tests/test_utils.cpp +++ b/tests/test_utils.cpp @@ -66,12 +66,9 @@ RobotModelHandler getTalosModelHandler() RobotModelHandler model_handler(model, "half_sitting", base_joint); // Add feet - model_handler.addFoot("left_sole_link", base_joint); - model_handler.addFoot("right_sole_link", base_joint); - model_handler.setFootReferencePlacement( - "left_sole_link", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.))); - model_handler.setFootReferencePlacement( - "right_sole_link", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., -0.1, 0.))); + Eigen::Matrix foot_quad{{0.1, 0.075, 0}, {-0.1, 0.075, 0}, {-0.1, -0.075, 0}, {0.1, -0.075, 0}}; + model_handler.addQuadFoot("left_sole_link", base_joint, foot_quad); + model_handler.addQuadFoot("right_sole_link", base_joint, foot_quad); return model_handler; } @@ -91,18 +88,10 @@ RobotModelHandler getSoloHandler() RobotModelHandler model_handler(model, "straight_standing", base_joint); // Add feet - model_handler.addFoot("FR_FOOT", base_joint); - model_handler.addFoot("FL_FOOT", base_joint); - model_handler.addFoot("HR_FOOT", base_joint); - model_handler.addFoot("HL_FOOT", base_joint); - model_handler.setFootReferencePlacement( - "FR_FOOT", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0.1, -0.1, 0.))); - model_handler.setFootReferencePlacement( - "FL_FOOT", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0.1, 0.1, 0.))); - model_handler.setFootReferencePlacement( - "HR_FOOT", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(-0.1, -0.1, 0.))); - model_handler.setFootReferencePlacement( - "HL_FOOT", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(-0.1, 0.1, 0.))); + model_handler.addPointFoot("FR_FOOT", base_joint); + model_handler.addPointFoot("FL_FOOT", base_joint); + model_handler.addPointFoot("HR_FOOT", base_joint); + model_handler.addPointFoot("HL_FOOT", base_joint); return model_handler; }