From a606b6d45143c9b1a9150483c88927afd5d22175 Mon Sep 17 00:00:00 2001 From: Iori Kumagai Date: Thu, 2 Nov 2023 16:34:07 +0900 Subject: [PATCH 1/5] Add function to update global vertices outside the constructor --- include/ForceColl/Contact.h | 24 +++++++- src/Contact.cpp | 109 +++++++++++++++++++++++++----------- 2 files changed, 98 insertions(+), 35 deletions(-) diff --git a/include/ForceColl/Contact.h b/include/ForceColl/Contact.h index 09b4e59..3c13dbc 100644 --- a/include/ForceColl/Contact.h +++ b/include/ForceColl/Contact.h @@ -85,6 +85,9 @@ class Contact return static_cast(graspMat_.cols()); } + /** \brief Update graspMat_ and vertexWithRidgeList_ according to the input pose. */ + virtual void updateGlobalVertices(const sva::PTransformd & pose) = 0; + /** \brief Calculate wrench. \param wrenchRatio wrench ratio of each ridge \param momentOrigin moment origin @@ -122,7 +125,10 @@ class Contact //! Local grasp matrix Eigen::Matrix localGraspMat_; - //! List of vertex with ridges + //! Friction pyramid + std::shared_ptr fricPyramid_; + + //! List of global vertex with ridges std::vector vertexWithRidgeList_; //! Maximum wrench in local frame that can be accepted by this contact @@ -150,6 +156,10 @@ class EmptyContact : public Contact { return "Empty"; } + + inline virtual void updateGlobalVertices(const sva::PTransformd & pose) + { + } }; /** \brief Surface contact. */ @@ -164,6 +174,9 @@ class SurfaceContact : public Contact //! Map of surface vertices in local coordinates static inline std::unordered_map> verticesMap; + //! List of local verticies + std::vector localVertices_; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -191,6 +204,9 @@ class SurfaceContact : public Contact return "Surface"; } + /** \brief Update graspMat_ and vertexWithRidgeList_ according to the input pose. */ + virtual void updateGlobalVertices(const sva::PTransformd & pose); + /** \brief Add markers to GUI. \param gui GUI \param category category of GUI entries @@ -217,6 +233,9 @@ class GraspContact : public Contact //! Map of grasp vertices in local coordinates static inline std::unordered_map> verticesMap; + //! List of local verticies + std::vector localVertices_; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -244,6 +263,9 @@ class GraspContact : public Contact return "Grasp"; } + /** \brief Update graspMat_ and vertexWithRidgeList_ according to the input pose. */ + virtual void updateGlobalVertices(const sva::PTransformd & pose); + /** \brief Add markers to GUI. \param gui GUI \param category category of GUI entries diff --git a/src/Contact.cpp b/src/Contact.cpp index 590f3fd..226545b 100644 --- a/src/Contact.cpp +++ b/src/Contact.cpp @@ -183,32 +183,27 @@ SurfaceContact::SurfaceContact(const std::string & name, std::optional maxWrench) : Contact(name, std::move(maxWrench)) { - // Set graspMat_ and vertexWithRidgeList_ - FrictionPyramid fricPyramid(fricCoeff); - - graspMat_.resize(6, static_cast(localVertices.size()) * fricPyramid.ridgeNum()); - localGraspMat_.resize(6, static_cast(localVertices.size()) * fricPyramid.ridgeNum()); + // Set graspMat_ and localRidgeList_ + fricPyramid_ = std::make_shared(fricCoeff); - const auto & globalRidgeList = fricPyramid.calcGlobalRidgeList(pose.rotation().transpose()); + localVertices_.resize(localVertices.size()); + std::copy(localVertices.begin(), localVertices.end(), localVertices_.begin()); - for(size_t vertexIdx = 0; vertexIdx < localVertices.size(); vertexIdx++) + localGraspMat_.resize(6, static_cast(localVertices_.size()) * fricPyramid_->ridgeNum()); + for(size_t vertexIdx = 0; vertexIdx < localVertices_.size(); vertexIdx++) { - const auto & localVertex = localVertices[vertexIdx]; - Eigen::Vector3d globalVertex = (sva::PTransformd(localVertex) * pose).translation(); - - for(size_t ridgeIdx = 0; ridgeIdx < globalRidgeList.size(); ridgeIdx++) + const auto & localVertex = localVertices_[vertexIdx]; + for(size_t ridgeIdx = 0; ridgeIdx < fricPyramid_->localRidgeList_.size(); ridgeIdx++) { - const auto & localRidge = fricPyramid.localRidgeList_[ridgeIdx]; - const auto & globalRidge = globalRidgeList[ridgeIdx]; + const auto & localRidge = fricPyramid_->localRidgeList_[ridgeIdx]; auto colIdx = - static_cast(vertexIdx) * fricPyramid.ridgeNum() + static_cast(ridgeIdx); + static_cast(vertexIdx) * fricPyramid_->ridgeNum() + static_cast(ridgeIdx); // The top 3 rows are moment, the bottom 3 rows are force. localGraspMat_.col(colIdx) << localVertex.cross(localRidge), localRidge; - graspMat_.col(colIdx) << globalVertex.cross(globalRidge), globalRidge; } - - vertexWithRidgeList_.push_back(VertexWithRidge(globalVertex, globalRidgeList)); } + + updateGlobalVertices(pose); } SurfaceContact::SurfaceContact(const mc_rtc::Configuration & mcRtcConfig) @@ -220,6 +215,31 @@ SurfaceContact::SurfaceContact(const mc_rtc::Configuration & mcRtcConfig) { } +void SurfaceContact::updateGlobalVertices(const sva::PTransformd & pose) +{ + graspMat_.resize(6, static_cast(localVertices_.size()) * fricPyramid_->ridgeNum()); + vertexWithRidgeList_.clear(); + + const auto & globalRidgeList = fricPyramid_->calcGlobalRidgeList(pose.rotation().transpose()); + + for(size_t vertexIdx = 0; vertexIdx < localVertices_.size(); vertexIdx++) + { + const auto & localVertex = localVertices_[vertexIdx]; + Eigen::Vector3d globalVertex = (sva::PTransformd(localVertex) * pose).translation(); + + for(size_t ridgeIdx = 0; ridgeIdx < globalRidgeList.size(); ridgeIdx++) + { + const auto & globalRidge = globalRidgeList[ridgeIdx]; + auto colIdx = + static_cast(vertexIdx) * fricPyramid_->ridgeNum() + static_cast(ridgeIdx); + // The top 3 rows are moment, the bottom 3 rows are force. + graspMat_.col(colIdx) << globalVertex.cross(globalRidge), globalRidge; + } + + vertexWithRidgeList_.push_back(VertexWithRidge(globalVertex, globalRidgeList)); + } +} + void SurfaceContact::addToGUI(mc_rtc::gui::StateBuilder & gui, const std::vector & category, double forceScale, @@ -255,34 +275,30 @@ GraspContact::GraspContact(const std::string & name, std::optional maxWrench) : Contact(name, std::move(maxWrench)) { - // Set graspMat_ and vertexWithRidgeList_ - FrictionPyramid fricPyramid(fricCoeff); + // Set graspMat_ and localRidgeList_ + fricPyramid_ = std::make_shared(fricCoeff); + + localVertices_.resize(localVertices.size()); + std::copy(localVertices.begin(), localVertices.end(), localVertices_.begin()); - graspMat_.resize(6, static_cast(localVertices.size()) * fricPyramid.ridgeNum()); - localGraspMat_.resize(6, static_cast(localVertices.size()) * fricPyramid.ridgeNum()); + localGraspMat_.resize(6, static_cast(localVertices_.size()) * fricPyramid_->ridgeNum()); - for(size_t vertexIdx = 0; vertexIdx < localVertices.size(); vertexIdx++) + for(size_t vertexIdx = 0; vertexIdx < localVertices_.size(); vertexIdx++) { - const auto & localVertexPose = localVertices[vertexIdx]; - sva::PTransformd globalVertexPose = localVertices[vertexIdx] * pose; + const auto & localVertexPose = localVertices_[vertexIdx]; const auto & localVertex = localVertexPose.translation(); - const auto & localRidgeList = fricPyramid.calcGlobalRidgeList(localVertexPose.rotation().transpose()); - const auto & globalVertex = globalVertexPose.translation(); - const auto & globalRidgeList = fricPyramid.calcGlobalRidgeList(globalVertexPose.rotation().transpose()); - - for(size_t ridgeIdx = 0; ridgeIdx < globalRidgeList.size(); ridgeIdx++) + const auto & localRidgeList = fricPyramid_->calcGlobalRidgeList(localVertexPose.rotation().transpose()); + for(size_t ridgeIdx = 0; ridgeIdx < localRidgeList.size(); ridgeIdx++) { const auto & localRidge = localRidgeList[ridgeIdx]; - const auto & globalRidge = globalRidgeList[ridgeIdx]; auto colIdx = - static_cast(vertexIdx) * fricPyramid.ridgeNum() + static_cast(ridgeIdx); + static_cast(vertexIdx) * fricPyramid_->ridgeNum() + static_cast(ridgeIdx); // The top 3 rows are moment, the bottom 3 rows are force. localGraspMat_.col(colIdx) << localVertex.cross(localRidge), localRidge; - graspMat_.col(colIdx) << globalVertex.cross(globalRidge), globalRidge; } - - vertexWithRidgeList_.push_back(VertexWithRidge(globalVertex, globalRidgeList)); } + + updateGlobalVertices(pose); } GraspContact::GraspContact(const mc_rtc::Configuration & mcRtcConfig) @@ -294,6 +310,31 @@ GraspContact::GraspContact(const mc_rtc::Configuration & mcRtcConfig) { } +void GraspContact::updateGlobalVertices(const sva::PTransformd & pose) +{ + graspMat_.resize(6, static_cast(localVertices_.size()) * fricPyramid_->ridgeNum()); + vertexWithRidgeList_.clear(); + + for(size_t vertexIdx = 0; vertexIdx < localVertices_.size(); vertexIdx++) + { + const auto & localVertexPose = localVertices_[vertexIdx]; + sva::PTransformd globalVertexPose = sva::PTransformd(localVertexPose) * pose; + const auto & globalVertex = globalVertexPose.translation(); + const auto & globalRidgeList = fricPyramid_->calcGlobalRidgeList(globalVertexPose.rotation().transpose()); + + for(size_t ridgeIdx = 0; ridgeIdx < globalRidgeList.size(); ridgeIdx++) + { + const auto & globalRidge = globalRidgeList[ridgeIdx]; + auto colIdx = + static_cast(vertexIdx) * fricPyramid_->ridgeNum() + static_cast(ridgeIdx); + // The top 3 rows are moment, the bottom 3 rows are force. + graspMat_.col(colIdx) << globalVertex.cross(globalRidge), globalRidge; + } + + vertexWithRidgeList_.push_back(VertexWithRidge(globalVertex, globalRidgeList)); + } +} + void GraspContact::addToGUI(mc_rtc::gui::StateBuilder & gui, const std::vector & category, double forceScale, From a64e2f1ca26d12001f429b553f64b9f63c301622 Mon Sep 17 00:00:00 2001 From: Iori Kumagai Date: Tue, 7 Nov 2023 11:25:04 +0900 Subject: [PATCH 2/5] Apply clang-format-fix --- include/ForceColl/Contact.h | 4 +--- src/Contact.cpp | 16 ++++++++-------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/include/ForceColl/Contact.h b/include/ForceColl/Contact.h index 3c13dbc..250e4e0 100644 --- a/include/ForceColl/Contact.h +++ b/include/ForceColl/Contact.h @@ -157,9 +157,7 @@ class EmptyContact : public Contact return "Empty"; } - inline virtual void updateGlobalVertices(const sva::PTransformd & pose) - { - } + inline virtual void updateGlobalVertices(const sva::PTransformd & pose) {} }; /** \brief Surface contact. */ diff --git a/src/Contact.cpp b/src/Contact.cpp index 226545b..9323d28 100644 --- a/src/Contact.cpp +++ b/src/Contact.cpp @@ -196,8 +196,8 @@ SurfaceContact::SurfaceContact(const std::string & name, for(size_t ridgeIdx = 0; ridgeIdx < fricPyramid_->localRidgeList_.size(); ridgeIdx++) { const auto & localRidge = fricPyramid_->localRidgeList_[ridgeIdx]; - auto colIdx = - static_cast(vertexIdx) * fricPyramid_->ridgeNum() + static_cast(ridgeIdx); + auto colIdx = static_cast(vertexIdx) * fricPyramid_->ridgeNum() + + static_cast(ridgeIdx); // The top 3 rows are moment, the bottom 3 rows are force. localGraspMat_.col(colIdx) << localVertex.cross(localRidge), localRidge; } @@ -230,8 +230,8 @@ void SurfaceContact::updateGlobalVertices(const sva::PTransformd & pose) for(size_t ridgeIdx = 0; ridgeIdx < globalRidgeList.size(); ridgeIdx++) { const auto & globalRidge = globalRidgeList[ridgeIdx]; - auto colIdx = - static_cast(vertexIdx) * fricPyramid_->ridgeNum() + static_cast(ridgeIdx); + auto colIdx = static_cast(vertexIdx) * fricPyramid_->ridgeNum() + + static_cast(ridgeIdx); // The top 3 rows are moment, the bottom 3 rows are force. graspMat_.col(colIdx) << globalVertex.cross(globalRidge), globalRidge; } @@ -291,8 +291,8 @@ GraspContact::GraspContact(const std::string & name, for(size_t ridgeIdx = 0; ridgeIdx < localRidgeList.size(); ridgeIdx++) { const auto & localRidge = localRidgeList[ridgeIdx]; - auto colIdx = - static_cast(vertexIdx) * fricPyramid_->ridgeNum() + static_cast(ridgeIdx); + auto colIdx = static_cast(vertexIdx) * fricPyramid_->ridgeNum() + + static_cast(ridgeIdx); // The top 3 rows are moment, the bottom 3 rows are force. localGraspMat_.col(colIdx) << localVertex.cross(localRidge), localRidge; } @@ -325,8 +325,8 @@ void GraspContact::updateGlobalVertices(const sva::PTransformd & pose) for(size_t ridgeIdx = 0; ridgeIdx < globalRidgeList.size(); ridgeIdx++) { const auto & globalRidge = globalRidgeList[ridgeIdx]; - auto colIdx = - static_cast(vertexIdx) * fricPyramid_->ridgeNum() + static_cast(ridgeIdx); + auto colIdx = static_cast(vertexIdx) * fricPyramid_->ridgeNum() + + static_cast(ridgeIdx); // The top 3 rows are moment, the bottom 3 rows are force. graspMat_.col(colIdx) << globalVertex.cross(globalRidge), globalRidge; } From 5c962994a51049355a95d8ab509505676d082a25 Mon Sep 17 00:00:00 2001 From: Iori Kumagai Date: Fri, 12 Apr 2024 20:01:15 +0900 Subject: [PATCH 3/5] Clarify override for updateGlobalVertices --- include/ForceColl/Contact.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/include/ForceColl/Contact.h b/include/ForceColl/Contact.h index 250e4e0..7da3551 100644 --- a/include/ForceColl/Contact.h +++ b/include/ForceColl/Contact.h @@ -157,7 +157,8 @@ class EmptyContact : public Contact return "Empty"; } - inline virtual void updateGlobalVertices(const sva::PTransformd & pose) {} + /** \brief Do nothing because EmptyContact does not have any vertices. */ + inline virtual void updateGlobalVertices(const sva::PTransformd & pose) override {} }; /** \brief Surface contact. */ @@ -203,7 +204,7 @@ class SurfaceContact : public Contact } /** \brief Update graspMat_ and vertexWithRidgeList_ according to the input pose. */ - virtual void updateGlobalVertices(const sva::PTransformd & pose); + virtual void updateGlobalVertices(const sva::PTransformd & pose) override; /** \brief Add markers to GUI. \param gui GUI @@ -262,7 +263,7 @@ class GraspContact : public Contact } /** \brief Update graspMat_ and vertexWithRidgeList_ according to the input pose. */ - virtual void updateGlobalVertices(const sva::PTransformd & pose); + virtual void updateGlobalVertices(const sva::PTransformd & pose) override; /** \brief Add markers to GUI. \param gui GUI From a6114ceddabd9263204b69e0a6dd2f31a95766cf Mon Sep 17 00:00:00 2001 From: Iori Kumagai Date: Fri, 12 Apr 2024 20:01:52 +0900 Subject: [PATCH 4/5] Add unit test for updateGlobalVertices --- tests/src/TestContact.cpp | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/tests/src/TestContact.cpp b/tests/src/TestContact.cpp index f0f6c70..f682cee 100644 --- 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( + "OriginalContact", 1.0, + std::vector{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( + "TargetContact", 1.0, + std::vector{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( + "OriginalContact", 1.0, + std::vector{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( + "TargetContact", 1.0, + std::vector{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); From 9b78f9f2dcd046852afd9c03f1fd9ec7bbf5793e Mon Sep 17 00:00:00 2001 From: Iori Kumagai Date: Fri, 12 Apr 2024 20:25:50 +0900 Subject: [PATCH 5/5] Apply clang-format-fix --- tests/src/TestContact.cpp | 50 ++++++++++++++++++++++----------------- 1 file changed, 28 insertions(+), 22 deletions(-) diff --git a/tests/src/TestContact.cpp b/tests/src/TestContact.cpp index f682cee..3651c81 100644 --- a/tests/src/TestContact.cpp +++ b/tests/src/TestContact.cpp @@ -132,38 +132,44 @@ TEST(TestContact, UpdateSurfaceContactVertices) { sva::PTransformd targetPose(sva::RotX(M_PI / 2), Eigen::Vector3d(0.0, 0.5, -0.5)); auto originalContact = std::make_shared( - "OriginalContact", 1.0, - std::vector{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()); + "OriginalContact", 1.0, + std::vector{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( - "TargetContact", 1.0, - std::vector{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); + "TargetContact", 1.0, + std::vector{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; + 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( - "OriginalContact", 1.0, - std::vector{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()); + "OriginalContact", 1.0, + std::vector{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( - "TargetContact", 1.0, - std::vector{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); + "TargetContact", 1.0, + std::vector{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; + 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)