Skip to content

Commit 3e29a8d

Browse files
ManifoldFRManifoldFR
authored
Merge pull request #27 from Simple-Robotics/topic/more-eigen-refs-II
Change all instances of taking argument "const Eigen::VectorXd &" to "const ConstVectorRef&"
2 parents a81c86b + dc332c8 commit 3e29a8d

20 files changed

+72
-100
lines changed

‎bindings/expose-mpc.cpp‎

Lines changed: 6 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111

1212
#include <eigenpy/deprecation-policy.hpp>
1313
#include <eigenpy/eigenpy.hpp>
14+
#include <eigenpy/std-unique-ptr.hpp>
1415
#include <eigenpy/std-vector.hpp>
1516

1617
#include "simple-mpc/mpc.hpp"
@@ -24,7 +25,7 @@ namespace simple_mpc
2425
namespace bp = boost::python;
2526
using eigenpy::StdVectorPythonVisitor;
2627

27-
void initialize(MPC & self, const bp::dict & settings, std::shared_ptr<OCPHandler> problem)
28+
MPC * createMPC(const bp::dict & settings, std::shared_ptr<OCPHandler> problem)
2829
{
2930
MPCSettings conf;
3031

@@ -42,7 +43,7 @@ namespace simple_mpc
4243
conf.T_contact = bp::extract<int>(settings["T_contact"]);
4344
conf.timestep = bp::extract<double>(settings["timestep"]);
4445

45-
self.initialize(conf, problem);
46+
return new MPC{conf, problem};
4647
}
4748

4849
bp::dict getSettings(MPC & self)
@@ -72,9 +73,8 @@ namespace simple_mpc
7273

7374
StdVectorPythonVisitor<std::vector<MapBool>, true>::expose("StdVec_MapBool");
7475

75-
bp::class_<MPC>("MPC", bp::no_init)
76-
.def(bp::init<>(bp::args("self")))
77-
.def("initialize", &initialize)
76+
bp::class_<MPC, boost::noncopyable>("MPC", bp::no_init)
77+
.def("__init__", bp::make_constructor(&createMPC, bp::default_call_policies()))
7878
.def("getSettings", &getSettings)
7979
.def("generateCycleHorizon", &MPC::generateCycleHorizon, bp::args("self", "contact_states"))
8080
.def("iterate", &MPC::iterate, bp::args("self", "x"))
@@ -103,11 +103,7 @@ namespace simple_mpc
103103
.def(
104104
"getCycleHorizon", &MPC::getCycleHorizon, "self"_a, bp::return_internal_reference<>(),
105105
"Get the cycle horizon.")
106-
.def(
107-
"getSolver", &MPC::getSolver, "self"_a,
108-
eigenpy::deprecated_member<eigenpy::DeprecationType::DEPRECATION, bp::return_internal_reference<>>(),
109-
"Get the SolverProxDDP object")
110-
.def_readonly("solver", &MPC::solver_)
106+
.add_property("solver", bp::make_getter(&MPC::solver_, eigenpy::ReturnInternalStdUniquePtr{}))
111107
.add_property("xs", &MPC::xs_)
112108
.add_property("us", &MPC::us_)
113109
.add_property("Ks", &MPC::Ks_);

‎bindings/expose-robot-handler.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@ namespace simple_mpc
2828
.def("difference", &RobotModelHandler::difference)
2929
.def("shapeState", &RobotModelHandler::shapeState)
3030
.def("getBaseFrameId", &RobotModelHandler::getBaseFrameId)
31-
.def("getReferenceState", &RobotModelHandler::getReferenceState, bp::return_internal_reference<>())
31+
.def("getReferenceState", &RobotModelHandler::getReferenceState)
3232
.def("getFootNb", &RobotModelHandler::getFootNb)
3333
.def("getFeetIds", &RobotModelHandler::getFeetIds, bp::return_internal_reference<>())
3434
.def("getFootName", &RobotModelHandler::getFootName, bp::return_internal_reference<>())

‎examples/go2_fulldynamics.py‎

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -83,8 +83,7 @@
8383
timestep=dt,
8484
)
8585

