Skip to content

Commit 145732f

Browse files
author
earlaud
committed
Mega clean : RobotHandlers only deals with foot nb, names only used to get foot nb + member variables named more explicitely
1 parent 89b411d commit 145732f

18 files changed

+264
-268
lines changed

‎benchmark/talos.cpp‎

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -152,36 +152,36 @@ int main()
152152
for (std::size_t i = 0; i < 10; i++)
153153
{
154154
std::map<std::string, bool> contact_state;
155-
contact_state.insert({model_handler.getFootName(0), true});
156-
contact_state.insert({model_handler.getFootName(1), true});
155+
contact_state.insert({model_handler.getFootFrameName(0), true});
156+
contact_state.insert({model_handler.getFootFrameName(1), true});
157157
contact_states.push_back(contact_state);
158158
}
159159
for (std::size_t i = 0; i < 50; i++)
160160
{
161161
std::map<std::string, bool> contact_state;
162-
contact_state.insert({model_handler.getFootName(0), true});
163-
contact_state.insert({model_handler.getFootName(1), false});
162+
contact_state.insert({model_handler.getFootFrameName(0), true});
163+
contact_state.insert({model_handler.getFootFrameName(1), false});
164164
contact_states.push_back(contact_state);
165165
}
166166
for (std::size_t i = 0; i < 10; i++)
167167
{
168168
std::map<std::string, bool> contact_state;
169-
contact_state.insert({model_handler.getFootName(0), true});
170-
contact_state.insert({model_handler.getFootName(1), true});
169+
contact_state.insert({model_handler.getFootFrameName(0), true});
170+
contact_state.insert({model_handler.getFootFrameName(1), true});
171171
contact_states.push_back(contact_state);
172172
}
173173
for (std::size_t i = 0; i < 50; i++)
174174
{
175175
std::map<std::string, bool> contact_state;
176-
contact_state.insert({model_handler.getFootName(0), false});
177-
contact_state.insert({model_handler.getFootName(1), true});
176+
contact_state.insert({model_handler.getFootFrameName(0), false});
177+
contact_state.insert({model_handler.getFootFrameName(1), true});
178178
contact_states.push_back(contact_state);
179179
}
180180
for (std::size_t i = 0; i < 10; i++)
181181
{
182182
std::map<std::string, bool> contact_state;
183-
contact_state.insert({model_handler.getFootName(0), true});
184-
contact_state.insert({model_handler.getFootName(1), true});
183+
contact_state.insert({model_handler.getFootFrameName(0), true});
184+
contact_state.insert({model_handler.getFootFrameName(1), true});
185185
contact_states.push_back(contact_state);
186186
}
187187

‎bindings/expose-robot-handler.cpp‎

Lines changed: 7 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -31,11 +31,11 @@ namespace simple_mpc
3131
.def("getBaseFrameId", &RobotModelHandler::getBaseFrameId)
3232
.def("getReferenceState", &RobotModelHandler::getReferenceState)
3333
.def("getFootNb", &RobotModelHandler::getFootNb)
34-
.def("getFeetIds", &RobotModelHandler::getFeetIds, bp::return_internal_reference<>())
35-
.def("getFootName", &RobotModelHandler::getFootName, bp::return_internal_reference<>())
36-
.def("getFeetNames", &RobotModelHandler::getFeetNames, bp::return_internal_reference<>())
37-
.def("getFootId", &RobotModelHandler::getFootId)
38-
.def("getRefFootId", &RobotModelHandler::getRefFootId)
34+
.def("getFeetFrameIds", &RobotModelHandler::getFeetFrameIds, bp::return_internal_reference<>())
35+
.def("getFootFrameName", &RobotModelHandler::getFootFrameName, bp::return_internal_reference<>())
36+
.def("getFeetFrameNames", &RobotModelHandler::getFeetFrameNames, bp::return_internal_reference<>())
37+
.def("getFootFrameId", &RobotModelHandler::getFootFrameId)
38+
.def("getFootRefFrameId", &RobotModelHandler::getFootRefFrameId)
3939
.def("getMass", &RobotModelHandler::getMass)
4040
.def("getModel", &RobotModelHandler::getModel, bp::return_internal_reference<>());
4141

