From 71d883eafcfaa843714b5c8f7d67a2d37f2a7c99 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 29 May 2024 05:38:24 -0700 Subject: [PATCH] Add cone to shapes tests Signed-off-by: Steve Peters --- test/common_test/simulation_features.cc | 70 +++++++++++++++++++++++-- test/common_test/worlds/shapes.world | 34 ++++++++++++ 2 files changed, 100 insertions(+), 4 deletions(-) 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 + + + + +