Skip to content

Changing ownership if the RobotData Object #24

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Dec 18, 2024
Merged

Changing ownership if the RobotData Object #24

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 4 additions & 5 deletions bindings/expose-centroidal.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,7 @@
namespace simple_mpc::python
{

auto * createCentroidal(
const bp::dict & settings, const RobotModelHandler & model_handler, const RobotDataHandler & data_handler)
auto * createCentroidal(const bp::dict & settings, const RobotModelHandler & model_handler)
{
CentroidalSettings conf;
conf.timestep = bp::extract<double>(settings["timestep"]);
Expand All @@ -25,7 +24,7 @@ namespace simple_mpc::python
conf.Lfoot = bp::extract<double>(settings["Lfoot"]);
conf.Wfoot = bp::extract<double>(settings["Wfoot"]);

return new CentroidalOCP(conf, model_handler, data_handler);
return new CentroidalOCP(conf, model_handler);
}

StageModel createCentStage(
Expand Down Expand Up @@ -113,8 +112,8 @@ namespace simple_mpc::python

bp::class_<CentroidalOCP, bp::bases<OCPHandler>, boost::noncopyable>("CentroidalOCP", bp::no_init)
.def(
"__init__", bp::make_constructor(
&createCentroidal, bp::default_call_policies(), ("settings"_a, "model_handler", "data_handler")))
"__init__",
bp::make_constructor(&createCentroidal, bp::default_call_policies(), ("settings"_a, "model_handler")))
.def("getSettings", &getSettingsCent)
.def("createStage", &createCentStage);
}
Expand Down
8 changes: 3 additions & 5 deletions bindings/expose-fulldynamics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,7 @@
namespace simple_mpc::python
{

auto * createFulldynamics(
const bp::dict & settings, const RobotModelHandler & model_handler, const RobotDataHandler & data_handler)
auto * createFulldynamics(const bp::dict & settings, const RobotModelHandler & model_handler)
{
FullDynamicsSettings conf;
conf.timestep = bp::extract<double>(settings["timestep"]);
Expand Down Expand Up @@ -41,7 +40,7 @@ namespace simple_mpc::python
conf.kinematics_limits = bp::extract<bool>(settings["kinematics_limits"]);
conf.force_cone = bp::extract<bool>(settings["force_cone"]);

return new FullDynamicsOCP(conf, model_handler, data_handler);
return new FullDynamicsOCP(conf, model_handler);
}

StageModel createFullStage(
Expand Down Expand Up @@ -138,8 +137,7 @@ namespace simple_mpc::python
bp::class_<FullDynamicsOCP, bp::bases<OCPHandler>, boost::noncopyable>("FullDynamicsOCP", bp::no_init)
.def(
"__init__",
bp::make_constructor(
&createFulldynamics, bp::default_call_policies(), ("settings"_a, "model_handler", "data_handler")))
bp::make_constructor(&createFulldynamics, bp::default_call_policies(), ("settings"_a, "model_handler")))
.def("getSettings", &getSettingsFull)
.def("createStage", &createFullStage);
}
Expand Down
8 changes: 3 additions & 5 deletions bindings/expose-kinodynamics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,7 @@
namespace simple_mpc::python
{

auto * createKinodynamics(
const bp::dict & settings, const RobotModelHandler & model_handler, const RobotDataHandler & data_handler)
auto * createKinodynamics(const bp::dict & settings, const RobotModelHandler & model_handler)
{
KinodynamicsSettings conf;
conf.timestep = bp::extract<double>(settings["timestep"]);
Expand All @@ -30,7 +29,7 @@ namespace simple_mpc::python
conf.kinematics_limits = bp::extract<bool>(settings["kinematics_limits"]);
conf.force_cone = bp::extract<bool>(settings["force_cone"]);

return new KinodynamicsOCP(conf, model_handler, data_handler);
return new KinodynamicsOCP(conf, model_handler);
}

bp::dict getSettingsKino(KinodynamicsOCP & self)
Expand Down Expand Up @@ -122,8 +121,7 @@ namespace simple_mpc::python
bp::class_<KinodynamicsOCP, bp::bases<OCPHandler>, boost::noncopyable>("KinodynamicsOCP", bp::no_init)
.def(
"__init__",
bp::make_constructor(
&createKinodynamics, bp::default_call_policies(), ("settings"_a, "model_handler", "data_handler")))
bp::make_constructor(&createKinodynamics, bp::default_call_policies(), ("settings"_a, "model_handler")))
.def("getSettings", &getSettingsKino)
.def("createStage", &createKinoStage);
}
Expand Down
4 changes: 2 additions & 2 deletions bindings/expose-problem.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ namespace simple_mpc
{
bp::register_ptr_to_python<std::shared_ptr<OCPHandler>>();
bp::class_<PyOCPHandler, boost::noncopyable>("OCPHandler", bp::no_init)
.def(bp::init<const RobotModelHandler &, const RobotDataHandler &>(("self"_a, "model_handler", "data_handler")))
.def(bp::init<const RobotModelHandler &>(("self"_a, "model_handler")))
.def(
"createStage", bp::pure_virtual(&OCPHandler::createStage),
("self"_a, "contact_map", "force_refs", "land_constraint"))
Expand All @@ -69,7 +69,7 @@ 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("getProblemState", bp::pure_virtual(&OCPHandler::getProblemState), bp::args("self"))
.def("getProblemState", bp::pure_virtual(&OCPHandler::getProblemState), bp::args("self", "data_handler"))
.def("getContactSupport", bp::pure_virtual(&OCPHandler::getContactSupport), bp::args("self", "t"))
.def(
"createProblem", &OCPHandler::createProblem,
Expand Down
2 changes: 1 addition & 1 deletion examples/go2_fulldynamics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@
)
T = 50

dynproblem = FullDynamicsOCP(problem_conf, model_handler, data_handler)
dynproblem = FullDynamicsOCP(problem_conf, model_handler)
dynproblem.createProblem(model_handler.getReferenceState(), T, force_size, gravity[2], False)

T_ds = 10
Expand Down
2 changes: 1 addition & 1 deletion examples/go2_kinodynamics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@
)
T = 50

