Skip to content

Commit f915d5d

Browse files
author
earlaud
committed
Bind get[Ref]FootPose[ByName]
1 parent 73dc091 commit f915d5d

File tree

6 files changed

+37
-21
lines changed

6 files changed

+37
-21
lines changed

‎bindings/expose-robot-handler.cpp‎

Lines changed: 18 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -46,8 +46,24 @@ namespace simple_mpc
4646
"updateInternalData", static_cast<void (RobotDataHandler::*)(const ConstVectorRef &, const bool)>(
4747
&RobotDataHandler::updateInternalData))
4848
.def("updateJacobiansMassMatrix", &RobotDataHandler::updateJacobiansMassMatrix)
49-
// .def("getRefFootPose", &RobotDataHandler::getRefFootPose, bp::return_internal_reference<>())
50-
// .def("getFootPose", &RobotDataHandler::getFootPose, bp::return_internal_reference<>())
49+
.def(
50+
"getRefFootPose",
51+
static_cast<const pinocchio::SE3 & (RobotDataHandler::*)(size_t) const>(&RobotDataHandler::getRefFootPose),
52+
bp::return_internal_reference<>())
53+
.def(
54+
"getRefFootPoseByName",
55+
static_cast<const pinocchio::SE3 & (RobotDataHandler::*)(const std::string &) const>(
56+
&RobotDataHandler::getRefFootPose),
57+
bp::return_internal_reference<>())
58+
.def(
59+
"getFootPose",
60+
static_cast<const pinocchio::SE3 & (RobotDataHandler::*)(size_t) const>(&RobotDataHandler::getFootPose),
61+
bp::return_internal_reference<>())
62+
.def(
63+
"getFootPoseByName",
64+
static_cast<const pinocchio::SE3 & (RobotDataHandler::*)(const std::string &) const>(
65+
&RobotDataHandler::getFootPose),
66+
bp::return_internal_reference<>())
5167
.def("getBaseFramePose", &RobotDataHandler::getBaseFramePose, bp::return_internal_reference<>())
5268
.def("getModelHandler", &RobotDataHandler::getModelHandler, bp::return_internal_reference<>())
5369
.def("getData", &RobotDataHandler::getData, bp::return_internal_reference<>())

‎examples/go2_fulldynamics.py‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -183,7 +183,7 @@
183183
x_measured = np.concatenate([q_meas, v_meas])
184184
mpc.getDataHandler().updateInternalData(x_measured, False)
185185

186-
ref_foot_pose = [mpc.getDataHandler().getRefFootPose(mpc.getModelHandler().getFeetNames()[i]) for i in range(4)]
186+
ref_foot_pose = [mpc.getDataHandler().getRefFootPose(i) for i in range(4)]
187187
for pose in ref_foot_pose:
188188
pose.translation[2] = 0
189189
device.showQuadrupedFeet(*ref_foot_pose)
@@ -258,10 +258,10 @@
258258
xss = [mpc.xs[0], mpc.xs[1]]
259259
uss = [mpc.us[0], mpc.us[1]]
260260

261-
FL_measured.append(mpc.getDataHandler().getFootPose("FL_foot").translation)
262-
FR_measured.append(mpc.getDataHandler().getFootPose("FR_foot").translation)
263-
RL_measured.append(mpc.getDataHandler().getFootPose("RL_foot").translation)
264-
RR_measured.append(mpc.getDataHandler().getFootPose("RR_foot").translation)
261+
FL_measured.append(mpc.getDataHandler().getFootPoseByName("FL_foot").translation)
262+
FR_measured.append(mpc.getDataHandler().getFootPoseByName("FR_foot").translation)
263+
RL_measured.append(mpc.getDataHandler().getFootPoseByName("RL_foot").translation)
264+
RR_measured.append(mpc.getDataHandler().getFootPoseByName("RR_foot").translation)
265265
FL_references.append(mpc.getReferencePose(0, "FL_foot").translation)
266266
FR_references.append(mpc.getReferencePose(0, "FR_foot").translation)
267267
RL_references.append(mpc.getReferencePose(0, "RL_foot").translation)

