diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index 801afd8..9c80201 100644 --- a/src/pyhpp/manipulation/device.cc +++ b/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(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(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) + 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 diff --git a/src/pyhpp/manipulation/device.hh b/src/pyhpp/manipulation/device.hh index ed22d4d..23cabda 100644 --- a/src/pyhpp/manipulation/device.hh +++ b/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 diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 678a5f2..eb6e472 100644 --- a/tests/CMakeLists.txt +++ b/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 . -# ============================================================================= -# 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) # =============================================================================