@@ -47,24 +47,8 @@ namespace simple_mpc
4747
"updateInternalData", static_cast<void (RobotDataHandler::*)(const ConstVectorRef &, const bool)>(
4848
&RobotDataHandler::updateInternalData))
4949
.def("updateJacobiansMassMatrix", &RobotDataHandler::updateJacobiansMassMatrix)
50-
.def(
51-
"getRefFootPose",
52-
static_cast<const pinocchio::SE3 & (RobotDataHandler::*)(size_t) const>(&RobotDataHandler::getRefFootPose),
53-
bp::return_internal_reference<>())
54-
.def(
55-
"getRefFootPoseByName",
56-
static_cast<const pinocchio::SE3 & (RobotDataHandler::*)(const std::string &) const>(
57-
&RobotDataHandler::getRefFootPose),
58-
bp::return_internal_reference<>())
59-
.def(
60-
"getFootPose",
61-
static_cast<const pinocchio::SE3 & (RobotDataHandler::*)(size_t) const>(&RobotDataHandler::getFootPose),
62-
bp::return_internal_reference<>())
63-
.def(
64-
"getFootPoseByName",
65-
static_cast<const pinocchio::SE3 & (RobotDataHandler::*)(const std::string &) const>(
66-
&RobotDataHandler::getFootPose),
67-
bp::return_internal_reference<>())
50+
.def("getFootRefPose", &RobotDataHandler::getFootRefPose, bp::return_internal_reference<>())
51+
.def("getFootPose", &RobotDataHandler::getFootPose, bp::return_internal_reference<>())
6852
.def("getBaseFramePose", &RobotDataHandler::getBaseFramePose, bp::return_internal_reference<>())
6953
.def("getModelHandler", &RobotDataHandler::getModelHandler, bp::return_internal_reference<>())
7054
.def("getData", &RobotDataHandler::getData, bp::return_internal_reference<>())

‎examples/go2_fulldynamics.py‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@
3838
nv = model_handler.getModel().nv
3939
nu = nv - 6
4040
force_size = 3
41-
nk = len(model_handler.getFeetNames())
41+
nk = len(model_handler.getFeetFrameNames())
4242
nf = force_size
4343

4444
gravity = np.array([0, 0, -9.81])
@@ -164,7 +164,7 @@
164164
x_measured = np.concatenate([q_meas, v_meas])
165165
mpc.getDataHandler().updateInternalData(x_measured, False)
166166

167-
ref_foot_pose = [mpc.getDataHandler().getRefFootPose(i) for i in range(4)]
167+
ref_foot_pose = [mpc.getDataHandler().getFootRefPose(i) for i in range(4)]
168168
for pose in ref_foot_pose:
169169
pose.translation[2] = 0
170170
device.showQuadrupedFeet(*ref_foot_pose)
@@ -239,10 +239,10 @@
239239
xss = [mpc.xs[0], mpc.xs[1]]
240240
uss = [mpc.us[0], mpc.us[1]]
241241

242-
FL_measured.append(mpc.getDataHandler().getFootPoseByName("FL_foot").translation)
243-
FR_measured.append(mpc.getDataHandler().getFootPoseByName("FR_foot").translation)
244-
RL_measured.append(mpc.getDataHandler().getFootPoseByName("RL_foot").translation)
245-
RR_measured.append(mpc.getDataHandler().getFootPoseByName("RR_foot").translation)
242+
FL_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("FL_foot")).translation)
243+
FR_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("FR_foot")).translation)
244+
RL_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("RL_foot")).translation)
245+
RR_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("RR_foot")).translation)
246246
FL_references.append(mpc.getReferencePose(0, "FL_foot").translation)
247247
FR_references.append(mpc.getReferencePose(0, "FR_foot").translation)
248248
RL_references.append(mpc.getReferencePose(0, "RL_foot").translation)

‎examples/go2_kinodynamics.py‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@
2929
nu = nv - 6
3030
nf = 12
3131
force_size = 3
32-
nk = len(model_handler.getFeetNames())
32+
nk = len(model_handler.getFeetFrameNames())
3333
gravity = np.array([0, 0, -9.81])
3434
fref = np.zeros(force_size)
3535
fref[2] = -model_handler.getMass() / nk * gravity[2]
@@ -168,10 +168,10 @@
168168
x_measured = np.concatenate([q_meas, v_meas])
169169

170170
device.showQuadrupedFeet(
171-
mpc.getDataHandler().getFootPoseByName("FL_foot"),
172-
mpc.getDataHandler().getFootPoseByName("FR_foot"),
173-
mpc.getDataHandler().getFootPoseByName("RL_foot"),
174-
mpc.getDataHandler().getFootPoseByName("RR_foot"),
171+
mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("FL_foot")),
172+
mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("FR_foot")),
173+
mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("RL_foot")),
174+
mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("RR_foot")),
175175
)
176176