86-
mpc = MPC()
87-
mpc.initialize(mpc_conf, dynproblem)
86+
mpc = MPC(mpc_conf, dynproblem)
8887

8988
""" Define contact sequence throughout horizon"""
9089
contact_phase_quadru = {
@@ -223,17 +222,17 @@
223222
solve_time.append(end - start)
224223

225224
a0 = (
226-
mpc.getSolver()
225+
mpc.solver
227226
.workspace.problem_data.stage_data[0]
228227
.dynamics_data.continuous_data.xdot[nv:]
229228
)
230229
a1 = (
231-
mpc.getSolver()
230+
mpc.solver
232231
.workspace.problem_data.stage_data[1]
233232
.dynamics_data.continuous_data.xdot[nv:]
234233
)
235234

236-
FL_f, FR_f, RL_f, RR_f, contact_states = extract_forces(mpc.getTrajOptProblem(), mpc.getSolver().workspace, 0)
235+
FL_f, FR_f, RL_f, RR_f, contact_states = extract_forces(mpc.getTrajOptProblem(), mpc.solver.workspace, 0)
237236
total_forces = np.concatenate((FL_f, FR_f, RL_f, RR_f))
238237
force_FL.append(FL_f)
239238
force_FR.append(FR_f)

‎examples/go2_kinodynamics.py‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -93,8 +93,7 @@
9393
timestep=0.01,
9494
)
9595

96-
mpc = MPC()
97-
mpc.initialize(mpc_conf, dynproblem)
96+
mpc = MPC(mpc_conf, dynproblem)
9897

9998
""" Define contact sequence throughout horizon"""
10099
contact_phase_quadru = {
@@ -227,12 +226,12 @@
227226
L_measured.append(mpc.getDataHandler().getData().hg.angular.copy())
228227

229228
a0 = (
230-
mpc.getSolver()
229+
mpc.solver
231230
.workspace.problem_data.stage_data[0]
232231
.dynamics_data.continuous_data.xdot[nv:]
233232
)
234233
a1 = (
235-
mpc.getSolver()
234+
mpc.solver
236235
.workspace.problem_data.stage_data[1]
237236
.dynamics_data.continuous_data.xdot[nv:]
238237
)

‎examples/talos_centroidal.py‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -87,8 +87,7 @@
8787
timestep=problem_conf["timestep"],
8888
)
8989

90-
mpc = MPC()
91-
mpc.initialize(mpc_conf, problem)
90+
mpc = MPC(mpc_conf, problem)
9291

9392
""" Define contact sequence throughout horizon"""
9493
contact_phase_double = {
@@ -210,7 +209,7 @@
210209
foot_ref = [mpc.getReferencePose(0, name) for name in model_handler.getFeetNames()]
211210
foot_ref_next = [mpc.getReferencePose(1, name) for name in model_handler.getFeetNames()]
212211
dH = (
213-
mpc.getSolver()
212+
mpc.solver
214213
.workspace.problem_data.stage_data[0]
215214
.dynamics_data.continuous_data.xdot[3:9]
216215
)
@@ -229,7 +228,7 @@
229228

230229
forces = (
231230
mpc.us[0][: nk * force_size]
232-
- 1 * mpc.getSolver().results.controlFeedbacks()[0] @ state_diff
231+
- 1 * mpc.solver.results.controlFeedbacks()[0] @ state_diff
233232
)
234233

235234

‎examples/talos_fulldynamics.py‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -112,8 +112,7 @@
112112
timestep=problem_conf["timestep"],
113113
)
114114

115-
mpc = MPC()
116-
mpc.initialize(mpc_conf, dynproblem)
115+
mpc = MPC(mpc_conf, dynproblem)
117116

118117
""" Define contact sequence throughout horizon"""
119118
contact_phase_double = {

‎examples/talos_kinodynamics.py‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -123,8 +123,7 @@
123123
timestep=problem_conf["timestep"],
124124
)
125125

126-
mpc = MPC()
127-
mpc.initialize(mpc_conf, problem)
126+
mpc = MPC(mpc_conf, problem)
128127

