wip add python bindings

Author: xuchenhan-tri

bindings/pydrake/geometry/geometry_py_scene_graph.cc

@@ -184,6 +184,8 @@ void DefineSceneGraphInspector(py::module m, T) {
             cls_doc.GetAllDeformableGeometryIds.doc)
         .def("GetConvexHull", &Class::GetConvexHull, py_rvp::reference_internal,
             py::arg("geometry_id"), cls_doc.GetConvexHull.doc)
+        .def("GetObbInGeometryFrame", &Class::GetObbInGeometryFrame,
+            py::arg("geometry_id"), cls_doc.GetObbInGeometryFrame.doc)
         .def("CollisionFiltered", &Class::CollisionFiltered,
             py::arg("geometry_id1"), py::arg("geometry_id2"),
             cls_doc.CollisionFiltered.doc)
@@ -566,16 +568,20 @@ void DefineQueryObject(py::module m, T) {
                 &Class::GetPoseInWorld),
             py::arg("frame_id"), py_rvp::reference_internal,
             cls_doc.GetPoseInWorld.doc_1args_frame_id)
-        .def("GetConfigurationsInWorld", &Class::GetConfigurationsInWorld,
-            py::arg("deformable_geometry_id"), py_rvp::copy,
-            cls_doc.GetConfigurationsInWorld.doc)
         .def("GetPoseInParent", &Class::GetPoseInParent, py::arg("frame_id"),
             py_rvp::reference_internal, cls_doc.GetPoseInParent.doc)
         .def("GetPoseInWorld",
             overload_cast_explicit<const math::RigidTransform<T>&, GeometryId>(
                 &Class::GetPoseInWorld),
             py::arg("geometry_id"), py_rvp::reference_internal,
             cls_doc.GetPoseInWorld.doc_1args_geometry_id)
+        .def("GetConfigurationsInWorld", &Class::GetConfigurationsInWorld,
+            py::arg("deformable_geometry_id"), py_rvp::copy,
+            cls_doc.GetConfigurationsInWorld.doc)
+        .def("ComputeAabbInWorld", &Class::ComputeAabbInWorld,
+            py::arg("geometry_id"), cls_doc.ComputeAabbInWorld.doc)
+        .def("ComputeObbInWorld", &Class::ComputeObbInWorld,
+            py::arg("geometry_id"), cls_doc.ComputeObbInWorld.doc)
         .def("ComputeSignedDistancePairwiseClosestPoints",
             &QueryObject<T>::ComputeSignedDistancePairwiseClosestPoints,
             py::arg("max_distance") = std::numeric_limits<double>::infinity(),

bindings/pydrake/geometry/test/scene_graph_test.py

@@ -279,6 +279,17 @@ def test_scene_graph_api(self, T):
         self.assertTrue(inspector.IsDeformableGeometry(geometry_id=sphere_4))
         self.assertEqual(inspector.GetAllDeformableGeometryIds(), [sphere_4])
         self.assertIsNone(inspector.GetConvexHull(geometry_id=sphere_3))
+        # Test GetObbInGeometryFrame for a geometry that supports it.
+        self.assertIsInstance(
+            inspector.GetObbInGeometryFrame(geometry_id=sphere_3), mut.Obb)
+        # Test GetObbInGeometryFrame for a geometry that does not support it.
+        half_space = scene_graph.RegisterAnchoredGeometry(
+            source_id=global_source,
+            geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
+                                          shape=mut.HalfSpace(),
+                                          name="half_space"))
+        self.assertIsNone(
+            inspector.GetObbInGeometryFrame(geometry_id=half_space))
         self.assertIsInstance(inspector.geometry_version(),
                               mut.GeometryVersion)
 
@@ -678,6 +689,13 @@ def test_query_object(self, T):
         self.assertIsInstance(
             query_object.GetPoseInWorld(geometry_id=geometry_id),
             RigidTransform_[T])
+        self.assertIsInstance(
+            query_object.ComputeAabbInWorld(
+                geometry_id=deformable_geometry_id),
+            mut.Aabb)
+        self.assertIsInstance(
+            query_object.ComputeObbInWorld(geometry_id=geometry_id),
+            mut.Obb)
 
         # Proximity queries -- all of these will produce empty results.
         results = query_object.ComputeSignedDistancePairwiseClosestPoints()

geometry/query_object.cc

