[test] Check that local wrench and global wrench are consistent.

isri-aist · Oct 30, 2023 · cb0e411 · cb0e411
1 parent 9b60d5a
commit cb0e411
Showing 1 changed file with 8 additions and 4 deletions.
diff --git a/tests/src/TestWrenchDistribution.cpp b/tests/src/TestWrenchDistribution.cpp
@@ -69,19 +69,21 @@ template<bool WithMaxWrench>
 void do_TestWrenchDistribution_ContainsGraspContact()
 {
   double fricCoeff = 0.5;
+  std::vector<sva::PTransformd> contactPoseList = {
+      sva::PTransformd::Identity(), sva::PTransformd(Eigen::Vector3d(0, -0.5, 0.5)),
+      sva::PTransformd(sva::RotY(M_PI / 2), Eigen::Vector3d(0.5, 0.5, 1.0))};
   auto leftFootContact = std::make_shared<ForceColl::SurfaceContact>(
       "LeftFootContact", fricCoeff,
       std::vector<Eigen::Vector3d>{Eigen::Vector3d(-0.1, -0.1, 0.0), Eigen::Vector3d(-0.1, 0.1, 0.0),
                                    Eigen::Vector3d(0.1, 0.0, 0.0)},
-      sva::PTransformd::Identity());
+      contactPoseList[0]);
   auto rightFootContact = std::make_shared<ForceColl::SurfaceContact>(
-      "RightFootContact", fricCoeff, std::vector<Eigen::Vector3d>{Eigen::Vector3d::Zero()},
-      sva::PTransformd(Eigen::Vector3d(0, -0.5, 0.5)));
+      "RightFootContact", fricCoeff, std::vector<Eigen::Vector3d>{Eigen::Vector3d::Zero()}, contactPoseList[1]);
   auto leftHandContact = std::make_shared<ForceColl::GraspContact>(
       "LeftHandContact", fricCoeff,
       std::vector<sva::PTransformd>{sva::PTransformd(Eigen::Vector3d(0.0, 0.0, -0.01)),
                                     sva::PTransformd(sva::RotX(M_PI), Eigen::Vector3d(0.0, 0.0, 0.01))},
-      sva::PTransformd(sva::RotY(M_PI / 2), Eigen::Vector3d(0.5, 0.5, 1.0)));
+      contactPoseList[2]);
 
   sva::ForceVecd maxWrench = sva::ForceVecd(Eigen::Vector3d(1.0, 1.0, 1.0), Eigen::Vector3d(1.0, 1.0, 10.0));
   if constexpr(WithMaxWrench)
@@ -109,6 +111,8 @@ void do_TestWrenchDistribution_ContainsGraspContact()
 
     EXPECT_GT(wrench.vector().norm(), 1e-10);
 
+    EXPECT_LT((wrench - contactPoseList[i].transMul(localWrench)).vector().norm(), 1e-10);
+
     if(contact->name_ == "LeftHandContact")
     {
       if constexpr(WithMaxWrench)