From 37e1312a9f8b4db7669c2d1b95fb9c48721ff974 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 7 Jan 2021 16:17:31 -0800 Subject: [PATCH 01/18] Add heightmap component mirroring mesh component Make dependency for dartsim plugin. Signed-off-by: Steve Peters --- CMakeLists.txt | 4 +- dartsim/CMakeLists.txt | 4 +- heightmap/CMakeLists.txt | 11 ++ .../physics/heightmap/HeightmapShape.hh | 139 ++++++++++++++++++ .../heightmap/detail/HeightmapShape.hh | 74 ++++++++++ 5 files changed, 229 insertions(+), 3 deletions(-) create mode 100644 heightmap/CMakeLists.txt create mode 100644 heightmap/include/ignition/physics/heightmap/HeightmapShape.hh create mode 100644 heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c2b20e9a..5aeeb8ca3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,7 +37,7 @@ message(STATUS "\n\n-- ====== Finding Dependencies ======") # Find ignition-common ign_find_package(ignition-common4 COMPONENTS graphics profiler - REQUIRED_BY mesh dartsim tpe tpelib) + REQUIRED_BY heightmap mesh dartsim tpe tpelib) set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR}) #-------------------------------------- @@ -91,7 +91,7 @@ set(IGNITION_PHYSICS_ENGINE_INSTALL_DIR # Configure the build #============================================================================ ign_configure_build(QUIT_IF_BUILD_ERRORS - COMPONENTS sdf mesh dartsim tpe) + COMPONENTS sdf heightmap mesh dartsim tpe) #============================================================================ diff --git a/dartsim/CMakeLists.txt b/dartsim/CMakeLists.txt index ca8a11cf2..83bc1b0cd 100644 --- a/dartsim/CMakeLists.txt +++ b/dartsim/CMakeLists.txt @@ -2,7 +2,7 @@ # This component expresses custom features of the dartsim plugin, which can # expose native dartsim data types. ign_add_component(dartsim INTERFACE - DEPENDS_ON_COMPONENTS sdf mesh + DEPENDS_ON_COMPONENTS sdf heightmap mesh GET_TARGET_NAME features) target_link_libraries(${features} INTERFACE ${DART_LIBRARIES}) @@ -29,6 +29,7 @@ target_link_libraries(${dartsim_plugin} PUBLIC ${features} ${PROJECT_LIBRARY_TARGET_NAME}-sdf + ${PROJECT_LIBRARY_TARGET_NAME}-heightmap ${PROJECT_LIBRARY_TARGET_NAME}-mesh ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ignition-math${IGN_MATH_VER}::eigen3 @@ -65,6 +66,7 @@ ign_build_tests( ignition-plugin${IGN_PLUGIN_VER}::loader ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ${PROJECT_LIBRARY_TARGET_NAME}-sdf + ${PROJECT_LIBRARY_TARGET_NAME}-heightmap ${PROJECT_LIBRARY_TARGET_NAME}-mesh TEST_LIST tests) diff --git a/heightmap/CMakeLists.txt b/heightmap/CMakeLists.txt new file mode 100644 index 000000000..43051af9f --- /dev/null +++ b/heightmap/CMakeLists.txt @@ -0,0 +1,11 @@ + +ign_add_component(heightmap INTERFACE + GET_TARGET_NAME heightmap) + +target_link_libraries(${heightmap} + INTERFACE + ignition-common${IGN_COMMON_VER}::graphics) + +install( + DIRECTORY include/ + DESTINATION "${IGN_INCLUDE_INSTALL_DIR_FULL}") diff --git a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh new file mode 100644 index 000000000..213ad2513 --- /dev/null +++ b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_PHYSICS_HEIGHTMAP_HEIGHTMAPSHAPE_HH_ +#define IGNITION_PHYSICS_HEIGHTMAP_HEIGHTMAPSHAPE_HH_ + +#include + +#include + +#include +#include + +namespace ignition +{ +namespace physics +{ +namespace heightmap +{ + IGN_PHYSICS_DECLARE_SHAPE_TYPE(HeightmapShape) + + ///////////////////////////////////////////////// + class GetHeightmapShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class HeightmapShape : public virtual Entity + { + public: using Dimensions = + typename FromPolicy::template Use; + + /// \brief Get the size of the triangle heightmap. + /// \returns the size of the triangle heightmap. + public: Dimensions GetSize() const; + + /// \brief Get the scaling factor that is being applied to the heightmap. + /// \returns the scaling factor that is being applied to the heightmap. + public: Dimensions GetScale() const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: virtual Dimensions GetHeightmapShapeSize( + const Identity &_heightmapID) const = 0; + + public: virtual Dimensions GetHeightmapShapeScale( + const Identity &_heightmapID) const = 0; + }; + }; + + ///////////////////////////////////////////////// + class SetHeightmapShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class HeightmapShape : public virtual Entity + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: void SetScale(const Dimensions &_dimensions); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: void SetHeightmapShapeScale( + const Identity &_heightmapID, + const Dimensions &_dimensions) = 0; + }; + }; + + ///////////////////////////////////////////////// + class AttachHeightmapShapeFeature + : public virtual FeatureWithRequirements + { + public: template + class Link : public virtual Feature::Link + { + public: using PoseType = + typename FromPolicy::template Use; + + public: using Dimensions = + typename FromPolicy::template Use; + + public: using ShapePtrType = HeightmapShapePtr; + + public: ShapePtrType AttachHeightmapShape( + const std::string &_name, + const ignition::common::HeightmapData &_heightmapData, + const PoseType &_pose = PoseType::Identity(), + const Dimensions &_scale = Dimensions::Ones()); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using PoseType = + typename FromPolicy::template Use; + + public: using Dimensions = + typename FromPolicy::template Use; + + public: virtual Identity AttachHeightmapShape( + const Identity &_linkID, + const std::string &_name, + const ignition::common::HeightmapData &_heightmapData, + const PoseType &_pose, + const Dimensions &_scale) = 0; + }; + }; +} +} +} + +#include + +#endif // IGNITION_PHYSICS_HEIGHTMAP_HEIGHTMAPSHAPE_HH_ diff --git a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh new file mode 100644 index 000000000..cf4a5f711 --- /dev/null +++ b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_PHYSICS_HEIGHTMAP_DETAIL_HEIGHTMAPSHAPE_HH_ +#define IGNITION_PHYSICS_HEIGHTMAP_DETAIL_HEIGHTMAPSHAPE_HH_ + +#include + +#include + +namespace ignition +{ +namespace physics +{ +namespace heightmap +{ + ///////////////////////////////////////////////// + template + auto GetHeightmapShapeProperties::HeightmapShape::GetSize() const + -> Dimensions + { + return this->template Interface() + ->GetHeightmapShapeSize(this->identity); + } + + ///////////////////////////////////////////////// + template + auto GetHeightmapShapeProperties::HeightmapShape::GetScale() const + -> Dimensions + { + return this->template Interface() + ->GetHeightmapShapeScale(this->identity); + } + + ///////////////////////////////////////////////// + template + void SetHeightmapShapeProperties::HeightmapShape::SetScale( + const Dimensions &_dimensions) + { + this->template Interface() + ->SetHeightmapShapeScale(this->identity, _dimensions); + } + + ///////////////////////////////////////////////// + template + auto AttachHeightmapShapeFeature::Link::AttachHeightmapShape( + const std::string &_name, + const ignition::common::HeightmapData &_heightmapData, + const PoseType &_pose, + const Dimensions &_scale) -> ShapePtrType + { + return ShapePtrType(this->pimpl, + this->template Interface() + ->AttachHeightmapShape(this->identity, _name, _heightmapData, _pose, _scale)); + } +} +} +} + +#endif // IGNITION_PHYSICS_MESH_DETAIL_MESHSHAPE_HH_ From 77d76610959a378d19f967c659830ccb29c65766 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 8 Jan 2021 12:45:24 -0800 Subject: [PATCH 02/18] Copy CustomMeshShape.* to CustomHeightmapShape.* Signed-off-by: Steve Peters --- dartsim/src/CustomHeightmapShape.cc | 227 ++++++++++++++++++++++++++++ dartsim/src/CustomHeightmapShape.hh | 42 +++++ 2 files changed, 269 insertions(+) create mode 100644 dartsim/src/CustomHeightmapShape.cc create mode 100644 dartsim/src/CustomHeightmapShape.hh diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc new file mode 100644 index 000000000..0fb8fd4a6 --- /dev/null +++ b/dartsim/src/CustomHeightmapShape.cc @@ -0,0 +1,227 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "CustomMeshShape.hh" + +#include +#include + +#include +#include + +namespace ignition { +namespace physics { +namespace dartsim { + +namespace { +///////////////////////////////////////////////// +unsigned int CheckNumVerticesPerFaces( + const ignition::common::SubMesh &_inputSubmesh, + const unsigned int _submeshIndex, + const std::string &_path) +{ + using namespace ignition::common; + + const SubMesh::PrimitiveType type = _inputSubmesh.SubMeshPrimitiveType(); + + const auto printWarning = [&](const std::string type_str) + { + ignwarn << "[dartsim::CustomMeshShape] The dartsim plugin does not support " + << type_str << " meshes, requested by submesh [" << _submeshIndex + << ":" << _inputSubmesh.Name() << "] in the input mesh [" << _path + << "]. This submesh will be ignored.\n"; + + return 0u; + }; + + if (SubMesh::POINTS == type) + return 1u; + + if (SubMesh::LINES == type) + return 2u; + + if (SubMesh::LINESTRIPS == type) + return printWarning("linestrip"); + + if (SubMesh::TRIANGLES == type) + return 3u; + + if (SubMesh::TRIFANS == type) + return printWarning("trifan"); + + if (SubMesh::TRISTRIPS == type) + return printWarning("tristrip"); + + ignwarn << "[dartsim::CustomMeshShape] One of the submeshes [" + << _submeshIndex << ":" << _inputSubmesh.Name() << "] in the input " + << "mesh [" << _path << "] has an unknown primitive type value [" + << type << "]. This submesh will be ignored.\n"; + + return 0; +} + +///////////////////////////////////////////////// +unsigned int GetPrimitiveType( + const ignition::common::SubMesh &_inputSubmesh) +{ + using namespace ignition::common; + + const SubMesh::PrimitiveType type = _inputSubmesh.SubMeshPrimitiveType(); + + if (SubMesh::POINTS == type) + return aiPrimitiveType_POINT; + + if (SubMesh::LINES == type) + return aiPrimitiveType_LINE; + + if (SubMesh::TRIANGLES == type) + return aiPrimitiveType_TRIANGLE; + + return 0; +} +} + +///////////////////////////////////////////////// +CustomMeshShape::CustomMeshShape( + const ignition::common::Mesh &_input, + const Eigen::Vector3d &_scale) + : dart::dynamics::MeshShape(_scale, nullptr) +{ + // Create the root + aiNode* node = new aiNode; + + // Allocate space for pointer arrays + const unsigned int numSubMeshes = _input.SubMeshCount(); + node->mNumMeshes = numSubMeshes; + node->mMeshes = new unsigned int[numSubMeshes]; + for (unsigned int i = 0; i < numSubMeshes; ++i) + node->mMeshes[i] = i; + + aiScene *scene = new aiScene; + scene->mNumMeshes = numSubMeshes; + scene->mMeshes = new aiMesh*[numSubMeshes]; + scene->mRootNode = node; + + // NOTE(MXG): We are ignoring materials and colors from the mesh, because + // gazebo will only use dartsim for physics, not for rendering. + scene->mMaterials = nullptr; + + // Fill in submesh contents + for (unsigned int i = 0; i < numSubMeshes; ++i) + { + const ignition::common::SubMeshPtr &inputSubmesh = + _input.SubMeshByIndex(i).lock(); + + scene->mMeshes[i] = nullptr; + + if (!inputSubmesh) + { + ignerr << "[dartsim::CustomMeshShape] One of the submeshes [" << i + << "] in the input mesh [" << _input.Path() << "] has expired!\n"; + continue; + } + + std::unique_ptr mesh = std::make_unique(); + mesh->mMaterialIndex = static_cast(-1); + + const unsigned int numVertices = inputSubmesh->VertexCount(); + if (inputSubmesh->NormalCount() != numVertices) + { + ignerr << "[dartsim::CustomMeshShape] One of the submeshes [" << i << ":" + << inputSubmesh->Name() << "] in the input mesh [" << _input.Path() + << "] does not have a normal count [" + << inputSubmesh->NormalCount() << "] that matches its vertex " + << "count [" << numVertices << "]. This submesh will be " + << "ignored!\n"; + continue; + } + + mesh->mNumVertices = numVertices; + mesh->mVertices = new aiVector3D[numVertices]; + mesh->mNormals = new aiVector3D[numVertices]; + + const unsigned int numVerticesPerFace = + CheckNumVerticesPerFaces(*inputSubmesh, i, _input.Path()); + if (0 == numVerticesPerFace) + continue; + + mesh->mPrimitiveTypes = GetPrimitiveType(*inputSubmesh); + if (0 == mesh->mPrimitiveTypes) + continue; + + const unsigned int numFaces = static_cast( + static_cast(inputSubmesh->IndexCount()) + /static_cast(numVerticesPerFace)); + + // Fill in the contents of each submesh + mesh->mNumFaces = numFaces; + mesh->mFaces = new aiFace[numFaces]; + unsigned int currentPrimitiveIndex = 0; + bool primitiveIndexOverflow = false; + for (unsigned int j = 0; j < numFaces; ++j) + { + mesh->mFaces[j].mNumIndices = numVerticesPerFace; + mesh->mFaces[j].mIndices = new unsigned int[numVerticesPerFace]; + + for (unsigned int k = 0; k < numVerticesPerFace; ++k) + { + int vertexIndex = inputSubmesh->Index(currentPrimitiveIndex); + if (vertexIndex == -1) + { + ignwarn << "[dartsim::CustomMeshShape] The submesh [" << i << ":" + << inputSubmesh->Name() << "] of mesh [" << _input.Path() + << "] overflowed at primitive index [" + << currentPrimitiveIndex << "]. Its expected number of " + << "primitive indices is [" << _input.IndexCount() + << "]. This submesh will be ignored.\n"; + primitiveIndexOverflow = true; + break; + } + + mesh->mFaces[j].mIndices[k] = static_cast(vertexIndex); + ++currentPrimitiveIndex; + } + + if (primitiveIndexOverflow) + break; + } + + if (primitiveIndexOverflow) + continue; + + for (unsigned int j = 0; j < numVertices; ++j) + { + const ignition::math::Vector3d &v = inputSubmesh->Vertex(j); + for (unsigned int k = 0; k < 3; ++k) + mesh->mVertices[j][k] = static_cast(v[k]); + + const ignition::math::Vector3d &n = inputSubmesh->Normal(j); + for (unsigned int k = 0; k < 3; ++k) + mesh->mNormals[j][k] = static_cast(n[k]); + } + + scene->mMeshes[i] = mesh.release(); + } + + this->mMesh = scene; + this->mIsBoundingBoxDirty = true; + this->mIsVolumeDirty = true; +} + +} +} +} diff --git a/dartsim/src/CustomHeightmapShape.hh b/dartsim/src/CustomHeightmapShape.hh new file mode 100644 index 000000000..9e9714646 --- /dev/null +++ b/dartsim/src/CustomHeightmapShape.hh @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMHEIGHTMAPSHAPE_HH_ +#define IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMHEIGHTMAPSHAPE_HH_ + +#include +#include + +namespace ignition { +namespace physics { +namespace dartsim { + +/// \brief This class creates a custom derivative of dartsim's HeightmapShape class +/// which allows an ignition::common::Heightmap to be converted into a HeightmapShape that +/// can be used by dartsim. +class CustomHeightmapShape : public dart::dynamics::HeightmapShape +{ + public: CustomHeightmapShape( + const ignition::common::HeightmapData &_input, + const Eigen::Vector3d &_scale); +}; + +} +} +} + +#endif // IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMHEIGHTMAPSHAPE_HH_ From a4d07ab2b51c12a2d2e6c0487b0ed963bdf053b9 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 8 Jan 2021 12:45:55 -0800 Subject: [PATCH 03/18] Start adding heightmap code still needs more changes in CustomHeightmapShape Signed-off-by: Steve Peters --- dartsim/src/CustomHeightmapShape.cc | 14 +++---- dartsim/src/CustomHeightmapShape.hh | 2 +- dartsim/src/SDFFeatures.cc | 15 +++++++ dartsim/src/ShapeFeatures.cc | 63 +++++++++++++++++++++++++++++ dartsim/src/ShapeFeatures.hh | 22 ++++++++++ 5 files changed, 108 insertions(+), 8 deletions(-) diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc index 0fb8fd4a6..95e5c58e7 100644 --- a/dartsim/src/CustomHeightmapShape.cc +++ b/dartsim/src/CustomHeightmapShape.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -15,7 +15,7 @@ * */ -#include "CustomMeshShape.hh" +#include "CustomHeightmapShape.hh" #include #include @@ -40,7 +40,7 @@ unsigned int CheckNumVerticesPerFaces( const auto printWarning = [&](const std::string type_str) { - ignwarn << "[dartsim::CustomMeshShape] The dartsim plugin does not support " + ignwarn << "[dartsim::CustomHeightmapShape] The dartsim plugin does not support " << type_str << " meshes, requested by submesh [" << _submeshIndex << ":" << _inputSubmesh.Name() << "] in the input mesh [" << _path << "]. This submesh will be ignored.\n"; @@ -66,7 +66,7 @@ unsigned int CheckNumVerticesPerFaces( if (SubMesh::TRISTRIPS == type) return printWarning("tristrip"); - ignwarn << "[dartsim::CustomMeshShape] One of the submeshes [" + ignwarn << "[dartsim::CustomHeightmapShape] One of the submeshes [" << _submeshIndex << ":" << _inputSubmesh.Name() << "] in the input " << "mesh [" << _path << "] has an unknown primitive type value [" << type << "]. This submesh will be ignored.\n"; @@ -96,10 +96,10 @@ unsigned int GetPrimitiveType( } ///////////////////////////////////////////////// -CustomMeshShape::CustomMeshShape( - const ignition::common::Mesh &_input, +CustomHeightmapShape::CustomHeightmapShape( + const ignition::common::HeightmapData &_input, const Eigen::Vector3d &_scale) - : dart::dynamics::MeshShape(_scale, nullptr) + : dart::dynamics::HeightmapShape(_scale, nullptr) { // Create the root aiNode* node = new aiNode; diff --git a/dartsim/src/CustomHeightmapShape.hh b/dartsim/src/CustomHeightmapShape.hh index 9e9714646..4c289fb32 100644 --- a/dartsim/src/CustomHeightmapShape.hh +++ b/dartsim/src/CustomHeightmapShape.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 6ee918f8b..02faf16ed 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -44,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -298,6 +300,17 @@ static ShapeAndTransform ConstructPlane( Eigen::Vector3d(planeDim, planeDim, planeDim)), tf}; } +///////////////////////////////////////////////// +static ShapeAndTransform ConstructHeightmap( + const ::sdf::Heightmap & /*_heightmap*/) +{ + // TODO(MXG): Look into what kind of heightmap URI we get here. Will it just + // be a local file name, or do we need to resolve the URI? + ignerr << "Heightmap construction from an SDF has not been implemented yet " + << "for dartsim.\n"; + return {nullptr}; +} + ///////////////////////////////////////////////// static ShapeAndTransform ConstructMesh( const ::sdf::Mesh & /*_mesh*/) @@ -323,6 +336,8 @@ static ShapeAndTransform ConstructGeometry( return ConstructPlane(*_geometry.PlaneShape()); else if (_geometry.MeshShape()) return ConstructMesh(*_geometry.MeshShape()); + else if (_geometry.HeightmapShape()) + return ConstructHeightmap(*_geometry.HeightmapShape()); return {nullptr}; } diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index 11d042325..4734fe9ff 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -201,6 +202,68 @@ Identity ShapeFeatures::AttachSphereShape( return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToHeightmapShape( + const Identity &_shapeID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_shapeID); + + const dart::dynamics::ShapePtr &shape = + shapeInfo->node->getShape(); + + if (dynamic_cast(shape.get())) + return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); + + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +LinearVector3d ShapeFeatures::GetHeightmapShapeSize( + const Identity &_heightmapID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_heightmapID); + + const dart::dynamics::HeightmapShape *heightmap = + static_cast( + shapeInfo->node->getShape().get()); + + return heightmap->getBoundingBox().getMax() - heightmap->getBoundingBox().getMin(); +} + +///////////////////////////////////////////////// +LinearVector3d ShapeFeatures::GetHeightmapShapeScale( + const Identity &_heightmapID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_heightmapID); + + const dart::dynamics::HeightmapShape *heightmap = + static_cast( + shapeInfo->node->getShape().get()); + + return heightmap->getScale(); +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachHeightmapShape( + const Identity &_linkID, + const std::string &_name, + const ignition::common::HeightmapData &_heightmapData, + const Pose3d &_pose, + const LinearVector3d &_scale) +{ + auto heightmap = std::make_shared(_heightmapData, _scale); + + DartBodyNode *bn = this->ReferenceInterface(_linkID)->link.get(); + dart::dynamics::ShapeNode *sn = + bn->createShapeNodeWith( + heightmap, bn->getName() + ":" + _name); + + sn->setRelativeTransform(_pose); + const std::size_t shapeID = this->AddShape({sn, _name}); + return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); +} + ///////////////////////////////////////////////// Identity ShapeFeatures::CastToMeshShape( const Identity &_shapeID) const diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index 9ec443714..5dfb998b2 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -56,6 +57,10 @@ struct ShapeFeatureList : FeatureList< // SetSphereShapeProperties, AttachSphereShapeFeature, + mesh::GetHeightMapShapeProperties, +// mesh::SetHeightMapShapeProperties, + mesh::AttachHeightMapShapeFeature, + mesh::GetMeshShapeProperties, // mesh::SetMeshShapeProperties, mesh::AttachMeshShapeFeature, @@ -122,6 +127,23 @@ class ShapeFeatures : const Pose3d &_pose) override; + // ----- Heightmap Features ----- + public: Identity CastToHeightmapShape( + const Identity &_shapeID) const override; + + public: LinearVector3d GetHeightmapShapeSize( + const Identity &_heightmapID) const override; + + public: LinearVector3d GetHeightmapShapeScale( + const Identity &_heightmapID) const override; + + public: Identity AttachHeightmapShape( + const Identity &_linkID, + const std::string &_name, + const ignition::common::HeightmapData &_heightmapData, + const Pose3d &_pose, + const LinearVector3d &_scale) override; + // ----- Mesh Features ----- public: Identity CastToMeshShape( const Identity &_shapeID) const override; From e1172e2dbcbbb868bd8ae72e666022f363284f13 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 26 Jan 2021 11:49:51 -0800 Subject: [PATCH 04/18] Latest prototyping, experimenting with API Signed-off-by: Steve Peters --- dartsim/src/CustomHeightmapShape.cc | 10 ++++++++-- dartsim/src/ShapeFeatures.cc | 1 + 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc index 95e5c58e7..3d8ac748f 100644 --- a/dartsim/src/CustomHeightmapShape.cc +++ b/dartsim/src/CustomHeightmapShape.cc @@ -98,9 +98,15 @@ unsigned int GetPrimitiveType( ///////////////////////////////////////////////// CustomHeightmapShape::CustomHeightmapShape( const ignition::common::HeightmapData &_input, - const Eigen::Vector3d &_scale) - : dart::dynamics::HeightmapShape(_scale, nullptr) + const Eigen::Vector3d &_size, + const int _subSampling) + : dart::dynamics::HeightmapShape() { + std::vector heights; + const bool flipY = false; + const int vertSize = (_input.GetWidth() * _subSampling) - _subSampling + 1; + ignition::math::Vector3d scale(_size(0) / vertSize, _size(1) / vertSize, 1.0); + _input.FillHeightMap(_subSampling, vertSize, _size, scale, flipY, heights); // Create the root aiNode* node = new aiNode; diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index 4734fe9ff..f9aa468e7 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -27,6 +27,7 @@ #include #include +#include "CustomHeightmapShape.hh" #include "CustomMeshShape.hh" namespace ignition { From 2c8cf7def74b86d3bc13743099fe7ba0a6db2e8f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 25 Feb 2021 20:01:59 -0800 Subject: [PATCH 05/18] fix compilation Signed-off-by: Louise Poubel --- dartsim/src/CustomHeightmapShape.hh | 2 +- dartsim/src/ShapeFeatures.cc | 12 ++++++------ dartsim/src/ShapeFeatures.hh | 6 +++--- .../ignition/physics/heightmap/HeightmapShape.hh | 2 +- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/dartsim/src/CustomHeightmapShape.hh b/dartsim/src/CustomHeightmapShape.hh index 4c289fb32..fff82bec4 100644 --- a/dartsim/src/CustomHeightmapShape.hh +++ b/dartsim/src/CustomHeightmapShape.hh @@ -28,7 +28,7 @@ namespace dartsim { /// \brief This class creates a custom derivative of dartsim's HeightmapShape class /// which allows an ignition::common::Heightmap to be converted into a HeightmapShape that /// can be used by dartsim. -class CustomHeightmapShape : public dart::dynamics::HeightmapShape +class CustomHeightmapShape : public dart::dynamics::HeightmapShape { public: CustomHeightmapShape( const ignition::common::HeightmapData &_input, diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index f9aa468e7..4636c6d70 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -203,7 +203,7 @@ Identity ShapeFeatures::AttachSphereShape( return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } -///////////////////////////////////////////////// +////////////////////////////////////////////////// Identity ShapeFeatures::CastToHeightmapShape( const Identity &_shapeID) const { @@ -212,7 +212,7 @@ Identity ShapeFeatures::CastToHeightmapShape( const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape(); - if (dynamic_cast(shape.get())) + if (dynamic_cast *>(shape.get())) return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); return this->GenerateInvalidId(); @@ -224,8 +224,8 @@ LinearVector3d ShapeFeatures::GetHeightmapShapeSize( { const auto *shapeInfo = this->ReferenceInterface(_heightmapID); - const dart::dynamics::HeightmapShape *heightmap = - static_cast( + const dart::dynamics::HeightmapShape *heightmap = + static_cast *>( shapeInfo->node->getShape().get()); return heightmap->getBoundingBox().getMax() - heightmap->getBoundingBox().getMin(); @@ -237,8 +237,8 @@ LinearVector3d ShapeFeatures::GetHeightmapShapeScale( { const auto *shapeInfo = this->ReferenceInterface(_heightmapID); - const dart::dynamics::HeightmapShape *heightmap = - static_cast( + const dart::dynamics::HeightmapShape *heightmap = + static_cast *>( shapeInfo->node->getShape().get()); return heightmap->getScale(); diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index 5dfb998b2..867ddd913 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -57,9 +57,9 @@ struct ShapeFeatureList : FeatureList< // SetSphereShapeProperties, AttachSphereShapeFeature, - mesh::GetHeightMapShapeProperties, -// mesh::SetHeightMapShapeProperties, - mesh::AttachHeightMapShapeFeature, + heightmap::GetHeightmapShapeProperties, +// heightmap::SetHeightmapShapeProperties, + heightmap::AttachHeightmapShapeFeature, mesh::GetMeshShapeProperties, // mesh::SetMeshShapeProperties, diff --git a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh index 213ad2513..4a6f2671e 100644 --- a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh @@ -20,7 +20,7 @@ #include -#include +#include #include #include From acfbafc79e67a28fae83bd78c1916638fc4b5f8b Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 2 Mar 2021 17:04:54 -0800 Subject: [PATCH 06/18] fix compilation again Signed-off-by: Louise Poubel --- dartsim/src/CustomHeightmapShape.cc | 254 +++++++++--------- dartsim/src/CustomHeightmapShape.hh | 3 +- dartsim/src/ShapeFeatures.cc | 6 +- dartsim/src/ShapeFeatures.hh | 3 +- .../physics/heightmap/HeightmapShape.hh | 6 +- .../heightmap/detail/HeightmapShape.hh | 6 +- 6 files changed, 144 insertions(+), 134 deletions(-) diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc index 3d8ac748f..7043f3cc2 100644 --- a/dartsim/src/CustomHeightmapShape.cc +++ b/dartsim/src/CustomHeightmapShape.cc @@ -19,6 +19,8 @@ #include #include +#include +#include #include #include @@ -99,133 +101,133 @@ unsigned int GetPrimitiveType( CustomHeightmapShape::CustomHeightmapShape( const ignition::common::HeightmapData &_input, const Eigen::Vector3d &_size, - const int _subSampling) - : dart::dynamics::HeightmapShape() + int _subSampling) + : dart::dynamics::HeightmapShape() { - std::vector heights; - const bool flipY = false; - const int vertSize = (_input.GetWidth() * _subSampling) - _subSampling + 1; - ignition::math::Vector3d scale(_size(0) / vertSize, _size(1) / vertSize, 1.0); - _input.FillHeightMap(_subSampling, vertSize, _size, scale, flipY, heights); - // Create the root - aiNode* node = new aiNode; - - // Allocate space for pointer arrays - const unsigned int numSubMeshes = _input.SubMeshCount(); - node->mNumMeshes = numSubMeshes; - node->mMeshes = new unsigned int[numSubMeshes]; - for (unsigned int i = 0; i < numSubMeshes; ++i) - node->mMeshes[i] = i; - - aiScene *scene = new aiScene; - scene->mNumMeshes = numSubMeshes; - scene->mMeshes = new aiMesh*[numSubMeshes]; - scene->mRootNode = node; - - // NOTE(MXG): We are ignoring materials and colors from the mesh, because - // gazebo will only use dartsim for physics, not for rendering. - scene->mMaterials = nullptr; - - // Fill in submesh contents - for (unsigned int i = 0; i < numSubMeshes; ++i) - { - const ignition::common::SubMeshPtr &inputSubmesh = - _input.SubMeshByIndex(i).lock(); - - scene->mMeshes[i] = nullptr; - - if (!inputSubmesh) - { - ignerr << "[dartsim::CustomMeshShape] One of the submeshes [" << i - << "] in the input mesh [" << _input.Path() << "] has expired!\n"; - continue; - } - - std::unique_ptr mesh = std::make_unique(); - mesh->mMaterialIndex = static_cast(-1); - - const unsigned int numVertices = inputSubmesh->VertexCount(); - if (inputSubmesh->NormalCount() != numVertices) - { - ignerr << "[dartsim::CustomMeshShape] One of the submeshes [" << i << ":" - << inputSubmesh->Name() << "] in the input mesh [" << _input.Path() - << "] does not have a normal count [" - << inputSubmesh->NormalCount() << "] that matches its vertex " - << "count [" << numVertices << "]. This submesh will be " - << "ignored!\n"; - continue; - } - - mesh->mNumVertices = numVertices; - mesh->mVertices = new aiVector3D[numVertices]; - mesh->mNormals = new aiVector3D[numVertices]; - - const unsigned int numVerticesPerFace = - CheckNumVerticesPerFaces(*inputSubmesh, i, _input.Path()); - if (0 == numVerticesPerFace) - continue; - - mesh->mPrimitiveTypes = GetPrimitiveType(*inputSubmesh); - if (0 == mesh->mPrimitiveTypes) - continue; - - const unsigned int numFaces = static_cast( - static_cast(inputSubmesh->IndexCount()) - /static_cast(numVerticesPerFace)); - - // Fill in the contents of each submesh - mesh->mNumFaces = numFaces; - mesh->mFaces = new aiFace[numFaces]; - unsigned int currentPrimitiveIndex = 0; - bool primitiveIndexOverflow = false; - for (unsigned int j = 0; j < numFaces; ++j) - { - mesh->mFaces[j].mNumIndices = numVerticesPerFace; - mesh->mFaces[j].mIndices = new unsigned int[numVerticesPerFace]; - - for (unsigned int k = 0; k < numVerticesPerFace; ++k) - { - int vertexIndex = inputSubmesh->Index(currentPrimitiveIndex); - if (vertexIndex == -1) - { - ignwarn << "[dartsim::CustomMeshShape] The submesh [" << i << ":" - << inputSubmesh->Name() << "] of mesh [" << _input.Path() - << "] overflowed at primitive index [" - << currentPrimitiveIndex << "]. Its expected number of " - << "primitive indices is [" << _input.IndexCount() - << "]. This submesh will be ignored.\n"; - primitiveIndexOverflow = true; - break; - } - - mesh->mFaces[j].mIndices[k] = static_cast(vertexIndex); - ++currentPrimitiveIndex; - } - - if (primitiveIndexOverflow) - break; - } - - if (primitiveIndexOverflow) - continue; - - for (unsigned int j = 0; j < numVertices; ++j) - { - const ignition::math::Vector3d &v = inputSubmesh->Vertex(j); - for (unsigned int k = 0; k < 3; ++k) - mesh->mVertices[j][k] = static_cast(v[k]); - - const ignition::math::Vector3d &n = inputSubmesh->Normal(j); - for (unsigned int k = 0; k < 3; ++k) - mesh->mNormals[j][k] = static_cast(n[k]); - } - - scene->mMeshes[i] = mesh.release(); - } - - this->mMesh = scene; - this->mIsBoundingBoxDirty = true; - this->mIsVolumeDirty = true; + // std::vector heights; + // const bool flipY = false; + // const int vertSize = (_input.GetWidth() * _subSampling) - _subSampling + 1; + // ignition::math::Vector3d scale(_size(0) / vertSize, _size(1) / vertSize, 1.0); + // _input.FillHeightMap(_subSampling, vertSize, _size, scale, flipY, heights); + // // Create the root + // aiNode* node = new aiNode; + + // // Allocate space for pointer arrays + // const unsigned int numSubMeshes = _input.SubMeshCount(); + // node->mNumMeshes = numSubMeshes; + // node->mMeshes = new unsigned int[numSubMeshes]; + // for (unsigned int i = 0; i < numSubMeshes; ++i) + // node->mMeshes[i] = i; + + // aiScene *scene = new aiScene; + // scene->mNumMeshes = numSubMeshes; + // scene->mMeshes = new aiMesh*[numSubMeshes]; + // scene->mRootNode = node; + + // // NOTE(MXG): We are ignoring materials and colors from the mesh, because + // // gazebo will only use dartsim for physics, not for rendering. + // scene->mMaterials = nullptr; + + // // Fill in submesh contents + // for (unsigned int i = 0; i < numSubMeshes; ++i) + // { + // const ignition::common::SubMeshPtr &inputSubmesh = + // _input.SubMeshByIndex(i).lock(); + + // scene->mMeshes[i] = nullptr; + + // if (!inputSubmesh) + // { + // ignerr << "[dartsim::CustomMeshShape] One of the submeshes [" << i + // << "] in the input mesh [" << _input.Path() << "] has expired!\n"; + // continue; + // } + + // std::unique_ptr mesh = std::make_unique(); + // mesh->mMaterialIndex = static_cast(-1); + + // const unsigned int numVertices = inputSubmesh->VertexCount(); + // if (inputSubmesh->NormalCount() != numVertices) + // { + // ignerr << "[dartsim::CustomMeshShape] One of the submeshes [" << i << ":" + // << inputSubmesh->Name() << "] in the input mesh [" << _input.Path() + // << "] does not have a normal count [" + // << inputSubmesh->NormalCount() << "] that matches its vertex " + // << "count [" << numVertices << "]. This submesh will be " + // << "ignored!\n"; + // continue; + // } + + // mesh->mNumVertices = numVertices; + // mesh->mVertices = new aiVector3D[numVertices]; + // mesh->mNormals = new aiVector3D[numVertices]; + + // const unsigned int numVerticesPerFace = + // CheckNumVerticesPerFaces(*inputSubmesh, i, _input.Path()); + // if (0 == numVerticesPerFace) + // continue; + + // mesh->mPrimitiveTypes = GetPrimitiveType(*inputSubmesh); + // if (0 == mesh->mPrimitiveTypes) + // continue; + + // const unsigned int numFaces = static_cast( + // static_cast(inputSubmesh->IndexCount()) + // /static_cast(numVerticesPerFace)); + + // // Fill in the contents of each submesh + // mesh->mNumFaces = numFaces; + // mesh->mFaces = new aiFace[numFaces]; + // unsigned int currentPrimitiveIndex = 0; + // bool primitiveIndexOverflow = false; + // for (unsigned int j = 0; j < numFaces; ++j) + // { + // mesh->mFaces[j].mNumIndices = numVerticesPerFace; + // mesh->mFaces[j].mIndices = new unsigned int[numVerticesPerFace]; + + // for (unsigned int k = 0; k < numVerticesPerFace; ++k) + // { + // int vertexIndex = inputSubmesh->Index(currentPrimitiveIndex); + // if (vertexIndex == -1) + // { + // ignwarn << "[dartsim::CustomMeshShape] The submesh [" << i << ":" + // << inputSubmesh->Name() << "] of mesh [" << _input.Path() + // << "] overflowed at primitive index [" + // << currentPrimitiveIndex << "]. Its expected number of " + // << "primitive indices is [" << _input.IndexCount() + // << "]. This submesh will be ignored.\n"; + // primitiveIndexOverflow = true; + // break; + // } + + // mesh->mFaces[j].mIndices[k] = static_cast(vertexIndex); + // ++currentPrimitiveIndex; + // } + + // if (primitiveIndexOverflow) + // break; + // } + + // if (primitiveIndexOverflow) + // continue; + + // for (unsigned int j = 0; j < numVertices; ++j) + // { + // const ignition::math::Vector3d &v = inputSubmesh->Vertex(j); + // for (unsigned int k = 0; k < 3; ++k) + // mesh->mVertices[j][k] = static_cast(v[k]); + + // const ignition::math::Vector3d &n = inputSubmesh->Normal(j); + // for (unsigned int k = 0; k < 3; ++k) + // mesh->mNormals[j][k] = static_cast(n[k]); + // } + + // scene->mMeshes[i] = mesh.release(); + // } + + // this->mMesh = scene; + // this->mIsBoundingBoxDirty = true; + // this->mIsVolumeDirty = true; } } diff --git a/dartsim/src/CustomHeightmapShape.hh b/dartsim/src/CustomHeightmapShape.hh index fff82bec4..41d2d4fb3 100644 --- a/dartsim/src/CustomHeightmapShape.hh +++ b/dartsim/src/CustomHeightmapShape.hh @@ -32,7 +32,8 @@ class CustomHeightmapShape : public dart::dynamics::HeightmapShape { public: CustomHeightmapShape( const ignition::common::HeightmapData &_input, - const Eigen::Vector3d &_scale); + const Eigen::Vector3d &_size, + const int _subSampling); }; } diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index 4636c6d70..870f63537 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -250,9 +250,11 @@ Identity ShapeFeatures::AttachHeightmapShape( const std::string &_name, const ignition::common::HeightmapData &_heightmapData, const Pose3d &_pose, - const LinearVector3d &_scale) + const LinearVector3d &_scale, + int _subSampling) { - auto heightmap = std::make_shared(_heightmapData, _scale); + auto heightmap = std::make_shared(_heightmapData, + _scale, _subSampling); DartBodyNode *bn = this->ReferenceInterface(_linkID)->link.get(); dart::dynamics::ShapeNode *sn = diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index 867ddd913..ce17f1769 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -142,7 +142,8 @@ class ShapeFeatures : const std::string &_name, const ignition::common::HeightmapData &_heightmapData, const Pose3d &_pose, - const LinearVector3d &_scale) override; + const LinearVector3d &_scale, + int _subSampling) override; // ----- Mesh Features ----- public: Identity CastToMeshShape( diff --git a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh index 4a6f2671e..9f26b03bd 100644 --- a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh @@ -110,7 +110,8 @@ namespace heightmap const std::string &_name, const ignition::common::HeightmapData &_heightmapData, const PoseType &_pose = PoseType::Identity(), - const Dimensions &_scale = Dimensions::Ones()); + const Dimensions &_scale = Dimensions::Ones(), + int _subSampling = 1); }; public: template @@ -127,7 +128,8 @@ namespace heightmap const std::string &_name, const ignition::common::HeightmapData &_heightmapData, const PoseType &_pose, - const Dimensions &_scale) = 0; + const Dimensions &_scale, + int _subSampling) = 0; }; }; } diff --git a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh index cf4a5f711..aca5cfe57 100644 --- a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh @@ -61,11 +61,13 @@ namespace heightmap const std::string &_name, const ignition::common::HeightmapData &_heightmapData, const PoseType &_pose, - const Dimensions &_scale) -> ShapePtrType + const Dimensions &_scale, + int _subSampling) -> ShapePtrType { return ShapePtrType(this->pimpl, this->template Interface() - ->AttachHeightmapShape(this->identity, _name, _heightmapData, _pose, _scale)); + ->AttachHeightmapShape(this->identity, _name, _heightmapData, _pose, + _scale, _subSampling)); } } } From 923ae1898890d74fba092a70b336c70e3b45cf0f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 2 Mar 2021 18:38:13 -0800 Subject: [PATCH 07/18] working, remove unnecessary bits Signed-off-by: Louise Poubel --- dartsim/src/CustomHeightmapShape.cc | 207 ++---------------- dartsim/src/CustomHeightmapShape.hh | 2 +- dartsim/src/ShapeFeatures.cc | 2 +- dartsim/src/ShapeFeatures.hh | 2 +- .../physics/heightmap/HeightmapShape.hh | 4 +- .../heightmap/detail/HeightmapShape.hh | 2 +- 6 files changed, 25 insertions(+), 194 deletions(-) diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc index 7043f3cc2..e87f0de42 100644 --- a/dartsim/src/CustomHeightmapShape.cc +++ b/dartsim/src/CustomHeightmapShape.cc @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -29,205 +30,35 @@ namespace ignition { namespace physics { namespace dartsim { -namespace { -///////////////////////////////////////////////// -unsigned int CheckNumVerticesPerFaces( - const ignition::common::SubMesh &_inputSubmesh, - const unsigned int _submeshIndex, - const std::string &_path) -{ - using namespace ignition::common; - - const SubMesh::PrimitiveType type = _inputSubmesh.SubMeshPrimitiveType(); - - const auto printWarning = [&](const std::string type_str) - { - ignwarn << "[dartsim::CustomHeightmapShape] The dartsim plugin does not support " - << type_str << " meshes, requested by submesh [" << _submeshIndex - << ":" << _inputSubmesh.Name() << "] in the input mesh [" << _path - << "]. This submesh will be ignored.\n"; - - return 0u; - }; - - if (SubMesh::POINTS == type) - return 1u; - - if (SubMesh::LINES == type) - return 2u; - - if (SubMesh::LINESTRIPS == type) - return printWarning("linestrip"); - - if (SubMesh::TRIANGLES == type) - return 3u; - - if (SubMesh::TRIFANS == type) - return printWarning("trifan"); - - if (SubMesh::TRISTRIPS == type) - return printWarning("tristrip"); - - ignwarn << "[dartsim::CustomHeightmapShape] One of the submeshes [" - << _submeshIndex << ":" << _inputSubmesh.Name() << "] in the input " - << "mesh [" << _path << "] has an unknown primitive type value [" - << type << "]. This submesh will be ignored.\n"; - - return 0; -} - -///////////////////////////////////////////////// -unsigned int GetPrimitiveType( - const ignition::common::SubMesh &_inputSubmesh) -{ - using namespace ignition::common; - - const SubMesh::PrimitiveType type = _inputSubmesh.SubMeshPrimitiveType(); - - if (SubMesh::POINTS == type) - return aiPrimitiveType_POINT; - - if (SubMesh::LINES == type) - return aiPrimitiveType_LINE; - - if (SubMesh::TRIANGLES == type) - return aiPrimitiveType_TRIANGLE; - - return 0; -} -} - ///////////////////////////////////////////////// CustomHeightmapShape::CustomHeightmapShape( - const ignition::common::HeightmapData &_input, + common::HeightmapData &_input, const Eigen::Vector3d &_size, int _subSampling) : dart::dynamics::HeightmapShape() { - // std::vector heights; - // const bool flipY = false; - // const int vertSize = (_input.GetWidth() * _subSampling) - _subSampling + 1; - // ignition::math::Vector3d scale(_size(0) / vertSize, _size(1) / vertSize, 1.0); - // _input.FillHeightMap(_subSampling, vertSize, _size, scale, flipY, heights); - // // Create the root - // aiNode* node = new aiNode; - - // // Allocate space for pointer arrays - // const unsigned int numSubMeshes = _input.SubMeshCount(); - // node->mNumMeshes = numSubMeshes; - // node->mMeshes = new unsigned int[numSubMeshes]; - // for (unsigned int i = 0; i < numSubMeshes; ++i) - // node->mMeshes[i] = i; - - // aiScene *scene = new aiScene; - // scene->mNumMeshes = numSubMeshes; - // scene->mMeshes = new aiMesh*[numSubMeshes]; - // scene->mRootNode = node; - - // // NOTE(MXG): We are ignoring materials and colors from the mesh, because - // // gazebo will only use dartsim for physics, not for rendering. - // scene->mMaterials = nullptr; - - // // Fill in submesh contents - // for (unsigned int i = 0; i < numSubMeshes; ++i) - // { - // const ignition::common::SubMeshPtr &inputSubmesh = - // _input.SubMeshByIndex(i).lock(); - - // scene->mMeshes[i] = nullptr; - - // if (!inputSubmesh) - // { - // ignerr << "[dartsim::CustomMeshShape] One of the submeshes [" << i - // << "] in the input mesh [" << _input.Path() << "] has expired!\n"; - // continue; - // } - - // std::unique_ptr mesh = std::make_unique(); - // mesh->mMaterialIndex = static_cast(-1); - - // const unsigned int numVertices = inputSubmesh->VertexCount(); - // if (inputSubmesh->NormalCount() != numVertices) - // { - // ignerr << "[dartsim::CustomMeshShape] One of the submeshes [" << i << ":" - // << inputSubmesh->Name() << "] in the input mesh [" << _input.Path() - // << "] does not have a normal count [" - // << inputSubmesh->NormalCount() << "] that matches its vertex " - // << "count [" << numVertices << "]. This submesh will be " - // << "ignored!\n"; - // continue; - // } - - // mesh->mNumVertices = numVertices; - // mesh->mVertices = new aiVector3D[numVertices]; - // mesh->mNormals = new aiVector3D[numVertices]; - - // const unsigned int numVerticesPerFace = - // CheckNumVerticesPerFaces(*inputSubmesh, i, _input.Path()); - // if (0 == numVerticesPerFace) - // continue; - - // mesh->mPrimitiveTypes = GetPrimitiveType(*inputSubmesh); - // if (0 == mesh->mPrimitiveTypes) - // continue; - - // const unsigned int numFaces = static_cast( - // static_cast(inputSubmesh->IndexCount()) - // /static_cast(numVerticesPerFace)); - - // // Fill in the contents of each submesh - // mesh->mNumFaces = numFaces; - // mesh->mFaces = new aiFace[numFaces]; - // unsigned int currentPrimitiveIndex = 0; - // bool primitiveIndexOverflow = false; - // for (unsigned int j = 0; j < numFaces; ++j) - // { - // mesh->mFaces[j].mNumIndices = numVerticesPerFace; - // mesh->mFaces[j].mIndices = new unsigned int[numVerticesPerFace]; - - // for (unsigned int k = 0; k < numVerticesPerFace; ++k) - // { - // int vertexIndex = inputSubmesh->Index(currentPrimitiveIndex); - // if (vertexIndex == -1) - // { - // ignwarn << "[dartsim::CustomMeshShape] The submesh [" << i << ":" - // << inputSubmesh->Name() << "] of mesh [" << _input.Path() - // << "] overflowed at primitive index [" - // << currentPrimitiveIndex << "]. Its expected number of " - // << "primitive indices is [" << _input.IndexCount() - // << "]. This submesh will be ignored.\n"; - // primitiveIndexOverflow = true; - // break; - // } - - // mesh->mFaces[j].mIndices[k] = static_cast(vertexIndex); - // ++currentPrimitiveIndex; - // } - - // if (primitiveIndexOverflow) - // break; - // } + double heightmapSizeZ = _input.MaxElevation(); + const bool flipY = false; + const int vertSize = (_input.Width() * _subSampling) - _subSampling + 1; + math::Vector3d scale; + scale.X(_size(0) / vertSize); + scale.Y(_size(1) / vertSize); - // if (primitiveIndexOverflow) - // continue; + if (math::equal(heightmapSizeZ, 0.0)) + scale.Z(1.0); + else + scale.Z(fabs(_size(2)) / heightmapSizeZ); - // for (unsigned int j = 0; j < numVertices; ++j) - // { - // const ignition::math::Vector3d &v = inputSubmesh->Vertex(j); - // for (unsigned int k = 0; k < 3; ++k) - // mesh->mVertices[j][k] = static_cast(v[k]); + auto sizeIgn = ignition::math::eigen3::convert(_size); - // const ignition::math::Vector3d &n = inputSubmesh->Normal(j); - // for (unsigned int k = 0; k < 3; ++k) - // mesh->mNormals[j][k] = static_cast(n[k]); - // } + std::vector heightsFloat; + _input.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY, + heightsFloat); - // scene->mMeshes[i] = mesh.release(); - // } + std::vector heightsDouble(heightsFloat.begin(), heightsFloat.end()); - // this->mMesh = scene; - // this->mIsBoundingBoxDirty = true; - // this->mIsVolumeDirty = true; + this->setHeightField(vertSize, vertSize, heightsDouble); + this->setScale(Vector3(scale.X(), scale.Y(), 1)); } } diff --git a/dartsim/src/CustomHeightmapShape.hh b/dartsim/src/CustomHeightmapShape.hh index 41d2d4fb3..59f96e587 100644 --- a/dartsim/src/CustomHeightmapShape.hh +++ b/dartsim/src/CustomHeightmapShape.hh @@ -31,7 +31,7 @@ namespace dartsim { class CustomHeightmapShape : public dart::dynamics::HeightmapShape { public: CustomHeightmapShape( - const ignition::common::HeightmapData &_input, + common::HeightmapData &_input, const Eigen::Vector3d &_size, const int _subSampling); }; diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index 870f63537..ec53c3477 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -248,7 +248,7 @@ LinearVector3d ShapeFeatures::GetHeightmapShapeScale( Identity ShapeFeatures::AttachHeightmapShape( const Identity &_linkID, const std::string &_name, - const ignition::common::HeightmapData &_heightmapData, + ignition::common::HeightmapData &_heightmapData, const Pose3d &_pose, const LinearVector3d &_scale, int _subSampling) diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index ce17f1769..65d899f22 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -140,7 +140,7 @@ class ShapeFeatures : public: Identity AttachHeightmapShape( const Identity &_linkID, const std::string &_name, - const ignition::common::HeightmapData &_heightmapData, + ignition::common::HeightmapData &_heightmapData, const Pose3d &_pose, const LinearVector3d &_scale, int _subSampling) override; diff --git a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh index 9f26b03bd..0cf430425 100644 --- a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh @@ -108,7 +108,7 @@ namespace heightmap public: ShapePtrType AttachHeightmapShape( const std::string &_name, - const ignition::common::HeightmapData &_heightmapData, + ignition::common::HeightmapData &_heightmapData, const PoseType &_pose = PoseType::Identity(), const Dimensions &_scale = Dimensions::Ones(), int _subSampling = 1); @@ -126,7 +126,7 @@ namespace heightmap public: virtual Identity AttachHeightmapShape( const Identity &_linkID, const std::string &_name, - const ignition::common::HeightmapData &_heightmapData, + ignition::common::HeightmapData &_heightmapData, const PoseType &_pose, const Dimensions &_scale, int _subSampling) = 0; diff --git a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh index aca5cfe57..2a50a5767 100644 --- a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh @@ -59,7 +59,7 @@ namespace heightmap template auto AttachHeightmapShapeFeature::Link::AttachHeightmapShape( const std::string &_name, - const ignition::common::HeightmapData &_heightmapData, + ignition::common::HeightmapData &_heightmapData, const PoseType &_pose, const Dimensions &_scale, int _subSampling) -> ShapePtrType From a19a66a1f0ff4ecde7cf1450570e8b793d823fc3 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 2 Mar 2021 18:59:01 -0800 Subject: [PATCH 08/18] tweaks Signed-off-by: Louise Poubel --- dartsim/src/CustomHeightmapShape.cc | 6 --- dartsim/src/CustomHeightmapShape.hh | 10 +++-- dartsim/src/SDFFeatures.cc | 2 - heightmap/CMakeLists.txt | 1 - .../physics/heightmap/HeightmapShape.hh | 41 +++++-------------- .../heightmap/detail/HeightmapShape.hh | 13 +----- 6 files changed, 20 insertions(+), 53 deletions(-) diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc index e87f0de42..7bde0a680 100644 --- a/dartsim/src/CustomHeightmapShape.cc +++ b/dartsim/src/CustomHeightmapShape.cc @@ -17,15 +17,9 @@ #include "CustomHeightmapShape.hh" -#include -#include #include -#include #include -#include -#include - namespace ignition { namespace physics { namespace dartsim { diff --git a/dartsim/src/CustomHeightmapShape.hh b/dartsim/src/CustomHeightmapShape.hh index 59f96e587..4425d080f 100644 --- a/dartsim/src/CustomHeightmapShape.hh +++ b/dartsim/src/CustomHeightmapShape.hh @@ -25,11 +25,15 @@ namespace ignition { namespace physics { namespace dartsim { -/// \brief This class creates a custom derivative of dartsim's HeightmapShape class -/// which allows an ignition::common::Heightmap to be converted into a HeightmapShape that -/// can be used by dartsim. +/// \brief This class creates a custom derivative of dartsim's HeightmapShape +/// class which allows an ignition::common::Heightmap to be converted into a +/// HeightmapShape that can be used by dartsim. class CustomHeightmapShape : public dart::dynamics::HeightmapShape { + /// \brief Constructor + /// \param[in] _input Holds heightmap data. + /// \param[in] _size Heightmap size in meters. + /// \param[in] _subSampling How much to subsample. public: CustomHeightmapShape( common::HeightmapData &_input, const Eigen::Vector3d &_size, diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 02faf16ed..0cc5eba0c 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -304,8 +304,6 @@ static ShapeAndTransform ConstructPlane( static ShapeAndTransform ConstructHeightmap( const ::sdf::Heightmap & /*_heightmap*/) { - // TODO(MXG): Look into what kind of heightmap URI we get here. Will it just - // be a local file name, or do we need to resolve the URI? ignerr << "Heightmap construction from an SDF has not been implemented yet " << "for dartsim.\n"; return {nullptr}; diff --git a/heightmap/CMakeLists.txt b/heightmap/CMakeLists.txt index 43051af9f..50efd33a0 100644 --- a/heightmap/CMakeLists.txt +++ b/heightmap/CMakeLists.txt @@ -1,4 +1,3 @@ - ign_add_component(heightmap INTERFACE GET_TARGET_NAME heightmap) diff --git a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh index 0cf430425..b92412b2f 100644 --- a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -43,8 +43,8 @@ namespace heightmap public: using Dimensions = typename FromPolicy::template Use; - /// \brief Get the size of the triangle heightmap. - /// \returns the size of the triangle heightmap. + /// \brief Get the size of the heightmap. + /// \returns the size of the heightmap. public: Dimensions GetSize() const; /// \brief Get the scaling factor that is being applied to the heightmap. @@ -67,31 +67,7 @@ namespace heightmap }; ///////////////////////////////////////////////// - class SetHeightmapShapeProperties - : public virtual FeatureWithRequirements - { - public: template - class HeightmapShape : public virtual Entity - { - public: using Dimensions = - typename FromPolicy::template Use; - - public: void SetScale(const Dimensions &_dimensions); - }; - - public: template - class Implementation : public virtual Feature::Implementation - { - public: using Dimensions = - typename FromPolicy::template Use; - - public: void SetHeightmapShapeScale( - const Identity &_heightmapID, - const Dimensions &_dimensions) = 0; - }; - }; - - ///////////////////////////////////////////////// + /// \brief Attach a heightmap shape to a link. class AttachHeightmapShapeFeature : public virtual FeatureWithRequirements { @@ -106,11 +82,16 @@ namespace heightmap public: using ShapePtrType = HeightmapShapePtr; + /// \brief Attach a heightmap shape to a link. + /// \param[in] _name Shape's name. + /// \param[in] _pose Position in the world. + /// \param[in] _size Heightmap total size in meters. + /// \param[in] _subSampling Increase sampling to improve resolution. public: ShapePtrType AttachHeightmapShape( const std::string &_name, ignition::common::HeightmapData &_heightmapData, - const PoseType &_pose = PoseType::Identity(), - const Dimensions &_scale = Dimensions::Ones(), + const PoseType &_pose, + const Dimensions &_size, int _subSampling = 1); }; diff --git a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh index 2a50a5767..93413c47c 100644 --- a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh @@ -46,28 +46,19 @@ namespace heightmap ->GetHeightmapShapeScale(this->identity); } - ///////////////////////////////////////////////// - template - void SetHeightmapShapeProperties::HeightmapShape::SetScale( - const Dimensions &_dimensions) - { - this->template Interface() - ->SetHeightmapShapeScale(this->identity, _dimensions); - } - ///////////////////////////////////////////////// template auto AttachHeightmapShapeFeature::Link::AttachHeightmapShape( const std::string &_name, ignition::common::HeightmapData &_heightmapData, const PoseType &_pose, - const Dimensions &_scale, + const Dimensions &_size, int _subSampling) -> ShapePtrType { return ShapePtrType(this->pimpl, this->template Interface() ->AttachHeightmapShape(this->identity, _name, _heightmapData, _pose, - _scale, _subSampling)); + _size, _subSampling)); } } } From bf4ff30bec9e270577b69ab8cd1669738be0eb61 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 17 Mar 2021 12:27:03 -0700 Subject: [PATCH 09/18] Support bullet detector Signed-off-by: Louise Poubel --- dartsim/src/CustomHeightmapShape.cc | 6 ++---- dartsim/src/CustomHeightmapShape.hh | 4 +++- dartsim/src/ShapeFeatures.cc | 12 ++++++------ 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc index 7bde0a680..1b85e2f48 100644 --- a/dartsim/src/CustomHeightmapShape.cc +++ b/dartsim/src/CustomHeightmapShape.cc @@ -29,7 +29,7 @@ CustomHeightmapShape::CustomHeightmapShape( common::HeightmapData &_input, const Eigen::Vector3d &_size, int _subSampling) - : dart::dynamics::HeightmapShape() + : dart::dynamics::HeightmapShape() { double heightmapSizeZ = _input.MaxElevation(); const bool flipY = false; @@ -49,9 +49,7 @@ CustomHeightmapShape::CustomHeightmapShape( _input.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY, heightsFloat); - std::vector heightsDouble(heightsFloat.begin(), heightsFloat.end()); - - this->setHeightField(vertSize, vertSize, heightsDouble); + this->setHeightField(vertSize, vertSize, heightsFloat); this->setScale(Vector3(scale.X(), scale.Y(), 1)); } diff --git a/dartsim/src/CustomHeightmapShape.hh b/dartsim/src/CustomHeightmapShape.hh index 4425d080f..b021b747c 100644 --- a/dartsim/src/CustomHeightmapShape.hh +++ b/dartsim/src/CustomHeightmapShape.hh @@ -28,7 +28,9 @@ namespace dartsim { /// \brief This class creates a custom derivative of dartsim's HeightmapShape /// class which allows an ignition::common::Heightmap to be converted into a /// HeightmapShape that can be used by dartsim. -class CustomHeightmapShape : public dart::dynamics::HeightmapShape +/// Using float precision because Bullet's collision detector doesn't support +/// double. common::HeightmapData also holds floats. +class CustomHeightmapShape : public dart::dynamics::HeightmapShape { /// \brief Constructor /// \param[in] _input Holds heightmap data. diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index ec53c3477..420ad06c7 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -212,7 +212,7 @@ Identity ShapeFeatures::CastToHeightmapShape( const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape(); - if (dynamic_cast *>(shape.get())) + if (dynamic_cast *>(shape.get())) return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); return this->GenerateInvalidId(); @@ -224,8 +224,8 @@ LinearVector3d ShapeFeatures::GetHeightmapShapeSize( { const auto *shapeInfo = this->ReferenceInterface(_heightmapID); - const dart::dynamics::HeightmapShape *heightmap = - static_cast *>( + const auto *heightmap = + static_cast *>( shapeInfo->node->getShape().get()); return heightmap->getBoundingBox().getMax() - heightmap->getBoundingBox().getMin(); @@ -237,11 +237,11 @@ LinearVector3d ShapeFeatures::GetHeightmapShapeScale( { const auto *shapeInfo = this->ReferenceInterface(_heightmapID); - const dart::dynamics::HeightmapShape *heightmap = - static_cast *>( + const auto *heightmap = + static_cast *>( shapeInfo->node->getShape().get()); - return heightmap->getScale(); + return heightmap->getScale().cast(); } ///////////////////////////////////////////////// From e87ae735d3defb7fbb7a5d7a27d7d98bff4157d8 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 17 Mar 2021 13:20:35 -0700 Subject: [PATCH 10/18] Add test, remove scale Signed-off-by: Louise Poubel --- dartsim/src/EntityManagement_TEST.cc | 17 +++++++++++++++++ dartsim/src/ShapeFeatures.cc | 17 ++--------------- dartsim/src/ShapeFeatures.hh | 5 +---- .../physics/heightmap/HeightmapShape.hh | 13 +++---------- .../physics/heightmap/detail/HeightmapShape.hh | 16 ++++------------ resources/heightmap_bowl.png | Bin 0 -> 11313 bytes 6 files changed, 27 insertions(+), 41 deletions(-) create mode 100644 resources/heightmap_bowl.png diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index 3fa84a53c..1adcb2439 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -19,6 +19,7 @@ #include +#include #include #include @@ -183,6 +184,22 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_NEAR(meshShapeScaledSize[0], 0.2553, 1e-4); EXPECT_NEAR(meshShapeScaledSize[1], 0.3831, 1e-4); EXPECT_NEAR(meshShapeScaledSize[2], 0.0489, 1e-4); + + auto heightmapLink = model->ConstructEmptyLink("heightmap_link"); + heightmapLink->AttachFixedJoint(child, "heightmap_joint"); + + auto heightmapFilename = IGNITION_PHYSICS_RESOURCE_DIR "/heightmap_bowl.png"; + ignition::common::ImageHeightmap data; + EXPECT_GE(0, data.Load(heightmapFilename)); + + const ignition::math::Vector3d size({129, 129, 10}); + auto heightmapShape = heightmapLink->AttachHeightmapShape("heightmap", data, + ignition::math::eigen3::convert(pose), + ignition::math::eigen3::convert(size)); + + EXPECT_NEAR(size.X(), heightmapShape->GetSize()[0], 1e-6); + EXPECT_NEAR(size.Y(), heightmapShape->GetSize()[1], 1e-6); + EXPECT_NEAR(size.Z(), heightmapShape->GetSize()[2], 1e-6); } TEST(EntityManagement_TEST, RemoveEntities) diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index 420ad06c7..fd54f9ccd 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -231,30 +231,17 @@ LinearVector3d ShapeFeatures::GetHeightmapShapeSize( return heightmap->getBoundingBox().getMax() - heightmap->getBoundingBox().getMin(); } -///////////////////////////////////////////////// -LinearVector3d ShapeFeatures::GetHeightmapShapeScale( - const Identity &_heightmapID) const -{ - const auto *shapeInfo = this->ReferenceInterface(_heightmapID); - - const auto *heightmap = - static_cast *>( - shapeInfo->node->getShape().get()); - - return heightmap->getScale().cast(); -} - ///////////////////////////////////////////////// Identity ShapeFeatures::AttachHeightmapShape( const Identity &_linkID, const std::string &_name, ignition::common::HeightmapData &_heightmapData, const Pose3d &_pose, - const LinearVector3d &_scale, + const LinearVector3d &_size, int _subSampling) { auto heightmap = std::make_shared(_heightmapData, - _scale, _subSampling); + _size, _subSampling); DartBodyNode *bn = this->ReferenceInterface(_linkID)->link.get(); dart::dynamics::ShapeNode *sn = diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index 65d899f22..c0ba4d39c 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -134,15 +134,12 @@ class ShapeFeatures : public: LinearVector3d GetHeightmapShapeSize( const Identity &_heightmapID) const override; - public: LinearVector3d GetHeightmapShapeScale( - const Identity &_heightmapID) const override; - public: Identity AttachHeightmapShape( const Identity &_linkID, const std::string &_name, ignition::common::HeightmapData &_heightmapData, const Pose3d &_pose, - const LinearVector3d &_scale, + const LinearVector3d &_size, int _subSampling) override; // ----- Mesh Features ----- diff --git a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh index b92412b2f..ad9110a72 100644 --- a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh @@ -43,13 +43,9 @@ namespace heightmap public: using Dimensions = typename FromPolicy::template Use; - /// \brief Get the size of the heightmap. - /// \returns the size of the heightmap. + /// \brief Get the size of the heightmap in meters. + /// \returns The size of the heightmap. public: Dimensions GetSize() const; - - /// \brief Get the scaling factor that is being applied to the heightmap. - /// \returns the scaling factor that is being applied to the heightmap. - public: Dimensions GetScale() const; }; public: template @@ -60,9 +56,6 @@ namespace heightmap public: virtual Dimensions GetHeightmapShapeSize( const Identity &_heightmapID) const = 0; - - public: virtual Dimensions GetHeightmapShapeScale( - const Identity &_heightmapID) const = 0; }; }; @@ -109,7 +102,7 @@ namespace heightmap const std::string &_name, ignition::common::HeightmapData &_heightmapData, const PoseType &_pose, - const Dimensions &_scale, + const Dimensions &_size, int _subSampling) = 0; }; }; diff --git a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh index 93413c47c..b437621dc 100644 --- a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh @@ -30,8 +30,8 @@ namespace heightmap { ///////////////////////////////////////////////// template - auto GetHeightmapShapeProperties::HeightmapShape::GetSize() const - -> Dimensions + auto GetHeightmapShapeProperties::HeightmapShape:: + GetSize() const -> Dimensions { return this->template Interface() ->GetHeightmapShapeSize(this->identity); @@ -39,16 +39,8 @@ namespace heightmap ///////////////////////////////////////////////// template - auto GetHeightmapShapeProperties::HeightmapShape::GetScale() const - -> Dimensions - { - return this->template Interface() - ->GetHeightmapShapeScale(this->identity); - } - - ///////////////////////////////////////////////// - template - auto AttachHeightmapShapeFeature::Link::AttachHeightmapShape( + auto AttachHeightmapShapeFeature::Link:: + AttachHeightmapShape( const std::string &_name, ignition::common::HeightmapData &_heightmapData, const PoseType &_pose, diff --git a/resources/heightmap_bowl.png b/resources/heightmap_bowl.png new file mode 100644 index 0000000000000000000000000000000000000000..52df5fcd073e26f738a40125f7f50a1fc33caa1d GIT binary patch literal 11313 zcmV-1EY8!3P)Px#24YJ`L;(K){{a7>y{D4^000SaNLh0L01FcU01FcV0GgZ_00007bV*G`2iye@ z4Ji<4g7ppn000?uMObu0Z*6U5Zgc=ca%Ew3Wn>_CX>@2HM@dakSAh-}001BWNklYrAjw3<$|Nr0V9xKelAxQ8plCpX+o2ar3rBZUgeip6(h=|CCKgj&}-}(ZX znfXEH2Z;|pNPYK{pZui0i_a(fOZ{GcJ_+RWpMI;C|91X+{rblrBk}osvfjG>Lp{R> z_I~rPpIlFMUL}8hFd#pl%mnbs^IrJm2ewL|PyG;p{OGN4-r%G6#`!Pk6^)1p#`**O zP5L+LUx)|>gNTR#0)YtB_s;jp2r#%_n{j^d=?~)a!x8Lvr+y0v1Q7&DkPITgAPJu5 zB?&^$h?xH~&XZm~?Rm<_2dh`8uK`9d2%MJ*5a-((b`vf2``y4aEV}>!+#fNuti2UW@uUYRv)> z0O6-ypO3!VxIYBvFWUAY=D5xd5OG!`0DyHM&(R?3gu_2WpOZF~nVHyjK{tnpxg{v|4p$qZ z3fX{Tlc1MkZMNL3oGz%Fs7*gRef&0$^#WN}^13U23Gft=k(YbIkXd7|2QfwgWh79( z`fSInf+89DtaS*kM+0#=JboX`nkWj<<%9kk-6(PnWj&oPp>~8GxhsIr4>DoC12pGj z@ej+q%7eqUJiv+%iQt$IKKZ_fE8$kNeF?`u^m#`glL;QmI@hVx{b8+&HpK4fquqP^ z-gq%Ur=ngv6snz7kS8V1S8@z;Zs+=b^OLc!);T=q6lUG>NRX7%1%kmelMXPD$2cU~ zP=a8B5gBA4ZG8cY$oUWzQ4t9lS-X$5z`ZNSF)$J%~tN zVu?Wn{4x zkV($#DhN3{i}qyguaCckZZ4JI;rFokek14!TPP@-gbupF2%YzG1|4veQ6Wft6Q77W z{T#pKhEhR6YGF^5o3)LM%yX;C;vM(V4_Qt#?IPCWqGQANrRe-{a@F(f8F|haNG38Y zEDUyjPqNZV`diiQB=w5J4fs%S0 zr?n02Ts6EnCdFeiKEn@qutOJN;@;k)-W~LT{%Sf6n=eYE)W0cYB z=&y5!b2KLYO=!O+$$-i@k1iR{J5nVf3mU65p+gQ#QBb7Mb6&_3ubdpC-Vt*T#?0mf!^{Xd6n6Ed;<`HAJcSaDg+&Kr zRY6i%1G`U{K>Nr1v`ZAUXFO}-jv^xEpmG+9=CI2oan22XH9_H7Lf?nrgc!8^t)2^Q zK1l+4uINk1`m&efo1e5ppS(r|M4cl(#SOZ8R2XogM}nuG%cM+1oCr##1exg9Z9oSe z`(Q+}a@EQ{l7U2hK@cc=bk0I5YT7v&FJ2Fig=^0wyXKOoHd;$^Lcw5p3L=sWBrNns ztxYJGJhk9DgLMy9RaT$wlu}RNqjaDPkmY_-?-@L{5#u}woKOU)#JLQJN9>lF+}8kJ zXwJF($NVcvJ`ORfbd!ixt)=#`F3BltV&x2Pz97tnGsAzd(_gSHe#MB)v(Oa+C{fOrgYbb+3Q1AOq+1Y9L|_u#x*1MB8D>DUW(*+fQ)wk zRS>}rqjhRMEv8k_arI13N8mWMSZxo?ey~qNf`L@LEYl-jarRxtdU`61?B@SN|y~=J+^YYk(svC49GR*?rQ%rQa!<%LHX%^dy_4{$s zeZNvn+YD7sx>}*$Nj)ve%rb#pZ^48;Cr5);zju8uHD-}-WhElJ`UU3MWW}sXY|A&P ztFc?Uz(Y>#CR%k_n)4)2l-Hj^N1p!s?8=vtDD({gr_X!yM$Z?rdoXn&*oUq|bW3rM zVd>J3{#niwRoU%$^qcvaihb355Oa5#<-h3{&#qIKb?U6vMoepSPV&jAlv}K{H6B{+ zv;MF{iteqKEj~vcS|W0ajS7#I#Ci@9m+tUisz%)yfmsD>l<7XA8x+}DT@`8A!Bv0v zi;sMA8J%N~l+K)3ubs-J3(==ZjaxVF-OK6|6P>K%IU2-6c#7$slmUR11gFu^$(L>~ zmBU6yeq|?LGD6&XXBV5#Nt+a5+Y{7zJy&#N2xyI|a<#QlMBtl9CDwT$Aq4YeXVx;G zk`iN9y+5FmUyDsuw;*xK*N7`{g1=`+-7V;VSMk~@!JT3iRjx;DgEv(eI7osQV8JzA ze+-kX^`H`1uWOgLyLM6G^F*3c&QXd-tSo3p&-h_3iWiu zG6mV?&J)^F{BQ+$X)R+MX&210W7~6BPGrJJ53^(hDL@` zY7w=UsFYS6Bl$&sBCH)-zzIxz#9mJo>?{2QQWr|rS9pS|8R*oh<2jw3-#_>?jUU6; zDui}>K#!=xS!6VvB^r*_L7SfdK0FD{F>v=>G5wrmH0Vw^pF|-Yl$#{mEu))`Q{oDI z^OKXzFwSPm=PvZhMJt=_yy14g)<_pilM#$%ul3sg&5N@IbXfxCH$qS4BrlTA@!0zOeK%U-CBXo z${ny`OoeFQPi8xav$TOY=INZ`h$&7h*}$<|69{$89<>2M;Weh{ma_=2Hvw0qBd!LT zP-t%Jn(k;}C6rwe+OWU&*gqg#3~;xQEtu$dr=aZSU6oZTX{JZSQz8L_U#!Mg2pWmP zvzt%MLH=QiULqY#e4EyFm`K!@9!^65?!ovqlT#2?c;k}?vER^|*FT5uB0ByKJA=xN^aSK=_)CoY?pI(TwT6zH?3L=1?T4=kM zR?7zNED_bNtm28fGD7-hHMtuoews)`vU@UM#5a+uNgOM7K2>!osUEUdYP*1P;4BV6 z6yi&{9@NQFUO%7&hlM{`VQ_O~tO9@ft89Q(b;z@pxH@N9@is24gLW8boAgU759V-#f_>kXx6op$E0$E4?H~)_{Uze3=H~F8l_S63hNm=(_UU3oyBYTK*a3Wl$elqW_BFK+}Rrk{^TEAUXaG1x#X zz&b1_N%d_ULsUGBCJYI|vasw-coxTF|l<}%CT*Z*aqXRNdE5y+gVI4=r)hRx7m7%5^46tlK>;mpsKx+2? zlz_1+x(jjYkWtJU4N! z4}v=|wvzx=y|eX(xEk^VnoppDmr=cpDb^O;0wH=ItaFF~b>NAmh)0^5TE3h@Y7Qg9!fSO8 z_43nj?WCc4`l3$=1Y%g>b_2WGh^h})Vt4X7&nCEAP_;-oZA|=*bE5&R*zhhYy319= zKjjHUqlOWkOucgbG+-9I@Ar%~<<4PfC3H0PQ$K}vU>HAnfVo1bz#0YT$+QRIVDT~= zLZftuy1d}o*9;+Dy86X^gwQ}y8itqg(%0fWFq$;D(*FwD1Zza62h9j6I@akf=HTXG zP*nhbY35-c2Q&t~!G&tka4HK2ceZnKywOUp@4Sv3pvVC(SMz~r@9$Y7uJ!-L4p!`}52fhPs z>6zi!hMY_rotAYfO@l)Ag%`L(W!&Ysy}9>OT^&>OM>r|A^5TTaDx5$G4S&HvXEzV4 ze6m8Ju5{QjUXYgY2G=&<`WshZLcM50h4NFs2=$EK?SRTU!+N5C$U}}@Y)^D=&CP&5 z^+20$lz^X16CV&S6%eU8R83SG3R_f_Q%3D#TahO8;AM+Y82zeOE6697dFMKzo7F{6 zmg~ykNs5FMYXKYD`x7zE-jXrVG^W3R?)-G-0_PBE{jr)@zd<{A;bOvjHZl}t_RwYB zmJLBG1tinjSLC#RznKY`#selR6YV(eOJA%PE23f>b2u*WG5gPU47`XyT=SybG$?UV z+1=Td?hbYes_1ydUMbQ$>EjXt?U0lWT!yvIx_}g3#OgBiP!n}-L=-9hLFQ`jy*S8+ zAQITC-Q9&-%_m+Kc@?jp7o_9xwiOVtXE<9acsLtGrdEo)ZvZcLD0BBD{Dh!M&m z4J3;qgtn-}X&{I#7v1G=m`r4>xag6C_{tUCy~d`Q3!pezrDY7%iR}-9zdXU! zvLF_1{tCw7r5}t3uHWhr^TiizfuVEOb8RgeFmbFdISDWYZTbDW3it7xzcH?jQTGkGu2Zfx{hNC*oTe{*qQBy?PKVgp8re zVNxRk?(i9Q`ap;18-c;_?dIR0Dny$R7i~JomFrYf>)=iQ0ZID5{s;fgPgO~g*E?cZ zLv>BMY%(DGT*rRO#z@|&2k*iS#>i|Ex_Bp6Xn;6kQgKpWE!!6jG@Xl=`49g*mskG# zHqB_!T-^oLdYT}Drbk6V#0&Zt=b%~Tx;xok8T<3&w;a-<7cP;w7H>;|&uDccr+}ad zxj*cIH|GDrqWumY`rC%yV?=e5{ALU@)+=zdf;hopfHjMGmf{b|K@hth3gL}E1cioj;XL$)c_?u$Kjzw~lzc^}*CkYU&*or8)=GlHO;EcT= zp8}Q^X16ee!yi_TYDkJt;VmA?0N@sn)H6No0r}qA;|neef4tAUmcQb+L@A6(%s?I< zvNdtE(6|>_s}Y`~D&;l9?TsHV#SYDTuVaM!gndSbNM#O3FR0=I@PeL#8o=Modt>`4 zg?#S4!vg;Ejw#mo@n=u$NHMiZ(ila z@a%5i?&?qXfIo(QajQx1mu|H5JCkzYltvRz6?11gT;oRnbrHc@rBX8QBAjA$Q5AzgI95e44;TPEoD>`8*5n$ z-_qI{5Gzyxdjiz@41ZW);tj1MySUoOlL;8yE{K zzDWb0H8e9ZVuuI#+5l0-4&OYgOSvP-fn(I=9SNftF#iD?ZJ+68WNG7gkZIeHmmrD1 zMz;6FNh&^(DSey^p01%bl(@&mdAzEHEbjNjn{o^>biZn=s?ns1FSF1_r;wGntQC(f zYV9%L4ee$nmt19$-4yJ)V3d-KUUCqpWAN^_jZ2pyuMc8xvKFQ=Rc#-_xZqn~0!#kl z?@5ZgA;S&QG+a`&tT4rb?8l%YRSJIEHP?=d=sNK?&MpmA_!>%v)Bq&hxy3WrhJakq z@=KiPpsc^-87=7~=3m7{l_nq2h~po!3>yA)%XS?phL|2HbXTbxG+Db%S(!i3^F|ZB zWg#U!M`Lq;OghZ%kLuqKutam&*NmB~={Y3Bi}`YUM+?D?P!aDLUD0&f`8EX0NM3rG zQlsf40l!o#7GQy`O4BMe-5@?P6$j_9$`u`ZBkemSBd78C1;h+L`Nb#_mhi=8ECDZV zz>y!E%%M=+CAshxzg{fMMWSKXMabgyG}9crjbpVUi6Stv>AHf)8vmiJ!4EVX#6y)< zv|GcuhA1hA8wO-kHnJ6%7*ZDtQ?OzH{C+;B!MpM+>-wq4az583Xr+S)O;>iR9wVK` zQPUXAX2o>#B9e!--uoqVtpl>>dNiO|6xj>5oimKa%Q*a;%%d@i1KdLNsELgI~gKX<&J&&QyUG@SUnZW zG)K-sirKGG+8q@LVv44LOW_W!Re#D2xuHe4T!0HV2;>d-)Lg5DAr#1|r;?1S)p>~G z;Y5)FqlCGvTHj+@ENJp&zf_SZy0eXEXx-x>t-E4yKJ^SP?J<*G8TmZ(LlVEjP&-=K zc!WesT_FZx9`i!p?T-gI5n@v2Rt+q(UJT?^e>*u~gOKEMV)WIl&4+J(QVhUkYS|-N zE5woqm&gmOErMxa4r9k9ei@NR5XJ*3T(k_m@V_Ich>b_??s%I}wyq>{opw%`>w=_w z2(s`7I|g~hf{+}HOY6xVR2Ra4@0A(!^Se<6qR%O3okF1o=TCBB%LxMR4vbn5iX&@ z=ry;(bWLcPQH+{(NDyFlHA}eFt3K|3=`Cyicq-0t!*TA9!PFIm-_5#u71o6Y(EWzMhF>`g z0-|TZxgK#!(6BUb%>`ZxF5mooD!dRflWs^Fj)96TLE|_jI#$;qN|8@;w5^6RSdKFS zCtNBKYMUY=t@r}YW;&CuBjKVjc(!tDMVne#EO}_g*1ai-_WT`PGi;;A2(noYG%=_4Llk#i53~6dqB#P<%G&oH+gcCm4sH~P`6s2vBls2pr znZz(covn;9d8$;J{_{ij`vcaLUWgWpud$R3?&#s7!gW!y*4h{mAxmE>%=7{H;`6D4 zsVi;cIU0GT-$oy{f$Sb$A8^~@Zg1>VVP-?1 zQ<153Q-vcA-imlqb>j=l9}8#BA<2x4gH3rsC>Y|pNr@@e_Q)>s`>7PvJTeu>%T{;( z$yPk5^V`ub?fFs-U9~bop(MMJ?4yRFo=09Ptw;&3!sU{tVV2{{#0uy9+sZ3j z3kg;_e}U%jDI}GA!5Dgjx$I^d3iFYz)Qhk>jH^EfzQ_;mRe}=!E!&VYC&cld)~21Y zKIvwm>GPI34A_Yk^&c|vr2Z-XZE5OD;n0bRWY`?Ca3C$LTZ)O6I&xFahzl$ek0AF8 z`@d>+oH4oHZKUcGa9tl$s+%+$qaE{2WXqMxzEB)DO#j|BXq~JaW8n>ptLpyD)IwL* z=5VTKld#&*81WXtX;l~vMN{_xb~qQ;>@lH;paxOEG~@(0cU+`6F599EW!L5XpJ42 zHpZ(R_kpYQq^;9x@z2oh2#M*S4fpY=b3a-eSkcr7R+Vs<=i6uYaL6j(THNmbxsZ)<=Q6~f#su#h< z!+YQ^6%+RVY@j((CTB(sodt*oysK9Ds(w)U!rqYEAj#PRGOVVm;fYo2q zSgg?sPpSPFagLujz_f|#v81h9-=l|Obu4>p*v}mZ0XHXTqm8n`qT=ed2&6g9?!fFIl9*Fff000YsNkl`5k=L2RYZGo^8E2}sqkeX#vt&}ox5aG8H* z5TvadBJGN@I~dy4Py7oZN3SXotgfXN?D!gM2EL8V+&{Ol;%yU zgsKR#3%v9o3)xzzJkyq4Xj1tBhD94QVXDtBO96Q4we@MBOaXvbi5dILW|*`2D{Vz&x-Xfdfos#W+eYk4 z`-s+JoiP878@DSv^LuBIRH*Hsp^sPo751vskk}fFBckF-mWPHlgE5!9e~$#1a^d%r zK2I1UQ#o+`cJ!;xf*v2o*;`LCfGtQmwrL8yaHz^gIj1*!Y$OTH`v&bgwN$-kic75C z3^@iFgt#i%wKAo#tMD+9XR1h?*|o6HMq*W_EN~kY&EevFk(r~@8*8eVB9Xwr9eWhi zh&YYzo#ju#i(uf%2y7&A12gl^IC6>qr8Z$-r({In;qUZ++#~Zwufa_Z1=FW(sux_# z(P)G|`YzXfDa3b;0vR{#Lc_6Vs7pI1_fuQIUV%q<&bmfwV#yWZsvU)e*1@%0 z&zEG?&ro(gkgX1hP31PvZEK3VES-cyY5Jez>F@}*wwaFt+1n`7!*-gXGhFz^8QyHz z(NizF6QZ#sBBKL((t@E2oA%1HQ%E|UmT~BBP6+59cOOr7A(GyPgJ;A<$()3Wjo|6It`=a ze6v1Fit2e{l%IzkXS&nk-jwSVnn@aZ1-j@g`M|kZYJFUkSgD9hm?(=G-IG^1;~_b~ z@4RbC<>>rcBsH`5a69I92~>^QXB#)|*ekq;%Ssa-$1iO*?t62u?ssJmk20&#K0kW6 zAuqqrI|cd02&UTNty0t?UfDHAVusmjNa04I6|3JZTJzD)PNk0}$VXkq{e;+6#lFoy zu}wQ=@K%@w`ge>QmWBau)YxTb_~723ZrJ@AUku>wc4G;P0M6k)lN)npJ(J9uBY0#5 zyCI^C9*_D(w~vBLxE^no=c4AAd>4+5*z$lYagZ1zHp{A~hHr=zz$c$1*O%cn!EFlon#Z&Xt*W%zhJxLq~5 zIgK#kbKdVK9z$JT7WtI}$QARw{@`f#Koip?mq6sr?jxLiKV)78aMLfsPe zA7}CY!;kgKpB<>q@N_fx1Y-z-jm+UXfqK+zxe z$ekIKsF~`^>VET+zet|{H+^RQ|9hi;dvwj6C}`#k=Ls&_qk?^#N|b-IMQ@Mvmql$b z`lIgAk2*$w-?0b9q*wVXzUp1QeDgQONuNP_s8!Qrq+NtXQbF@W8IhZh_Ro&(&!6`C zGVg;yFB};9SeNVGUFGdwR3rDq6C>UHqfhv&gex!Z-s)yG(OqD{=4@j`HEUqeRk!7G zgc-kSuo_6-CkiYb&YvLuH?%F+JV@{-9NfF~-sW=tc*ULs;&A_T&wGATU!+UK6tCCo z6rgLGoOE@3@~@Zg{Rf7#Y(c_bj0las@&GI$w=U?e@tqmOFG+n*BlZ&;Egz zs<=q}g?xk$sl@=%gfB8p1Ft6iWXa#}Chq#EewH6})m}E>?F4g653MVJc)h1Tn$ee= z^02FK+Mv9$)P@MCu!%x}of+#5;BTLNJLI>;=Y3?ru)E!s4<{jRGMr|fmInAm&M5C! z;t>Hi1lor+U3BTo8s(#li6N5R*5PH>XeGHN1J^{rY>fwTh->+KlACL5I?R}9?b8eB z+ab@`pErLxe9ss8`olJDmotou&U?5B*6}GZ#`Rj=c7n}mw#!wZTI7A+@5Ski>EaNq z?K_$}G>o6&0-`zPqj&B1+qS@K9M?~Fm7a|y4;YJkXbN_56Y6|dYJS!fT-`%g0KEwx z+Bv3mmT}eFv6I)I!sIvJB01bG(j}tQJ+>_0`O{zoOl_qojPJfuolm%Q^ioeK>(;WSfQWIC(m!q2+&J;FTk!j}n{6E|!pSMOQ z*YzGhW^82L(i{Zu&Gc*WE0VL5nb!-_rCu9s>S_aOJdNr4qU6`&?uj-2k8VliE+{+l01Z|Ibb(kOAktb=YzLX zNEahI9E{t- zG$jw=2r*M37DYs~V0h@9aXlK#8cT%8Q~>gayUr=ynXX4{xzeK5%o9pcir^P1lM;nM+^Fb-ztI%jKdhy%?+Ava%~93lSKI zYYs447I}MmJk$V6CuwVMbu{DBBrR(=1i=!%9myA~WX!|S0rJ-%aD39txoFeVoV$zo z@!TSW{Z%k?zFGIBQb5FRnF&f!NjYk zXS*pbxzA0r@95VfT8<`02pYNSe7FQzxEy#ouuk#~9g2m?X5=+|4)_2G)hdvL&l?}; zYp)903-lYpEN8#7APn`RnIm^$*tPB0{IlR2S&R=Z(8E;+KVqk~q~2Bhv$Vw!KG-P!dbjD~Jnc-51b135@x3*y;)smGXp z{8BC*eJ@j+z`Vnav^geM3kzw|#9b_$uC$km;~TTHGAUCK=!kfDDNfLBiRMJ>eqlt! zmK>RdAtB8IQ-?8GPr<=}AiluhgmbNMY%v{N$~|mASD1d`0yM>dlGy>$961qNTM-ZK zC}}K2lM6A)iL~YRA@VEJzjhn0CDJmbWzSmUrRUd;qF7H$X2$h4`uZ=0n@C<~Ia?Ts zR+{~f|MMT=DtlN7hKZo Date: Wed, 17 Mar 2021 13:26:13 -0700 Subject: [PATCH 11/18] codecheck Signed-off-by: Louise Poubel --- dartsim/src/ShapeFeatures.cc | 3 ++- .../ignition/physics/heightmap/detail/HeightmapShape.hh | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index fd54f9ccd..b4b7c94dd 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -228,7 +228,8 @@ LinearVector3d ShapeFeatures::GetHeightmapShapeSize( static_cast *>( shapeInfo->node->getShape().get()); - return heightmap->getBoundingBox().getMax() - heightmap->getBoundingBox().getMin(); + return heightmap->getBoundingBox().getMax() - + heightmap->getBoundingBox().getMin(); } ///////////////////////////////////////////////// diff --git a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh index b437621dc..fea43f5c8 100644 --- a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh @@ -49,8 +49,8 @@ namespace heightmap { return ShapePtrType(this->pimpl, this->template Interface() - ->AttachHeightmapShape(this->identity, _name, _heightmapData, _pose, - _size, _subSampling)); + ->AttachHeightmapShape(this->identity, _name, _heightmapData, + _pose, _size, _subSampling)); } } } From dddb1059b5efaa0e1d9e034e7f794921cabc8be0 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 17 Mar 2021 13:29:31 -0700 Subject: [PATCH 12/18] docs Signed-off-by: Louise Poubel --- tutorials/03_physics_plugins.md | 2 ++ tutorials/08-implementing-a-custom-feature.md | 15 ++++++++------- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/tutorials/03_physics_plugins.md b/tutorials/03_physics_plugins.md index 7eed91016..d8b16bcb0 100644 --- a/tutorials/03_physics_plugins.md +++ b/tutorials/03_physics_plugins.md @@ -155,3 +155,5 @@ The following is a table of implemented `Features` of Dartsim and TPE-Plugin. | mesh::AttachMeshShapeFeature | ✓ | ✓ | | ForwardStep | ✓ | ✓ | | GetContactsFromLastStepFeature | ✓ | ✕ | +| heightmap::GetHeightmapShapeProperties | ✓ | ✓ | +| heightmap::AttachHeightmapShapeFeature | ✓ | ✓ | diff --git a/tutorials/08-implementing-a-custom-feature.md b/tutorials/08-implementing-a-custom-feature.md index a77787717..0bf0862f2 100644 --- a/tutorials/08-implementing-a-custom-feature.md +++ b/tutorials/08-implementing-a-custom-feature.md @@ -23,6 +23,7 @@ Below is the general structure of the `ign-physics` repository: ign-physics ├── dartsim Files for dartsim plugin component. ├── tpe Files for tpe plugin component. +├── heightmap Files for heightmap component. ├── include/ignition/physics Header files. ├── mesh Files for mesh component. ├── resources Model and mesh resource files used by tests. @@ -181,11 +182,11 @@ dart::simulation::WorldPtr RetrieveWorld::World The new defined feature file is placed in `dartsim/include/ignition/physics/dartsim`: ``` dartsim -├── worlds -├── src +├── worlds +├── src ├── include/ignition/physics/dartsim │ ├── World.hh -└── CMakeLists.txt +└── CMakeLists.txt ``` As seen above, after including the necessary library of `dartsim` and `ign-physics`, @@ -214,13 +215,13 @@ and implemented in [CustomFeatures.cc](https://github.com/ignitionrobotics/ign-p These files are place as follows: ``` dartsim -├── worlds +├── worlds ├── src │ ├── CustomFeatures.hh │ ├── CustomFeatures.cc -│ ├── ... -├── include/ignition/physics/dartsim -└── CMakeLists.txt +│ ├── ... +├── include/ignition/physics/dartsim +└── CMakeLists.txt ``` We display them here for convenience: From 8b0257e02d86aa2f37a351720b50765613d3008b Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 17 Mar 2021 13:32:53 -0700 Subject: [PATCH 13/18] fix Signed-off-by: Louise Poubel --- tutorials/03_physics_plugins.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tutorials/03_physics_plugins.md b/tutorials/03_physics_plugins.md index d8b16bcb0..b4096c854 100644 --- a/tutorials/03_physics_plugins.md +++ b/tutorials/03_physics_plugins.md @@ -155,5 +155,5 @@ The following is a table of implemented `Features` of Dartsim and TPE-Plugin. | mesh::AttachMeshShapeFeature | ✓ | ✓ | | ForwardStep | ✓ | ✓ | | GetContactsFromLastStepFeature | ✓ | ✕ | -| heightmap::GetHeightmapShapeProperties | ✓ | ✓ | -| heightmap::AttachHeightmapShapeFeature | ✓ | ✓ | +| heightmap::GetHeightmapShapeProperties | ✓ | | +| heightmap::AttachHeightmapShapeFeature | ✓ | | From 457c052cdf5385ebc9bac04445a4957acab9da0a Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 27 Apr 2021 18:54:19 -0700 Subject: [PATCH 14/18] const HeightmapData Signed-off-by: Louise Poubel --- dartsim/src/CustomHeightmapShape.cc | 20 +++++++++++++++++-- dartsim/src/CustomHeightmapShape.hh | 2 +- dartsim/src/ShapeFeatures.cc | 2 +- dartsim/src/ShapeFeatures.hh | 2 +- .../physics/heightmap/HeightmapShape.hh | 4 ++-- .../heightmap/detail/HeightmapShape.hh | 2 +- 6 files changed, 24 insertions(+), 8 deletions(-) diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc index 1b85e2f48..425f9fb3b 100644 --- a/dartsim/src/CustomHeightmapShape.cc +++ b/dartsim/src/CustomHeightmapShape.cc @@ -18,6 +18,8 @@ #include "CustomHeightmapShape.hh" #include +#include +#include #include namespace ignition { @@ -26,7 +28,7 @@ namespace dartsim { ///////////////////////////////////////////////// CustomHeightmapShape::CustomHeightmapShape( - common::HeightmapData &_input, + const common::HeightmapData &_input, const Eigen::Vector3d &_size, int _subSampling) : dart::dynamics::HeightmapShape() @@ -45,8 +47,22 @@ CustomHeightmapShape::CustomHeightmapShape( auto sizeIgn = ignition::math::eigen3::convert(_size); + // We need to make a copy here in order to use the non-const FillHeightMap + // function + common::ImageHeightmap copyData; + try + { + const auto &imageData = dynamic_cast(_input); + copyData.Load(imageData.Filename()); + } + catch(const std::bad_cast &) + { + ignerr << "Only image heightmaps are supported at the moment." << std::endl; + return; + } + std::vector heightsFloat; - _input.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY, + copyData.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY, heightsFloat); this->setHeightField(vertSize, vertSize, heightsFloat); diff --git a/dartsim/src/CustomHeightmapShape.hh b/dartsim/src/CustomHeightmapShape.hh index b021b747c..68883c191 100644 --- a/dartsim/src/CustomHeightmapShape.hh +++ b/dartsim/src/CustomHeightmapShape.hh @@ -37,7 +37,7 @@ class CustomHeightmapShape : public dart::dynamics::HeightmapShape /// \param[in] _size Heightmap size in meters. /// \param[in] _subSampling How much to subsample. public: CustomHeightmapShape( - common::HeightmapData &_input, + const common::HeightmapData &_input, const Eigen::Vector3d &_size, const int _subSampling); }; diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index 0f7f63aa0..30008f6dd 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -358,7 +358,7 @@ LinearVector3d ShapeFeatures::GetHeightmapShapeSize( Identity ShapeFeatures::AttachHeightmapShape( const Identity &_linkID, const std::string &_name, - ignition::common::HeightmapData &_heightmapData, + const common::HeightmapData &_heightmapData, const Pose3d &_pose, const LinearVector3d &_size, int _subSampling) diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index f0ed2aee0..160723b14 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -175,7 +175,7 @@ class ShapeFeatures : public: Identity AttachHeightmapShape( const Identity &_linkID, const std::string &_name, - ignition::common::HeightmapData &_heightmapData, + const common::HeightmapData &_heightmapData, const Pose3d &_pose, const LinearVector3d &_size, int _subSampling) override; diff --git a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh index ad9110a72..7bc12cb1c 100644 --- a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh @@ -82,7 +82,7 @@ namespace heightmap /// \param[in] _subSampling Increase sampling to improve resolution. public: ShapePtrType AttachHeightmapShape( const std::string &_name, - ignition::common::HeightmapData &_heightmapData, + const common::HeightmapData &_heightmapData, const PoseType &_pose, const Dimensions &_size, int _subSampling = 1); @@ -100,7 +100,7 @@ namespace heightmap public: virtual Identity AttachHeightmapShape( const Identity &_linkID, const std::string &_name, - ignition::common::HeightmapData &_heightmapData, + const common::HeightmapData &_heightmapData, const PoseType &_pose, const Dimensions &_size, int _subSampling) = 0; diff --git a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh index fea43f5c8..022c863e7 100644 --- a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh @@ -42,7 +42,7 @@ namespace heightmap auto AttachHeightmapShapeFeature::Link:: AttachHeightmapShape( const std::string &_name, - ignition::common::HeightmapData &_heightmapData, + const common::HeightmapData &_heightmapData, const PoseType &_pose, const Dimensions &_size, int _subSampling) -> ShapePtrType From 736f04bce33c401cb62f2c73c6efaf55eb0bb346 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 30 Jun 2021 19:38:29 -0700 Subject: [PATCH 15/18] PR feedback Signed-off-by: Louise Poubel --- dartsim/src/CustomHeightmapShape.cc | 1 - dartsim/src/CustomHeightmapShape.hh | 1 - dartsim/src/EntityManagement_TEST.cc | 7 +++++-- .../include/ignition/physics/heightmap/HeightmapShape.hh | 1 + 4 files changed, 6 insertions(+), 4 deletions(-) diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc index 425f9fb3b..3829f0275 100644 --- a/dartsim/src/CustomHeightmapShape.cc +++ b/dartsim/src/CustomHeightmapShape.cc @@ -68,7 +68,6 @@ CustomHeightmapShape::CustomHeightmapShape( this->setHeightField(vertSize, vertSize, heightsFloat); this->setScale(Vector3(scale.X(), scale.Y(), 1)); } - } } } diff --git a/dartsim/src/CustomHeightmapShape.hh b/dartsim/src/CustomHeightmapShape.hh index 68883c191..4c641ceec 100644 --- a/dartsim/src/CustomHeightmapShape.hh +++ b/dartsim/src/CustomHeightmapShape.hh @@ -41,7 +41,6 @@ class CustomHeightmapShape : public dart::dynamics::HeightmapShape const Eigen::Vector3d &_size, const int _subSampling); }; - } } } diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index f2815c02d..0a0b1b136 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -21,6 +21,7 @@ #include #include +#include #include @@ -168,7 +169,8 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto meshLink = model->ConstructEmptyLink("mesh_link"); meshLink->AttachFixedJoint(child, "fixed"); - const std::string meshFilename = IGNITION_PHYSICS_RESOURCE_DIR "/chassis.dae"; + const std::string meshFilename = ignition::common::joinPaths( + IGNITION_PHYSICS_RESOURCE_DIR, "chassis.dae"); auto &meshManager = *ignition::common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshFilename); @@ -206,7 +208,8 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto heightmapLink = model->ConstructEmptyLink("heightmap_link"); heightmapLink->AttachFixedJoint(child, "heightmap_joint"); - auto heightmapFilename = IGNITION_PHYSICS_RESOURCE_DIR "/heightmap_bowl.png"; + auto heightmapFilename = ignition::common::joinPaths( + IGNITION_PHYSICS_RESOURCE_DIR, "heightmap_bowl.png"); ignition::common::ImageHeightmap data; EXPECT_GE(0, data.Load(heightmapFilename)); diff --git a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh index 7bc12cb1c..0f400181e 100644 --- a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh @@ -77,6 +77,7 @@ namespace heightmap /// \brief Attach a heightmap shape to a link. /// \param[in] _name Shape's name. + /// \param[in] _heightmapData Contains the 3D data for the heigthtmap. /// \param[in] _pose Position in the world. /// \param[in] _size Heightmap total size in meters. /// \param[in] _subSampling Increase sampling to improve resolution. From e700b5b98a27f5adcfca848922efbc50bd48a042 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 8 Jul 2021 10:15:40 -0700 Subject: [PATCH 16/18] Expect 0 from ImageHeightmap::Load Signed-off-by: Steve Peters --- dartsim/src/EntityManagement_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index 0a0b1b136..9b90b3dc6 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -211,7 +211,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto heightmapFilename = ignition::common::joinPaths( IGNITION_PHYSICS_RESOURCE_DIR, "heightmap_bowl.png"); ignition::common::ImageHeightmap data; - EXPECT_GE(0, data.Load(heightmapFilename)); + EXPECT_EQ(0, data.Load(heightmapFilename)); const ignition::math::Vector3d size({129, 129, 10}); auto heightmapShape = heightmapLink->AttachHeightmapShape("heightmap", data, From 4a1edc346e4be960153b312c8a04c6eb780b9a2e Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 8 Jul 2021 10:27:33 -0700 Subject: [PATCH 17/18] shorten line length Signed-off-by: Steve Peters --- dartsim/src/CustomHeightmapShape.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc index 3829f0275..3d5a287d6 100644 --- a/dartsim/src/CustomHeightmapShape.cc +++ b/dartsim/src/CustomHeightmapShape.cc @@ -52,8 +52,8 @@ CustomHeightmapShape::CustomHeightmapShape( common::ImageHeightmap copyData; try { - const auto &imageData = dynamic_cast(_input); - copyData.Load(imageData.Filename()); + const auto &image = dynamic_cast(_input); + copyData.Load(image.Filename()); } catch(const std::bad_cast &) { From 63cbe64a163a31c22a6d29e5283a5f174e676bd6 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Thu, 8 Jul 2021 13:58:50 -0700 Subject: [PATCH 18/18] Test coverage of CastToHeightmapShape Signed-off-by: Steven Peters --- dartsim/src/EntityManagement_TEST.cc | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index 9b90b3dc6..a97ddc7ac 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -221,6 +221,15 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_NEAR(size.X(), heightmapShape->GetSize()[0], 1e-6); EXPECT_NEAR(size.Y(), heightmapShape->GetSize()[1], 1e-6); EXPECT_NEAR(size.Z(), heightmapShape->GetSize()[2], 1e-6); + + auto heightmapShapeGeneric = heightmapLink->GetShape("heightmap"); + ASSERT_NE(nullptr, heightmapShapeGeneric); + EXPECT_EQ(nullptr, heightmapShapeGeneric->CastToBoxShape()); + auto heightmapShapeRecast = heightmapShapeGeneric->CastToHeightmapShape(); + ASSERT_NE(nullptr, heightmapShapeRecast); + EXPECT_NEAR(size.X(), heightmapShapeRecast->GetSize()[0], 1e-6); + EXPECT_NEAR(size.Y(), heightmapShapeRecast->GetSize()[1], 1e-6); + EXPECT_NEAR(size.Z(), heightmapShapeRecast->GetSize()[2], 1e-6); } TEST(EntityManagement_TEST, RemoveEntities)