Skip to content

Commit

Permalink
[geometry] Add GeometryInstance overload without unique_ptr
Browse files Browse the repository at this point in the history
Deprecate unique_ptr getter in pydrake (not C++).
  • Loading branch information
jwnimmer-tri committed Apr 24, 2023
1 parent 8d6b352 commit 89e141d
Show file tree
Hide file tree
Showing 8 changed files with 39 additions and 26 deletions.
11 changes: 8 additions & 3 deletions bindings/pydrake/geometry/geometry_py_common.cc
Original file line number Diff line number Diff line change
Expand Up @@ -86,8 +86,8 @@ void DoScalarIndependentDefinitions(py::module m) {
constexpr auto& cls_doc = doc.GeometryInstance;
py::class_<Class> cls(m, "GeometryInstance", cls_doc.doc);
cls // BR
.def(py::init<const math::RigidTransform<double>&,
std::unique_ptr<Shape>, const std::string&>(),
.def(py::init<const math::RigidTransform<double>&, const Shape&,
const std::string&>(),
py::arg("X_PG"), py::arg("shape"), py::arg("name"),
cls_doc.ctor.doc)
.def("id", &Class::id, cls_doc.id.doc)
Expand All @@ -96,7 +96,6 @@ void DoScalarIndependentDefinitions(py::module m) {
"set_pose", &Class::set_pose, py::arg("X_PG"), cls_doc.set_pose.doc)
.def("shape", &Class::shape, py_rvp::reference_internal,
cls_doc.shape.doc)
.def("release_shape", &Class::release_shape, cls_doc.release_shape.doc)
.def("name", &Class::name, cls_doc.name.doc)
.def("set_name", &Class::set_name, cls_doc.set_name.doc)
.def("set_proximity_properties", &Class::set_proximity_properties,
Expand All @@ -121,6 +120,12 @@ void DoScalarIndependentDefinitions(py::module m) {
.def("perception_properties", &Class::perception_properties,
py_rvp::reference_internal, cls_doc.perception_properties.doc);
DefCopyAndDeepCopy(&cls);
constexpr char doc_release_deprecated[] =
"Ownership transfer doesn't make sense for Python. Just use shape() "
"instead. This function will be removed on or after 2023-08-01.";
cls.def("release_shape",
WrapDeprecated(doc_release_deprecated, &Class::release_shape),
doc_release_deprecated);
}

// GeometryProperties
Expand Down
3 changes: 2 additions & 1 deletion bindings/pydrake/geometry/test/common_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,6 @@ def test_geometry_instance_api(self):
geometry.set_pose(RigidTransform([1, 0, 0]))
self.assertIsInstance(geometry.pose(), RigidTransform)
self.assertIsInstance(geometry.shape(), mut.Shape)
self.assertIsInstance(geometry.release_shape(), mut.Shape)
self.assertEqual(geometry.name(), "sphere")
geometry.set_name("funky")
self.assertEqual(geometry.name(), "funky")
Expand All @@ -88,6 +87,8 @@ def test_geometry_instance_api(self):
mut.PerceptionProperties)
self.assertIsInstance(geometry.perception_properties(),
mut.PerceptionProperties)
with catch_drake_warnings(expected_count=1):
self.assertIsInstance(geometry.release_shape(), mut.Shape)

def test_geometry_properties_api(self):
# Test perception/ illustration properties (specifically Rgba).
Expand Down
16 changes: 11 additions & 5 deletions geometry/geometry_instance.cc
Original file line number Diff line number Diff line change
Expand Up @@ -5,20 +5,26 @@
namespace drake {
namespace geometry {

GeometryInstance::GeometryInstance(const math::RigidTransform<double>& X_PG,
const Shape& shape, const std::string& name)
: GeometryInstance(X_PG, shape.Clone(), name) {}

GeometryInstance::GeometryInstance(const math::RigidTransform<double>& X_PG,
std::unique_ptr<Shape> shape,
const std::string& name)
: id_(GeometryId::get_new_id()),
X_PG_(X_PG),
shape_(std::move(shape)) {
: id_(GeometryId::get_new_id()), X_PG_(X_PG) {
DRAKE_THROW_UNLESS(shape != nullptr);
shape_ = std::move(shape);
set_name(name);
}

void GeometryInstance::set_name(const std::string& name) {
name_ = internal::CanonicalizeStringName(name);
if (name_.empty()) {
throw std::logic_error("GeometryInstance given the name '" + name +
"' which is an empty canonical string");
throw std::logic_error(fmt::format(
"GeometryInstance given the name '{}' which is an empty canonical "
"string",
name));
}
}

Expand Down
14 changes: 11 additions & 3 deletions geometry/geometry_instance.h
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,12 @@ class GeometryInstance {
@param name The name of the geometry (must satisfy the name requirements).
@throws std::exception if the canonicalized version of `name` is empty.
*/
GeometryInstance(const math::RigidTransform<double>& X_PG, const Shape& shape,
const std::string& name);

/** (Advanced) Overload that transfers ownership of `shape` (for performance).
@pre shape is non-null.
@exclude_from_pydrake_mkdoc{Cannot transfer ownership.} */
GeometryInstance(const math::RigidTransform<double>& X_PG,
std::unique_ptr<Shape> shape, const std::string& name);

Expand All @@ -105,13 +111,15 @@ class GeometryInstance {
void set_pose(const math::RigidTransformd& X_PG) { X_PG_ = X_PG; }

/** Returns the underlying shape specification for this geometry instance.
@pre release_shape() has not been called. */
@pre release_shape() has not been called nor has this been moved-from. */
const Shape& shape() const {
DRAKE_DEMAND(shape_ != nullptr);
DRAKE_THROW_UNLESS(shape_ != nullptr);
return *shape_;
}

/** Releases the shape from the instance. */
/** (Advanced) Transfers ownership of this geometry instance's underlying
shape specification to the caller.
@pre release_shape() has not been called nor has this been moved-from. */
std::unique_ptr<Shape> release_shape() { return std::move(shape_); }

/** Returns the *canonicalized* name for the instance.
Expand Down
7 changes: 2 additions & 5 deletions geometry/scene_graph_inspector.cc
Original file line number Diff line number Diff line change
Expand Up @@ -284,11 +284,8 @@ void SceneGraphInspector<T>::Reify(GeometryId geometry_id,
template <typename T>
std::unique_ptr<GeometryInstance> SceneGraphInspector<T>::CloneGeometryInstance(
GeometryId id) const {
const std::string name = GetName(id);
const math::RigidTransformd X_PG = GetPoseInFrame(id);
std::unique_ptr<Shape> shape = GetShape(id).Clone();
auto geometry_instance =
std::make_unique<GeometryInstance>(X_PG, std::move(shape), name);
auto geometry_instance = std::make_unique<GeometryInstance>(
GetPoseInFrame(id), GetShape(id), GetName(id));
if (const auto* props = GetProximityProperties(id)) {
geometry_instance->set_proximity_properties(*props);
}
Expand Down
9 changes: 3 additions & 6 deletions geometry/test/geometry_instance_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -28,14 +28,12 @@ GTEST_TEST(GeometryInstanceTest, IsCopyable) {

GTEST_TEST(GeometryInstanceTest, IdCopies) {
RigidTransformd pose = RigidTransformd::Identity();
auto shape = make_unique<Sphere>(1.0);
GeometryInstance geometry_a{pose, move(shape), "geometry_a"};
GeometryInstance geometry_a{pose, Sphere(1.0), "geometry_a"};
GeometryInstance geometry_b(geometry_a);
EXPECT_EQ(geometry_a.id(), geometry_b.id());
EXPECT_EQ(geometry_a.name(), geometry_b.name());

shape = make_unique<Sphere>(2.0);
GeometryInstance geometry_c{pose, move(shape), "geometry_c"};
GeometryInstance geometry_c{pose, Sphere(2.0), "geometry_c"};
EXPECT_NE(geometry_a.id(), geometry_c.id());
EXPECT_NE(geometry_a.name(), geometry_c.name());
geometry_c = geometry_a;
Expand All @@ -50,8 +48,7 @@ GTEST_TEST(GeometryInstanceTest, CanonicalName) {
RigidTransformd pose = RigidTransformd::Identity();

auto make_instance = [&pose](const std::string& name) {
auto shape = make_unique<Sphere>(1.0);
return GeometryInstance{pose, move(shape), name};
return GeometryInstance{pose, Sphere(1.0), name};
};

const std::string canonical = "name";
Expand Down
2 changes: 1 addition & 1 deletion geometry/test/scene_graph_inspector_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -128,7 +128,7 @@ GTEST_TEST(SceneGraphInspector, CloneGeometryInstance) {

// Geometry with no properties; confirm the other properties.
const GeometryInstance original(RigidTransformd(
Eigen::Vector3d(1, 2, 3)), make_unique<Sphere>(1.5), "test_sphere");
Eigen::Vector3d(1, 2, 3)), Sphere(1.5), "test_sphere");
const GeometryId geometry_id = tester.mutable_state().RegisterGeometry(
source_id, frame_id, make_unique<GeometryInstance>(original));

Expand Down
3 changes: 1 addition & 2 deletions multibody/plant/test/contact_properties_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -94,9 +94,8 @@ class ContactPropertiesTest : public ::testing::Test {
const std::optional<double>& hydro_modulus = std::nullopt,
const std::optional<geometry::internal::HydroelasticType>&
compliance_type = std::nullopt) {
auto sphere = make_unique<Sphere>(1.0);
auto result =
make_unique<GeometryInstance>(RigidTransformd(), move(sphere), name);
make_unique<GeometryInstance>(RigidTransformd(), Sphere(1.0), name);
geometry::ProximityProperties props;
geometry::AddContactMaterial(hunt_crossley_dissipation, k, mu, &props);
if (tau.has_value()) {
Expand Down

0 comments on commit 89e141d

Please sign in to comment.