From 4f1b1dff0c86f7d9f178c1a364f3645be1fa2351 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Thu, 13 Jun 2024 13:16:25 -0400 Subject: [PATCH] Add Cone as a collision shape (#639) * Use mesh for DART collisions as cone not supported. * Add cone to shapes tests Signed-off-by: Benjamin Perseghetti Signed-off-by: Steve Peters Co-authored-by: Steve Peters Co-authored-by: Steve Peters --- README.md | 2 +- bullet-featherstone/README.md | 2 + bullet-featherstone/src/SDFFeatures.cc | 9 ++ bullet-featherstone/src/ShapeFeatures.cc | 80 +++++++++++ bullet-featherstone/src/ShapeFeatures.hh | 21 +++ bullet/src/SDFFeatures.cc | 11 +- dartsim/src/CustomConeMeshShape.cc | 62 +++++++++ dartsim/src/CustomConeMeshShape.hh | 44 +++++++ dartsim/src/SDFFeatures.cc | 20 +++ dartsim/src/SDFFeatures_TEST.cc | 5 +- dartsim/src/ShapeFeatures.cc | 70 ++++++++++ dartsim/src/ShapeFeatures.hh | 22 ++++ include/gz/physics/ConeShape.hh | 154 ++++++++++++++++++++++ include/gz/physics/detail/ConeShape.hh | 84 ++++++++++++ test/common_test/simulation_features.cc | 70 +++++++++- test/common_test/worlds/shapes.world | 34 +++++ tpe/lib/src/Collision.cc | 6 + tpe/lib/src/Shape.cc | 39 ++++++ tpe/lib/src/Shape.hh | 39 ++++++ tpe/plugin/src/SDFFeatures.cc | 9 ++ tpe/plugin/src/ShapeFeatures.cc | 103 +++++++++++++-- tpe/plugin/src/ShapeFeatures.hh | 23 +++- tpe/plugin/src/SimulationFeatures_TEST.cc | 38 +++++- 23 files changed, 921 insertions(+), 26 deletions(-) create mode 100644 dartsim/src/CustomConeMeshShape.cc create mode 100644 dartsim/src/CustomConeMeshShape.hh create mode 100644 include/gz/physics/ConeShape.hh create mode 100644 include/gz/physics/detail/ConeShape.hh diff --git a/README.md b/README.md index 3aa42a185..264ed1736 100644 --- a/README.md +++ b/README.md @@ -58,7 +58,7 @@ Gazebo Physics provides the following functionality: at runtime. * Features for common aspects of rigid body dynamic simulation - Construct model from [SDFormat](http://sdformat.org/) file. - - Collision shapes (such as box, sphere, cylinder, capsule, ellipsoid, mesh, heightmap). + - Collision shapes (such as box, sphere, cylinder, cone, capsule, ellipsoid, mesh, heightmap). - Joint types (such as revolute, prismatic, fixed, ball, screw, universal). - Step simulation, get/set state, apply inputs. * Reference implementation of physics plugin using diff --git a/bullet-featherstone/README.md b/bullet-featherstone/README.md index 5444cd089..932842b68 100644 --- a/bullet-featherstone/README.md +++ b/bullet-featherstone/README.md @@ -65,6 +65,8 @@ These are the specific physics API features implemented. * AttachBoxShapeFeature * GetCapsuleShapeProperties * AttachCapsuleShapeFeature + * GetConeShapeProperties + * AttachConeShapeFeature * GetCylinderShapeProperties * AttachCylinderShapeFeature * GetEllipsoidShapeProperties diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 0679fb831..3ad3b3f5b 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -963,6 +964,14 @@ bool SDFFeatures::AddSdfCollision( const auto radius = sphere->Radius(); shape = std::make_unique(static_cast(radius)); } + else if (const auto *cone = geom->ConeShape()) + { + const auto radius = static_cast(cone->Radius()); + const auto height = static_cast(cone->Length()); + shape = + std::make_unique(radius, height); + shape->setMargin(0.0); + } else if (const auto *cylinder = geom->CylinderShape()) { const auto radius = static_cast(cylinder->Radius()); diff --git a/bullet-featherstone/src/ShapeFeatures.cc b/bullet-featherstone/src/ShapeFeatures.cc index 1f05f51fc..038ad8357 100644 --- a/bullet-featherstone/src/ShapeFeatures.cc +++ b/bullet-featherstone/src/ShapeFeatures.cc @@ -179,6 +179,86 @@ Identity ShapeFeatures::AttachCapsuleShape( return identity; } +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToConeShape(const Identity &_shapeID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_shapeID); + if (shapeInfo != nullptr) + { + const auto &shape = shapeInfo->collider; + if (dynamic_cast(shape.get())) + return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); + } + + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetConeShapeRadius( + const Identity &_coneID) const +{ + auto it = this->collisions.find(_coneID); + if (it != this->collisions.end() && it->second != nullptr) + { + if (it->second->collider != nullptr) + { + auto *cone = static_cast( + it->second->collider.get()); + if (cone) + { + return cone->getRadius(); + } + } + } + + return -1; +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetConeShapeHeight( + const Identity &_coneID) const +{ + auto it = this->collisions.find(_coneID); + if (it != this->collisions.end() && it->second != nullptr) + { + if (it->second->collider != nullptr) + { + auto *cone = static_cast( + it->second->collider.get()); + if (cone) + { + return cone->getHeight(); + } + } + } + + return -1; +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachConeShape( + const Identity &_linkID, + const std::string &_name, + const double _radius, + const double _height, + const Pose3d &_pose) +{ + const auto radius = static_cast(_radius); + const auto height = static_cast(_height); + auto shape = + std::make_unique(radius, height); + shape->setMargin(0.0); + + auto identity = this->AddCollision( + CollisionInfo{ + _name, + std::move(shape), + _linkID, + _pose}); + + return identity; +} + ///////////////////////////////////////////////// Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const { diff --git a/bullet-featherstone/src/ShapeFeatures.hh b/bullet-featherstone/src/ShapeFeatures.hh index 998c97e66..4bcd86fac 100644 --- a/bullet-featherstone/src/ShapeFeatures.hh +++ b/bullet-featherstone/src/ShapeFeatures.hh @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -42,6 +43,9 @@ struct ShapeFeatureList : FeatureList< GetCapsuleShapeProperties, AttachCapsuleShapeFeature, + GetConeShapeProperties, + AttachConeShapeFeature, + GetCylinderShapeProperties, AttachCylinderShapeFeature, @@ -90,6 +94,23 @@ class ShapeFeatures : double _length, const Pose3d &_pose) override; + // ----- Cone Features ----- + public: Identity CastToConeShape( + const Identity &_shapeID) const override; + + public: double GetConeShapeRadius( + const Identity &_coneID) const override; + + public: double GetConeShapeHeight( + const Identity &_coneID) const override; + + public: Identity AttachConeShape( + const Identity &_linkID, + const std::string &_name, + double _radius, + double _height, + const Pose3d &_pose) override; + // ----- Cylinder Features ----- public: Identity CastToCylinderShape( const Identity &_shapeID) const override; diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc index efdb37b67..2d69cc61f 100644 --- a/bullet/src/SDFFeatures.cc +++ b/bullet/src/SDFFeatures.cc @@ -22,9 +22,10 @@ #include #include #include +#include +#include #include #include -#include #include #include @@ -228,6 +229,14 @@ Identity SDFFeatures::ConstructSdfCollision( const auto radius = static_cast(sphere->Radius()); shape = std::make_shared(radius); } + else if (geom->ConeShape()) + { + const auto cone = geom->ConeShape(); + const auto radius = static_cast(cone->Radius()); + const auto height = static_cast(cone->Length()); + shape = + std::make_shared(radius, height); + } else if (geom->CylinderShape()) { const auto cylinder = geom->CylinderShape(); diff --git a/dartsim/src/CustomConeMeshShape.cc b/dartsim/src/CustomConeMeshShape.cc new file mode 100644 index 000000000..a3f8bf7ed --- /dev/null +++ b/dartsim/src/CustomConeMeshShape.cc @@ -0,0 +1,62 @@ +/* + * 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 "CustomConeMeshShape.hh" + +#include +#include + +#include +#include +#include + +namespace gz { +namespace physics { +namespace dartsim { + +///////////////////////////////////////////////// +const gz::common::Mesh* MakeCustomConeMesh( + const gz::math::Coned &_cone, + int _meshRings, + int _meshSegments) +{ + common::MeshManager *meshMgr = common::MeshManager::Instance(); + std::string coneMeshName = std::string("cone_mesh") + + "_" + std::to_string(_cone.Radius()) + + "_" + std::to_string(_cone.Length()); + meshMgr->CreateCone( + coneMeshName, + _cone.Radius(), + _cone.Length(), + _meshRings, _meshSegments); + return meshMgr->MeshByName(coneMeshName); +} + +///////////////////////////////////////////////// +CustomConeMeshShape::CustomConeMeshShape( + const gz::math::Coned &_cone, + int _meshRings, + int _meshSegments) + : CustomMeshShape(*MakeCustomConeMesh(_cone, _meshRings, _meshSegments), + Eigen::Vector3d(1, 1, 1)), + cone(_cone) +{ +} + +} +} +} diff --git a/dartsim/src/CustomConeMeshShape.hh b/dartsim/src/CustomConeMeshShape.hh new file mode 100644 index 000000000..64e0e9cec --- /dev/null +++ b/dartsim/src/CustomConeMeshShape.hh @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2024 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 GZ_PHYSICS_DARTSIM_SRC_CUSTOMCONEMESHSHAPE_HH_ +#define GZ_PHYSICS_DARTSIM_SRC_CUSTOMCONEMESHSHAPE_HH_ + +#include +#include "CustomMeshShape.hh" + +namespace gz { +namespace physics { +namespace dartsim { + +/// \brief This class creates a custom derivative of the CustomMeshShape to +/// allow casting to a Cone shape. +class CustomConeMeshShape : public CustomMeshShape +{ + public: CustomConeMeshShape( + const gz::math::Coned &_cone, + int _meshRings = 1, + int _meshSegments = 36); + + public: gz::math::Coned cone; +}; + +} +} +} + +#endif // GZ_PHYSICS_DARTSIM_SRC_CUSTOMMESHSHAPE_HH_ diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 08a1183f9..3993dceb6 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -20,12 +20,14 @@ #include #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -49,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -65,6 +68,7 @@ #include #include "AddedMassFeatures.hh" +#include "CustomConeMeshShape.hh" #include "CustomMeshShape.hh" namespace gz { @@ -340,11 +344,27 @@ static ShapeAndTransform ConstructGeometry( { return ConstructCapsule(*_geometry.CapsuleShape()); } + else if (_geometry.ConeShape()) + { + // TODO(anyone): Replace this code when Cone is supported by DART + gzwarn << "DART: Cone is not a supported collision geomerty" + << " primitive, using generated mesh of a cone instead" + << std::endl; + auto mesh = + std::make_shared(_geometry.ConeShape()->Shape()); + auto mesh2 = std::dynamic_pointer_cast(mesh); + return {mesh2}; + } else if (_geometry.CylinderShape()) + { return ConstructCylinder(*_geometry.CylinderShape()); + } else if (_geometry.EllipsoidShape()) { // TODO(anyone): Replace this code when Ellipsoid is supported by DART + gzwarn << "DART: Ellipsoid is not a supported collision geomerty" + << " primitive, using generated mesh of an ellipsoid instead" + << std::endl; common::MeshManager *meshMgr = common::MeshManager::Instance(); std::string ellipsoidMeshName = std::string("ellipsoid_mesh") + "_" + std::to_string(_geometry.EllipsoidShape()->Radii().X()) diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index d3db87464..613bb8144 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -972,10 +972,11 @@ TEST_P(SDFFeatures_TEST, Shapes) auto dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); - ASSERT_EQ(5u, dartWorld->getNumSkeletons()); + ASSERT_EQ(6u, dartWorld->getNumSkeletons()); int count{0}; - for (auto name : {"sphere", "box", "cylinder", "capsule", "ellipsoid"}) + for (auto name : {"sphere", "box", "cylinder", "capsule", "ellipsoid", + "cone"}) { const auto skeleton = dartWorld->getSkeleton(count++); ASSERT_NE(nullptr, skeleton); diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index 8b42a0dbb..5b19c17a0 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -17,7 +17,9 @@ #include "ShapeFeatures.hh" +#include #include +#include #include #include @@ -32,6 +34,7 @@ #include #include +#include "CustomConeMeshShape.hh" #include "CustomHeightmapShape.hh" #include "CustomMeshShape.hh" @@ -161,6 +164,70 @@ Identity ShapeFeatures::AttachCapsuleShape( return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToConeShape(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(); +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetConeShapeRadius( + const Identity &_coneID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_coneID); + + auto *coneShape = + static_cast( + shapeInfo->node->getShape().get()); + + return coneShape->cone.Radius(); +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetConeShapeHeight( + const Identity &_coneID) const +{ + const auto *shapeInfo = this->ReferenceInterface(_coneID); + + auto *coneShape = + static_cast( + shapeInfo->node->getShape().get()); + + return coneShape->cone.Length(); +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachConeShape( + const Identity &_linkID, + const std::string &_name, + const double _radius, + const double _height, + const Pose3d &_pose) +{ + gzwarn << "DART: Cone is not a supported collision geomerty" + << " primitive, using generated mesh of a cone instead" + << std::endl; + auto mesh = + std::make_shared(gz::math::Coned(_height, _radius)); + + DartBodyNode *bn = this->ReferenceInterface(_linkID)->link.get(); + dart::dynamics::ShapeNode *sn = + bn->createShapeNodeWith( + mesh, 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::CastToCylinderShape(const Identity &_shapeID) const { @@ -255,6 +322,9 @@ Identity ShapeFeatures::AttachEllipsoidShape( const Vector3d &_radii, const Pose3d &_pose) { + gzwarn << "DART: Ellipsoid is not a supported collision geomerty" + << " primitive, using generated mesh of an ellipsoid instead" + << std::endl; common::MeshManager *meshMgr = common::MeshManager::Instance(); std::string ellipsoidMeshName = _name + "_ellipsoid_mesh" + "_" + std::to_string(_radii[0]) diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index e83992419..b112856c2 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -55,6 +56,10 @@ struct ShapeFeatureList : FeatureList< // SetCapsulerShapeProperties, AttachCapsuleShapeFeature, + GetConeShapeProperties, +// SetConeShapeProperties, + AttachConeShapeFeature, + GetCylinderShapeProperties, // SetCylinderShapeProperties, AttachCylinderShapeFeature, @@ -121,6 +126,23 @@ class ShapeFeatures : double _length, const Pose3d &_pose) override; + // ----- Cone Features ----- + public: Identity CastToConeShape( + const Identity &_shapeID) const override; + + public: double GetConeShapeRadius( + const Identity &_coneID) const override; + + public: double GetConeShapeHeight( + const Identity &_coneID) const override; + + public: Identity AttachConeShape( + const Identity &_linkID, + const std::string &_name, + double _radius, + double _height, + const Pose3d &_pose) override; + // ----- Cylinder Features ----- public: Identity CastToCylinderShape( const Identity &_shapeID) const override; diff --git a/include/gz/physics/ConeShape.hh b/include/gz/physics/ConeShape.hh new file mode 100644 index 000000000..143d3490a --- /dev/null +++ b/include/gz/physics/ConeShape.hh @@ -0,0 +1,154 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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 GZ_PHYSICS_CONESHAPE_HH_ +#define GZ_PHYSICS_CONESHAPE_HH_ + +#include + +#include +#include + +namespace gz +{ + namespace physics + { + GZ_PHYSICS_DECLARE_SHAPE_TYPE(ConeShape) + + class GZ_PHYSICS_VISIBLE GetConeShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class ConeShape : public virtual Entity + { + public: using Scalar = typename PolicyT::Scalar; + + /// \brief Get the radius of this ConeShape + /// \return the radius of this ConeShape + public: Scalar GetRadius() const; + + /// \brief Get the height (length along the local z-axis) of this + /// ConeShape. + /// \return the height of this ConeShape + public: Scalar GetHeight() const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: virtual Scalar GetConeShapeRadius( + const Identity &_coneID) const = 0; + + public: virtual Scalar GetConeShapeHeight( + const Identity &_coneID) const = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature sets the ConeShape properties such as + /// the cone radius and height. + class GZ_PHYSICS_VISIBLE SetConeShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class ConeShape : public virtual Entity + { + public: using Scalar = typename PolicyT::Scalar; + + /// \brief Set the radius of this ConeShape + /// \param[in] _radius + /// The desired radius of this ConeShape + public: void SetRadius(Scalar _radius); + + /// \brief Set the height of this ConeShape + /// \param[in] _height + /// The desired height of this ConeShape + public: void SetHeight(Scalar _height); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: virtual void SetConeShapeRadius( + const Identity &_coneID, Scalar _radius) = 0; + + public: virtual void SetConeShapeHeight( + const Identity &_coneID, Scalar _height) = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature constructs a new cone shape and attaches the + /// desired pose in the link frame. The pose could be defined to be the + /// cone center point in actual implementation. + class GZ_PHYSICS_VISIBLE AttachConeShapeFeature + : public virtual FeatureWithRequirements + { + public: template + class Link : public virtual Feature::Link + { + public: using Scalar = typename PolicyT::Scalar; + + public: using PoseType = + typename FromPolicy::template Use; + + public: using ShapePtrType = ConeShapePtr; + + /// \brief Rigidly attach a ConeShape to this link. + /// \param[in] _name + /// \param[in] _radius + /// The radius of the cone. + /// \param[in] _height + /// The height of the cone. + /// \param[in] _pose + /// The desired pose of the ConeShape relative to the Link frame. + /// \returns a ShapePtrType to the newly constructed ConeShape + public: ShapePtrType AttachConeShape( + const std::string &_name = "cone", + Scalar _radius = 1.0, + Scalar _height = 1.0, + const PoseType &_pose = PoseType::Identity()); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: using PoseType = + typename FromPolicy::template Use; + + public: virtual Identity AttachConeShape( + const Identity &_linkID, + const std::string &_name, + Scalar _radius, + Scalar _height, + const PoseType &_pose) = 0; + }; + }; + } +} + +#include + +#endif diff --git a/include/gz/physics/detail/ConeShape.hh b/include/gz/physics/detail/ConeShape.hh new file mode 100644 index 000000000..0630b281a --- /dev/null +++ b/include/gz/physics/detail/ConeShape.hh @@ -0,0 +1,84 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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 GZ_PHYSICS_DETAIL_CONESHAPE_HH_ +#define GZ_PHYSICS_DETAIL_CONESHAPE_HH_ + +#include + +#include + +namespace gz +{ + namespace physics + { + ///////////////////////////////////////////////// + template + auto GetConeShapeProperties::ConeShape + ::GetRadius() const -> Scalar + { + return this->template Interface() + ->GetConeShapeRadius(this->identity); + } + + ///////////////////////////////////////////////// + template + auto GetConeShapeProperties::ConeShape + ::GetHeight() const -> Scalar + { + return this->template Interface() + ->GetConeShapeHeight(this->identity); + } + + ///////////////////////////////////////////////// + template + void SetConeShapeProperties::ConeShape + ::SetRadius(Scalar _radius) + { + this->template Interface() + ->SetConeShapeRadius(this->identity, _radius); + } + + ///////////////////////////////////////////////// + template + void SetConeShapeProperties::ConeShape + ::SetHeight(Scalar _height) + { + this->template Interface() + ->SetConeShapeHeight(this->identity, _height); + } + + ///////////////////////////////////////////////// + template + auto AttachConeShapeFeature::Link + ::AttachConeShape( + const std::string &_name, + Scalar _radius, + Scalar _height, + const PoseType &_pose) -> ShapePtrType + { + return ShapePtrType(this->pimpl, + this->template Interface() + ->AttachConeShape( + this->identity, _name, _radius, _height, _pose)); + } + } +} + +#endif diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index d92e474f9..ec0c05072 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -36,13 +36,14 @@ #include #include "gz/physics/BoxShape.hh" -#include #include "gz/physics/ContactProperties.hh" -#include "gz/physics/CylinderShape.hh" #include "gz/physics/CapsuleShape.hh" +#include "gz/physics/ConeShape.hh" +#include "gz/physics/CylinderShape.hh" #include "gz/physics/EllipsoidShape.hh" #include #include +#include #include "gz/physics/SphereShape.hh" #include @@ -79,11 +80,13 @@ struct Features : gz::physics::FeatureList< gz::physics::AttachBoxShapeFeature, gz::physics::AttachSphereShapeFeature, + gz::physics::AttachConeShapeFeature, gz::physics::AttachCylinderShapeFeature, gz::physics::AttachEllipsoidShapeFeature, gz::physics::AttachCapsuleShapeFeature, gz::physics::GetSphereShapeProperties, gz::physics::GetBoxShapeProperties, + gz::physics::GetConeShapeProperties, gz::physics::GetCylinderShapeProperties, gz::physics::GetCapsuleShapeProperties, gz::physics::GetEllipsoidShapeProperties @@ -264,7 +267,7 @@ TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest, EXPECT_TRUE(checkedOutput); contacts = world->GetContactsFromLastStep(); - EXPECT_EQ(4u, contacts.size()); + EXPECT_EQ(5u, contacts.size()); world->SetCollisionPairMaxContacts(0u); EXPECT_EQ(0u, world->GetCollisionPairMaxContacts()); @@ -372,11 +375,13 @@ struct FeaturesShapeFeatures : gz::physics::FeatureList< gz::physics::AttachBoxShapeFeature, gz::physics::AttachSphereShapeFeature, + gz::physics::AttachConeShapeFeature, gz::physics::AttachCylinderShapeFeature, gz::physics::AttachEllipsoidShapeFeature, gz::physics::AttachCapsuleShapeFeature, gz::physics::GetSphereShapeProperties, gz::physics::GetBoxShapeProperties, + gz::physics::GetConeShapeProperties, gz::physics::GetCylinderShapeProperties, gz::physics::GetCapsuleShapeProperties, gz::physics::GetEllipsoidShapeProperties @@ -445,6 +450,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) auto cylinderLink = cylinder->GetLink(0); auto cylinderCollision = cylinderLink->GetShape(0); auto cylinderShape = cylinderCollision->CastToCylinderShape(); + ASSERT_NE(nullptr, cylinderShape); EXPECT_NEAR(0.5, cylinderShape->GetRadius(), 1e-6); EXPECT_NEAR(1.1, cylinderShape->GetHeight(), 1e-6); @@ -452,6 +458,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) "cylinder2", 3.0, 4.0, Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, cylinderLink->GetShapeCount()); EXPECT_EQ(cylinder2, cylinderLink->GetShape(1)); + ASSERT_NE(nullptr, cylinderLink->GetShape(1)->CastToCylinderShape()); EXPECT_NEAR(3.0, cylinderLink->GetShape(1)->CastToCylinderShape()->GetRadius(), 1e-6); @@ -463,6 +470,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) auto ellipsoidLink = ellipsoid->GetLink(0); auto ellipsoidCollision = ellipsoidLink->GetShape(0); auto ellipsoidShape = ellipsoidCollision->CastToEllipsoidShape(); + ASSERT_NE(nullptr, ellipsoidShape); EXPECT_TRUE( gz::math::Vector3d(0.2, 0.3, 0.5).Equal( gz::math::eigen3::convert(ellipsoidShape->GetRadii()), 0.1)); @@ -478,6 +486,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) auto capsuleLink = capsule->GetLink(0); auto capsuleCollision = capsuleLink->GetShape(0); auto capsuleShape = capsuleCollision->CastToCapsuleShape(); + ASSERT_NE(nullptr, capsuleShape); EXPECT_NEAR(0.2, capsuleShape->GetRadius(), 1e-6); EXPECT_NEAR(0.6, capsuleShape->GetLength(), 1e-6); @@ -486,6 +495,26 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) EXPECT_EQ(2u, capsuleLink->GetShapeCount()); EXPECT_EQ(capsule2, capsuleLink->GetShape(1)); + auto cone = world->GetModel("cone"); + auto coneLink = cone->GetLink(0); + auto coneCollision = coneLink->GetShape(0); + auto coneShape = coneCollision->CastToConeShape(); + ASSERT_NE(nullptr, coneShape); + EXPECT_NEAR(0.5, coneShape->GetRadius(), 1e-6); + EXPECT_NEAR(1.1, coneShape->GetHeight(), 1e-6); + + auto cone2 = coneLink->AttachConeShape( + "cone2", 3.0, 4.0, Eigen::Isometry3d::Identity()); + EXPECT_EQ(2u, coneLink->GetShapeCount()); + EXPECT_EQ(cone2, coneLink->GetShape(1)); + ASSERT_NE(nullptr, coneLink->GetShape(1)->CastToConeShape()); + EXPECT_NEAR(3.0, + coneLink->GetShape(1)->CastToConeShape()->GetRadius(), + 1e-6); + EXPECT_NEAR(4.0, + coneLink->GetShape(1)->CastToConeShape()->GetHeight(), + 1e-6); + // Test the bounding boxes in the local frames auto sphereAABB = sphereCollision->GetAxisAlignedBoundingBox(*sphereCollision); @@ -497,6 +526,8 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) ellipsoidCollision->GetAxisAlignedBoundingBox(*ellipsoidCollision); auto capsuleAABB = capsuleCollision->GetAxisAlignedBoundingBox(*capsuleCollision); + auto coneAABB = + coneCollision->GetAxisAlignedBoundingBox(*coneCollision); EXPECT_TRUE(gz::math::Vector3d(-1, -1, -1).Equal( gz::math::eigen3::convert(sphereAABB).Min(), 0.1)); @@ -518,6 +549,10 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) gz::math::eigen3::convert(capsuleAABB).Min(), 0.1)); EXPECT_TRUE(gz::math::Vector3d(0.2, 0.2, 0.5).Equal( gz::math::eigen3::convert(capsuleAABB).Max(), 0.1)); + EXPECT_EQ(gz::math::Vector3d(-0.5, -0.5, -0.55), + gz::math::eigen3::convert(coneAABB).Min()); + EXPECT_EQ(gz::math::Vector3d(0.5, 0.5, 0.55), + gz::math::eigen3::convert(coneAABB).Max()); // check model AABB. By default, the AABBs are in world frame auto sphereModelAABB = sphere->GetAxisAlignedBoundingBox(); @@ -525,6 +560,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) auto cylinderModelAABB = cylinder->GetAxisAlignedBoundingBox(); auto ellipsoidModelAABB = ellipsoid->GetAxisAlignedBoundingBox(); auto capsuleModelAABB = capsule->GetAxisAlignedBoundingBox(); + auto coneModelAABB = cone->GetAxisAlignedBoundingBox(); EXPECT_EQ(gz::math::Vector3d(-1, 0.5, -0.5), gz::math::eigen3::convert(sphereModelAABB).Min()); @@ -546,6 +582,10 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) gz::math::eigen3::convert(capsuleModelAABB).Min()); EXPECT_EQ(gz::math::Vector3d(0.2, -2.8, 1), gz::math::eigen3::convert(capsuleModelAABB).Max()); + EXPECT_EQ(gz::math::Vector3d(-3, -9.5, -1.5), + gz::math::eigen3::convert(coneModelAABB).Min()); + EXPECT_EQ(gz::math::Vector3d(3, -3.5, 2.5), + gz::math::eigen3::convert(coneModelAABB).Max()); } } @@ -780,6 +820,10 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) auto ellipsoidFreeGroup = ellipsoid->FindFreeGroup(); EXPECT_NE(nullptr, ellipsoidFreeGroup); + auto cone = world->GetModel("cone"); + auto coneFreeGroup = cone->FindFreeGroup(); + EXPECT_NE(nullptr, coneFreeGroup); + auto box = world->GetModel("box"); // step and get contacts @@ -794,6 +838,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) unsigned int contactBoxCylinder = 0u; unsigned int contactBoxCapsule = 0u; unsigned int contactBoxEllipsoid = 0u; + unsigned int contactBoxCone = 0u; for (auto &contact : contacts) { @@ -827,6 +872,11 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) { contactBoxEllipsoid++; } + else if ((m1->GetName() == "box" && m2->GetName() == "cone") || + (m1->GetName() == "cone" && m2->GetName() == "box")) + { + contactBoxCone++; + } else { FAIL() << "There should not be contacts between: " @@ -837,6 +887,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) EXPECT_NE(0u, contactBoxCylinder); EXPECT_NE(0u, contactBoxCapsule); EXPECT_NE(0u, contactBoxEllipsoid); + EXPECT_NE(0u, contactBoxCone); // move sphere away sphereFreeGroup->SetWorldPose(gz::math::eigen3::convert( @@ -848,12 +899,13 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) contacts = world->GetContactsFromLastStep(); // large box in the middle should be intersecting with cylinder, capsule, - // ellipsoid + // ellipsoid, cone EXPECT_NE(0u, contacts.size()); contactBoxCylinder = 0u; contactBoxCapsule = 0u; contactBoxEllipsoid = 0u; + contactBoxCone = 0u; for (auto contact : contacts) { const auto &contactPoint = @@ -881,6 +933,11 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) { contactBoxEllipsoid++; } + else if ((m1->GetName() == "box" && m2->GetName() == "cone") || + (m1->GetName() == "cone" && m2->GetName() == "box")) + { + contactBoxCone++; + } else { FAIL() << "There should only be contacts between box and cylinder"; @@ -889,6 +946,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) EXPECT_NE(0u, contactBoxCylinder); EXPECT_NE(0u, contactBoxCapsule); EXPECT_NE(0u, contactBoxEllipsoid); + EXPECT_NE(0u, contactBoxCone); // move cylinder away cylinderFreeGroup->SetWorldPose(gz::math::eigen3::convert( @@ -902,6 +960,10 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) ellipsoidFreeGroup->SetWorldPose(gz::math::eigen3::convert( gz::math::Pose3d(0, -100, -100, 0, 0, 0))); + // move cone away + coneFreeGroup->SetWorldPose(gz::math::eigen3::convert( + gz::math::Pose3d(100, -100, 0.5, 0, 0, 0))); + // step and get contacts checkedOutput = StepWorld(world, false).first; EXPECT_FALSE(checkedOutput); diff --git a/test/common_test/worlds/shapes.world b/test/common_test/worlds/shapes.world index cb3bce5d9..0b66579d6 100644 --- a/test/common_test/worlds/shapes.world +++ b/test/common_test/worlds/shapes.world @@ -165,5 +165,39 @@ + + + 0 -6.5 0.5 0 0 0 + + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + 0 0 -0.275 0 0 0 + + + + + 0.5 + 1.1 + + + + + + + 0.5 + 1.1 + + + + + diff --git a/tpe/lib/src/Collision.cc b/tpe/lib/src/Collision.cc index 114851f1a..1a340e0a4 100644 --- a/tpe/lib/src/Collision.cc +++ b/tpe/lib/src/Collision.cc @@ -81,6 +81,12 @@ void Collision::SetShape(const Shape &_shape) static_cast(&_shape); this->dataPtr->shape.reset(new CapsuleShape(*typedShape)); } + else if (_shape.GetType() == ShapeType::CONE) + { + const ConeShape *typedShape = + static_cast(&_shape); + this->dataPtr->shape.reset(new ConeShape(*typedShape)); + } else if (_shape.GetType() == ShapeType::CYLINDER) { const CylinderShape *typedShape = diff --git a/tpe/lib/src/Shape.cc b/tpe/lib/src/Shape.cc index 44fbc0e1f..4f1d12126 100644 --- a/tpe/lib/src/Shape.cc +++ b/tpe/lib/src/Shape.cc @@ -115,6 +115,45 @@ void CapsuleShape::UpdateBoundingBox() this->bbox = math::AxisAlignedBox(-halfSize, halfSize); } +////////////////////////////////////////////////// +ConeShape::ConeShape() : Shape() +{ + this->type = ShapeType::CONE; +} + +////////////////////////////////////////////////// +double ConeShape::GetRadius() const +{ + return this->radius; +} + +////////////////////////////////////////////////// +void ConeShape::SetRadius(double _radius) +{ + this->radius = _radius; + this->dirty = true; +} + +////////////////////////////////////////////////// +double ConeShape::GetLength() const +{ + return this->length; +} + +////////////////////////////////////////////////// +void ConeShape::SetLength(double _length) +{ + this->length = _length; + this->dirty = true; +} + +////////////////////////////////////////////////// +void ConeShape::UpdateBoundingBox() +{ + math::Vector3d halfSize(this->radius, this->radius, this->length*0.5); + this->bbox = math::AxisAlignedBox(-halfSize, halfSize); +} + ////////////////////////////////////////////////// CylinderShape::CylinderShape() : Shape() { diff --git a/tpe/lib/src/Shape.hh b/tpe/lib/src/Shape.hh index 1cdb15415..55b5374f4 100644 --- a/tpe/lib/src/Shape.hh +++ b/tpe/lib/src/Shape.hh @@ -59,6 +59,9 @@ enum class GZ_PHYSICS_TPELIB_VISIBLE ShapeType /// \brief A ellipsoid shape. ELLIPSOID = 7, + + /// \brief A cone shape. + CONE = 10, }; @@ -153,6 +156,42 @@ class GZ_PHYSICS_TPELIB_VISIBLE CapsuleShape : public Shape private: double length = 0.0; }; +/// \brief Cone geometry +class GZ_PHYSICS_TPELIB_VISIBLE ConeShape : public Shape +{ + /// \brief Constructor + public: ConeShape(); + + /// \brief Destructor + public: virtual ~ConeShape() = default; + + /// \brief Get cone radius + /// \return cone radius + public: double GetRadius() const; + + /// \brief Set cone radius + /// \param[in] _radius Cone radius + public: void SetRadius(double _radius); + + /// \brief Get cone length + /// \return Cone length + public: double GetLength() const; + + /// \brief Set cone length + /// \param[in] _length Cone length + public: void SetLength(double _length); + + // Documentation inherited + protected: virtual void UpdateBoundingBox() override; + + /// \brief Cone radius + private: double radius = 0.0; + + /// \brief Cone length + private: double length = 0.0; +}; + + /// \brief Cylinder geometry class GZ_PHYSICS_TPELIB_VISIBLE CylinderShape : public Shape { diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index 409e222c7..4286fdb49 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -292,6 +293,14 @@ Identity SDFFeatures::ConstructSdfCollision( shape.SetLength(capsuleSdf->Length()); collision->SetShape(shape); } + else if (geom->Type() == ::sdf::GeometryType::CONE) + { + const auto coneSdf = geom->ConeShape(); + tpelib::ConeShape shape; + shape.SetRadius(coneSdf->Radius()); + shape.SetLength(coneSdf->Length()); + collision->SetShape(shape); + } else if (geom->Type() == ::sdf::GeometryType::CYLINDER) { const auto cylinderSdf = geom->CylinderShape(); diff --git a/tpe/plugin/src/ShapeFeatures.cc b/tpe/plugin/src/ShapeFeatures.cc index c01b0b9c4..c4f6b7f84 100644 --- a/tpe/plugin/src/ShapeFeatures.cc +++ b/tpe/plugin/src/ShapeFeatures.cc @@ -82,19 +82,6 @@ Identity ShapeFeatures::AttachBoxShape( return this->GenerateInvalidId(); } -///////////////////////////////////////////////// -Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const -{ - auto it = this->collisions.find(_shapeID); - if (it != this->collisions.end() && it->second != nullptr) - { - auto *shape = it->second->collision->GetShape(); - if (shape != nullptr && dynamic_cast(shape)) - return this->GenerateIdentity(_shapeID, it->second); - } - return this->GenerateInvalidId(); -} - ///////////////////////////////////////////////// Identity ShapeFeatures::CastToCapsuleShape(const Identity &_shapeID) const { @@ -172,6 +159,96 @@ Identity ShapeFeatures::AttachCapsuleShape( return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToConeShape(const Identity &_shapeID) const +{ + auto it = this->collisions.find(_shapeID); + if (it != this->collisions.end() && it->second != nullptr) + { + auto *shape = it->second->collision->GetShape(); + if (shape != nullptr && dynamic_cast(shape)) + return this->GenerateIdentity(_shapeID, it->second); + } + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetConeShapeRadius( + const Identity &_coneID) const +{ + // assume _coneID ~= _collisionID + auto it = this->collisions.find(_coneID); + if (it != this->collisions.end() && it->second != nullptr) + { + auto *shape = it->second->collision->GetShape(); + if (shape != nullptr) + { + auto *cone = static_cast(shape); + return cone->GetRadius(); + } + } + // return invalid radius if no collision found + return -1.0; +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetConeShapeHeight( + const Identity &_coneID) const +{ + // assume _coneID ~= _collisionID + auto it = this->collisions.find(_coneID); + if (it != this->collisions.end() && it->second != nullptr) + { + auto *shape = it->second->collision->GetShape(); + if (shape != nullptr) + { + auto *cone = static_cast(shape); + return cone->GetLength(); + } + } + // return invalid height if no collision found + return -1.0; +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachConeShape( + const Identity &_linkID, + const std::string &_name, + const double _radius, + const double _height, + const Pose3d &_pose) +{ + auto it = this->links.find(_linkID); + if (it != this->links.end() && it->second != nullptr) + { + auto &collision = static_cast( + it->second->link->AddCollision()); + collision.SetName(_name); + collision.SetPose(math::eigen3::convert(_pose)); + + tpelib::ConeShape coneshape; + coneshape.SetRadius(_radius); + coneshape.SetLength(_height); + collision.SetShape(coneshape); + + return this->AddCollision(_linkID, collision); + } + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const +{ + auto it = this->collisions.find(_shapeID); + if (it != this->collisions.end() && it->second != nullptr) + { + auto *shape = it->second->collision->GetShape(); + if (shape != nullptr && dynamic_cast(shape)) + return this->GenerateIdentity(_shapeID, it->second); + } + return this->GenerateInvalidId(); +} + ///////////////////////////////////////////////// double ShapeFeatures::GetCylinderShapeRadius( const Identity &_cylinderID) const diff --git a/tpe/plugin/src/ShapeFeatures.hh b/tpe/plugin/src/ShapeFeatures.hh index e09564e38..6b90dbc5c 100644 --- a/tpe/plugin/src/ShapeFeatures.hh +++ b/tpe/plugin/src/ShapeFeatures.hh @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -42,6 +43,9 @@ struct ShapeFeatureList : FeatureList< GetCapsuleShapeProperties, AttachCapsuleShapeFeature, + GetConeShapeProperties, + AttachConeShapeFeature, + GetCylinderShapeProperties, AttachCylinderShapeFeature, @@ -89,6 +93,23 @@ class ShapeFeatures : double _length, const Pose3d &_pose) override; + // ----- Cone Features ----- + public: Identity CastToConeShape( + const Identity &_shapeID) const override; + + public: double GetConeShapeRadius( + const Identity &_coneID) const override; + + public: double GetConeShapeHeight( + const Identity &_coneID) const override; + + public: Identity AttachConeShape( + const Identity &_linkID, + const std::string &_name, + double _radius, + double _height, + const Pose3d &_pose) override; + // ----- Cylinder Features ----- public: Identity CastToCylinderShape( const Identity &_shapeID) const override; @@ -106,7 +127,7 @@ class ShapeFeatures : double _height, const Pose3d &_pose) override; - // ----- Capsule Features ----- + // ----- Ellipsoid Features ----- public: Identity CastToEllipsoidShape( const Identity &_shapeID) const override; diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index ddecee5de..cf44d5485 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -603,6 +603,10 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) auto ellipsoidFreeGroup = ellipsoid->FindFreeGroup(); EXPECT_NE(nullptr, ellipsoidFreeGroup); + auto cone = world->GetModel("cone"); + auto coneFreeGroup = cone->FindFreeGroup(); + EXPECT_NE(nullptr, coneFreeGroup); + auto box = world->GetModel("box"); // step and get contacts @@ -611,12 +615,13 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) auto contacts = world->GetContactsFromLastStep(); // large box in the middle should be intersecting with sphere, cylinder, - // capsule and ellipsoid - EXPECT_EQ(4u, contacts.size()); + // capsule, ellipsoid, and cone + EXPECT_EQ(5u, contacts.size()); unsigned int contactBoxSphere = 0u; unsigned int contactBoxCylinder = 0u; unsigned int contactBoxCapsule = 0u; unsigned int contactBoxEllipsoid = 0u; + unsigned int contactBoxCone = 0u; for (auto &contact : contacts) { @@ -661,6 +666,16 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } + else if ((m1->GetName() == "box" && m2->GetName() == "cone") || + (m1->GetName() == "cone" && m2->GetName() == "box")) + { + contactBoxCone++; + Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -6.5, 0.5); + EXPECT_TRUE(physics::test::Equal(expectedContactPos, + contactPoint.point, 1e-6)) + << "expected: " << expectedContactPos << "\n" + << " actual: " << contactPoint.point; + } else { FAIL() << "There should not be contacts between: " @@ -671,6 +686,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_EQ(1u, contactBoxCylinder); EXPECT_EQ(1u, contactBoxCapsule); EXPECT_EQ(1u, contactBoxEllipsoid); + EXPECT_EQ(1u, contactBoxCone); // move sphere away sphereFreeGroup->SetWorldPose(math::eigen3::convert( @@ -682,12 +698,13 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) contacts = world->GetContactsFromLastStep(); // large box in the middle should be intersecting with cylinder, capsule, - // ellipsoid - EXPECT_EQ(3u, contacts.size()); + // ellipsoid, and cone + EXPECT_EQ(4u, contacts.size()); contactBoxCylinder = 0u; contactBoxCapsule = 0u; contactBoxEllipsoid = 0u; + contactBoxCone = 0u; for (auto contact : contacts) { const auto &contactPoint = contact.Get<::TestContactPoint>(); @@ -723,6 +740,14 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } + else if ((m1->GetName() == "box" && m2->GetName() == "cone") || + (m1->GetName() == "cone" && m2->GetName() == "box")) + { + contactBoxCone++; + Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -6.5, 0.5); + EXPECT_TRUE(physics::test::Equal(expectedContactPos, + contactPoint.point, 1e-6)); + } else { FAIL() << "There should only be contacts between box and cylinder"; @@ -731,6 +756,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_EQ(1u, contactBoxCylinder); EXPECT_EQ(1u, contactBoxCapsule); EXPECT_EQ(1u, contactBoxEllipsoid); + EXPECT_EQ(1u, contactBoxCone); // move cylinder away cylinderFreeGroup->SetWorldPose(math::eigen3::convert( @@ -744,6 +770,10 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) ellipsoidFreeGroup->SetWorldPose(math::eigen3::convert( math::Pose3d(0, -100, -100, 0, 0, 0))); + // move cone away + coneFreeGroup->SetWorldPose(math::eigen3::convert( + math::Pose3d(0, -100, -200, 0, 0, 0))); + // step and get contacts checkedOutput = StepWorld(world, false); EXPECT_FALSE(checkedOutput);