dynproblem = KinodynamicsOCP(problem_conf, model_handler, data_handler)
dynproblem = KinodynamicsOCP(problem_conf, model_handler)
dynproblem.createProblem(model_handler.getReferenceState(), T, force_size, gravity[2], False)

T_ds = 10
Expand Down
2 changes: 1 addition & 1 deletion examples/talos_centroidal.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@
)
T = 100

problem = CentroidalOCP(problem_conf, model_handler, data_handler)
problem = CentroidalOCP(problem_conf, model_handler)
problem.createProblem(data_handler.getCentroidalState(), T, 6, gravity[2], False)

T_ds = 20
Expand Down
2 changes: 1 addition & 1 deletion examples/talos_fulldynamics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@
)

T = 100
dynproblem = FullDynamicsOCP(problem_conf, model_handler, data_handler)
dynproblem = FullDynamicsOCP(problem_conf, model_handler)
dynproblem.createProblem(x0, T, 6, gravity[2], False)

""" Define feet trajectory """
Expand Down
2 changes: 1 addition & 1 deletion examples/talos_kinodynamics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@

T = 100

problem = KinodynamicsOCP(problem_conf, model_handler, data_handler)
problem = KinodynamicsOCP(problem_conf, model_handler)
problem.createProblem(model_handler.getReferenceState(), T, 6, gravity[2], False)

""" Define MPC object """
Expand Down
9 changes: 3 additions & 6 deletions include/simple-mpc/centroidal-dynamics.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,10 +59,7 @@ namespace simple_mpc
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

// Constructor
explicit CentroidalOCP(
const CentroidalSettings & settings,
const RobotModelHandler & model_handler,
const RobotDataHandler & data_handler);
explicit CentroidalOCP(const CentroidalSettings & settings, const RobotModelHandler & model_handler);

SIMPLE_MPC_DEFINE_DEFAULT_MOVE_CTORS(CentroidalOCP);

Expand All @@ -77,7 +74,7 @@ namespace simple_mpc

// Manage terminal cost and constraint
CostStack createTerminalCost() override;
void createTerminalConstraint() override;
void createTerminalConstraint(const Eigen::Vector3d & com_ref) override;
void updateTerminalConstraint(const Eigen::Vector3d & com_ref) override;

