Expose contact surfaces from device

src/pyhpp/manipulation/device.cc

@@ -84,6 +84,46 @@ void Device::setJointBounds(const char* jointName,
     joint->upperBound(i, vMax);  // Set upper bound for DOF i
   }
 }
+
+boost::python::list Device::contactSurfaceNames() {
+  auto manipDevice = std::dynamic_pointer_cast<hpp::manipulation::Device>(obj);
+  boost::python::list result;
+  for (const auto& entry : manipDevice->jointAndShapes.map) {
+    result.append(entry.first);
+  }
+  return result;
+}
+
+boost::python::dict Device::contactSurfaces() {
+  auto manipDevice = std::dynamic_pointer_cast<hpp::manipulation::Device>(obj);
+  boost::python::dict result;
+
+  for (const auto& entry : manipDevice->jointAndShapes.map) {
+    const std::string& name = entry.first;
+    const auto& jointAndShapes = entry.second;
+    boost::python::list surfacesList;
+
+    for (const auto& jas : jointAndShapes) {
+      boost::python::dict surfaceDict;
+      // jas.first is JointPtr_t, jas.second is Shape_t (vector<vector3_t>)
+      std::string jointName = jas.first ? jas.first->name() : "universe";
+      surfaceDict["joint"] = jointName;
+
+      boost::python::list points;
+      for (const auto& pt : jas.second) {
+        boost::python::list point;
+        point.append(pt[0]);
+        point.append(pt[1]);
+        point.append(pt[2]);
+        points.append(point);
+      }
+      surfaceDict["points"] = points;
+      surfacesList.append(surfaceDict);
+    }
+    result[name] = surfacesList;
+  }
+  return result;
+}
 boost::python::list Device::getJointConfig(const char* jointName) {
   try {
     Frame frame = obj->getFrameByName(jointName);
@@ -184,7 +224,11 @@ void exposeDevice() {
       .def("asPinDevice", &asPinDevice)
       .def("getJointNames", &Device::getJointNames)
       .def("getJointConfig", &Device::getJointConfig)
-      .def("setJointBounds", &Device::setJointBounds);
+      .def("setJointBounds", &Device::setJointBounds)
+      .def("contactSurfaceNames", &Device::contactSurfaceNames,
+           "Return list of contact surface names registered on device")
+      .def("contactSurfaces", &Device::contactSurfaces,
+           "Return dict mapping surface names to list of {joint, points}");
 }
 }  // namespace manipulation
 }  // namespace pyhpp

src/pyhpp/manipulation/device.hh

@@ -83,6 +83,8 @@ struct Device : public pyhpp::pinocchio::Device {
   boost::python::list getJointConfig(const char* jointName);
   boost::python::list getJointNames();
   void setJointBounds(const char* jointName, boost::python::list jointBounds);
+  boost::python::list contactSurfaceNames();
+  boost::python::dict contactSurfaces();
 };  // struct Device
 }  // namespace manipulation
 }  // namespace pyhpp

tests/CMakeLists.txt

@@ -12,21 +12,6 @@
 # details.  You should have received a copy of the GNU Lesser General Public
 # License along with hpp-python  If not, see <http://www.gnu.org/licenses/>.
 
-# =============================================================================
-# Existing Python unit tests (run as simple scripts)
-# =============================================================================
-
-foreach(UNIT_TEST ${PYTHON_UNIT_TESTS})
-  add_python_unit_test(${UNIT_TEST} tests/${UNIT_TEST}.py src)
-endforeach()
-
-set_tests_properties(
-  ${PYTHON_UNIT_TESTS}
-  PROPERTIES
-    ENVIRONMENT_MODIFICATION
-    "ROS_PACKAGE_PATH=path_list_prepend:${example-robot-data_INCLUDE_DIRS}/../share;ROS_PACKAGE_PATH=path_list_prepend:${hpp-environments_INCLUDE_DIRS}/../share;"
-)
-
 # =============================================================================
 # unittest-based unit tests (uses Python's built-in unittest framework)
 # =============================================================================