129128
""" Define contact sequence throughout horizon"""
130129
contact_phase_double = {
@@ -212,12 +211,12 @@
212211
end = time.time()
213212
print("MPC iterate = " + str(end - start))
214213
a0 = (
215-
mpc.getSolver()
214+
mpc.solver
216215
.workspace.problem_data.stage_data[0]
217216
.dynamics_data.continuous_data.xdot[nv:]
218217
)
219218
a1 = (
220-
mpc.getSolver()
219+
mpc.solver
221220
.workspace.problem_data.stage_data[1]
222221
.dynamics_data.continuous_data.xdot[nv:]
223222
)

‎include/simple-mpc/centroidal-dynamics.hpp‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -87,14 +87,13 @@ namespace simple_mpc
8787

8888
// Getters and setters for contact forces
8989
void setReferenceForces(const std::size_t t, const std::map<std::string, Eigen::VectorXd> & force_refs) override;
90-
void
91-
setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref) override;
90+
void setReferenceForce(const std::size_t t, const std::string & ee_name, const ConstVectorRef & force_ref) override;
9291
const Eigen::VectorXd getReferenceForce(const std::size_t t, const std::string & ee_name) override;
9392
void computeControlFromForces(const std::map<std::string, Eigen::VectorXd> & force_refs);
9493
const Eigen::VectorXd getVelocityBase(const std::size_t t) override;
95-
void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override;
94+
void setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) override;
9695
const Eigen::VectorXd getPoseBase(const std::size_t t) override;
97-
void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override;
96+
void setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) override;
9897
const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
9998
size_t getContactSupport(const std::size_t t) override;
10099

‎include/simple-mpc/fulldynamics.hpp‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -92,14 +92,13 @@ namespace simple_mpc
9292
void setReferencePoses(const std::size_t t, const std::map<std::string, pinocchio::SE3> & pose_refs) override;
9393
void setTerminalReferencePose(const std::string & ee_name, const pinocchio::SE3 & pose_ref) override;
9494
void setReferenceForces(const std::size_t t, const std::map<std::string, Eigen::VectorXd> & force_refs) override;
95-
void
96-
setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref) override;
95+
void setReferenceForce(const std::size_t t, const std::string & ee_name, const ConstVectorRef & force_ref) override;
9796
const pinocchio::SE3 getReferencePose(const std::size_t t, const std::string & cost_name) override;
9897
const Eigen::VectorXd getReferenceForce(const std::size_t t, const std::string & cost_name) override;
9998
const Eigen::VectorXd getVelocityBase(const std::size_t t) override;
100-
void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override;
99+
void setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) override;
101100
const Eigen::VectorXd getPoseBase(const std::size_t t) override;
102-
void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override;
101+
void setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) override;
103102
const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
104103
size_t getContactSupport(const std::size_t t) override;
105104
FullDynamicsSettings getSettings()

‎include/simple-mpc/kinodynamics.hpp‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -80,14 +80,13 @@ namespace simple_mpc
8080
void setReferencePoses(const std::size_t i, const std::map<std::string, pinocchio::SE3> & pose_refs) override;
8181
void setTerminalReferencePose(const std::string & ee_name, const pinocchio::SE3 & pose_ref) override;
8282
void setReferenceForces(const std::size_t i, const std::map<std::string, Eigen::VectorXd> & force_refs) override;
83-
void
84-
setReferenceForce(const std::size_t i, const std::string & ee_name, const Eigen::VectorXd & force_ref) override;
83+
void setReferenceForce(const std::size_t i, const std::string & ee_name, const ConstVectorRef & force_ref) override;
8584
const Eigen::VectorXd getReferenceForce(const std::size_t i, const std::string & cost_name) override;
8685
const pinocchio::SE3 getReferencePose(const std::size_t i, const std::string & cost_name) override;
8786
const Eigen::VectorXd getVelocityBase(const std::size_t t) override;
8887
const Eigen::VectorXd getPoseBase(const std::size_t t) override;
89-
void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override;
90-
void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override;
88+
void setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) override;
89+
void setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) override;
9190
const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
9291
size_t getContactSupport(const std::size_t t) override;
9392

0 commit comments

Comments
 (0)