Use getFeetNb instead of doing len() by hand

Author: EtienneAr

bindings/expose-robot-handler.cpp

@@ -30,6 +30,7 @@ namespace simple_mpc
         .def("getBaseFrameName", &RobotModelHandler::getBaseFrameName)
         .def("getBaseFrameId", &RobotModelHandler::getBaseFrameId)
         .def("getReferenceState", &RobotModelHandler::getReferenceState)
+        .def("getFeetNb", &RobotModelHandler::getFeetNb)
         .def("getFootNb", &RobotModelHandler::getFootNb)
         .def("getFeetFrameIds", &RobotModelHandler::getFeetFrameIds, bp::return_internal_reference<>())
         .def("getFootFrameName", &RobotModelHandler::getFootFrameName, bp::return_internal_reference<>())

examples/go2_fulldynamics.py

@@ -34,7 +34,7 @@
 nv = model_handler.getModel().nv
 nu = nv - 6
 force_size = 3
-nk = len(model_handler.getFeetFrameNames())
+nk = model_handler.getFeetNb()
 nf = force_size
 
 gravity = np.array([0, 0, -9.81])

examples/go2_kinodynamics.py

@@ -25,7 +25,7 @@
 nu = nv - 6
 nf = 12
 force_size = 3
-nk = len(model_handler.getFeetFrameNames())
+nk = model_handler.getFeetNb()
 gravity = np.array([0, 0, -9.81])
 fref = np.zeros(force_size)
 fref[2] = -model_handler.getMass() / nk * gravity[2]

examples/talos_centroidal.py

@@ -29,11 +29,11 @@
 
 x0 = np.zeros(9)
 x0[:3] = data_handler.getData().com[0]
-nu = model_handler.getModel().nv - 6 + len(model_handler.getFeetFrameNames()) * 6
+nu = model_handler.getModel().nv - 6 + model_handler.getFeetNb() * 6
 
 gravity = np.array([0, 0, -9.81])
 fref = np.zeros(6)
-fref[2] = -model_handler.getMass() / len(model_handler.getFeetFrameNames()) * gravity[2]
+fref[2] = -model_handler.getMass() / model_handler.getFeetNb() * gravity[2]
 u0 = np.concatenate((fref, fref))
 
 w_control_linear = np.ones(3) * 0.001

examples/talos_kinodynamics.py

@@ -34,7 +34,7 @@
 """ Define kinodynamics problem """
 gravity = np.array([0, 0, -9.81])
 fref = np.zeros(6)
-fref[2] = -model_handler.getMass() / len(model_handler.getFeetFrameNames()) * gravity[2]
+fref[2] = -model_handler.getMass() / model_handler.getFeetNb() * gravity[2]
 u0 = np.concatenate((fref, fref, np.zeros(nv - 6)))
 
 w_basepos = [0, 0, 1000, 1000, 1000, 1000]