From 0045ecc3c3751186aac12262d8fd6f47100f7aef Mon Sep 17 00:00:00 2001
From: Masaki Murooka <m-murooka@aist.go.jp>
Date: Wed, 10 Jul 2024 16:16:54 +0900
Subject: [PATCH] Add updateLocalVertices method to {Surface,Grasp}Contact.

---
 include/ForceColl/Contact.h |  6 +++++
 src/Contact.cpp             | 52 ++++++++++++++++++++-----------------
 2 files changed, 34 insertions(+), 24 deletions(-)

diff --git a/include/ForceColl/Contact.h b/include/ForceColl/Contact.h
index 412e53b..e160f19 100644
--- a/include/ForceColl/Contact.h
+++ b/include/ForceColl/Contact.h
@@ -206,6 +206,9 @@ class SurfaceContact : public Contact
     return "Surface";
   }
 
+  /** \brief Update localVertices_ and localGraspMat_ according to the input pose. */
+  void updateLocalVertices(const std::vector<Eigen::Vector3d> & localVertices);
+
   /** \brief Update graspMat_ and vertexWithRidgeList_ according to the input pose. */
   virtual void updateGlobalVertices(const sva::PTransformd & pose) override;
 
@@ -265,6 +268,9 @@ class GraspContact : public Contact
     return "Grasp";
   }
 
+  /** \brief Update localVertices_ and localGraspMat_ according to the input pose. */
+  void updateLocalVertices(const std::vector<sva::PTransformd> & localVertices);
+
   /** \brief Update graspMat_ and vertexWithRidgeList_ according to the input pose. */
   virtual void updateGlobalVertices(const sva::PTransformd & pose) override;
 
diff --git a/src/Contact.cpp b/src/Contact.cpp
index 432f984..38548cc 100644
--- a/src/Contact.cpp
+++ b/src/Contact.cpp
@@ -188,9 +188,22 @@ SurfaceContact::SurfaceContact(const std::string & name,
                                std::optional<sva::ForceVecd> maxWrench)
 : Contact(name, std::move(maxWrench))
 {
-  // Set graspMat_ and localRidgeList_
   fricPyramid_ = std::make_shared<FrictionPyramid>(fricCoeff);
+  updateLocalVertices(localVertices);
+  updateGlobalVertices(pose);
+}
+
+SurfaceContact::SurfaceContact(const mc_rtc::Configuration & mcRtcConfig)
+: SurfaceContact(mcRtcConfig("name"),
+                 mcRtcConfig("fricCoeff"),
+                 verticesMap.at(mcRtcConfig("verticesName")),
+                 mcRtcConfig("pose"),
+                 mcRtcConfig("maxWrench", std::optional<sva::ForceVecd>{}))
+{
+}
 
+void SurfaceContact::updateLocalVertices(const std::vector<Eigen::Vector3d> & localVertices)
+{
   localVertices_.resize(localVertices.size());
   std::copy(localVertices.begin(), localVertices.end(), localVertices_.begin());
 
@@ -207,17 +220,6 @@ SurfaceContact::SurfaceContact(const std::string & name,
       localGraspMat_.col(colIdx) << localVertex.cross(localRidge), localRidge;
     }
   }
-
-  updateGlobalVertices(pose);
-}
-
-SurfaceContact::SurfaceContact(const mc_rtc::Configuration & mcRtcConfig)
-: SurfaceContact(mcRtcConfig("name"),
-                 mcRtcConfig("fricCoeff"),
-                 verticesMap.at(mcRtcConfig("verticesName")),
-                 mcRtcConfig("pose"),
-                 mcRtcConfig("maxWrench", std::optional<sva::ForceVecd>{}))
-{
 }
 
 void SurfaceContact::updateGlobalVertices(const sva::PTransformd & pose)
@@ -280,9 +282,22 @@ GraspContact::GraspContact(const std::string & name,
                            std::optional<sva::ForceVecd> maxWrench)
 : Contact(name, std::move(maxWrench))
 {
-  // Set graspMat_ and localRidgeList_
   fricPyramid_ = std::make_shared<FrictionPyramid>(fricCoeff);
+  updateLocalVertices(localVertices);
+  updateGlobalVertices(pose);
+}
+
+GraspContact::GraspContact(const mc_rtc::Configuration & mcRtcConfig)
+: GraspContact(mcRtcConfig("name"),
+               mcRtcConfig("fricCoeff"),
+               verticesMap.at(mcRtcConfig("verticesName")),
+               mcRtcConfig("pose"),
+               mcRtcConfig("maxWrench", std::optional<sva::ForceVecd>{}))
+{
+}
 
+void GraspContact::updateLocalVertices(const std::vector<sva::PTransformd> & localVertices)
+{
   localVertices_.resize(localVertices.size());
   std::copy(localVertices.begin(), localVertices.end(), localVertices_.begin());
 
@@ -302,17 +317,6 @@ GraspContact::GraspContact(const std::string & name,
       localGraspMat_.col(colIdx) << localVertex.cross(localRidge), localRidge;
     }
   }
-
-  updateGlobalVertices(pose);
-}
-
-GraspContact::GraspContact(const mc_rtc::Configuration & mcRtcConfig)
-: GraspContact(mcRtcConfig("name"),
-               mcRtcConfig("fricCoeff"),
-               verticesMap.at(mcRtcConfig("verticesName")),
-               mcRtcConfig("pose"),
-               mcRtcConfig("maxWrench", std::optional<sva::ForceVecd>{}))
-{
 }
 
 void GraspContact::updateGlobalVertices(const sva::PTransformd & pose)