diff --git a/geometry/BUILD.bazel b/geometry/BUILD.bazel index 4912d922c22f..9aecaebffda2 100644 --- a/geometry/BUILD.bazel +++ b/geometry/BUILD.bazel @@ -51,6 +51,7 @@ drake_cc_library( "//geometry/query_results:penetration_as_point_pair", "//geometry/query_results:signed_distance_pair", "@fcl", + "@tinyobjloader", ], ) @@ -245,10 +246,15 @@ drake_cc_library( drake_cc_googletest( name = "proximity_engine_test", + data = [ + "test/forbidden_two_cubes.obj", + "test/quad_cube.obj", + ], deps = [ ":proximity_engine", ":shape_specification", "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:expect_throws_message", ], ) diff --git a/geometry/geometry_visualization.cc b/geometry/geometry_visualization.cc index 58008036cdf5..e2e454e8fbd9 100644 --- a/geometry/geometry_visualization.cc +++ b/geometry/geometry_visualization.cc @@ -111,6 +111,16 @@ class ShapeToLcm : public ShapeReifier { geometry_data_.string_data = mesh.filename(); } + // For visualization, Convex is the same as Mesh. + void ImplementGeometry(const Convex& mesh, void*) override { + geometry_data_.type = geometry_data_.MESH; + geometry_data_.num_float_data = 3; + geometry_data_.float_data.push_back(static_cast(mesh.scale())); + geometry_data_.float_data.push_back(static_cast(mesh.scale())); + geometry_data_.float_data.push_back(static_cast(mesh.scale())); + geometry_data_.string_data = mesh.filename(); + } + private: lcmt_viewer_geometry_data geometry_data_{}; // The transform from the geometry frame to its parent frame. diff --git a/geometry/proximity_engine.cc b/geometry/proximity_engine.cc index 782b73ba171f..10134d141418 100644 --- a/geometry/proximity_engine.cc +++ b/geometry/proximity_engine.cc @@ -1,15 +1,21 @@ #include "drake/geometry/proximity_engine.h" #include +#include #include #include +#include #include #include +#include #include #include +#include #include #include +#include +#include #include "drake/common/default_scalars.h" #include "drake/common/sorted_vectors_have_intersection.h" @@ -637,6 +643,144 @@ class ProximityEngine::Impl : public ShapeReifier { TakeShapeOwnership(fcl_sphere, user_data); } + + // + // Convert vertices from tinyobj format to FCL format. + // + // Vertices from tinyobj are in a vector of floating-points like this: + // attrib.vertices = {c0,c1,c2, c3,c4,c5, c6,c7,c8,...} + // = {x, y, z, x, y, z, x, y, z,...} + // We will convert to a vector of Vector3d for FCL like this: + // vertices = {{c0,c1,c2}, {c3,c4,c5}, {c6,c7,c8},...} + // = { v0, v1, v2, ...} + // + // The size of `attrib.vertices` is three times the number of vertices. + // + std::vector TinyObjToFclVertices(const tinyobj::attrib_t& attrib, + const double scale) const { + int num_coords = attrib.vertices.size(); + DRAKE_DEMAND(num_coords % 3 == 0); + std::vector vertices; + vertices.reserve(num_coords / 3); + + auto iter = attrib.vertices.begin(); + while (iter != attrib.vertices.end()) { + // We increment `iter` three times for x, y, and z coordinates. + double x = *(iter++) * scale; + double y = *(iter++) * scale; + double z = *(iter++) * scale; + vertices.emplace_back(x, y, z); + } + + return vertices; + } + + // + // Convert faces from tinyobj to FCL. + // + // + // A tinyobj mesh has an integer array storing the number of vertices of + // each polygonal face. + // mesh.num_face_vertices = {n0,n1,n2,...} + // face0 has n0 vertices. + // face1 has n1 vertices. + // face2 has n2 vertices. + // ... + // A tinyobj mesh has a vector of vertices that belong to the faces. + // mesh.indices = {v0_0, v0_1,..., v0_n0-1, + // v1_0, v1_1,..., v1_n1-1, + // v2_0, v2_1,..., v2_n2-1, + // ...} + // face0 has vertices v0_0, v0_1,...,v0_n0-1. + // face1 has vertices v1_0, v1_1,...,v1_n1-1. + // face2 has vertices v2_0, v2_1,...,v2_n2-1. + // ... + // For fcl::Convex, the `faces` is as an array of integers in this format. + // faces = { n0, v0_0,v0_1,...,v0_n0-1, + // n1, v1_0,v1_1,...,v1_n1-1, + // n2, v2_0,v2_1,...,v2_n2-1, + // ...} + // where n_i is the number of vertices of face_i. + // + int TinyObjToFclFaces(const tinyobj::mesh_t& mesh, + std::vector* faces) const { + auto iter = mesh.indices.begin(); + for (const int& num : mesh.num_face_vertices) { + faces->push_back(num); + std::for_each(iter, iter + num, [faces](const tinyobj::index_t& index) { + faces->push_back(index.vertex_index); + }); + iter += num; + } + + return mesh.num_face_vertices.size(); + } + + void ImplementGeometry(const Convex& convex, void* user_data) override { + // We use tiny_obj_loader to read the .obj file of the convex shape. + tinyobj::attrib_t attrib; + std::vector shapes; + std::vector materials; + std::string err; + // We keep polygonal faces without triangulating them. Some algorithms for + // convex geometry perform better with fewer faces. + bool do_tinyobj_triangulation = false; + // We use default value (NULL) for the base directory of .mtl file (material + // description), so it will be searched from the working directory. + const char* mtl_basedir = nullptr; + bool ret = tinyobj::LoadObj(&attrib, &shapes, &materials, &err, + convex.filename().c_str(), mtl_basedir, do_tinyobj_triangulation); + if (!ret || !err.empty()) { + throw std::runtime_error("Error parsing file '" + convex.filename() + + "' : " + err); + } + + // TODO(DamrongGuoy) Check that the input is a valid convex polyhedron. + // 1. Each face is a planar polygon. + // 2. Each face is a convex polygon. + // 3. The polyhedron is convex. + + // + // Now we convert tinyobj data for fcl::Convex. + // + + if (shapes.size() != 1) { + throw std::runtime_error("For Convex geometry, the .obj file must have " + "one and only one object defined in it."); + } + + std::vector vertices = + TinyObjToFclVertices(attrib, convex.scale()); + + const tinyobj::mesh_t& mesh = shapes[0].mesh; + + // We will have `faces.size()` larger than the number of faces. For each + // face_i, the vector `faces` contains both the number and indices of its + // vertices: + // faces = { n0, v0_0,v0_1,...,v0_n0-1, + // n1, v1_0,v1_1,...,v1_n1-1, + // n2, v2_0,v2_1,...,v2_n2-1, + // ...} + // where n_i is the number of vertices of face_i. + // + std::vector faces; + int num_faces = TinyObjToFclFaces(mesh, &faces); + + convex_objects_.emplace_back(move(vertices), num_faces, move(faces)); + ConvexData& object = convex_objects_.back(); + + // Create fcl::Convex. + auto fcl_convex = make_shared( + object.vertices.size(), object.vertices.data(), + object.num_faces, object.faces.data()); + TakeShapeOwnership(fcl_convex, user_data); + + // TODO(DamrongGuoy): Per f2f with SeanCurtis-TRI, we want ProximityEngine + // to own vertices and face by a map from filename. This way we won't have + // to read the same file again and again when we create multiple Convex + // objects from the same file. + } + std::vector> ComputeSignedDistancePairwiseClosestPoints( const std::vector& dynamic_map, @@ -873,6 +1017,23 @@ class ProximityEngine::Impl : public ShapeReifier { // The mechanism for dictating collision filtering. CollisionFilterLegacy collision_filter_; + // The data needed by fcl::Convex. + struct ConvexData { + // TODO(DamrongGuoy) We will switch to shared_ptr> later. + // For now, we force callers to use move semantics (&& rvalue reference) + // for efficiency. + ConvexData(std::vector&& v, int n, std::vector&& f): + vertices(move(v)), num_faces(n), faces(move(f)) { + } + + std::vector vertices; + int num_faces; + std::vector faces; + }; + + // The vector containing data for each convex object. + std::vector convex_objects_; + // The tolerance that determines when the iterative process would terminate. // @see ProximityEngine::set_distance_tolerance() for more details. double distance_tolerance_{1E-6}; diff --git a/geometry/shape_specification.cc b/geometry/shape_specification.cc index 179fb6f7dd22..d5ea8edf2f28 100644 --- a/geometry/shape_specification.cc +++ b/geometry/shape_specification.cc @@ -71,5 +71,8 @@ Mesh::Mesh(const std::string& absolute_filename, double scale) filename_); } +Convex::Convex(const std::string& absolute_filename, double scale) + : Shape(ShapeTag()), filename_(absolute_filename), scale_(scale) {} + } // namespace geometry } // namespace drake diff --git a/geometry/shape_specification.h b/geometry/shape_specification.h index e172559eb213..380a5d010c13 100644 --- a/geometry/shape_specification.h +++ b/geometry/shape_specification.h @@ -192,6 +192,35 @@ class Mesh final : public Shape { double scale_; }; +/** Support for convex shapes. */ +class Convex final : public Shape { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Convex) + + /** Constructs a convex shape specification from the file located at the + given _absolute_ file path. Optionally uniformly scaled by the given scale + factor. + @param absolute_filename The file name with absolute path. We only + support an .obj file with only one polyhedron. + We assume that the polyhedron is convex. + @param scale An optional scale to coordinates. + + @throws std::runtime_error if the .obj file doesn't define a single object. + This can happen if it is empty, if there are + multiple object-name statements (e.g., + "o object_name"), or if there are faces defined + outside a single object-name statement. + */ + explicit Convex(const std::string& absolute_filename, double scale = 1.0); + + const std::string& filename() const { return filename_; } + double scale() const { return scale_; } + + private: + std::string filename_; + double scale_; +}; + /** The interface for converting shape descriptions to real shapes. Any entity that consumes shape descriptions _must_ implement this interface. @@ -250,6 +279,7 @@ class ShapeReifier { void* user_data) = 0; virtual void ImplementGeometry(const Box& box, void* user_data) = 0; virtual void ImplementGeometry(const Mesh& mesh, void* user_data) = 0; + virtual void ImplementGeometry(const Convex& convex, void* user_data) = 0; }; template diff --git a/geometry/test/forbidden_two_cubes.obj b/geometry/test/forbidden_two_cubes.obj new file mode 100644 index 000000000000..d221cfffa84f --- /dev/null +++ b/geometry/test/forbidden_two_cubes.obj @@ -0,0 +1,37 @@ +o cube1 +v 1.000000 -1.000000 -1.000000 +v 1.000000 -1.000000 1.000000 +v -1.000000 -1.000000 1.000000 +v -1.000000 -1.000000 -1.000000 +v 1.000000 1.000000 -1.000000 +v 1.000000 1.000000 1.000000 +v -1.000000 1.000000 1.000000 +v -1.000000 1.000000 -1.000000 +vn 0.0000 -1.0000 0.0000 +vn 0.0000 1.0000 0.0000 +vn 1.0000 0.0000 0.0000 +vn -0.0000 -0.0000 1.0000 +vn -1.0000 -0.0000 -0.0000 +vn 0.0000 0.0000 -1.0000 +f 1//1 2//1 3//1 4//1 +f 5//2 8//2 7//2 6//2 +f 1//3 5//3 6//3 2//3 +f 2//4 6//4 7//4 3//4 +f 3//5 7//5 8//5 4//5 +f 5//6 1//6 4//6 8//6 + +o cube2 +v 5.000000 -1.000000 -1.000000 +v 5.000000 -1.000000 1.000000 +v 3.000000 -1.000000 1.000000 +v 3.000000 -1.000000 -1.000000 +v 5.000000 1.000000 -1.000000 +v 5.000000 1.000000 1.000000 +v 3.000000 1.000000 1.000000 +v 3.000000 1.000000 -1.000000 +f 9//1 10//1 11//1 12//1 +f 13//2 16//2 15//2 14//2 +f 9//3 13//3 14//3 10//3 +f 10//4 14//4 15//4 11//4 +f 11//5 15//5 16//5 12//5 +f 13//6 9//6 12//6 16//6 diff --git a/geometry/test/proximity_engine_test.cc b/geometry/test/proximity_engine_test.cc index c780078127d1..c01cd0b14b32 100644 --- a/geometry/test/proximity_engine_test.cc +++ b/geometry/test/proximity_engine_test.cc @@ -5,7 +5,9 @@ #include #include +#include "drake/common/find_resource.h" #include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/test_utilities/expect_throws_message.h" #include "drake/geometry/shape_specification.h" namespace drake { @@ -89,6 +91,17 @@ GTEST_TEST(ProximityEngineTests, AddMixedGeometry) { EXPECT_EQ(engine.num_dynamic(), 1); } +// Tests for reading .obj files.------------------------------------------------ + +// Tests exception when we read an .obj file with two objects into Convex +GTEST_TEST(ProximityEngineTests, ExceptionTwoObjectsInObjFileForConvex) { + ProximityEngine engine; + Convex convex{drake::FindResourceOrThrow( + "drake/geometry/test/forbidden_two_cubes.obj"), 1.0}; + DRAKE_EXPECT_THROWS_MESSAGE(engine.AddDynamicGeometry(convex), + std::runtime_error, ".*one and only one object.*"); +} + // Tests for copy/move semantics. --------------------------------------------- // Tests the copy semantics of the ProximityEngine -- the copy is a complete, @@ -888,7 +901,8 @@ class BoxPenetrationTest : public ::testing::Test { TangentSphere, TangentBox, TangentStandingCylinder, - TangentProneCylinder + TangentProneCylinder, + TangentConvex }; // The test that produces *bad* results based on the box orientation. Not @@ -1011,6 +1025,8 @@ class BoxPenetrationTest : public ::testing::Test { return "standing cylinder"; case TangentProneCylinder: return "prone cylinder"; + case TangentConvex: + return "convex"; } return "undefined shape"; } @@ -1027,6 +1043,8 @@ class BoxPenetrationTest : public ::testing::Test { case TangentStandingCylinder: case TangentProneCylinder: return tangent_cylinder_; + case TangentConvex: + return tangent_convex_; } // GCC considers this function ill-formed - no apparent return value. This // exception alleviates its concern. @@ -1045,6 +1063,9 @@ class BoxPenetrationTest : public ::testing::Test { pose.translation() = Vector3d{0, 0, -kRadius}; break; case TangentBox: + // The tangent convex is a cube of the same size as the tangent box. + // That is why we give them the same pose. + case TangentConvex: case TangentStandingCylinder: pose.translation() = Vector3d{0, 0, -kLength / 2}; break; @@ -1073,6 +1094,10 @@ class BoxPenetrationTest : public ::testing::Test { const Box tangent_box_{kLength, kLength, kLength}; const HalfSpace tangent_plane_; // Default construct the z = 0 plane. const Cylinder tangent_cylinder_{kRadius, kLength}; + // We scale the convex shape by 5.0 to match the tangent_box_ of size 10.0. + // The file "quad_cube.obj" contains the cube of size 2.0. + const Convex tangent_convex_{drake::FindResourceOrThrow( + "drake/geometry/test/quad_cube.obj"), 5.0}; const Vector3d p_WC_{0, 0, -kDepth}; const Vector3d p_BoC_B_{-0.5, -0.5, -0.5}; @@ -1130,6 +1155,16 @@ TEST_F(BoxPenetrationTest, TangentProneCylinder2) { TestCollision2(TangentProneCylinder, 1e-4); } +TEST_F(BoxPenetrationTest, TangentConvex1) { + // TODO(DamrongGuoy): We should check why we cannot use a smaller tolerance. + TestCollision1(TangentConvex, 1e-3); +} + +TEST_F(BoxPenetrationTest, TangentConvex2) { + // TODO(DamrongGuoy): We should check why we cannot use a smaller tolerance. + TestCollision2(TangentConvex, 1e-3); +} + } // namespace } // namespace internal } // namespace geometry diff --git a/geometry/test/quad_cube.obj b/geometry/test/quad_cube.obj new file mode 100644 index 000000000000..d3c496bd83a4 --- /dev/null +++ b/geometry/test/quad_cube.obj @@ -0,0 +1,20 @@ +v 1.000000 -1.000000 -1.000000 +v 1.000000 -1.000000 1.000000 +v -1.000000 -1.000000 1.000000 +v -1.000000 -1.000000 -1.000000 +v 1.000000 1.000000 -1.000000 +v 1.000000 1.000000 1.000001 +v -1.000000 1.000000 1.000000 +v -1.000000 1.000000 -1.000000 +vn 0.0000 -1.0000 0.0000 +vn 0.0000 1.0000 0.0000 +vn 1.0000 0.0000 0.0000 +vn -0.0000 -0.0000 1.0000 +vn -1.0000 -0.0000 -0.0000 +vn 0.0000 0.0000 -1.0000 +f 1//1 2//1 3//1 4//1 +f 5//2 8//2 7//2 6//2 +f 1//3 5//3 6//3 2//3 +f 2//4 6//4 7//4 3//4 +f 3//5 7//5 8//5 4//5 +f 5//6 1//6 4//6 8//6 diff --git a/geometry/test/shape_specification_test.cc b/geometry/test/shape_specification_test.cc index 5c558441056c..70b261a4dd03 100644 --- a/geometry/test/shape_specification_test.cc +++ b/geometry/test/shape_specification_test.cc @@ -38,10 +38,16 @@ class ReifierTest : public ShapeReifier, public ::testing::Test { // supported. EXPECT_TRUE(false) << "Don't test Meshes until they are fully supported"; } + void ImplementGeometry(const Convex&, void* data) override { + received_user_data_ = data; + convex_made_ = true; + } void Reset() { + box_made_ = false; sphere_made_ = false; half_space_made_ = false; cylinder_made_ = false; + convex_made_ = false; received_user_data_ = nullptr; } @@ -50,6 +56,7 @@ class ReifierTest : public ShapeReifier, public ::testing::Test { bool sphere_made_{false}; bool cylinder_made_{false}; bool half_space_made_{false}; + bool convex_made_{false}; void* received_user_data_{nullptr}; }; @@ -70,6 +77,7 @@ TEST_F(ReifierTest, ReificationDifferentiation) { EXPECT_FALSE(half_space_made_); EXPECT_FALSE(cylinder_made_); EXPECT_FALSE(box_made_); + EXPECT_FALSE(convex_made_); Reset(); @@ -79,6 +87,7 @@ TEST_F(ReifierTest, ReificationDifferentiation) { EXPECT_TRUE(half_space_made_); EXPECT_FALSE(cylinder_made_); EXPECT_FALSE(box_made_); + EXPECT_FALSE(convex_made_); Reset(); @@ -88,6 +97,7 @@ TEST_F(ReifierTest, ReificationDifferentiation) { EXPECT_FALSE(half_space_made_); EXPECT_TRUE(cylinder_made_); EXPECT_FALSE(box_made_); + EXPECT_FALSE(convex_made_); Reset(); @@ -97,6 +107,17 @@ TEST_F(ReifierTest, ReificationDifferentiation) { EXPECT_FALSE(half_space_made_); EXPECT_FALSE(cylinder_made_); EXPECT_TRUE(box_made_); + EXPECT_FALSE(convex_made_); + + Reset(); + + Convex convex{"fictitious_name.obj", 1.0}; + convex.Reify(this); + EXPECT_FALSE(sphere_made_); + EXPECT_FALSE(half_space_made_); + EXPECT_FALSE(cylinder_made_); + EXPECT_FALSE(box_made_); + EXPECT_TRUE(convex_made_); } // Confirms that the ReifiableShape properly clones the right types.