From ef9e019620e4170ecd50a9d5d7d784447921a80e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 22 Jan 2021 13:44:16 +0100 Subject: [PATCH 1/7] Added Ellipsoid and Capsule shapes to TPE Signed-off-by: ahcorde --- tpe/lib/src/Collision.cc | 12 +++ tpe/lib/src/Collision_TEST.cc | 26 +++++++ tpe/lib/src/Shape.cc | 98 ++++++++++++++++++++++- tpe/lib/src/Shape.hh | 81 +++++++++++++++++++ tpe/lib/src/Shape_TEST.cc | 41 ++++++++++ tpe/plugin/src/SDFFeatures.cc | 17 ++++ tpe/plugin/src/ShapeFeatures.cc | 133 ++++++++++++++++++++++++++++++++ tpe/plugin/src/ShapeFeatures.hh | 36 +++++++++ 8 files changed, 442 insertions(+), 2 deletions(-) diff --git a/tpe/lib/src/Collision.cc b/tpe/lib/src/Collision.cc index cd1a6dadc..184b9510b 100644 --- a/tpe/lib/src/Collision.cc +++ b/tpe/lib/src/Collision.cc @@ -75,12 +75,24 @@ void Collision::SetShape(const Shape &_shape) const BoxShape *typedShape = static_cast(&_shape); this->dataPtr->shape.reset(new BoxShape(*typedShape)); } + else if (_shape.GetType() == ShapeType::CAPSULE) + { + const CapsuleShape *typedShape = + static_cast(&_shape); + this->dataPtr->shape.reset(new CapsuleShape(*typedShape)); + } else if (_shape.GetType() == ShapeType::CYLINDER) { const CylinderShape *typedShape = static_cast(&_shape); this->dataPtr->shape.reset(new CylinderShape(*typedShape)); } + else if (_shape.GetType() == ShapeType::ELLIPSOID) + { + const EllipsoidShape *typedShape = + static_cast(&_shape); + this->dataPtr->shape.reset(new EllipsoidShape(*typedShape)); + } else if (_shape.GetType() == ShapeType::SPHERE) { const SphereShape *typedShape = dynamic_cast(&_shape); diff --git a/tpe/lib/src/Collision_TEST.cc b/tpe/lib/src/Collision_TEST.cc index d1bc6d1fa..80f104010 100644 --- a/tpe/lib/src/Collision_TEST.cc +++ b/tpe/lib/src/Collision_TEST.cc @@ -68,6 +68,20 @@ TEST(Collision, BoxShape) result->GetBoundingBox().Max()); } +///////////////////////////////////////////////// +TEST(Collision, CapsuleShape) +{ + Collision collision; + CapsuleShape CapsuleShape; + CapsuleShape.SetRadius(2.0); + CapsuleShape.SetLength(3.0); + collision.SetShape(CapsuleShape); + auto result = collision.GetShape(); + ASSERT_NE(nullptr, result); + EXPECT_EQ(ignition::math::Vector3d(2.0, 2.0, 3.5), + result->GetBoundingBox().Max()); +} + ///////////////////////////////////////////////// TEST(Collision, CylinderShape) { @@ -82,6 +96,18 @@ TEST(Collision, CylinderShape) result->GetBoundingBox().Max()); } +///////////////////////////////////////////////// +TEST(Collision, EllipsoidShape) +{ + Collision collision; + EllipsoidShape EllipsoidShape; + EllipsoidShape.SetRadii({1.0, 2.0, 1.3}); + collision.SetShape(EllipsoidShape); + auto result = collision.GetShape(); + ASSERT_NE(nullptr, result); + EXPECT_EQ(ignition::math::Vector3d(1.0, 2.0, 1.3), + result->GetBoundingBox().Max());} + ///////////////////////////////////////////////// TEST(Collision, SphereShape) { diff --git a/tpe/lib/src/Shape.cc b/tpe/lib/src/Shape.cc index 6337aebd3..f497bf06c 100644 --- a/tpe/lib/src/Shape.cc +++ b/tpe/lib/src/Shape.cc @@ -97,6 +97,61 @@ void BoxShape::UpdateBoundingBox() this->bbox = math::AxisAlignedBox(-halfSize, halfSize); } +////////////////////////////////////////////////// +CapsuleShape::CapsuleShape() : Shape() +{ + this->type = ShapeType::CAPSULE; +} + +////////////////////////////////////////////////// +CapsuleShape::CapsuleShape(const CapsuleShape &_other) + : Shape() +{ + *this = _other; +} + +////////////////////////////////////////////////// +Shape &CapsuleShape::operator=(const Shape &_other) +{ + auto other = static_cast(&_other); + this->radius = other->radius; + this->length = other->length; + return *this; +} + +////////////////////////////////////////////////// +double CapsuleShape::GetRadius() const +{ + return this->radius; +} + +////////////////////////////////////////////////// +void CapsuleShape::SetRadius(double _radius) +{ + this->radius = _radius; + this->dirty = true; +} + +////////////////////////////////////////////////// +double CapsuleShape::GetLength() const +{ + return this->length; +} + +////////////////////////////////////////////////// +void CapsuleShape::SetLength(double _length) +{ + this->length = _length; + this->dirty = true; +} + +////////////////////////////////////////////////// +void CapsuleShape::UpdateBoundingBox() +{ + math::Vector3d halfSize(this->radius, this->radius, this->length*0.5 + this->radius); + this->bbox = math::AxisAlignedBox(-halfSize, halfSize); +} + ////////////////////////////////////////////////// CylinderShape::CylinderShape() : Shape() { @@ -152,6 +207,47 @@ void CylinderShape::UpdateBoundingBox() this->bbox = math::AxisAlignedBox(-halfSize, halfSize); } +////////////////////////////////////////////////// +EllipsoidShape::EllipsoidShape() : Shape() +{ + this->type = ShapeType::ELLIPSOID; +} + +////////////////////////////////////////////////// +EllipsoidShape::EllipsoidShape(const EllipsoidShape &_other) + : Shape() +{ + *this = _other; +} + +////////////////////////////////////////////////// +Shape &EllipsoidShape::operator=(const Shape &_other) +{ + auto other = static_cast(&_other); + this->radii = other->radii; + return *this; +} + +////////////////////////////////////////////////// +math::Vector3d EllipsoidShape::GetRadii() const +{ + return this->radii; +} + +////////////////////////////////////////////////// +void EllipsoidShape::SetRadii(math::Vector3d _radii) +{ + this->radii = _radii; + this->dirty = true; +} + +////////////////////////////////////////////////// +void EllipsoidShape::UpdateBoundingBox() +{ + math::Vector3d halfSize(this->radii.X(), this->radii.Y(), this->radii.Z()); + this->bbox = math::AxisAlignedBox(-halfSize, halfSize); +} + ////////////////////////////////////////////////// SphereShape::SphereShape() : Shape() { @@ -245,5 +341,3 @@ void MeshShape::UpdateBoundingBox() this->bbox = math::AxisAlignedBox( this->scale * this->meshAABB.Min(), this->scale * this->meshAABB.Max()); } - - diff --git a/tpe/lib/src/Shape.hh b/tpe/lib/src/Shape.hh index d1d7ddc59..2bd1499d1 100644 --- a/tpe/lib/src/Shape.hh +++ b/tpe/lib/src/Shape.hh @@ -53,6 +53,12 @@ enum class IGNITION_PHYSICS_TPELIB_VISIBLE ShapeType /// \brief A mesh shape. MESH = 5, + + /// \brief A capsule shape. + CAPSULE = 4, + + /// \brief A ellipsoid shape. + ELLIPSOID = 5, }; @@ -120,6 +126,49 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE BoxShape : public Shape IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING }; +/// \brief Cylinder geometry +class IGNITION_PHYSICS_TPELIB_VISIBLE CapsuleShape : public Shape +{ + /// \brief Constructor + public: CapsuleShape(); + + /// \brief Copy Constructor + /// \param[in] _other shape to copy from + public: CapsuleShape(const CapsuleShape &_other); + + /// \brief Destructor + public: ~CapsuleShape() = default; + + /// \brief Assignment operator + /// \param[in] _other shape to copy from + public: Shape &operator=(const Shape &_other); + + /// \brief Get capsule radius + /// \return capsule radius + public: double GetRadius() const; + + /// \brief Set capsule radius + /// \param[in] _radius Cylinder radius + public: void SetRadius(double _radius); + + /// \brief Get capsule length + /// \return Cylinder length + public: double GetLength() const; + + /// \brief Set capsule length + /// \param[in] _length Cylinder length + public: void SetLength(double _length); + + // Documentation inherited + protected: virtual void UpdateBoundingBox() override; + + /// \brief Cylinder radius + private: double radius = 0.0; + + /// \brief Cylinder length + private: double length = 0.0; +}; + /// \brief Cylinder geometry class IGNITION_PHYSICS_TPELIB_VISIBLE CylinderShape : public Shape { @@ -163,6 +212,38 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE CylinderShape : public Shape private: double length = 0.0; }; +/// \brief Ellipsoid geometry +class IGNITION_PHYSICS_TPELIB_VISIBLE EllipsoidShape : public Shape +{ + /// \brief Constructor + public: EllipsoidShape(); + + /// \brief Copy Constructor + /// \param[in] _other shape to copy from + public: EllipsoidShape(const EllipsoidShape &_other); + + /// \brief Destructor + public: ~EllipsoidShape() = default; + + /// \brief Assignment operator + /// \param[in] _other shape to copy from + public: Shape &operator=(const Shape &_other); + + /// \brief Get ellipsoid radius + /// \return ellipsoid radius + public: math::Vector3d GetRadii() const; + + /// \brief Set ellipsoid radius + /// \param[in] _radius ellipsoid radius + public: void SetRadii(math::Vector3d _radii); + + // Documentation inherited + protected: virtual void UpdateBoundingBox() override; + + /// \brief ellipsoid radius + private: math::Vector3d radii = math::Vector3d::Zero; +}; + /// \brief Sphere geometry class IGNITION_PHYSICS_TPELIB_VISIBLE SphereShape : public Shape { diff --git a/tpe/lib/src/Shape_TEST.cc b/tpe/lib/src/Shape_TEST.cc index 767402b6f..251cf52d4 100644 --- a/tpe/lib/src/Shape_TEST.cc +++ b/tpe/lib/src/Shape_TEST.cc @@ -45,6 +45,28 @@ TEST(Shape, BoxShape) EXPECT_EQ(math::Vector3d(0.6, 1.8, 2.9), bbox.Max()); } +///////////////////////////////////////////////// +TEST(Shape, CapsuleShape) +{ + CapsuleShape shape; + EXPECT_EQ(ShapeType::CAPSULE, shape.GetType()); + math::AxisAlignedBox empty = shape.GetBoundingBox(); + EXPECT_EQ(math::Vector3d::Zero, empty.Center()); + EXPECT_EQ(math::Vector3d::Zero, empty.Size()); + + double radius = 0.6; + double length = 2.8; + shape.SetRadius(radius); + EXPECT_NEAR(radius, shape.GetRadius(), 1e-6); + shape.SetLength(length); + EXPECT_NEAR(length, shape.GetLength(), 1e-6); + math::AxisAlignedBox bbox = shape.GetBoundingBox(); + EXPECT_EQ(math::Vector3d::Zero, bbox.Center()); + EXPECT_EQ(math::Vector3d(1.2, 1.2, 4.0), bbox.Size()); + EXPECT_EQ(math::Vector3d(-0.6, -0.6, -2.0), bbox.Min()); + EXPECT_EQ(math::Vector3d(0.6, 0.6, 2.0), bbox.Max()); +} + ///////////////////////////////////////////////// TEST(Shape, CylinderShape) { @@ -67,6 +89,25 @@ TEST(Shape, CylinderShape) EXPECT_EQ(math::Vector3d(0.6, 0.6, 1.4), bbox.Max()); } +///////////////////////////////////////////////// +TEST(Shape, EllipsoidShape) +{ + EllipsoidShape shape; + EXPECT_EQ(ShapeType::ELLIPSOID, shape.GetType()); + math::AxisAlignedBox empty = shape.GetBoundingBox(); + EXPECT_EQ(math::Vector3d::Zero, empty.Center()); + EXPECT_EQ(math::Vector3d::Zero, empty.Size()); + + math::Vector3d radii(0.5, 0.6, 0.7); + shape.SetRadii(radii); + EXPECT_EQ(radii, shape.GetRadii()); + math::AxisAlignedBox bbox = shape.GetBoundingBox(); + EXPECT_EQ(math::Vector3d::Zero, bbox.Center()); + EXPECT_EQ(math::Vector3d(1.0, 1.2, 1.4), bbox.Size()); + EXPECT_EQ(math::Vector3d(-0.5, -0.6, -0.7), bbox.Min()); + EXPECT_EQ(math::Vector3d(0.5, 0.6, 0.7), bbox.Max()); +} + ///////////////////////////////////////////////// TEST(Shape, SphereShape) { diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index 08890c832..94704803c 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -18,7 +18,9 @@ #include "SDFFeatures.hh" #include +#include #include +#include #include #include #include @@ -250,6 +252,14 @@ Identity SDFFeatures::ConstructSdfCollision( shape.SetSize(boxSdf->Size()); collision->SetShape(shape); } + else if (geom->Type() == ::sdf::GeometryType::CAPSULE) + { + const auto capsuleSdf = geom->CapsuleShape(); + tpelib::CapsuleShape shape; + shape.SetRadius(capsuleSdf->Radius()); + shape.SetLength(capsuleSdf->Length()); + collision->SetShape(shape); + } else if (geom->Type() == ::sdf::GeometryType::CYLINDER) { const auto cylinderSdf = geom->CylinderShape(); @@ -258,6 +268,13 @@ Identity SDFFeatures::ConstructSdfCollision( shape.SetLength(cylinderSdf->Length()); collision->SetShape(shape); } + else if (geom->Type() == ::sdf::GeometryType::ELLIPSOID) + { + const auto ellipsoidSdf = geom->EllipsoidShape(); + tpelib::EllipsoidShape shape; + shape.SetRadii(ellipsoidSdf->Radii()); + collision->SetShape(shape); + } else if (geom->Type() == ::sdf::GeometryType::SPHERE) { const auto sphereSdf = geom->SphereShape(); diff --git a/tpe/plugin/src/ShapeFeatures.cc b/tpe/plugin/src/ShapeFeatures.cc index 2fd585ad1..a64eacfbd 100644 --- a/tpe/plugin/src/ShapeFeatures.cc +++ b/tpe/plugin/src/ShapeFeatures.cc @@ -95,6 +95,83 @@ Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToCapsuleShape(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::GetCapsuleShapeRadius( + const Identity &_capsuleID) const +{ + // assume _capsuleID ~= _collisionID + auto it = this->collisions.find(_capsuleID); + if (it != this->collisions.end() && it->second != nullptr) + { + auto *shape = it->second->collision->GetShape(); + if (shape != nullptr) + { + auto *capsule = static_cast(shape); + return capsule->GetRadius(); + } + } + // return invalid radius if no collision found + return -1.0; +} + +///////////////////////////////////////////////// +double ShapeFeatures::GetCapsuleShapeHeight( + const Identity &_capsuleID) const +{ + // assume _capsuleID ~= _collisionID + auto it = this->collisions.find(_capsuleID); + if (it != this->collisions.end() && it->second != nullptr) + { + auto *shape = it->second->collision->GetShape(); + if (shape != nullptr) + { + auto *capsule = static_cast(shape); + return capsule->GetLength(); + } + } + // return invalid height if no collision found + return -1.0; +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachCapsuleShape( + 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::CapsuleShape capsuleshape; + capsuleshape.SetRadius(_radius); + capsuleshape.SetLength(_height); + collision.SetShape(capsuleshape); + + return this->AddCollision(_linkID, collision); + } + return this->GenerateInvalidId(); +} + ///////////////////////////////////////////////// double ShapeFeatures::GetCylinderShapeRadius( const Identity &_cylinderID) const @@ -159,6 +236,62 @@ Identity ShapeFeatures::AttachCylinderShape( return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToEllipsoidShape(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(); +} + +///////////////////////////////////////////////// +Vector3d ShapeFeatures::GetEllipsoidShapeRadii( + const Identity &_capsuleID) const +{ + // assume _capsuleID ~= _collisionID + auto it = this->collisions.find(_capsuleID); + if (it != this->collisions.end() && it->second != nullptr) + { + auto *shape = it->second->collision->GetShape(); + if (shape != nullptr) + { + auto *capsule = static_cast(shape); + return math::eigen3::convert(capsule->GetRadii()); + } + } + // return invalid radius if no collision found + return math::eigen3::convert(math::Vector3d(-1.0, -1.0, -1.0)); +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachEllipsoidShape( + const Identity &_linkID, + const std::string &_name, + Vector3d _radii, + 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::EllipsoidShape capsuleshape; + capsuleshape.SetRadii(math::eigen3::convert(_radii)); + collision.SetShape(capsuleshape); + + return this->AddCollision(_linkID, collision); + } + return this->GenerateInvalidId(); +} + ///////////////////////////////////////////////// Identity ShapeFeatures::CastToSphereShape( const Identity &_shapeID) const diff --git a/tpe/plugin/src/ShapeFeatures.hh b/tpe/plugin/src/ShapeFeatures.hh index 5fca7de61..5132dce7a 100644 --- a/tpe/plugin/src/ShapeFeatures.hh +++ b/tpe/plugin/src/ShapeFeatures.hh @@ -22,7 +22,9 @@ #include #include +#include #include +#include #include #include @@ -37,9 +39,15 @@ struct ShapeFeatureList : FeatureList< AttachBoxShapeFeature, GetShapeBoundingBox, + GetCapsuleShapeProperties, + AttachCapsuleShapeFeature, + GetCylinderShapeProperties, AttachCylinderShapeFeature, + GetEllipsoidShapeProperties, + AttachEllipsoidShapeFeature, + GetSphereShapeProperties, AttachSphereShapeFeature, @@ -64,6 +72,22 @@ class ShapeFeatures : const LinearVector3d &_size, const Pose3d &_pose) override; + // ----- Capsule Features ----- + public: Identity CastToCapsuleShape( + const Identity &_shapeID) const override; + + public: double GetCapsuleShapeRadius( + const Identity &_capsuleID) const override; + + public: double GetCapsuleShapeHeight( + const Identity &_capsuleID) const override; + + public: Identity AttachCapsuleShape( + const Identity &_linkID, + const std::string &_name, + double _radius, + double _height, + const Pose3d &_pose) override; // ----- Cylinder Features ----- public: Identity CastToCylinderShape( @@ -82,6 +106,18 @@ class ShapeFeatures : double _height, const Pose3d &_pose) override; + // ----- Capsule Features ----- + public: Identity CastToEllipsoidShape( + const Identity &_shapeID) const override; + + public: Vector3d GetEllipsoidShapeRadii( + const Identity &_capsuleID) const override; + + public: Identity AttachEllipsoidShape( + const Identity &_linkID, + const std::string &_name, + Vector3d _radii, + const Pose3d &_pose) override; // ----- Sphere Features ----- public: Identity CastToSphereShape( From 6c4336f0c4b61a85cb191d865acd434ae6369d87 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 12 Apr 2021 22:20:36 +0200 Subject: [PATCH 2/7] Fixed renamed method Signed-off-by: ahcorde --- tpe/plugin/src/ShapeFeatures.cc | 2 +- tpe/plugin/src/ShapeFeatures.hh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tpe/plugin/src/ShapeFeatures.cc b/tpe/plugin/src/ShapeFeatures.cc index a64eacfbd..91a1e88ae 100644 --- a/tpe/plugin/src/ShapeFeatures.cc +++ b/tpe/plugin/src/ShapeFeatures.cc @@ -128,7 +128,7 @@ double ShapeFeatures::GetCapsuleShapeRadius( } ///////////////////////////////////////////////// -double ShapeFeatures::GetCapsuleShapeHeight( +double ShapeFeatures::GetCapsuleShapeLength( const Identity &_capsuleID) const { // assume _capsuleID ~= _collisionID diff --git a/tpe/plugin/src/ShapeFeatures.hh b/tpe/plugin/src/ShapeFeatures.hh index 5132dce7a..8cce3f138 100644 --- a/tpe/plugin/src/ShapeFeatures.hh +++ b/tpe/plugin/src/ShapeFeatures.hh @@ -79,7 +79,7 @@ class ShapeFeatures : public: double GetCapsuleShapeRadius( const Identity &_capsuleID) const override; - public: double GetCapsuleShapeHeight( + public: double GetCapsuleShapeLength( const Identity &_capsuleID) const override; public: Identity AttachCapsuleShape( From 3a79c63603626c6d6c26376063f781047bd18b36 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 12 Apr 2021 22:26:39 +0200 Subject: [PATCH 3/7] Fixed ShapeType enum Signed-off-by: ahcorde --- tpe/lib/src/Shape.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tpe/lib/src/Shape.hh b/tpe/lib/src/Shape.hh index 0a24e2401..0872150bb 100644 --- a/tpe/lib/src/Shape.hh +++ b/tpe/lib/src/Shape.hh @@ -55,10 +55,10 @@ enum class IGNITION_PHYSICS_TPELIB_VISIBLE ShapeType MESH = 5, /// \brief A capsule shape. - CAPSULE = 4, + CAPSULE = 6, /// \brief A ellipsoid shape. - ELLIPSOID = 5, + ELLIPSOID = 7, }; From 8f403269fb200f3c7bb190e8be792ce6fe994d50 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 12 Apr 2021 22:45:22 +0200 Subject: [PATCH 4/7] Added more tests Signed-off-by: ahcorde --- tpe/plugin/src/SimulationFeatures_TEST.cc | 24 ++++++++++++++++++ tpe/plugin/worlds/shapes.world | 31 +++++++++++++++++++++++ 2 files changed, 55 insertions(+) diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index 9901d09b1..dbcd32017 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -204,6 +204,19 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) EXPECT_EQ(2u, cylinderLink->GetShapeCount()); EXPECT_EQ(cylinder2, cylinderLink->GetShape(1)); + auto ellipsoid = world->GetModel("ellipsoid"); + auto ellipsoidLink = ellipsoid->GetLink(0); + auto ellipsoidCollision = ellipsoidLink->GetShape(0); + auto ellipsoidShape = ellipsoidCollision->CastToEllipsoidShape(); + EXPECT_EQ( + ignition::math::Vector3d(0.2, 0.3, 0.5), + ignition::math::eigen3::convert(ellipsoidShape->GetRadii())); + + auto ellipsoid2 = ellipsoidLink->AttachCylinderShape( + "ellipsoid2", 3.0, 4.0, Eigen::Isometry3d::Identity()); + EXPECT_EQ(2u, ellipsoidLink->GetShapeCount()); + EXPECT_EQ(ellipsoid2, ellipsoidLink->GetShape(1)); + // Test the bounding boxes in the local frames auto sphereAABB = sphereCollision->GetAxisAlignedBoundingBox(*sphereCollision); @@ -211,6 +224,8 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) boxCollision->GetAxisAlignedBoundingBox(*boxCollision); auto cylinderAABB = cylinderCollision->GetAxisAlignedBoundingBox(*cylinderCollision); + auto ellipsoidAABB = + ellipsoidCollision->GetAxisAlignedBoundingBox(*ellipsoidCollision); EXPECT_EQ(ignition::math::Vector3d(-1, -1, -1), ignition::math::eigen3::convert(sphereAABB).Min()); @@ -224,11 +239,16 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) ignition::math::eigen3::convert(cylinderAABB).Min()); EXPECT_EQ(ignition::math::Vector3d(0.5, 0.5, 0.55), ignition::math::eigen3::convert(cylinderAABB).Max()); + EXPECT_EQ(ignition::math::Vector3d(-0.2, -0.3, -0.5), + ignition::math::eigen3::convert(ellipsoidAABB).Min()); + EXPECT_EQ(ignition::math::Vector3d(0.2, 0.3, 0.5), + ignition::math::eigen3::convert(ellipsoidAABB).Max()); // check model AABB. By default, the AABBs are in world frame auto sphereModelAABB = sphere->GetAxisAlignedBoundingBox(); auto boxModelAABB = box->GetAxisAlignedBoundingBox(); auto cylinderModelAABB = cylinder->GetAxisAlignedBoundingBox(); + auto ellipsoidModelAABB = ellipsoid->GetAxisAlignedBoundingBox(); EXPECT_EQ(ignition::math::Vector3d(-1, 0.5, -0.5), ignition::math::eigen3::convert(sphereModelAABB).Min()); EXPECT_EQ(ignition::math::Vector3d(1, 2.5, 1.5), @@ -241,6 +261,10 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) ignition::math::eigen3::convert(cylinderModelAABB).Min()); EXPECT_EQ(ignition::math::Vector3d(3, 1.5, 2.5), ignition::math::eigen3::convert(cylinderModelAABB).Max()); + EXPECT_EQ(ignition::math::Vector3d(-3, -8, 3), + ignition::math::eigen3::convert(ellipsoidModelAABB).Min()); + EXPECT_EQ(ignition::math::Vector3d(3, -2, 7), + ignition::math::eigen3::convert(ellipsoidModelAABB).Max()); } } diff --git a/tpe/plugin/worlds/shapes.world b/tpe/plugin/worlds/shapes.world index bbddf19be..ae4fff6cc 100644 --- a/tpe/plugin/worlds/shapes.world +++ b/tpe/plugin/worlds/shapes.world @@ -96,5 +96,36 @@ + + + 0 -5 5 0 0 0 + + + + 0.068 + 0 + 0 + 0.058 + 0 + 0.026 + + 1.0 + + + + + 0.2 0.3 0.5 + + + + + + + 0.2 0.3 0.5 + + + + + From 65f21ef90e28b5a408b6eb36ec15fbef9c281a41 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 18 Jun 2021 12:57:22 +0200 Subject: [PATCH 5/7] Improved tests and other fixes Signed-off-by: ahcorde --- tpe/lib/src/CollisionDetector_TEST.cc | 94 +++++++++++++++++- tpe/lib/src/Shape.cc | 2 +- tpe/lib/src/Shape.hh | 10 +- tpe/plugin/src/SimulationFeatures_TEST.cc | 110 +++++++++++++++++++--- tpe/plugin/worlds/shapes.world | 40 +++++++- 5 files changed, 235 insertions(+), 21 deletions(-) diff --git a/tpe/lib/src/CollisionDetector_TEST.cc b/tpe/lib/src/CollisionDetector_TEST.cc index 6305a6c03..e7aefc2dd 100644 --- a/tpe/lib/src/CollisionDetector_TEST.cc +++ b/tpe/lib/src/CollisionDetector_TEST.cc @@ -113,6 +113,26 @@ TEST(CollisionDetector, CheckCollisions) cylinderShapeC.SetLength(4); collisionC->SetShape(cylinderShapeC); + // model D + std::shared_ptr modelD(new Model); + Entity &linkDEnt = modelD->AddLink(); + Link *linkD = static_cast(&linkDEnt); + Entity &collisionDEnt = linkD->AddCollision(); + Collision *collisionD = static_cast(&collisionDEnt); + CapsuleShape capsuleShapeD; + capsuleShapeD.SetRadius(0.2); + capsuleShapeD.SetLength(0.6); + collisionD->SetShape(capsuleShapeD); + + // model E + std::shared_ptr modelE(new Model); + Entity &linkEEnt = modelE->AddLink(); + Link *linkE = static_cast(&linkEEnt); + Entity &collisionEEnt = linkE->AddCollision(); + Collision *collisionE = static_cast(&collisionEEnt); + EllipsoidShape ellipsoidShapeE; + ellipsoidShapeE.SetRadii({2, 2, 0.5}); + collisionE->SetShape(ellipsoidShapeE); // check collisions CollisionDetector cd; @@ -121,14 +141,18 @@ TEST(CollisionDetector, CheckCollisions) modelA->SetPose(math::Pose3d(100, 0, 0, 0, 0, 0)); modelB->SetPose(math::Pose3d(0, 0, 0, 0, 0, 0)); modelC->SetPose(math::Pose3d(-100, 0, 0, 0, 0, 0)); + modelD->SetPose(math::Pose3d(-200, 0, 0, 0, 0, 0)); + modelE->SetPose(math::Pose3d(200, 0, 0, 0, 0, 0)); entities[modelA->GetId()] = modelA; entities[modelB->GetId()] = modelB; entities[modelC->GetId()] = modelC; + entities[modelD->GetId()] = modelD; + entities[modelE->GetId()] = modelE; std::vector contacts = cd.CheckCollisions(entities); EXPECT_TRUE(contacts.empty()); - // collision between model A and B but not model C + // collision between model A and B but not model C, D and E modelA->SetPose(math::Pose3d(2, 2, 2, 0, 0, 0)); modelB->SetPose(math::Pose3d(0, 0, 0, 0, 0, 0)); modelC->SetPose(math::Pose3d(100, 0, 0, 0, 0, 0)); @@ -140,6 +164,8 @@ TEST(CollisionDetector, CheckCollisions) EXPECT_TRUE(c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()); EXPECT_TRUE(c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId()); EXPECT_TRUE(c.entity1 != modelC->GetId() && c.entity2 != modelC->GetId()); + EXPECT_TRUE(c.entity1 != modelD->GetId() && c.entity2 != modelD->GetId()); + EXPECT_TRUE(c.entity1 != modelE->GetId() && c.entity2 != modelE->GetId()); EXPECT_NE(c.entity1, c.entity2); } // check single contact point @@ -151,8 +177,12 @@ TEST(CollisionDetector, CheckCollisions) (contacts[0].entity2 == modelB->GetId())); EXPECT_TRUE((contacts[0].entity1 != modelC->GetId()) && (contacts[0].entity2 != modelC->GetId())); + EXPECT_TRUE((contacts[0].entity1 != modelD->GetId()) && + (contacts[0].entity2 != modelD->GetId())); + EXPECT_TRUE((contacts[0].entity1 != modelE->GetId()) && + (contacts[0].entity2 != modelE->GetId())); - // collision between model A and C but not model B + // collision between model A and C but not model B, D and E modelA->SetPose(math::Pose3d(2, 1, 2, 0, 0, 0)); modelB->SetPose(math::Pose3d(100, 0, 0, 0, 0, 0)); modelC->SetPose(math::Pose3d(2, 3, 4, 0, 0, 0)); @@ -164,6 +194,8 @@ TEST(CollisionDetector, CheckCollisions) EXPECT_TRUE(c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()); EXPECT_TRUE(c.entity1 != modelB->GetId() && c.entity2 != modelB->GetId()); EXPECT_TRUE(c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId()); + EXPECT_TRUE(c.entity1 != modelD->GetId() && c.entity2 != modelD->GetId()); + EXPECT_TRUE(c.entity1 != modelE->GetId() && c.entity2 != modelE->GetId()); EXPECT_NE(c.entity1, c.entity2); } // check single contact point @@ -175,6 +207,10 @@ TEST(CollisionDetector, CheckCollisions) (contacts[0].entity2 != modelB->GetId())); EXPECT_TRUE((contacts[0].entity1 == modelC->GetId()) || (contacts[0].entity2 == modelC->GetId())); + EXPECT_TRUE((contacts[0].entity1 != modelD->GetId()) && + (contacts[0].entity2 != modelD->GetId())); + EXPECT_TRUE((contacts[0].entity1 != modelE->GetId()) && + (contacts[0].entity2 != modelE->GetId())); // collision between model A and B, and B and C, but not A and C modelA->SetPose(math::Pose3d(-2, -2, -2, 0, 0, 0)); @@ -196,6 +232,27 @@ TEST(CollisionDetector, CheckCollisions) else if ((c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()) && (c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId())) FAIL() << "There should be no contacts between model A and C"; + else if ((c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()) && + (c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId())) + FAIL() << "There should be no contacts between model A and D"; + else if ((c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()) && + (c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId())) + FAIL() << "There should be no contacts between model A and E"; + else if ((c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId()) && + (c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId())) + FAIL() << "There should be no contacts between model B and D"; + else if ((c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId()) && + (c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId())) + FAIL() << "There should be no contacts between model B and E"; + else if ((c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId()) && + (c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId())) + FAIL() << "There should be no contacts between model C and D"; + else if ((c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId()) && + (c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId())) + FAIL() << "There should be no contacts between model C and E"; + else if ((c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId()) && + (c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId())) + FAIL() << "There should be no contacts between model D and E"; } EXPECT_EQ(8u, contactAB); EXPECT_EQ(8u, contactBC); @@ -240,6 +297,39 @@ TEST(CollisionDetector, CheckCollisions) EXPECT_EQ(8u, contacts.size()); contacts = cd.CheckCollisions(entities, true); EXPECT_EQ(1u, contacts.size()); + + entities[modelC->GetId()] = modelC; + // collision between model E and B but not model A, C, D + modelA->SetPose(math::Pose3d(100, 0, 0, 0, 0, 0)); + modelB->SetPose(math::Pose3d(0, 0, 0, 0, 0, 0)); + modelC->SetPose(math::Pose3d(-100, 0, 0, 0, 0, 0)); + modelD->SetPose(math::Pose3d(-200, 0, 0, 0, 0, 0)); + modelE->SetPose(math::Pose3d(5, 0, 0, 0, 0, 0)); + contacts = cd.CheckCollisions(entities); + EXPECT_EQ(8u, contacts.size()); + + for (const auto &c : contacts) + { + EXPECT_TRUE(c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId()); + EXPECT_TRUE(c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId()); + EXPECT_TRUE(c.entity1 != modelA->GetId() && c.entity2 != modelA->GetId()); + EXPECT_TRUE(c.entity1 != modelC->GetId() && c.entity2 != modelC->GetId()); + EXPECT_TRUE(c.entity1 != modelD->GetId() && c.entity2 != modelD->GetId()); + EXPECT_NE(c.entity1, c.entity2); + } + // check single contact point + contacts = cd.CheckCollisions(entities, true); + EXPECT_EQ(1u, contacts.size()); + EXPECT_TRUE((contacts[0].entity1 != modelA->GetId()) && + (contacts[0].entity2 != modelA->GetId())); + EXPECT_TRUE((contacts[0].entity1 == modelB->GetId()) || + (contacts[0].entity2 == modelB->GetId())); + EXPECT_TRUE((contacts[0].entity1 != modelC->GetId()) && + (contacts[0].entity2 != modelC->GetId())); + EXPECT_TRUE((contacts[0].entity1 != modelD->GetId()) && + (contacts[0].entity2 != modelD->GetId())); + EXPECT_TRUE((contacts[0].entity1 == modelE->GetId()) || + (contacts[0].entity2 == modelE->GetId())); } ///////////////////////////////////////////////// diff --git a/tpe/lib/src/Shape.cc b/tpe/lib/src/Shape.cc index f497bf06c..abd2dbff8 100644 --- a/tpe/lib/src/Shape.cc +++ b/tpe/lib/src/Shape.cc @@ -235,7 +235,7 @@ math::Vector3d EllipsoidShape::GetRadii() const } ////////////////////////////////////////////////// -void EllipsoidShape::SetRadii(math::Vector3d _radii) +void EllipsoidShape::SetRadii(const math::Vector3d &_radii) { this->radii = _radii; this->dirty = true; diff --git a/tpe/lib/src/Shape.hh b/tpe/lib/src/Shape.hh index 0872150bb..da99eed23 100644 --- a/tpe/lib/src/Shape.hh +++ b/tpe/lib/src/Shape.hh @@ -126,7 +126,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE BoxShape : public Shape IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING }; -/// \brief Cylinder geometry +/// \brief Capsule geometry class IGNITION_PHYSICS_TPELIB_VISIBLE CapsuleShape : public Shape { /// \brief Constructor @@ -152,7 +152,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE CapsuleShape : public Shape public: void SetRadius(double _radius); /// \brief Get capsule length - /// \return Cylinder length + /// \return Capsule length public: double GetLength() const; /// \brief Set capsule length @@ -162,10 +162,10 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE CapsuleShape : public Shape // Documentation inherited protected: virtual void UpdateBoundingBox() override; - /// \brief Cylinder radius + /// \brief Capsule radius private: double radius = 0.0; - /// \brief Cylinder length + /// \brief Capsule length private: double length = 0.0; }; @@ -235,7 +235,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE EllipsoidShape : public Shape /// \brief Set ellipsoid radius /// \param[in] _radius ellipsoid radius - public: void SetRadii(math::Vector3d _radii); + public: void SetRadii(const math::Vector3d &_radii); // Documentation inherited protected: virtual void UpdateBoundingBox() override; diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index dbcd32017..85befd8c8 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -212,11 +212,25 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) ignition::math::Vector3d(0.2, 0.3, 0.5), ignition::math::eigen3::convert(ellipsoidShape->GetRadii())); - auto ellipsoid2 = ellipsoidLink->AttachCylinderShape( - "ellipsoid2", 3.0, 4.0, Eigen::Isometry3d::Identity()); + auto ellipsoid2 = ellipsoidLink->AttachEllipsoidShape( + "ellipsoid2", + ignition::math::eigen3::convert(ignition::math::Vector3d(0.2, 0.3, 0.5)), + Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, ellipsoidLink->GetShapeCount()); EXPECT_EQ(ellipsoid2, ellipsoidLink->GetShape(1)); + auto capsule = world->GetModel("capsule"); + auto capsuleLink = capsule->GetLink(0); + auto capsuleCollision = capsuleLink->GetShape(0); + auto capsuleShape = capsuleCollision->CastToCapsuleShape(); + EXPECT_NEAR(0.2, capsuleShape->GetRadius(), 1e-6); + EXPECT_NEAR(0.6, capsuleShape->GetLength(), 1e-6); + + auto capsule2 = capsuleLink->AttachCapsuleShape( + "capsule2", 0.2, 0.6, Eigen::Isometry3d::Identity()); + EXPECT_EQ(2u, capsuleLink->GetShapeCount()); + EXPECT_EQ(capsule2, capsuleLink->GetShape(1)); + // Test the bounding boxes in the local frames auto sphereAABB = sphereCollision->GetAxisAlignedBoundingBox(*sphereCollision); @@ -226,6 +240,8 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) cylinderCollision->GetAxisAlignedBoundingBox(*cylinderCollision); auto ellipsoidAABB = ellipsoidCollision->GetAxisAlignedBoundingBox(*ellipsoidCollision); + auto capsuleAABB = + capsuleCollision->GetAxisAlignedBoundingBox(*capsuleCollision); EXPECT_EQ(ignition::math::Vector3d(-1, -1, -1), ignition::math::eigen3::convert(sphereAABB).Min()); @@ -243,12 +259,17 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) ignition::math::eigen3::convert(ellipsoidAABB).Min()); EXPECT_EQ(ignition::math::Vector3d(0.2, 0.3, 0.5), ignition::math::eigen3::convert(ellipsoidAABB).Max()); + EXPECT_EQ(ignition::math::Vector3d(-0.2, -0.2, -0.5), + ignition::math::eigen3::convert(capsuleAABB).Min()); + EXPECT_EQ(ignition::math::Vector3d(0.2, 0.2, 0.5), + ignition::math::eigen3::convert(capsuleAABB).Max()); // check model AABB. By default, the AABBs are in world frame auto sphereModelAABB = sphere->GetAxisAlignedBoundingBox(); auto boxModelAABB = box->GetAxisAlignedBoundingBox(); auto cylinderModelAABB = cylinder->GetAxisAlignedBoundingBox(); auto ellipsoidModelAABB = ellipsoid->GetAxisAlignedBoundingBox(); + auto capsuleModelAABB = capsule->GetAxisAlignedBoundingBox(); EXPECT_EQ(ignition::math::Vector3d(-1, 0.5, -0.5), ignition::math::eigen3::convert(sphereModelAABB).Min()); EXPECT_EQ(ignition::math::Vector3d(1, 2.5, 1.5), @@ -261,10 +282,14 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) ignition::math::eigen3::convert(cylinderModelAABB).Min()); EXPECT_EQ(ignition::math::Vector3d(3, 1.5, 2.5), ignition::math::eigen3::convert(cylinderModelAABB).Max()); - EXPECT_EQ(ignition::math::Vector3d(-3, -8, 3), + EXPECT_EQ(ignition::math::Vector3d(-0.2, -5.3, 0.2), ignition::math::eigen3::convert(ellipsoidModelAABB).Min()); - EXPECT_EQ(ignition::math::Vector3d(3, -2, 7), + EXPECT_EQ(ignition::math::Vector3d(0.2, -4.7, 1.2), ignition::math::eigen3::convert(ellipsoidModelAABB).Max()); + EXPECT_EQ(ignition::math::Vector3d(-0.2, -3.2, 0), + ignition::math::eigen3::convert(capsuleModelAABB).Min()); + EXPECT_EQ(ignition::math::Vector3d(0.2, -2.8, 1), + ignition::math::eigen3::convert(capsuleModelAABB).Max()); } } @@ -367,6 +392,14 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) auto cylinderFreeGroup = cylinder->FindFreeGroup(); EXPECT_NE(nullptr, cylinderFreeGroup); + auto capsule = world->GetModel("capsule"); + auto capsuleFreeGroup = capsule->FindFreeGroup(); + EXPECT_NE(nullptr, capsuleFreeGroup); + + auto ellipsoid = world->GetModel("ellipsoid"); + auto ellipsoidFreeGroup = ellipsoid->FindFreeGroup(); + EXPECT_NE(nullptr, ellipsoidFreeGroup); + auto box = world->GetModel("box"); // step and get contacts @@ -374,10 +407,13 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_TRUE(checkedOutput); auto contacts = world->GetContactsFromLastStep(); - // large box in the middle should be intersecting with sphere and cylinder - EXPECT_EQ(2u, contacts.size()); + // large box in the middle should be intersecting with sphere, cylinder, + // capsule and ellipsoid + EXPECT_EQ(4u, contacts.size()); unsigned int contactBoxSphere = 0u; unsigned int contactBoxCylinder = 0u; + unsigned int contactBoxCapsule = 0u; + unsigned int contactBoxEllipsoid = 0u; for (auto &contact : contacts) { @@ -406,6 +442,22 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } + else if ((m1->GetName() == "box" && m2->GetName() == "capsule") || + (m1->GetName() == "capsule" && m2->GetName() == "box")) + { + contactBoxCapsule++; + Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -3, 0.5); + EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos, + contactPoint.point, 1e-6)); + } + else if ((m1->GetName() == "box" && m2->GetName() == "ellipsoid") || + (m1->GetName() == "ellipsoid" && m2->GetName() == "box")) + { + contactBoxEllipsoid++; + Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -5, 0.6); + EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos, + contactPoint.point, 1e-6)); + } else { FAIL() << "There should not be contacts between: " @@ -414,6 +466,8 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) } EXPECT_EQ(1u, contactBoxSphere); EXPECT_EQ(1u, contactBoxCylinder); + EXPECT_EQ(1u, contactBoxCapsule); + EXPECT_EQ(1u, contactBoxEllipsoid); // move sphere away sphereFreeGroup->SetWorldPose(ignition::math::eigen3::convert( @@ -424,10 +478,13 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_FALSE(checkedOutput); contacts = world->GetContactsFromLastStep(); - // large box in the middle should be intersecting with cylinder - EXPECT_EQ(1u, contacts.size()); + // large box in the middle should be intersecting with cylinder, capsule, + // ellipsoid + EXPECT_EQ(3u, contacts.size()); contactBoxCylinder = 0u; + contactBoxCapsule = 0u; + contactBoxEllipsoid = 0u; for (auto contact : contacts) { const auto &contactPoint = contact.Get<::ContactPoint>(); @@ -441,20 +498,49 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) auto m2 = c2->GetLink()->GetModel(); if ((m1->GetName() == "box" && m2->GetName() == "cylinder") || (m1->GetName() == "cylinder" && m2->GetName() == "box")) + { contactBoxCylinder++; + Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0, -1.5, 0.5); + EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos, + contactPoint.point, 1e-6)); + } + else if ((m1->GetName() == "box" && m2->GetName() == "capsule") || + (m1->GetName() == "capsule" && m2->GetName() == "box")) + { + contactBoxCapsule++; + Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -3, 0.5); + EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos, + contactPoint.point, 1e-6)); + } + else if ((m1->GetName() == "box" && m2->GetName() == "ellipsoid") || + (m1->GetName() == "ellipsoid" && m2->GetName() == "box")) + { + contactBoxEllipsoid++; + Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -5, 0.6); + EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos, + contactPoint.point, 1e-6)); + } else + { FAIL() << "There should only be contacts between box and cylinder"; - - Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0, -1.5, 0.5); - EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos, - contactPoint.point, 1e-6)); + } } EXPECT_EQ(1u, contactBoxCylinder); + EXPECT_EQ(1u, contactBoxCapsule); + EXPECT_EQ(1u, contactBoxEllipsoid); // move cylinder away cylinderFreeGroup->SetWorldPose(ignition::math::eigen3::convert( ignition::math::Pose3d(0, -100, 0.5, 0, 0, 0))); + // move capsule away + capsuleFreeGroup->SetWorldPose(ignition::math::eigen3::convert( + ignition::math::Pose3d(0, -100, 100, 0, 0, 0))); + + // move ellipsoid away + ellipsoidFreeGroup->SetWorldPose(ignition::math::eigen3::convert( + ignition::math::Pose3d(0, -100, -100, 0, 0, 0))); + // step and get contacts checkedOutput = StepWorld(world, false); EXPECT_FALSE(checkedOutput); diff --git a/tpe/plugin/worlds/shapes.world b/tpe/plugin/worlds/shapes.world index ae4fff6cc..cb3bce5d9 100644 --- a/tpe/plugin/worlds/shapes.world +++ b/tpe/plugin/worlds/shapes.world @@ -97,8 +97,46 @@ + + 0 -3.0 0.5 0 0 0 + + + + 0.074154 + 0 + 0 + 0.074154 + 0 + 0.018769 + + 1.0 + + + + + 0.2 + 0.6 + + + + + + + 0.2 + 0.6 + + + + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 + + + + + - 0 -5 5 0 0 0 + 0 -5 0.7 0 0 0 From 6df64446b2de9a5134b5dd5eb7f94959bd122b21 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 23 Jun 2021 08:41:05 +0200 Subject: [PATCH 6/7] Fixed windows warning Signed-off-by: ahcorde --- tpe/lib/src/Shape.hh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tpe/lib/src/Shape.hh b/tpe/lib/src/Shape.hh index da99eed23..406f6f35c 100644 --- a/tpe/lib/src/Shape.hh +++ b/tpe/lib/src/Shape.hh @@ -240,8 +240,10 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE EllipsoidShape : public Shape // Documentation inherited protected: virtual void UpdateBoundingBox() override; + IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief ellipsoid radius private: math::Vector3d radii = math::Vector3d::Zero; + IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING }; /// \brief Sphere geometry From cf373b163bda3c920dcb1b1cabdcc1087abbd6c1 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 25 Jun 2021 09:32:38 +0200 Subject: [PATCH 7/7] Added feedback Signed-off-by: ahcorde --- tpe/lib/src/CollisionDetector_TEST.cc | 32 +++++++++++++++++++++++++++ tpe/plugin/src/ShapeFeatures.cc | 4 ++-- 2 files changed, 34 insertions(+), 2 deletions(-) diff --git a/tpe/lib/src/CollisionDetector_TEST.cc b/tpe/lib/src/CollisionDetector_TEST.cc index e7aefc2dd..9d2e99acf 100644 --- a/tpe/lib/src/CollisionDetector_TEST.cc +++ b/tpe/lib/src/CollisionDetector_TEST.cc @@ -330,6 +330,38 @@ TEST(CollisionDetector, CheckCollisions) (contacts[0].entity2 != modelD->GetId())); EXPECT_TRUE((contacts[0].entity1 == modelE->GetId()) || (contacts[0].entity2 == modelE->GetId())); + + // collision between model D and B but not model A, C, E + modelA->SetPose(math::Pose3d(100, 0, 0, 0, 0, 0)); + modelB->SetPose(math::Pose3d(0, 0, 0, 0, 0, 0)); + modelC->SetPose(math::Pose3d(-100, 0, 0, 0, 0, 0)); + modelD->SetPose(math::Pose3d(5, 0, 0, 0, 0, 0)); + modelE->SetPose(math::Pose3d(200, 0, 0, 0, 0, 0)); + contacts = cd.CheckCollisions(entities); + EXPECT_EQ(8u, contacts.size()); + + for (const auto &c : contacts) + { + EXPECT_TRUE(c.entity1 != modelA->GetId() && c.entity2 != modelA->GetId()); + EXPECT_TRUE(c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId()); + EXPECT_TRUE(c.entity1 != modelC->GetId() && c.entity2 != modelC->GetId()); + EXPECT_TRUE(c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId()); + EXPECT_TRUE(c.entity1 != modelE->GetId() && c.entity2 != modelE->GetId()); + EXPECT_NE(c.entity1, c.entity2); + } + // check single contact point + contacts = cd.CheckCollisions(entities, true); + EXPECT_EQ(1u, contacts.size()); + EXPECT_TRUE((contacts[0].entity1 != modelA->GetId()) && + (contacts[0].entity2 != modelA->GetId())); + EXPECT_TRUE((contacts[0].entity1 == modelB->GetId()) || + (contacts[0].entity2 == modelB->GetId())); + EXPECT_TRUE((contacts[0].entity1 != modelC->GetId()) && + (contacts[0].entity2 != modelC->GetId())); + EXPECT_TRUE((contacts[0].entity1 == modelD->GetId()) || + (contacts[0].entity2 == modelD->GetId())); + EXPECT_TRUE((contacts[0].entity1 != modelE->GetId()) && + (contacts[0].entity2 != modelE->GetId())); } ///////////////////////////////////////////////// diff --git a/tpe/plugin/src/ShapeFeatures.cc b/tpe/plugin/src/ShapeFeatures.cc index 91a1e88ae..e26fc7f68 100644 --- a/tpe/plugin/src/ShapeFeatures.cc +++ b/tpe/plugin/src/ShapeFeatures.cc @@ -151,7 +151,7 @@ Identity ShapeFeatures::AttachCapsuleShape( const Identity &_linkID, const std::string &_name, const double _radius, - const double _height, + const double _length, const Pose3d &_pose) { auto it = this->links.find(_linkID); @@ -164,7 +164,7 @@ Identity ShapeFeatures::AttachCapsuleShape( tpelib::CapsuleShape capsuleshape; capsuleshape.SetRadius(_radius); - capsuleshape.SetLength(_height); + capsuleshape.SetLength(_length); collision.SetShape(capsuleshape); return this->AddCollision(_linkID, collision);