Add unit test for updateGlobalVertices

isri-aist · Apr 12, 2024 · a6114ce · a6114ce
1 parent 5c96299
commit a6114ce
Showing 1 changed file with 38 additions and 0 deletions.
diff --git a/tests/src/TestContact.cpp b/tests/src/TestContact.cpp
@@ -128,6 +128,44 @@ TEST(TestContact, calcWrench)
   ForceColl::calcWrenchList(ForceColl::getContactVecFromMap(contactUnorderedMap), wrenchRatio);
 }
 
+TEST(TestContact, UpdateSurfaceContactVertices)
+{
+  sva::PTransformd targetPose(sva::RotX(M_PI / 2), Eigen::Vector3d(0.0, 0.5, -0.5));
+  auto originalContact = std::make_shared<ForceColl::SurfaceContact>(
+    "OriginalContact", 1.0,
+    std::vector<Eigen::Vector3d>{Eigen::Vector3d(-0.1, -0.1, 0.0), Eigen::Vector3d(-0.1, 0.1, 0.0), Eigen::Vector3d(0.0, 0.0, 0.1)},
+    sva::PTransformd::Identity());
+  auto targetContact = std::make_shared<ForceColl::SurfaceContact>(
+    "TargetContact", 1.0,
+    std::vector<Eigen::Vector3d>{Eigen::Vector3d(-0.1, -0.1, 0.0), Eigen::Vector3d(-0.1, 0.1, 0.0), Eigen::Vector3d(0.0, 0.0, 0.1)},
+    targetPose);
+  originalContact->updateGlobalVertices(targetPose);
+  EXPECT_LT((originalContact->graspMat_ - targetContact->graspMat_).norm(), 1e-8) << "originalContact:\n"
+                                                                                  << originalContact->graspMat_ << std::endl
+                                                                                  << "targetContact:\n"
+                                                                                  << targetContact->graspMat_ << std::endl;
+}
+
+TEST(TestContact, UpdateGraspContactVertices)
+{
+  sva::PTransformd targetPose(sva::RotX(M_PI / 2), Eigen::Vector3d(0.0, 0.5, -0.5));
+  auto originalContact = std::make_shared<ForceColl::GraspContact>(
+    "OriginalContact", 1.0,
+    std::vector<sva::PTransformd>{sva::PTransformd(sva::RotX(-1 * M_PI / 2), Eigen::Vector3d(-0.1, -0.1, 0.0)),
+      sva::PTransformd(sva::RotX(M_PI / 2), Eigen::Vector3d(-0.1, 0.1, 0.0)), sva::PTransformd(sva::RotX(0.0), Eigen::Vector3d(0.0, 0.0, 0.1))},
+    sva::PTransformd::Identity());
+  auto targetContact = std::make_shared<ForceColl::GraspContact>(
+    "TargetContact", 1.0,
+    std::vector<sva::PTransformd>{sva::PTransformd(sva::RotX(-1 * M_PI / 2), Eigen::Vector3d(-0.1, -0.1, 0.0)),
+      sva::PTransformd(sva::RotX(M_PI / 2), Eigen::Vector3d(-0.1, 0.1, 0.0)), sva::PTransformd(sva::RotX(0.0), Eigen::Vector3d(0.0, 0.0, 0.1))},
+    targetPose);
+  originalContact->updateGlobalVertices(targetPose);
+  EXPECT_LT((originalContact->graspMat_ - targetContact->graspMat_).norm(), 1e-8) << "originalContact:\n"
+                                                                                  << originalContact->graspMat_ << std::endl
+                                                                                  << "targetContact:\n"
+                                                                                  << targetContact->graspMat_ << std::endl;
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);