177177
force_FL = []
@@ -217,10 +217,10 @@
217217
force_RL.append(mpc.us[0][6:9])
218218
force_RR.append(mpc.us[0][9:12])
219219

220-
FL_measured.append(mpc.getDataHandler().getFootPoseByName("FL_foot").translation)
221-
FR_measured.append(mpc.getDataHandler().getFootPoseByName("FR_foot").translation)
222-
RL_measured.append(mpc.getDataHandler().getFootPoseByName("RL_foot").translation)
223-
RR_measured.append(mpc.getDataHandler().getFootPoseByName("RR_foot").translation)
220+
FL_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("FL_foot")).translation)
221+
FR_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("FR_foot")).translation)
222+
RL_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("RL_foot")).translation)
223+
RR_measured.append(mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("RR_foot")).translation)
224224
FL_references.append(mpc.getReferencePose(0, "FL_foot").translation)
225225
FR_references.append(mpc.getReferencePose(0, "FR_foot").translation)
226226
RL_references.append(mpc.getReferencePose(0, "RL_foot").translation)

‎examples/talos_centroidal.py‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -24,11 +24,11 @@
2424

2525
x0 = np.zeros(9)
2626
x0[:3] = data_handler.getData().com[0]
27-
nu = model_handler.getModel().nv - 6 + len(model_handler.getFeetNames()) * 6
27+
nu = model_handler.getModel().nv - 6 + len(model_handler.getFeetFrameNames()) * 6
2828

2929
gravity = np.array([0, 0, -9.81])
3030
fref = np.zeros(6)
31-
fref[2] = -model_handler.getMass() / len(model_handler.getFeetNames()) * gravity[2]
31+
fref[2] = -model_handler.getMass() / len(model_handler.getFeetFrameNames()) * gravity[2]
3232
u0 = np.concatenate((fref, fref))
3333

3434
w_control_linear = np.ones(3) * 0.001
@@ -125,8 +125,8 @@
125125
x_centroidal = mpc.getDataHandler().getCentroidalState()
126126

127127
device.showTargetToTrack(
128-
mpc.getDataHandler().getFootPoseByName("left_sole_link"),
129-
mpc.getDataHandler().getFootPoseByName("right_sole_link"),
128+
mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("left_sole_link")),
129+
mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("right_sole_link")),
130130
)
131131

132132
v = np.zeros(6)
@@ -157,8 +157,8 @@
157157
)
158158

159159
contact_states = mpc.ocp_handler.getContactState(0)
160-
foot_ref = [mpc.getReferencePose(0, name) for name in model_handler.getFeetNames()]
161-
foot_ref_next = [mpc.getReferencePose(1, name) for name in model_handler.getFeetNames()]
160+
foot_ref = [mpc.getReferencePose(0, name) for name in model_handler.getFeetFrameNames()]
161+
foot_ref_next = [mpc.getReferencePose(1, name) for name in model_handler.getFeetFrameNames()]
162162
dH = mpc.getStateDerivative(0)[3:9]
163163

164164
for j in range(10):

‎examples/talos_fulldynamics.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -158,8 +158,8 @@
158158
takeoff_LF = -1
159159
takeoff_RF = -1
160160
device.showTargetToTrack(
161-
mpc.getDataHandler().getFootPoseByName("left_sole_link"),
162-
mpc.getDataHandler().getFootPoseByName("right_sole_link"),
161+
mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("left_sole_link")),
162+
mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("right_sole_link")),
163163
)
164164

165165
v = np.zeros(6)

‎examples/talos_kinodynamics.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@
3636
""" Define kinodynamics problem """
3737
gravity = np.array([0, 0, -9.81])
3838
fref = np.zeros(6)
39-
fref[2] = -model_handler.getMass() / len(model_handler.getFeetNames()) * gravity[2]
39+
fref[2] = -model_handler.getMass() / len(model_handler.getFeetFrameNames()) * gravity[2]
4040
u0 = np.concatenate((fref, fref, np.zeros(nv - 6)))
4141

4242
w_basepos = [0, 0, 1000, 1000, 1000, 1000]
@@ -182,8 +182,8 @@
182182
force_size = 6
183183

184184
device.showTargetToTrack(
185-
mpc.getDataHandler().getFootPoseByName("left_sole_link"),
186-
mpc.getDataHandler().getFootPoseByName("right_sole_link"),
185+
mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("left_sole_link")),
186+
mpc.getDataHandler().getFootPose(mpc.getModelHandler().getFootNb("right_sole_link")),
187187
)
188188

189189
v = np.zeros(6)

0 commit comments

Comments
 (0)