// Getters and setters for pose not implemented
Expand All @@ -98,7 +95,7 @@ namespace simple_mpc
void setVelocityBase(const std::size_t t, const Eigen::VectorXd & 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;
const Eigen::VectorXd getProblemState() override;
const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
size_t getContactSupport(const std::size_t t) override;

CentroidalSettings getSettings()
Expand Down
9 changes: 3 additions & 6 deletions include/simple-mpc/fulldynamics.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,10 +69,7 @@ namespace simple_mpc
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

// Constructors
explicit FullDynamicsOCP(
const FullDynamicsSettings & settings,
const RobotModelHandler & model_handler,
const RobotDataHandler & data_handler);
explicit FullDynamicsOCP(const FullDynamicsSettings & settings, const RobotModelHandler & model_handler);

SIMPLE_MPC_DEFINE_DEFAULT_MOVE_CTORS(FullDynamicsOCP);

Expand All @@ -87,7 +84,7 @@ namespace simple_mpc

// Manage terminal cost and constraint
CostStack createTerminalCost() override;
void createTerminalConstraint() override;
void createTerminalConstraint(const Eigen::Vector3d & com_ref) override;
void updateTerminalConstraint(const Eigen::Vector3d & com_ref) override;

// Getters and setters
Expand All @@ -103,7 +100,7 @@ namespace simple_mpc
void setVelocityBase(const std::size_t t, const Eigen::VectorXd & 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;
const Eigen::VectorXd getProblemState() override;
const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
size_t getContactSupport(const std::size_t t) override;
FullDynamicsSettings getSettings()
{
Expand Down
9 changes: 3 additions & 6 deletions include/simple-mpc/kinodynamics.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,10 +57,7 @@ namespace simple_mpc
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

// Constructor
explicit KinodynamicsOCP(
const KinodynamicsSettings & settings,
const RobotModelHandler & model_handler,
const RobotDataHandler & data_handler);
explicit KinodynamicsOCP(const KinodynamicsSettings & settings, const RobotModelHandler & model_handler);

SIMPLE_MPC_DEFINE_DEFAULT_MOVE_CTORS(KinodynamicsOCP);

Expand All @@ -75,7 +72,7 @@ namespace simple_mpc

// Manage terminal cost and constraint
CostStack createTerminalCost() override;
void createTerminalConstraint() override;
void createTerminalConstraint(const Eigen::Vector3d & com_ref) override;
void updateTerminalConstraint(const Eigen::Vector3d & com_ref) override;

// Getters and setters
Expand All @@ -91,7 +88,7 @@ namespace simple_mpc
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;
const Eigen::VectorXd getProblemState() override;
const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
size_t getContactSupport(const std::size_t t) override;

void computeControlFromForces(const std::map<std::string, Eigen::VectorXd> & force_refs);
Expand Down
4 changes: 3 additions & 1 deletion include/simple-mpc/mpc.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,8 @@ namespace simple_mpc
Eigen::Vector3d com0_;
LocomotionType now_;

std::shared_ptr<RobotDataHandler> data_handler_;

public:
std::shared_ptr<SolverProxDDP> solver_;
Vector6d velocity_base_;
Expand Down Expand Up @@ -136,7 +138,7 @@ namespace simple_mpc
}
const RobotDataHandler & getDataHandler() const
{
return ocp_handler_->getDataHandler();
return *data_handler_;
}
const RobotModelHandler & getModelHandler() const
{
Expand Down
15 changes: 3 additions & 12 deletions include/simple-mpc/ocp-handler.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ namespace simple_mpc
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

/// Constructor
explicit OCPHandler(const RobotModelHandler & model_handler, const RobotDataHandler & data_handler);
explicit OCPHandler(const RobotModelHandler & model_handler);
SIMPLE_MPC_DEFINE_DEFAULT_MOVE_CTORS(OCPHandler);
virtual ~OCPHandler();

Expand All @@ -71,7 +71,7 @@ namespace simple_mpc

virtual void updateTerminalConstraint(const Eigen::Vector3d & com_ref) = 0;

virtual void createTerminalConstraint() = 0;
virtual void createTerminalConstraint(const Eigen::Vector3d & com_ref) = 0;

// Setter and getter for poses reference
virtual void
Expand All @@ -93,7 +93,7 @@ namespace simple_mpc
virtual void
setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref) = 0;
virtual const Eigen::VectorXd getReferenceForce(const std::size_t t, const std::string & ee_name) = 0;
virtual const Eigen::VectorXd getProblemState() = 0;
virtual const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) = 0;
virtual size_t getContactSupport(const std::size_t t) = 0;

/// Common functions for all problems
Expand Down Expand Up @@ -135,14 +135,6 @@ namespace simple_mpc
{
return model_handler_;
}
const RobotDataHandler & getDataHandler() const
{
return data_handler_;
}
RobotDataHandler & getDataHandler()
{
return data_handler_;
}
int getNu()
{
return nu_;
Expand All @@ -158,7 +150,6 @@ namespace simple_mpc
bool terminal_constraint_ = false;

/// The robot model
RobotDataHandler data_handler_;
RobotModelHandler model_handler_;

/// The reference shooting problem storing all shooting nodes
Expand Down
8 changes: 4 additions & 4 deletions include/simple-mpc/python/py-ocp-handler.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,9 +89,9 @@ namespace simple_mpc
SIMPLE_MPC_PYTHON_OVERRIDE_PURE(aligator::CostStackTpl<double>, "createTerminalCost", );
}

void createTerminalConstraint() override
void createTerminalConstraint(const Eigen::Vector3d & com_ref) override
{
SIMPLE_MPC_PYTHON_OVERRIDE_PURE(void, "createTerminalConstraint", );
SIMPLE_MPC_PYTHON_OVERRIDE_PURE(void, "createTerminalConstraint", com_ref);
}

void updateTerminalConstraint(const Eigen::Vector3d & com_ref) override
Expand Down Expand Up @@ -155,9 +155,9 @@ namespace simple_mpc
SIMPLE_MPC_PYTHON_OVERRIDE_PURE(Eigen::VectorXd, "getPoseBase", t);
}

