diff --git a/bindings/expose-mpc.cpp b/bindings/expose-mpc.cpp index 131409a1..b4243d4f 100644 --- a/bindings/expose-mpc.cpp +++ b/bindings/expose-mpc.cpp @@ -79,10 +79,8 @@ namespace simple_mpc .def("getReferencePose", &MPC::getReferencePose, bp::args("self", "t", "ee_name")) .def("setTerminalReferencePose", &MPC::setTerminalReferencePose, bp::args("self", "ee_name", "pose_ref")) .def_readwrite("velocity_base", &MPC::velocity_base_) - .def_readwrite("pose_base", &MPC::pose_base_) + .def_readwrite("x_reference", &MPC::x_reference_) .def_readonly("ocp_handler", &MPC::ocp_handler_) - .def("setPoseBase", &MPC::setPoseBase, ("self"_a, "pose_base")) - .def("getPoseBase", &MPC::getPoseBase, ("self"_a, "t")) .def("switchToWalk", &MPC::switchToWalk, ("self"_a, "velocity_base")) .def("switchToStand", &MPC::switchToStand, "self"_a) .def("getFootTakeoffCycle", &MPC::getFootTakeoffCycle, ("self"_a, "ee_name")) diff --git a/bindings/expose-problem.cpp b/bindings/expose-problem.cpp index 59fc061f..a5aa3d50 100644 --- a/bindings/expose-problem.cpp +++ b/bindings/expose-problem.cpp @@ -69,6 +69,8 @@ namespace simple_mpc .def("getVelocityBase", bp::pure_virtual(&OCPHandler::getVelocityBase), bp::args("self", "t")) .def("setPoseBase", bp::pure_virtual(&OCPHandler::setPoseBase), bp::args("self", "t", "pose_base")) .def("getPoseBase", bp::pure_virtual(&OCPHandler::getPoseBase), bp::args("self", "t")) + .def("setReferenceState", bp::pure_virtual(&OCPHandler::setReferenceState), bp::args("self", "t", "x_ref")) + .def("getReferenceState", bp::pure_virtual(&OCPHandler::getReferenceState), bp::args("self", "t")) .def("getProblemState", bp::pure_virtual(&OCPHandler::getProblemState), bp::args("self", "data_handler")) .def("getContactSupport", bp::pure_virtual(&OCPHandler::getContactSupport), bp::args("self", "t")) .def("getContactState", bp::pure_virtual(&OCPHandler::getContactState), bp::args("self", "t")) diff --git a/include/simple-mpc/centroidal-dynamics.hpp b/include/simple-mpc/centroidal-dynamics.hpp index 413d9191..e065546e 100644 --- a/include/simple-mpc/centroidal-dynamics.hpp +++ b/include/simple-mpc/centroidal-dynamics.hpp @@ -97,6 +97,8 @@ namespace simple_mpc const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override; size_t getContactSupport(const std::size_t t) override; std::vector getContactState(const std::size_t t) override; + void setReferenceState(const std::size_t t, const ConstVectorRef & x_ref) override; + const ConstVectorRef getReferenceState(const std::size_t t) override; CentroidalSettings getSettings() { diff --git a/include/simple-mpc/fulldynamics.hpp b/include/simple-mpc/fulldynamics.hpp index 05c2a449..f3137896 100644 --- a/include/simple-mpc/fulldynamics.hpp +++ b/include/simple-mpc/fulldynamics.hpp @@ -102,6 +102,8 @@ namespace simple_mpc const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override; size_t getContactSupport(const std::size_t t) override; std::vector getContactState(const std::size_t t) override; + void setReferenceState(const std::size_t t, const ConstVectorRef & x_ref) override; + const ConstVectorRef getReferenceState(const std::size_t t) override; FullDynamicsSettings getSettings() { return settings_; @@ -112,9 +114,6 @@ namespace simple_mpc FullDynamicsSettings settings_; ProximalSettings prox_settings_; - // State reference - Eigen::VectorXd x0_; - // Actuation matrix Eigen::MatrixXd actuation_matrix_; diff --git a/include/simple-mpc/kinodynamics.hpp b/include/simple-mpc/kinodynamics.hpp index 00370687..9dad3e4d 100644 --- a/include/simple-mpc/kinodynamics.hpp +++ b/include/simple-mpc/kinodynamics.hpp @@ -90,6 +90,8 @@ namespace simple_mpc const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override; size_t getContactSupport(const std::size_t t) override; std::vector getContactState(const std::size_t t) override; + void setReferenceState(const std::size_t t, const ConstVectorRef & x_ref) override; + const ConstVectorRef getReferenceState(const std::size_t t) override; void computeControlFromForces(const std::map & force_refs); @@ -100,7 +102,6 @@ namespace simple_mpc protected: KinodynamicsSettings settings_; - Eigen::VectorXd x0_; }; } // namespace simple_mpc diff --git a/include/simple-mpc/mpc.hpp b/include/simple-mpc/mpc.hpp index a3e09a77..bb9f1150 100644 --- a/include/simple-mpc/mpc.hpp +++ b/include/simple-mpc/mpc.hpp @@ -88,7 +88,6 @@ namespace simple_mpc public: std::unique_ptr solver_; Vector6d velocity_base_; - Vector7d pose_base_; Eigen::Vector3d next_pose_; Eigen::Vector2d twist_vect_; MPCSettings settings_; @@ -120,18 +119,10 @@ namespace simple_mpc velocity_base_ = v; } - void setPoseBaseFromSE3(const pin::SE3 & pose_ref) + void setReferenceState(const VectorXd & state_ref) { - Eigen::Map q{pose_base_.tail<4>().data()}; - pose_base_.head<3>() = pose_ref.translation(); - q = pose_ref.rotation(); + x_reference_ = state_ref; } - SIMPLE_MPC_DEPRECATED void setPoseBase(const Vector7d & pose_ref) - { - pose_base_ = pose_ref; - } - - ConstVectorRef getPoseBase(const std::size_t t) const; // getters and setters TrajOptProblem & getTrajOptProblem(); @@ -198,6 +189,7 @@ namespace simple_mpc std::vector us_; // Riccati gains std::vector Ks_; + VectorXd x_reference_; // Initial quantities VectorXd x0_; diff --git a/include/simple-mpc/ocp-handler.hpp b/include/simple-mpc/ocp-handler.hpp index 1e07794f..04e6eb75 100644 --- a/include/simple-mpc/ocp-handler.hpp +++ b/include/simple-mpc/ocp-handler.hpp @@ -96,6 +96,8 @@ namespace simple_mpc virtual const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) = 0; virtual size_t getContactSupport(const std::size_t t) = 0; virtual std::vector getContactState(const std::size_t t) = 0; + virtual void setReferenceState(const std::size_t t, const ConstVectorRef & x_ref) = 0; + virtual const ConstVectorRef getReferenceState(const std::size_t t) = 0; /// Common functions for all problems @@ -107,7 +109,7 @@ namespace simple_mpc const double gravity, const bool terminal_constraint); - // Setter and getter for control reference + // Setter and getter for state and control reference void setReferenceControl(const std::size_t t, const ConstVectorRef & u_ref); ConstVectorRef getReferenceControl(const std::size_t t); @@ -150,6 +152,9 @@ namespace simple_mpc bool problem_initialized_ = false; bool terminal_constraint_ = false; + /// State reference + Eigen::VectorXd x0_; + /// The robot model RobotModelHandler model_handler_; diff --git a/include/simple-mpc/python/py-ocp-handler.hpp b/include/simple-mpc/python/py-ocp-handler.hpp index 1a5dff01..f8b50a0d 100644 --- a/include/simple-mpc/python/py-ocp-handler.hpp +++ b/include/simple-mpc/python/py-ocp-handler.hpp @@ -170,6 +170,16 @@ namespace simple_mpc SIMPLE_MPC_PYTHON_OVERRIDE_PURE(std::vector, "getContactState", t); } + void setReferenceState(const std::size_t t, const ConstVectorRef & x_ref) override + { + SIMPLE_MPC_PYTHON_OVERRIDE_PURE(void, "setReferenceState", t, x_ref); + } + + const ConstVectorRef getReferenceState(const std::size_t t) override + { + SIMPLE_MPC_PYTHON_OVERRIDE_PURE(ConstVectorRef, "getReferenceState", t); + } + void setReferenceControl(const std::size_t t, const ConstVectorRef & u_ref) { SIMPLE_MPC_PYTHON_OVERRIDE(void, OCPHandler, setReferenceControl, t, u_ref); diff --git a/src/centroidal-dynamics.cpp b/src/centroidal-dynamics.cpp index 2778fbac..57eebc77 100644 --- a/src/centroidal-dynamics.cpp +++ b/src/centroidal-dynamics.cpp @@ -33,6 +33,7 @@ namespace simple_mpc control_ref_.resize(nu_); control_ref_.setZero(); com_ref_.setZero(); + x0_.resize(9); } StageModel CentroidalOCP::createStage( @@ -225,6 +226,7 @@ namespace simple_mpc void CentroidalOCP::setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) { + assert(velocity_base.size() == 6 && "velocity_base not of the right size"); CostStack * cs = getCostStack(t); QuadraticResidualCost * qcm = cs->getComponent("linear_mom_cost"); LinearMomentumResidual * cfm = qcm->getResidual(); @@ -246,10 +248,7 @@ namespace simple_mpc void CentroidalOCP::setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) { - if (pose_base.size() != 7) - { - throw std::runtime_error("pose_base size should be 7"); - } + assert(pose_base.size() == 3 && "pose_base not of the right size"); CostStack * cs = getCostStack(t); QuadraticResidualCost * qrc = cs->getComponent("com_cost"); CentroidalCoMResidual * cfr = qrc->getResidual(); @@ -290,6 +289,20 @@ namespace simple_mpc return contact_state; } + 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)); + } + + const ConstVectorRef CentroidalOCP::getReferenceState(const std::size_t t) + { + x0_.head(3) = getPoseBase(t); + x0_.tail(6) = getVelocityBase(t); + return x0_; + } + CostStack CentroidalOCP::createTerminalCost() { auto ter_space = VectorSpace(nx_); diff --git a/src/fulldynamics.cpp b/src/fulldynamics.cpp index 32787f09..6578cf5c 100644 --- a/src/fulldynamics.cpp +++ b/src/fulldynamics.cpp @@ -88,7 +88,7 @@ namespace simple_mpc auto space = MultibodyPhaseSpace(model_handler_.getModel()); auto rcost = CostStack(space, nu_); - rcost.addCost("state_cost", QuadraticStateCost(space, nu_, x0_, settings_.w_x)); + rcost.addCost("state_cost", QuadraticStateCost(space, nu_, model_handler_.getReferenceState(), settings_.w_x)); rcost.addCost("control_cost", QuadraticControlCost(space, Eigen::VectorXd::Zero(nu_), settings_.w_u)); auto cent_mom = CentroidalMomentumResidual(space.ndx(), nu_, model_handler_.getModel(), Eigen::VectorXd::Zero(6)); @@ -337,6 +337,7 @@ namespace simple_mpc } CostStack * cs = getCostStack(t); QuadraticStateCost * qc = cs->getComponent("state_cost"); + x0_ = getReferenceState(t); x0_.segment(nq_, 6) = velocity_base; qc->setTarget(x0_); } @@ -356,6 +357,7 @@ namespace simple_mpc } CostStack * cs = getCostStack(t); QuadraticStateCost * qc = cs->getComponent("state_cost"); + x0_ = getReferenceState(t); x0_.head(7) = pose_base; qc->setTarget(x0_); } @@ -398,6 +400,21 @@ namespace simple_mpc return contact_state; } + void FullDynamicsOCP::setReferenceState(const std::size_t t, const ConstVectorRef & x_ref) + { + assert(x_ref.size() == nq_ + nv_ && "x_ref not of the right size"); + CostStack * cs = getCostStack(t); + QuadraticStateCost * qc = cs->getComponent("state_cost"); + qc->setTarget(x_ref); + } + + const ConstVectorRef FullDynamicsOCP::getReferenceState(const std::size_t t) + { + CostStack * cs = getCostStack(t); + QuadraticStateCost * qc = cs->getComponent("state_cost"); + return qc->getTarget(); + } + CostStack FullDynamicsOCP::createTerminalCost() { auto ter_space = MultibodyPhaseSpace(model_handler_.getModel()); @@ -405,7 +422,8 @@ namespace simple_mpc auto cent_mom = CentroidalMomentumResidual(ter_space.ndx(), nu_, model_handler_.getModel(), Eigen::VectorXd::Zero(6)); - term_cost.addCost("state_cost", QuadraticStateCost(ter_space, nu_, x0_, settings_.w_x)); + term_cost.addCost( + "state_cost", QuadraticStateCost(ter_space, nu_, model_handler_.getReferenceState(), settings_.w_x)); /* term_cost.addCost( "centroidal_cost", QuadraticResidualCost(ter_space, cent_mom, settings_.w_cent)); */ diff --git a/src/kinodynamics.cpp b/src/kinodynamics.cpp index 246a47bd..7d3044a5 100644 --- a/src/kinodynamics.cpp +++ b/src/kinodynamics.cpp @@ -57,7 +57,7 @@ namespace simple_mpc auto centder_mom = CentroidalMomentumDerivativeResidual( space.ndx(), model_handler_.getModel(), settings_.gravity, contact_states, model_handler_.getFeetIds(), settings_.force_size); - rcost.addCost("state_cost", QuadraticStateCost(space, nu_, x0_, settings_.w_x)); + 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)); @@ -278,6 +278,7 @@ namespace simple_mpc } CostStack * cs = getCostStack(t); QuadraticStateCost * qc = cs->getComponent("state_cost"); + x0_ = getReferenceState(t); x0_.segment(nq_, 6) = velocity_base; qc->setTarget(x0_); } @@ -297,6 +298,7 @@ namespace simple_mpc } CostStack * cs = getCostStack(t); QuadraticStateCost * qc = cs->getComponent("state_cost"); + x0_ = getReferenceState(t); x0_.head(7) = pose_base; qc->setTarget(x0_); } @@ -306,6 +308,21 @@ namespace simple_mpc return data_handler.getState(); } + void KinodynamicsOCP::setReferenceState(const std::size_t t, const ConstVectorRef & x_ref) + { + assert(x_ref.size() == nq_ + nv_ && "x_ref not of the right size"); + CostStack * cs = getCostStack(t); + QuadraticStateCost * qc = cs->getComponent("state_cost"); + qc->setTarget(x_ref); + } + + const ConstVectorRef KinodynamicsOCP::getReferenceState(const std::size_t t) + { + CostStack * cs = getCostStack(t); + QuadraticStateCost * qc = cs->getComponent("state_cost"); + return qc->getTarget(); + } + size_t KinodynamicsOCP::getContactSupport(const std::size_t t) { KinodynamicsFwdDynamics * ode = @@ -337,7 +354,8 @@ namespace simple_mpc auto cent_mom = CentroidalMomentumResidual(ter_space.ndx(), nu_, model_handler_.getModel(), Eigen::VectorXd::Zero(6)); - term_cost.addCost("state_cost", QuadraticStateCost(ter_space, nu_, x0_, settings_.w_x)); + term_cost.addCost( + "state_cost", QuadraticStateCost(ter_space, nu_, model_handler_.getReferenceState(), settings_.w_x)); term_cost.addCost("centroidal_cost", QuadraticResidualCost(ter_space, cent_mom, settings_.w_cent * 10)); return term_cost; diff --git a/src/mpc.cpp b/src/mpc.cpp index 917fcd12..f7449212 100644 --- a/src/mpc.cpp +++ b/src/mpc.cpp @@ -36,6 +36,7 @@ namespace simple_mpc foot_trajectories_.updateApex(settings.swing_apex); x0_ = ocp_handler_->getProblemState(*data_handler_); + x_reference_ = ocp_handler_->getReferenceState(0); solver_ = std::make_unique(settings_.TOL, settings_.mu_init, maxiters, aligator::QUIET); solver_->rollout_type_ = aligator::RolloutType::LINEAR; @@ -89,7 +90,7 @@ namespace simple_mpc com0_ = data_handler_->getData().com[0]; now_ = WALKING; - pose_base_ = x0_.head<7>(); + velocity_base_.setZero(); next_pose_.setZero(); twist_vect_.setZero(); @@ -302,8 +303,8 @@ namespace simple_mpc } } + ocp_handler_->setReferenceState(ocp_handler_->getSize() - 1, x_reference_); ocp_handler_->setVelocityBase(ocp_handler_->getSize() - 1, velocity_base_); - ocp_handler_->setPoseBase(ocp_handler_->getSize() - 1, pose_base_); Eigen::Vector3d com_ref; com_ref << 0, 0, 0; @@ -332,11 +333,6 @@ namespace simple_mpc return ocp_handler_->getReferencePose(t, ee_name); } - ConstVectorRef MPC::getPoseBase(const std::size_t t) const - { - return ocp_handler_->getPoseBase(t); - } - TrajOptProblem & MPC::getTrajOptProblem() { return ocp_handler_->getProblem(); diff --git a/tests/problem.cpp b/tests/problem.cpp index da820120..89af17fc 100644 --- a/tests/problem.cpp +++ b/tests/problem.cpp @@ -94,6 +94,13 @@ BOOST_AUTO_TEST_CASE(fulldynamics) pose_base << 0, 0, 2, 0, 0, 0, 1; fdproblem.setPoseBase(2, pose_base); BOOST_CHECK_EQUAL(fdproblem.getPoseBase(2), pose_base); + + Eigen::VectorXd new_q = pinocchio::randomConfiguration(model_handler.getModel()); + Eigen::VectorXd new_x(model_handler.getModel().nq + model_handler.getModel().nv); + new_x.head(model_handler.getModel().nq) = new_q; + new_x.tail(model_handler.getModel().nv).setRandom(); + fdproblem.setReferenceState(2, new_x); + BOOST_CHECK_EQUAL(fdproblem.getReferenceState(2), new_x); } BOOST_AUTO_TEST_CASE(kinodynamics) @@ -179,6 +186,13 @@ BOOST_AUTO_TEST_CASE(kinodynamics) pose_base << 0, 0, 2, 0, 0, 0, 1; knproblem.setPoseBase(2, pose_base); BOOST_CHECK_EQUAL(knproblem.getPoseBase(2), pose_base); + + Eigen::VectorXd new_q = pinocchio::randomConfiguration(model_handler.getModel()); + Eigen::VectorXd new_x(model_handler.getModel().nq + model_handler.getModel().nv); + new_x.head(model_handler.getModel().nq) = new_q; + new_x.tail(model_handler.getModel().nv).setRandom(); + knproblem.setReferenceState(2, new_x); + BOOST_CHECK_EQUAL(knproblem.getReferenceState(2), new_x); } BOOST_AUTO_TEST_CASE(centroidal) @@ -260,10 +274,15 @@ BOOST_AUTO_TEST_CASE(centroidal) BOOST_CHECK_EQUAL(cproblem.getReferencePose(3, "left_sole_link"), new_poses.at("left_sole_link")); BOOST_CHECK_EQUAL(cproblem.getReferencePose(3, "right_sole_link"), new_poses.at("right_sole_link")); - Eigen::VectorXd pose_base(7); - pose_base << 0, 0, 2, 0, 0, 0, 1; + Eigen::VectorXd pose_base(3); + pose_base << 0, 0, 2; cproblem.setPoseBase(2, pose_base); - BOOST_CHECK_EQUAL(cproblem.getPoseBase(2), pose_base.head(3)); + BOOST_CHECK_EQUAL(cproblem.getPoseBase(2), pose_base); + + Eigen::VectorXd new_x(9); + new_x << 0, 0, 1, 0, 0.1, 0.1, 0.2, 0.2, 0.2; + cproblem.setReferenceState(2, new_x); + BOOST_CHECK_EQUAL(cproblem.getReferenceState(2), new_x); } BOOST_AUTO_TEST_CASE(centroidal_solo)