‎examples/go2_kinodynamics.py‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -172,10 +172,10 @@
172172
x_measured = np.concatenate([q_meas, v_meas])
173173

174174
device.showQuadrupedFeet(
175-
mpc.getDataHandler().getFootPose("FL_foot"),
176-
mpc.getDataHandler().getFootPose("FR_foot"),
177-
mpc.getDataHandler().getFootPose("RL_foot"),
178-
mpc.getDataHandler().getFootPose("RR_foot"),
175+
mpc.getDataHandler().getFootPoseByName("FL_foot"),
176+
mpc.getDataHandler().getFootPoseByName("FR_foot"),
177+
mpc.getDataHandler().getFootPoseByName("RL_foot"),
178+
mpc.getDataHandler().getFootPoseByName("RR_foot"),
179179
)
180180

181181
force_FL = []
@@ -222,10 +222,10 @@
222222
force_RL.append(mpc.us[0][6:9])
223223
force_RR.append(mpc.us[0][9:12])
224224

225-
FL_measured.append(mpc.getDataHandler().getFootPose("FL_foot").translation)
226-
FR_measured.append(mpc.getDataHandler().getFootPose("FR_foot").translation)
227-
RL_measured.append(mpc.getDataHandler().getFootPose("RL_foot").translation)
228-
RR_measured.append(mpc.getDataHandler().getFootPose("RR_foot").translation)
225+
FL_measured.append(mpc.getDataHandler().getFootPoseByName("FL_foot").translation)
226+
FR_measured.append(mpc.getDataHandler().getFootPoseByName("FR_foot").translation)
227+
RL_measured.append(mpc.getDataHandler().getFootPoseByName("RL_foot").translation)
228+
RR_measured.append(mpc.getDataHandler().getFootPoseByName("RR_foot").translation)
229229
FL_references.append(mpc.getReferencePose(0, "FL_foot").translation)
230230
FR_references.append(mpc.getReferencePose(0, "FR_foot").translation)
231231
RL_references.append(mpc.getReferencePose(0, "RL_foot").translation)

‎examples/talos_centroidal.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -161,8 +161,8 @@
161161
x_centroidal = mpc.getDataHandler().getCentroidalState()
162162

163163
device.showTargetToTrack(
164-
mpc.getDataHandler().getFootPose("left_sole_link"),
165-
mpc.getDataHandler().getFootPose("right_sole_link"),
164+
mpc.getDataHandler().getFootPoseByName("left_sole_link"),
165+
mpc.getDataHandler().getFootPoseByName("right_sole_link"),
166166
)
167167

168168
v = np.zeros(6)

‎examples/talos_fulldynamics.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -152,8 +152,8 @@
152152
takeoff_LF = -1
153153
takeoff_RF = -1
154154
device.showTargetToTrack(
155-
mpc.getDataHandler().getFootPose("left_sole_link"),
156-
mpc.getDataHandler().getFootPose("right_sole_link"),
155+
mpc.getDataHandler().getFootPoseByName("left_sole_link"),
156+
mpc.getDataHandler().getFootPoseByName("right_sole_link"),
157157
)
158158

159159
v = np.zeros(6)

‎examples/talos_kinodynamics.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -174,8 +174,8 @@
174174
force_size = 6
175175

176176
device.showTargetToTrack(
177-
mpc.getDataHandler().getFootPose("left_sole_link"),
178-
mpc.getDataHandler().getFootPose("right_sole_link"),
177+
mpc.getDataHandler().getFootPoseByName("left_sole_link"),
178+
mpc.getDataHandler().getFootPoseByName("right_sole_link"),
179179
)
180180

181181
v = np.zeros(6)

0 commit comments

Comments
 (0)