Skip to content

Commit

Permalink
Support deformable geometries with perception role (RobotLocomotion#2…
Browse files Browse the repository at this point in the history 
…1094)

Deformable geometries with perception role registers driven meshes in
GeometryState that are driven by the configuration of the control meshes
in SceneGraph's configuration update. These driven meshes are registered
and rendered in accepting RenderEngines.

Miscellaneous features in support of this:

* Conversion from RenderMesh to TriangleSurfaceMesh
* Conversion of VectorX<T> to VectorX<double>
* A new data structure DrivenMeshData that employs the same caching
  mechanism as KinematicsData
  • Loading branch information
@xuchenhan-tri
xuchenhan-tri authored Mar 18, 2024
1 parent c6de711 commit b925e46
Show file tree
Hide file tree
Showing 11 changed files with 531 additions and 59 deletions.
1 change: 1 addition & 0 deletions geometry/BUILD.bazel
View file
Original file line number Diff line number Diff line change
Expand Up @@ -261,6 +261,7 @@ drake_cc_library(
":internal_frame",
":internal_geometry",
":kinematics_vector",
":mesh_deformation_interpolator",
":proximity_engine",
":utilities",
"//geometry/proximity:make_convex_hull_mesh",
Expand Down
181 changes: 163 additions & 18 deletions geometry/geometry_state.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include "drake/geometry/geometry_instance.h"
#include "drake/geometry/geometry_roles.h"
#include "drake/geometry/proximity/make_convex_hull_mesh.h"
#include "drake/geometry/proximity/volume_to_surface_mesh.h"
#include "drake/geometry/proximity_engine.h"
#include "drake/geometry/proximity_properties.h"
#include "drake/geometry/render/render_engine.h"
Expand All @@ -26,11 +27,14 @@ namespace drake {
namespace geometry {

using internal::convert_to_double;
using internal::DrivenTriangleMesh;
using internal::FrameNameSet;
using internal::HydroelasticType;
using internal::InternalFrame;
using internal::InternalGeometry;
using internal::MakeRenderMeshFromTriangleSurfaceMesh;
using internal::ProximityEngine;
using internal::RenderMesh;
using math::RigidTransform;
using math::RigidTransformd;
using render::ColorRenderCamera;
Expand All @@ -46,6 +50,37 @@ using systems::sensors::ImageDepth32F;
using systems::sensors::ImageLabel16I;
using systems::sensors::ImageRgba8U;

namespace internal {

template <typename T>
void DrivenMeshData::SetControlMeshPositions(
const std::unordered_map<GeometryId, VectorX<T>>& q_WGs) {
for (auto& [id, meshes] : driven_meshes_) {
DRAKE_DEMAND(q_WGs.contains(id));
// To prevent unnecessary copying, this returns a reference for T=double and
// returns a copy otherwise.
const VectorX<double>& q_WG =
geometry::internal::convert_to_double(q_WGs.at(id));
// The meshes are partitions of the overall geometry and each of them knows
// how to locate its own coordinates from within the full set of q_WG.
for (auto& mesh : meshes) {
mesh.SetControlMeshPositions(q_WG);
}
}
}

void DrivenMeshData::SetMeshes(
GeometryId id, std::vector<DrivenTriangleMesh> driven_meshes,
std::vector<RenderMesh> render_meshes) {
DRAKE_DEMAND(!driven_meshes.empty());
DRAKE_DEMAND(!render_meshes.empty());
DRAKE_DEMAND(driven_meshes.size() == render_meshes.size());
driven_meshes_.emplace(id, std::move(driven_meshes));
render_meshes_.emplace(id, std::move(render_meshes));
}

} // namespace internal

//-----------------------------------------------------------------------------

// These utility methods help streamline the desired semantics of map lookups.
Expand Down Expand Up @@ -186,6 +221,7 @@ GeometryState<T>::GeometryState(const GeometryState<U>& source)
frames_(source.frames_),
geometries_(source.geometries_),
frame_index_to_id_map_(source.frame_index_to_id_map_),
driven_perception_meshes_(source.driven_perception_meshes_),
geometry_engine_(
std::move(source.geometry_engine_->template ToScalarType<T>())),
render_engines_(source.render_engines_),
Expand Down Expand Up @@ -1122,6 +1158,10 @@ void GeometryState<T>::AssignRole(SourceId source_id, GeometryId geometry_id,

geometry.SetRole(std::move(properties));

if (geometry.is_deformable()) {
RegisterDrivenPerceptionMesh(geometry_id);
}

const bool added_to_renderer = AddToCompatibleRenderersUnchecked(geometry);

if (!added_to_renderer && render_engines_.size() > 0) {
Expand Down Expand Up @@ -1296,9 +1336,14 @@ void GeometryState<T>::AddRenderer(
"renderer", "accepting", set<string>{});
if (accepting_renderers.empty() || accepting_renderers.contains(name)) {
const GeometryId id = id_geo_pair.first;
accepted |= render_engine->RegisterVisual(
id, geometry.shape(), *properties, RigidTransformd(geometry.X_FG()),
geometry.is_dynamic());
if (geometry.is_deformable()) {
accepted |= render_engine->RegisterDeformableVisual(
id, driven_perception_meshes_.render_meshes(id), *properties);
} else {
accepted |= render_engine->RegisterVisual(
id, geometry.shape(), *properties,
RigidTransformd(geometry.X_FG()), geometry.is_dynamic());
}
}
}
}
Expand Down Expand Up @@ -1516,10 +1561,25 @@ void GeometryState<T>::FinalizePoseUpdate(
template <typename T>
void GeometryState<T>::FinalizeConfigurationUpdate(
const internal::KinematicsData<T>& kinematics_data,
const internal::DrivenMeshData& driven_meshes,
internal::ProximityEngine<T>* proximity_engine,
std::vector<render::RenderEngine*>) const {
std::vector<render::RenderEngine*> render_engines) const {
proximity_engine->UpdateDeformableVertexPositions(kinematics_data.q_WGs);
// TODO(xuchenhan-tri): Update render engine as necessary.
for (const auto& [id, meshes] : driven_meshes.driven_meshes()) {
// Vertex positions of driven meshes.
std::vector<VectorX<double>> q_WDs(meshes.size());
// Vertex normals of driven meshes.
std::vector<VectorX<double>> nhats_W(meshes.size());
for (int i = 0; i < ssize(meshes); ++i) {
// TODO(xuchenhan-tri): Consider eliminating the copy here if performance
// is an issue.
q_WDs[i] = meshes[i].GetDrivenVertexPositions();
nhats_W[i] = meshes[i].GetDrivenVertexNormals();
}
for (auto* render_engine : render_engines) {
render_engine->UpdateDeformableConfigurations(id, q_WDs, nhats_W);
}
}
}

template <typename T>
Expand Down Expand Up @@ -1736,23 +1796,26 @@ bool GeometryState<T>::RemoveFromRendererUnchecked(
template <typename T>
bool GeometryState<T>::AddToCompatibleRenderersUnchecked(
const internal::InternalGeometry& geometry) {
const PerceptionProperties& properties = *geometry.perception_properties();

const RigidTransformd& X_WG =
convert_to_double(kinematics_data_.X_WGs.at(geometry.id()));

bool added_to_renderer = false;
auto accepting_renderers =
properties.GetPropertyOrDefault("renderer", "accepting", set<string>{});

bool added_to_renderer{false};
geometry.perception_properties()->GetPropertyOrDefault(
"renderer", "accepting", set<string>{});
std::vector<render::RenderEngine*> candidate_renderers;
for (auto& [name, engine] : render_engines_) {
// If no "accepting_renderer" has been specified, every renderer will be
// given the chance to register the geometry.
if (accepting_renderers.empty() || accepting_renderers.contains(name)) {
added_to_renderer =
engine->RegisterVisual(geometry.id(), geometry.shape(), properties,
X_WG, geometry.is_dynamic()) ||
added_to_renderer;
candidate_renderers.emplace_back(engine.get_mutable());
}
}
if (candidate_renderers.empty()) return false;
if (geometry.is_deformable()) {
added_to_renderer = AddDeformableToCompatibleRenderersUnchecked(
geometry, &candidate_renderers);
} else {
added_to_renderer =
AddRigidToCompatibleRenderersUnchecked(geometry, &candidate_renderers);
}
if (added_to_renderer) {
// Increment version number only if some renderer picks up the role
// assignment.
Expand All @@ -1761,6 +1824,81 @@ bool GeometryState<T>::AddToCompatibleRenderersUnchecked(
return added_to_renderer;
}

template <typename T>
bool GeometryState<T>::AddRigidToCompatibleRenderersUnchecked(
const internal::InternalGeometry& geometry,
std::vector<render::RenderEngine*>* candidate_renderers) {
const PerceptionProperties& properties = *geometry.perception_properties();

const RigidTransformd& X_WG =
convert_to_double(kinematics_data_.X_WGs.at(geometry.id()));

bool added_to_renderer{false};
for (auto& engine : *candidate_renderers) {
added_to_renderer =
engine->RegisterVisual(geometry.id(), geometry.shape(), properties,
X_WG, geometry.is_dynamic()) ||
added_to_renderer;
}
return added_to_renderer;
}

template <typename T>
bool GeometryState<T>::AddDeformableToCompatibleRenderersUnchecked(
const internal::InternalGeometry& geometry,
std::vector<render::RenderEngine*>* candidate_renderers) {
const GeometryId id = geometry.id();
const PerceptionProperties& properties = *geometry.perception_properties();
bool added_to_renderer{false};
for (auto& engine : *candidate_renderers) {
added_to_renderer =
engine->RegisterDeformableVisual(
id, driven_perception_meshes_.render_meshes(id), properties) ||
added_to_renderer;
}
return added_to_renderer;
}

template <typename T>
void GeometryState<T>::RegisterDrivenPerceptionMesh(GeometryId geometry_id) {
InternalGeometry& geometry = geometries_[geometry_id];
DRAKE_DEMAND(geometry.is_deformable());
DRAKE_DEMAND(geometry.has_perception_role());
const PerceptionProperties& properties = *geometry.perception_properties();
// TODO(xuchenhan-tri): Each render engine customizes its own default diffuse
// color. By setting a default value here, we are subverting the engine,
// preventing it from applying its own logic. Lack of a diffuse color should
// propagate all the way down to the engine for the engine to resolve.
const auto default_rgba = properties.GetPropertyOrDefault(
"phong", "diffuse", Rgba{1.0, 1.0, 1.0, 1.0});

const VolumeMesh<double>* control_mesh_ptr = geometry.reference_mesh();
DRAKE_DEMAND(control_mesh_ptr != nullptr);
const VolumeMesh<double>& control_mesh = *control_mesh_ptr;

const string render_meshes_file =
properties.GetPropertyOrDefault("deformable", "embedded_mesh", string{});
std::vector<RenderMesh> render_meshes;
std::vector<DrivenTriangleMesh> driven_meshes;
if (render_meshes_file.empty()) {
// If no render mesh is specified, use the surface triangle mesh of the
// control volume mesh as the render mesh.
driven_meshes.emplace_back(internal::MakeDrivenSurfaceMesh(control_mesh));
render_meshes.emplace_back(MakeRenderMeshFromTriangleSurfaceMesh(
driven_meshes.back().triangle_surface_mesh(), properties,
default_rgba));
} else {
render_meshes = internal::LoadRenderMeshesFromObj(render_meshes_file,
properties, default_rgba);
for (const internal::RenderMesh& render_mesh : render_meshes) {
driven_meshes.emplace_back(MakeTriangleSurfaceMesh(render_mesh),
control_mesh);
}
}
driven_perception_meshes_.SetMeshes(geometry_id, std::move(driven_meshes),
std::move(render_meshes));
}

template <typename T>
void GeometryState<T>::RemoveFromAllRenderersUnchecked(GeometryId id) {
for (auto& name_engine_pair : render_engines_) {
Expand Down Expand Up @@ -1808,8 +1946,12 @@ bool GeometryState<T>::RemovePerceptionRole(GeometryId geometry_id) {
if (!geometry->has_perception_role()) return false;

// Geometry has a perception role; do the work to remove it from whichever
// render engines it happens to present in.
// render engines it happens to be present in and also remove its driven
// perception meshes.
RemoveFromAllRenderersUnchecked(geometry_id);
if (IsDeformableGeometry(geometry_id)) {
driven_perception_meshes_.Remove(geometry_id);
}
geometry->RemovePerceptionRole();
return true;
}
Expand Down Expand Up @@ -1878,3 +2020,6 @@ template GeometryState<Expression>::GeometryState(const GeometryState<AutoDiffXd

DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
class ::drake::geometry::GeometryState)
DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
(&drake::geometry::internal::DrivenMeshData::
template SetControlMeshPositions<T>))
Loading

0 comments on commit b925e46

Please sign in to comment.