const Eigen::VectorXd getProblemState() override
const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override
{
SIMPLE_MPC_PYTHON_OVERRIDE_PURE(Eigen::VectorXd, "getProblemState", );
SIMPLE_MPC_PYTHON_OVERRIDE_PURE(Eigen::VectorXd, "getProblemState", data_handler);
}

std::size_t getContactSupport(const std::size_t t) override
Expand Down
13 changes: 6 additions & 7 deletions src/centroidal-dynamics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,8 @@ namespace simple_mpc
using CentroidalFrictionConeResidual = CentroidalFrictionConeResidualTpl<double>;
using IntegratorEuler = dynamics::IntegratorEulerTpl<double>;

CentroidalOCP::CentroidalOCP(
const CentroidalSettings & settings, const RobotModelHandler & model_handler, const RobotDataHandler & data_handler)
: Base(model_handler, data_handler)
CentroidalOCP::CentroidalOCP(const CentroidalSettings & settings, const RobotModelHandler & model_handler)
: Base(model_handler)
, settings_(settings)
{
nx_ = 9;
Expand Down Expand Up @@ -258,9 +257,9 @@ namespace simple_mpc
cfr->setReference(com_ref_);
}

const Eigen::VectorXd CentroidalOCP::getProblemState()
const Eigen::VectorXd CentroidalOCP::getProblemState(const RobotDataHandler & data_handler)
{
return data_handler_.getCentroidalState();
return data_handler.getCentroidalState();
}

size_t CentroidalOCP::getContactSupport(const std::size_t t)
Expand Down Expand Up @@ -291,13 +290,13 @@ namespace simple_mpc
return term_cost;
}

void CentroidalOCP::createTerminalConstraint()
void CentroidalOCP::createTerminalConstraint(const Eigen::Vector3d & com_ref)
{
if (!problem_initialized_)
{
throw std::runtime_error("Create problem first!");
}
CentroidalCoMResidual com_cstr = CentroidalCoMResidual(ndx_, nu_, data_handler_.getData().com[0]);
CentroidalCoMResidual com_cstr = CentroidalCoMResidual(ndx_, nu_, com_ref);

// problem_->addTerminalConstraint(com_cstr, EqualityConstraint());
terminal_constraint_ = false;
Expand Down
Loading