@@ -85,40 +85,41 @@ const RigidTransform<T>& QueryObject<T>::GetPoseInWorld(
 }
 
 template <typename T>
-Aabb QueryObject<T>::ComputeAabbInWorld(GeometryId geometry_id) const {
+const VectorX<T>& QueryObject<T>::GetConfigurationsInWorld(
+    GeometryId geometry_id) const {
   ThrowIfNotCallable();
+
   FullConfigurationUpdate();
   const GeometryState<T>& state = geometry_state();
-  return state.ComputeAabbInWorld(geometry_id);
+  return state.get_configurations_in_world(geometry_id);
 }
 
 template <typename T>
-Obb QueryObject<T>::ComputeObbInWorld(GeometryId geometry_id) const {
+std::vector<VectorX<T>> QueryObject<T>::GetDrivenMeshConfigurationsInWorld(
+    GeometryId geometry_id, Role role) const {
   ThrowIfNotCallable();
-  FullPoseUpdate();
+  FullConfigurationUpdate();
   const GeometryState<T>& state = geometry_state();
-  return state.ComputeObbInWorld(geometry_id);
+  return state.GetDrivenMeshConfigurationsInWorld(geometry_id, role);
 }
 
 template <typename T>
-const VectorX<T>& QueryObject<T>::GetConfigurationsInWorld(
-    GeometryId geometry_id) const {
+Aabb QueryObject<T>::ComputeAabbInWorld(GeometryId geometry_id) const {
   ThrowIfNotCallable();
-
   FullConfigurationUpdate();
   const GeometryState<T>& state = geometry_state();
-  return state.get_configurations_in_world(geometry_id);
+  return state.ComputeAabbInWorld(geometry_id);
 }
 
 template <typename T>
-std::vector<VectorX<T>> QueryObject<T>::GetDrivenMeshConfigurationsInWorld(
-    GeometryId geometry_id, Role role) const {
+Obb QueryObject<T>::ComputeObbInWorld(GeometryId geometry_id) const {
   ThrowIfNotCallable();
-  FullConfigurationUpdate();
+  FullPoseUpdate();
   const GeometryState<T>& state = geometry_state();
-  return state.GetDrivenMeshConfigurationsInWorld(geometry_id, role);
+  return state.ComputeObbInWorld(geometry_id);
 }
 
+
 template <typename T>
 std::vector<PenetrationAsPointPair<T>>
 QueryObject<T>::ComputePointPairPenetration() const {

geometry/query_object.h

@@ -172,18 +172,6 @@ class QueryObject {
    is deformable.  */
   const math::RigidTransform<T>& GetPoseInWorld(GeometryId geometry_id) const;
 
-  /** Reports the axis-aligned bounding box of the geometry indicated by
-   `geometry_id` in the world frame.
-   @throws std::exception if the geometry `geometry_id` is not valid.
-   @throws std::exception if the geometry is not deformable.  */
-  Aabb ComputeAabbInWorld(GeometryId geometry_id) const;
-
-  /** Reports the oriented bounding box of the geometry indicated by
-   `geometry_id` in the world frame.
-   @throws std::exception if the geometry `geometry_id` is not valid.
-   @throws std::exception if the geometry is deformable.  */
-  Obb ComputeObbInWorld(GeometryId geometry_id) const;
-
   /** Reports the configuration of the deformable geometry indicated by
    `deformable_geometry_id` relative to the world frame.
    @sa GetPoseInWorld().
@@ -204,6 +192,19 @@ class QueryObject {
    @experimental */
   std::vector<VectorX<T>> GetDrivenMeshConfigurationsInWorld(
       GeometryId deformable_geometry_id, Role role) const;
+
+  /** Reports the axis-aligned bounding box of the geometry indicated by
+   `geometry_id` in the world frame.
+   @throws std::exception if the geometry `geometry_id` is not valid.
+   @throws std::exception if the geometry is not deformable.  */
+  Aabb ComputeAabbInWorld(GeometryId geometry_id) const;
+
+  /** Reports the oriented bounding box of the geometry indicated by
+   `geometry_id` in the world frame.
+   @throws std::exception if the geometry `geometry_id` is not valid.
+   @throws std::exception if the geometry is deformable.  */
+  Obb ComputeObbInWorld(GeometryId geometry_id) const;
+
   //@}
 
   /**