Merge pull request RobotLocomotion#11583 from mpetersen94/bind/frame_…

…pose_vector pydrake geometry: Add bindings for FramePoseVector functions
YinjieLin · Jun 5, 2019 · 0d09623 · 0d09623
2 parents 57649ae + e6088f3
commit 0d09623
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 2 deletions.
diff --git a/bindings/pydrake/geometry_py.cc b/bindings/pydrake/geometry_py.cc
@@ -82,7 +82,21 @@ PYBIND11_MODULE(geometry, m) {
         return std::make_unique<FramePoseVector<T>>(source_id, ids);
       }),
           py::arg("source_id"), py::arg("ids"),
-          doc.FrameKinematicsVector.ctor.doc_deprecated_2args);
+          doc.FrameKinematicsVector.ctor.doc_deprecated_2args)
+      .def("clear", &FramePoseVector<T>::clear,
+          doc.FrameKinematicsVector.clear.doc)
+      .def("set_value", &FramePoseVector<T>::set_value, py::arg("id"),
+          py::arg("value"), doc.FrameKinematicsVector.set_value.doc)
+      .def(
+          "size", &FramePoseVector<T>::size, doc.FrameKinematicsVector.size.doc)
+      // This intentionally copies the value to avoid segfaults from accessing
+      // the result after clear() is called. (see #11583)
+      .def("value", &FramePoseVector<T>::value, py::arg("id"),
+          doc.FrameKinematicsVector.value.doc)
+      .def("has_id", &FramePoseVector<T>::has_id, py::arg("id"),
+          doc.FrameKinematicsVector.has_id.doc)
+      .def("frame_ids", &FramePoseVector<T>::frame_ids,
+          doc.FrameKinematicsVector.frame_ids.doc);
   AddValueInstantiation<FramePoseVector<T>>(m);
 
   py::class_<QueryObject<T>>(m, "QueryObject", doc.QueryObject.doc)

diff --git a/bindings/pydrake/test/geometry_test.py b/bindings/pydrake/test/geometry_test.py
@@ -4,6 +4,7 @@
 import warnings
 
 from pydrake.common import FindResourceOrThrow
+from pydrake.common.eigen_geometry import Isometry3
 from pydrake.common.test_utilities.deprecation import catch_drake_warnings
 from pydrake.lcm import DrakeMockLcm
 from pydrake.systems.framework import DiagramBuilder, InputPort, OutputPort
@@ -41,7 +42,16 @@ def test_scene_graph_api(self):
                 scene_graph=scene_graph, lcm=lcm)
 
     def test_frame_pose_vector_api(self):
-        mut.FramePoseVector()
+        obj = mut.FramePoseVector()
+        frame_id = mut.FrameId.get_new_id()
+        obj.set_value(id=frame_id, value=Isometry3.Identity())
+        self.assertEqual(obj.size(), 1)
+        self.assertIsInstance(obj.value(id=frame_id), Isometry3)
+        self.assertTrue(obj.has_id(id=frame_id))
+        self.assertIsInstance(obj.frame_ids(), list)
+        self.assertIsInstance(obj.frame_ids()[0], mut.FrameId)
+        obj.clear()
+        self.assertEqual(obj.size(), 0)
         with catch_drake_warnings(expected_count=1):
             mut.FramePoseVector(source_id=mut.SourceId.get_new_id(),
                                 ids=[mut.FrameId.get_new_id()])