From e48f4a41d0d7553f902443fc40aa53c6d2d2f952 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Wed, 18 Dec 2024 15:36:04 +0100 Subject: [PATCH 1/5] MPC : remove default ctor --- bindings/expose-mpc.cpp | 7 +++---- include/simple-mpc/mpc.hpp | 4 +--- src/mpc.cpp | 14 +++----------- 3 files changed, 7 insertions(+), 18 deletions(-) diff --git a/bindings/expose-mpc.cpp b/bindings/expose-mpc.cpp index c372e072..2a09032e 100644 --- a/bindings/expose-mpc.cpp +++ b/bindings/expose-mpc.cpp @@ -24,7 +24,7 @@ namespace simple_mpc namespace bp = boost::python; using eigenpy::StdVectorPythonVisitor; - void initialize(MPC & self, const bp::dict & settings, std::shared_ptr problem) + MPC * createMPC(const bp::dict & settings, std::shared_ptr problem) { MPCSettings conf; @@ -42,7 +42,7 @@ namespace simple_mpc conf.T_contact = bp::extract(settings["T_contact"]); conf.timestep = bp::extract(settings["timestep"]); - self.initialize(conf, problem); + return new MPC{conf, problem}; } bp::dict getSettings(MPC & self) @@ -73,8 +73,7 @@ namespace simple_mpc StdVectorPythonVisitor, true>::expose("StdVec_MapBool"); bp::class_("MPC", bp::no_init) - .def(bp::init<>(bp::args("self"))) - .def("initialize", &initialize) + .def("__init__", bp::make_constructor(&createMPC, bp::default_call_policies())) .def("getSettings", &getSettings) .def("generateCycleHorizon", &MPC::generateCycleHorizon, bp::args("self", "contact_states")) .def("iterate", &MPC::iterate, bp::args("self", "x")) diff --git a/include/simple-mpc/mpc.hpp b/include/simple-mpc/mpc.hpp index ddeb4cc2..555bf0b4 100644 --- a/include/simple-mpc/mpc.hpp +++ b/include/simple-mpc/mpc.hpp @@ -87,16 +87,14 @@ namespace simple_mpc MPCSettings settings_; std::shared_ptr ocp_handler_; - explicit MPC(); explicit MPC(const MPCSettings & settings, std::shared_ptr problem); - void initialize(const MPCSettings & settings, std::shared_ptr problem); // Generate the cycle walking problem along which we will iterate // the receding horizon void generateCycleHorizon(const std::vector> & contact_states); // Perform one iteration of MPC - void iterate(const Eigen::VectorXd & x); + void iterate(const ConstVectorRef & x); void updateCycleTiming(const bool updateOnlyHorizon); diff --git a/src/mpc.cpp b/src/mpc.cpp index 8762a75b..e094b71b 100644 --- a/src/mpc.cpp +++ b/src/mpc.cpp @@ -16,19 +16,11 @@ namespace simple_mpc using namespace aligator; constexpr std::size_t maxiters = 100; - MPC::MPC() - { - } - MPC::MPC(const MPCSettings & settings, std::shared_ptr problem) + : settings_(settings) + , ocp_handler_(problem) { - initialize(settings, problem); - } - void MPC::initialize(const MPCSettings & settings, std::shared_ptr problem) - { - settings_ = settings; - ocp_handler_ = problem; data_handler_ = std::make_shared(ocp_handler_->getModelHandler()); data_handler_->updateInternalData(ocp_handler_->getModelHandler().getReferenceState(), true); std::map starting_poses; @@ -189,7 +181,7 @@ namespace simple_mpc } } - void MPC::iterate(const Eigen::VectorXd & x) + void MPC::iterate(const ConstVectorRef & x) { data_handler_->updateInternalData(x, false); From 7b9e7bc02f8177baccbdfb5202c56b0d30c52882 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Wed, 18 Dec 2024 15:44:33 +0100 Subject: [PATCH 2/5] MPC : solver_ is now a unique_ptr --- bindings/expose-mpc.cpp | 9 +++------ include/simple-mpc/mpc.hpp | 18 ++++++------------ src/mpc.cpp | 2 +- 3 files changed, 10 insertions(+), 19 deletions(-) diff --git a/bindings/expose-mpc.cpp b/bindings/expose-mpc.cpp index 2a09032e..d0b9ac46 100644 --- a/bindings/expose-mpc.cpp +++ b/bindings/expose-mpc.cpp @@ -11,6 +11,7 @@ #include #include +#include #include #include "simple-mpc/mpc.hpp" @@ -72,7 +73,7 @@ namespace simple_mpc StdVectorPythonVisitor, true>::expose("StdVec_MapBool"); - bp::class_("MPC", bp::no_init) + bp::class_("MPC", bp::no_init) .def("__init__", bp::make_constructor(&createMPC, bp::default_call_policies())) .def("getSettings", &getSettings) .def("generateCycleHorizon", &MPC::generateCycleHorizon, bp::args("self", "contact_states")) @@ -102,11 +103,7 @@ namespace simple_mpc .def( "getCycleHorizon", &MPC::getCycleHorizon, "self"_a, bp::return_internal_reference<>(), "Get the cycle horizon.") - .def( - "getSolver", &MPC::getSolver, "self"_a, - eigenpy::deprecated_member>(), - "Get the SolverProxDDP object") - .def_readonly("solver", &MPC::solver_) + .add_property("solver", bp::make_getter(&MPC::solver_, eigenpy::ReturnInternalStdUniquePtr{})) .add_property("xs", &MPC::xs_) .add_property("us", &MPC::us_) .add_property("Ks", &MPC::Ks_); diff --git a/include/simple-mpc/mpc.hpp b/include/simple-mpc/mpc.hpp index 555bf0b4..eca56af7 100644 --- a/include/simple-mpc/mpc.hpp +++ b/include/simple-mpc/mpc.hpp @@ -79,7 +79,7 @@ namespace simple_mpc std::shared_ptr data_handler_; public: - std::shared_ptr solver_; + std::unique_ptr solver_; Vector6d velocity_base_; Vector7d pose_base_; Eigen::Vector3d next_pose_; @@ -129,11 +129,6 @@ namespace simple_mpc // getters and setters TrajOptProblem & getTrajOptProblem(); - SIMPLE_MPC_DEPRECATED_MESSAGE("The MPC::solver_ member is now public.") - SolverProxDDP & getSolver() - { - return *solver_; - } const RobotDataHandler & getDataHandler() const { return *data_handler_; @@ -184,15 +179,14 @@ namespace simple_mpc std::map> foot_takeoff_times_, foot_land_times_; // Solution vectors for state and control - std::vector xs_; - std::vector us_; - + std::vector xs_; + std::vector us_; // Riccati gains - std::vector Ks_; + std::vector Ks_; // Initial quantities - Eigen::VectorXd x0_; - Eigen::VectorXd u0_; + VectorXd x0_; + VectorXd u0_; }; } // namespace simple_mpc diff --git a/src/mpc.cpp b/src/mpc.cpp index e094b71b..74d36af0 100644 --- a/src/mpc.cpp +++ b/src/mpc.cpp @@ -37,7 +37,7 @@ namespace simple_mpc foot_trajectories_.updateApex(settings.swing_apex); x0_ = ocp_handler_->getProblemState(*data_handler_); - solver_ = std::make_shared(settings_.TOL, settings_.mu_init, maxiters, aligator::QUIET); + solver_ = std::make_unique(settings_.TOL, settings_.mu_init, maxiters, aligator::QUIET); solver_->rollout_type_ = aligator::RolloutType::LINEAR; if (settings_.num_threads > 1) From f588bf7fcdd3e10efb61ce3778093f3e8f16b99a Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Wed, 18 Dec 2024 15:48:14 +0100 Subject: [PATCH 3/5] Change all instances of taking argument "const Eigen::VectorXd &" to "const ConstVectorRef&" --- include/simple-mpc/centroidal-dynamics.hpp | 7 +++---- include/simple-mpc/fulldynamics.hpp | 7 +++---- include/simple-mpc/kinodynamics.hpp | 7 +++---- include/simple-mpc/ocp-handler.hpp | 10 +++++----- include/simple-mpc/python/py-ocp-handler.hpp | 8 ++++---- include/simple-mpc/robot-handler.hpp | 10 +++++----- src/centroidal-dynamics.cpp | 6 +++--- src/fulldynamics.cpp | 8 ++++---- src/kinodynamics.cpp | 8 ++++---- src/ocp-handler.cpp | 4 ++-- src/robot-handler.cpp | 8 ++++---- 11 files changed, 40 insertions(+), 43 deletions(-) diff --git a/include/simple-mpc/centroidal-dynamics.hpp b/include/simple-mpc/centroidal-dynamics.hpp index 8d625385..c08d4f14 100644 --- a/include/simple-mpc/centroidal-dynamics.hpp +++ b/include/simple-mpc/centroidal-dynamics.hpp @@ -87,14 +87,13 @@ namespace simple_mpc // Getters and setters for contact forces void setReferenceForces(const std::size_t t, const std::map & force_refs) override; - void - setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref) override; + void setReferenceForce(const std::size_t t, const std::string & ee_name, const ConstVectorRef & force_ref) override; const Eigen::VectorXd getReferenceForce(const std::size_t t, const std::string & ee_name) override; void computeControlFromForces(const std::map & force_refs); const Eigen::VectorXd getVelocityBase(const std::size_t t) override; - void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override; + void setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) override; const Eigen::VectorXd getPoseBase(const std::size_t t) override; - void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override; + void setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) override; const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override; size_t getContactSupport(const std::size_t t) override; diff --git a/include/simple-mpc/fulldynamics.hpp b/include/simple-mpc/fulldynamics.hpp index 5e0f4a87..c885f5cf 100644 --- a/include/simple-mpc/fulldynamics.hpp +++ b/include/simple-mpc/fulldynamics.hpp @@ -92,14 +92,13 @@ namespace simple_mpc void setReferencePoses(const std::size_t t, const std::map & pose_refs) override; void setTerminalReferencePose(const std::string & ee_name, const pinocchio::SE3 & pose_ref) override; void setReferenceForces(const std::size_t t, const std::map & force_refs) override; - void - setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref) override; + void setReferenceForce(const std::size_t t, const std::string & ee_name, const ConstVectorRef & force_ref) override; const pinocchio::SE3 getReferencePose(const std::size_t t, const std::string & cost_name) override; const Eigen::VectorXd getReferenceForce(const std::size_t t, const std::string & cost_name) override; const Eigen::VectorXd getVelocityBase(const std::size_t t) override; - void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override; + void setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) override; const Eigen::VectorXd getPoseBase(const std::size_t t) override; - void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override; + void setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) override; const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override; size_t getContactSupport(const std::size_t t) override; FullDynamicsSettings getSettings() diff --git a/include/simple-mpc/kinodynamics.hpp b/include/simple-mpc/kinodynamics.hpp index dd25401a..787122b6 100644 --- a/include/simple-mpc/kinodynamics.hpp +++ b/include/simple-mpc/kinodynamics.hpp @@ -80,14 +80,13 @@ namespace simple_mpc void setReferencePoses(const std::size_t i, const std::map & pose_refs) override; void setTerminalReferencePose(const std::string & ee_name, const pinocchio::SE3 & pose_ref) override; void setReferenceForces(const std::size_t i, const std::map & force_refs) override; - void - setReferenceForce(const std::size_t i, const std::string & ee_name, const Eigen::VectorXd & force_ref) override; + void setReferenceForce(const std::size_t i, const std::string & ee_name, const ConstVectorRef & force_ref) override; const Eigen::VectorXd getReferenceForce(const std::size_t i, const std::string & cost_name) override; const pinocchio::SE3 getReferencePose(const std::size_t i, const std::string & cost_name) override; const Eigen::VectorXd getVelocityBase(const std::size_t t) override; const Eigen::VectorXd getPoseBase(const std::size_t t) override; - void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override; - void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override; + void setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) override; + void setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) override; const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override; size_t getContactSupport(const std::size_t t) override; diff --git a/include/simple-mpc/ocp-handler.hpp b/include/simple-mpc/ocp-handler.hpp index ef2f7860..c84d9a2d 100644 --- a/include/simple-mpc/ocp-handler.hpp +++ b/include/simple-mpc/ocp-handler.hpp @@ -82,16 +82,16 @@ namespace simple_mpc // Setter and getter for base velocity virtual const Eigen::VectorXd getVelocityBase(const std::size_t t) = 0; - virtual void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) = 0; + virtual void setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) = 0; // Setter and getter for base pose virtual const Eigen::VectorXd getPoseBase(const std::size_t t) = 0; - virtual void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) = 0; + virtual void setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) = 0; // Setter and getter for forces reference virtual void setReferenceForces(const std::size_t t, const std::map & force_refs) = 0; virtual void - setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref) = 0; + setReferenceForce(const std::size_t t, const std::string & ee_name, const ConstVectorRef & force_ref) = 0; virtual const Eigen::VectorXd getReferenceForce(const std::size_t t, const std::string & ee_name) = 0; virtual const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) = 0; virtual size_t getContactSupport(const std::size_t t) = 0; @@ -100,14 +100,14 @@ namespace simple_mpc // Create one TrajOptProblem from contact sequence void createProblem( - const Eigen::VectorXd & x0, + const ConstVectorRef & x0, const size_t horizon, const int force_size, const double gravity, const bool terminal_constraint); // Setter and getter for control reference - void setReferenceControl(const std::size_t t, const Eigen::VectorXd & u_ref); + void setReferenceControl(const std::size_t t, const ConstVectorRef & u_ref); ConstVectorRef getReferenceControl(const std::size_t t); // Getter for various objects and quantities diff --git a/include/simple-mpc/python/py-ocp-handler.hpp b/include/simple-mpc/python/py-ocp-handler.hpp index 1a04acdb..422667f0 100644 --- a/include/simple-mpc/python/py-ocp-handler.hpp +++ b/include/simple-mpc/python/py-ocp-handler.hpp @@ -125,7 +125,7 @@ namespace simple_mpc } void - setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref) override + setReferenceForce(const std::size_t t, const std::string & ee_name, const ConstVectorRef & force_ref) override { SIMPLE_MPC_PYTHON_OVERRIDE_PURE(void, "setReferenceForce", t, ee_name, force_ref); } @@ -135,7 +135,7 @@ namespace simple_mpc SIMPLE_MPC_PYTHON_OVERRIDE_PURE(Eigen::VectorXd, "getReferenceForce", t, ee_name); } - void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override + void setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) override { SIMPLE_MPC_PYTHON_OVERRIDE_PURE(void, "setVelocityBase", t, velocity_base); } @@ -145,7 +145,7 @@ namespace simple_mpc SIMPLE_MPC_PYTHON_OVERRIDE_PURE(Eigen::VectorXd, "getVelocityBase", t); } - void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override + void setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) override { SIMPLE_MPC_PYTHON_OVERRIDE_PURE(void, "setPoseBase", t, pose_base); } @@ -165,7 +165,7 @@ namespace simple_mpc SIMPLE_MPC_PYTHON_OVERRIDE_PURE(std::size_t, "getContactSupport", t); } - void setReferenceControl(const std::size_t t, const Eigen::VectorXd & u_ref) + void setReferenceControl(const std::size_t t, const ConstVectorRef & u_ref) { SIMPLE_MPC_PYTHON_OVERRIDE(void, OCPHandler, setReferenceControl, t, u_ref); } diff --git a/include/simple-mpc/robot-handler.hpp b/include/simple-mpc/robot-handler.hpp index fd8be9af..5da9afd8 100644 --- a/include/simple-mpc/robot-handler.hpp +++ b/include/simple-mpc/robot-handler.hpp @@ -111,7 +111,7 @@ namespace simple_mpc * @return Eigen::VectorXd The vector that must be integrated during a unit of * time to go from x1 to x2. */ - Eigen::VectorXd difference(const Eigen::VectorXd & x1, const Eigen::VectorXd & x2) const; + Eigen::VectorXd difference(const ConstVectorRef & x1, const ConstVectorRef & x2) const; /** * @brief Compute reduced state from measures by concatenating q,v of the @@ -121,10 +121,10 @@ namespace simple_mpc * @param v Velocity vector of the full model * @return const Eigen::VectorXd State vector of the reduced model. */ - Eigen::VectorXd shapeState(const Eigen::VectorXd & q, const Eigen::VectorXd & v) const; + Eigen::VectorXd shapeState(const ConstVectorRef & q, const ConstVectorRef & v) const; // Const getters - const Eigen::VectorXd & getReferenceState() const + const ConstVectorRef & getReferenceState() const { return reference_state_; } @@ -195,8 +195,8 @@ namespace simple_mpc RobotDataHandler(const RobotModelHandler & model_handler); // Set new robot state - void updateInternalData(const Eigen::VectorXd & x, const bool updateJacobians); - void updateJacobiansMassMatrix(const Eigen::VectorXd & x); + void updateInternalData(const ConstVectorRef & x, const bool updateJacobians); + void updateJacobiansMassMatrix(const ConstVectorRef & x); // Const getters const SE3 & getRefFootPose(const std::string & foot_name) const diff --git a/src/centroidal-dynamics.cpp b/src/centroidal-dynamics.cpp index 011945ae..bf671e79 100644 --- a/src/centroidal-dynamics.cpp +++ b/src/centroidal-dynamics.cpp @@ -189,7 +189,7 @@ namespace simple_mpc } void - CentroidalOCP::setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref) + CentroidalOCP::setReferenceForce(const std::size_t t, const std::string & ee_name, const ConstVectorRef & force_ref) { std::vector hname = model_handler_.getFeetNames(); std::vector::iterator it = std::find(hname.begin(), hname.end(), ee_name); @@ -223,7 +223,7 @@ namespace simple_mpc return v; } - void CentroidalOCP::setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) + void CentroidalOCP::setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) { CostStack * cs = getCostStack(t); QuadraticResidualCost * qcm = cs->getComponent("linear_mom_cost"); @@ -244,7 +244,7 @@ namespace simple_mpc return cfr->getReference(); } - void CentroidalOCP::setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) + void CentroidalOCP::setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) { if (pose_base.size() != 7) { diff --git a/src/fulldynamics.cpp b/src/fulldynamics.cpp index 45a41f13..2cef30ee 100644 --- a/src/fulldynamics.cpp +++ b/src/fulldynamics.cpp @@ -287,8 +287,8 @@ namespace simple_mpc } } - void FullDynamicsOCP::setReferenceForce( - const std::size_t i, const std::string & ee_name, const Eigen::VectorXd & force_ref) + void + FullDynamicsOCP::setReferenceForce(const std::size_t i, const std::string & ee_name, const ConstVectorRef & force_ref) { CostStack * cs = getCostStack(i); QuadraticResidualCost * qrc = cs->getComponent(ee_name + "_force_cost"); @@ -329,7 +329,7 @@ namespace simple_mpc return qc->getTarget().segment(nq_, 6); } - void FullDynamicsOCP::setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) + void FullDynamicsOCP::setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) { if (velocity_base.size() != 6) { @@ -348,7 +348,7 @@ namespace simple_mpc return qc->getTarget().head(7); }; - void FullDynamicsOCP::setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) + void FullDynamicsOCP::setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) { if (pose_base.size() != 7) { diff --git a/src/kinodynamics.cpp b/src/kinodynamics.cpp index 5577d656..059b55f6 100644 --- a/src/kinodynamics.cpp +++ b/src/kinodynamics.cpp @@ -244,8 +244,8 @@ namespace simple_mpc setReferenceControl(i, control_ref_); } - void KinodynamicsOCP::setReferenceForce( - const std::size_t i, const std::string & ee_name, const Eigen::VectorXd & force_ref) + 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::iterator it = std::find(hname.begin(), hname.end(), ee_name); @@ -270,7 +270,7 @@ namespace simple_mpc return qc->getTarget().segment(nq_, 6); } - void KinodynamicsOCP::setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) + void KinodynamicsOCP::setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) { if (velocity_base.size() != 6) { @@ -289,7 +289,7 @@ namespace simple_mpc return qc->getTarget().head(7); }; - void KinodynamicsOCP::setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) + void KinodynamicsOCP::setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) { if (pose_base.size() != 7) { diff --git a/src/ocp-handler.cpp b/src/ocp-handler.cpp index f767f9b1..6ff6692b 100644 --- a/src/ocp-handler.cpp +++ b/src/ocp-handler.cpp @@ -55,7 +55,7 @@ namespace simple_mpc return stage_models; } - void OCPHandler::setReferenceControl(const std::size_t t, const Eigen::VectorXd & u_ref) + void OCPHandler::setReferenceControl(const std::size_t t, const ConstVectorRef & u_ref) { CostStack * cs = getCostStack(t); QuadraticControlCost * qc = cs->getComponent("control_cost"); @@ -94,7 +94,7 @@ namespace simple_mpc } void OCPHandler::createProblem( - const Eigen::VectorXd & x0, + const ConstVectorRef & x0, const size_t horizon, const int force_size, const double gravity, diff --git a/src/robot-handler.cpp b/src/robot-handler.cpp index da1099c0..d66774b8 100644 --- a/src/robot-handler.cpp +++ b/src/robot-handler.cpp @@ -66,7 +66,7 @@ namespace simple_mpc return frame_id; } - Eigen::VectorXd RobotModelHandler::shapeState(const Eigen::VectorXd & q, const Eigen::VectorXd & v) const + Eigen::VectorXd RobotModelHandler::shapeState(const ConstVectorRef & q, const ConstVectorRef & v) const { const long nq_full = model_full_.nq; const long nv_full = model_full_.nv; @@ -97,7 +97,7 @@ namespace simple_mpc return x; } - Eigen::VectorXd RobotModelHandler::difference(const Eigen::VectorXd & x1, const Eigen::VectorXd & x2) const + Eigen::VectorXd RobotModelHandler::difference(const ConstVectorRef & x1, const ConstVectorRef & x2) const { const size_t nq = (size_t)model_.nq; const size_t nv = (size_t)model_.nv; @@ -131,7 +131,7 @@ namespace simple_mpc updateInternalData(model_handler.getReferenceState(), true); } - void RobotDataHandler::updateInternalData(const Eigen::VectorXd & x, const bool updateJacobians) + void RobotDataHandler::updateInternalData(const ConstVectorRef & x, const bool updateJacobians) { const Eigen::Block q = x.head(model_handler_.getModel().nq); const Eigen::Block v = x.tail(model_handler_.getModel().nv); @@ -147,7 +147,7 @@ namespace simple_mpc } } - void RobotDataHandler::updateJacobiansMassMatrix(const Eigen::VectorXd & x) + void RobotDataHandler::updateJacobiansMassMatrix(const ConstVectorRef & x) { const Eigen::Block q = x.head(model_handler_.getModel().nq); const Eigen::Block v = x.tail(model_handler_.getModel().nv); From 32a7ec81e1adb9d4293db1775a556453e4a0c58b Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Wed, 18 Dec 2024 16:31:16 +0100 Subject: [PATCH 4/5] robot-handler.hpp : fix return type of getReferenceStte() --- bindings/expose-robot-handler.cpp | 2 +- include/simple-mpc/robot-handler.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/bindings/expose-robot-handler.cpp b/bindings/expose-robot-handler.cpp index 10dd0e84..2c77edfe 100644 --- a/bindings/expose-robot-handler.cpp +++ b/bindings/expose-robot-handler.cpp @@ -28,7 +28,7 @@ namespace simple_mpc .def("difference", &RobotModelHandler::difference) .def("shapeState", &RobotModelHandler::shapeState) .def("getBaseFrameId", &RobotModelHandler::getBaseFrameId) - .def("getReferenceState", &RobotModelHandler::getReferenceState, bp::return_internal_reference<>()) + .def("getReferenceState", &RobotModelHandler::getReferenceState) .def("getFootNb", &RobotModelHandler::getFootNb) .def("getFeetIds", &RobotModelHandler::getFeetIds, bp::return_internal_reference<>()) .def("getFootName", &RobotModelHandler::getFootName, bp::return_internal_reference<>()) diff --git a/include/simple-mpc/robot-handler.hpp b/include/simple-mpc/robot-handler.hpp index 5da9afd8..d5d21b63 100644 --- a/include/simple-mpc/robot-handler.hpp +++ b/include/simple-mpc/robot-handler.hpp @@ -124,7 +124,7 @@ namespace simple_mpc Eigen::VectorXd shapeState(const ConstVectorRef & q, const ConstVectorRef & v) const; // Const getters - const ConstVectorRef & getReferenceState() const + ConstVectorRef getReferenceState() const { return reference_state_; } From dc332c8fa0474c397dce5adf3e3aec8a03a68ef4 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Wed, 18 Dec 2024 18:27:23 +0100 Subject: [PATCH 5/5] Fix examples in python --- examples/go2_fulldynamics.py | 9 ++++----- examples/go2_kinodynamics.py | 7 +++---- examples/talos_centroidal.py | 7 +++---- examples/talos_fulldynamics.py | 3 +-- examples/talos_kinodynamics.py | 7 +++---- 5 files changed, 14 insertions(+), 19 deletions(-) diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py index e328afe6..b6486b11 100644 --- a/examples/go2_fulldynamics.py +++ b/examples/go2_fulldynamics.py @@ -83,8 +83,7 @@ timestep=dt, ) -mpc = MPC() -mpc.initialize(mpc_conf, dynproblem) +mpc = MPC(mpc_conf, dynproblem) """ Define contact sequence throughout horizon""" contact_phase_quadru = { @@ -223,17 +222,17 @@ solve_time.append(end - start) a0 = ( - mpc.getSolver() + mpc.solver .workspace.problem_data.stage_data[0] .dynamics_data.continuous_data.xdot[nv:] ) a1 = ( - mpc.getSolver() + mpc.solver .workspace.problem_data.stage_data[1] .dynamics_data.continuous_data.xdot[nv:] ) - FL_f, FR_f, RL_f, RR_f, contact_states = extract_forces(mpc.getTrajOptProblem(), mpc.getSolver().workspace, 0) + FL_f, FR_f, RL_f, RR_f, contact_states = extract_forces(mpc.getTrajOptProblem(), mpc.solver.workspace, 0) total_forces = np.concatenate((FL_f, FR_f, RL_f, RR_f)) force_FL.append(FL_f) force_FR.append(FR_f) diff --git a/examples/go2_kinodynamics.py b/examples/go2_kinodynamics.py index 07e95791..666aee3f 100644 --- a/examples/go2_kinodynamics.py +++ b/examples/go2_kinodynamics.py @@ -93,8 +93,7 @@ timestep=0.01, ) -mpc = MPC() -mpc.initialize(mpc_conf, dynproblem) +mpc = MPC(mpc_conf, dynproblem) """ Define contact sequence throughout horizon""" contact_phase_quadru = { @@ -227,12 +226,12 @@ L_measured.append(mpc.getDataHandler().getData().hg.angular.copy()) a0 = ( - mpc.getSolver() + mpc.solver .workspace.problem_data.stage_data[0] .dynamics_data.continuous_data.xdot[nv:] ) a1 = ( - mpc.getSolver() + mpc.solver .workspace.problem_data.stage_data[1] .dynamics_data.continuous_data.xdot[nv:] ) diff --git a/examples/talos_centroidal.py b/examples/talos_centroidal.py index a99c02ea..43e187e9 100644 --- a/examples/talos_centroidal.py +++ b/examples/talos_centroidal.py @@ -87,8 +87,7 @@ timestep=problem_conf["timestep"], ) -mpc = MPC() -mpc.initialize(mpc_conf, problem) +mpc = MPC(mpc_conf, problem) """ Define contact sequence throughout horizon""" contact_phase_double = { @@ -210,7 +209,7 @@ 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.getSolver() + mpc.solver .workspace.problem_data.stage_data[0] .dynamics_data.continuous_data.xdot[3:9] ) @@ -229,7 +228,7 @@ forces = ( mpc.us[0][: nk * force_size] - - 1 * mpc.getSolver().results.controlFeedbacks()[0] @ state_diff + - 1 * mpc.solver.results.controlFeedbacks()[0] @ state_diff ) diff --git a/examples/talos_fulldynamics.py b/examples/talos_fulldynamics.py index 6b328632..e65400a8 100644 --- a/examples/talos_fulldynamics.py +++ b/examples/talos_fulldynamics.py @@ -112,8 +112,7 @@ timestep=problem_conf["timestep"], ) -mpc = MPC() -mpc.initialize(mpc_conf, dynproblem) +mpc = MPC(mpc_conf, dynproblem) """ Define contact sequence throughout horizon""" contact_phase_double = { diff --git a/examples/talos_kinodynamics.py b/examples/talos_kinodynamics.py index 63f75916..3d6549e0 100644 --- a/examples/talos_kinodynamics.py +++ b/examples/talos_kinodynamics.py @@ -123,8 +123,7 @@ timestep=problem_conf["timestep"], ) -mpc = MPC() -mpc.initialize(mpc_conf, problem) +mpc = MPC(mpc_conf, problem) """ Define contact sequence throughout horizon""" contact_phase_double = { @@ -212,12 +211,12 @@ end = time.time() print("MPC iterate = " + str(end - start)) a0 = ( - mpc.getSolver() + mpc.solver .workspace.problem_data.stage_data[0] .dynamics_data.continuous_data.xdot[nv:] ) a1 = ( - mpc.getSolver() + mpc.solver .workspace.problem_data.stage_data[1] .dynamics_data.continuous_data.xdot[nv:] )