Skip to content

Commit

Permalink
Merge pull request #4 from orikuma/update-global-vertices-of-contact-…
Browse files Browse the repository at this point in the history 
…constraint

Add function to update global vertices outside the constructor
  • Loading branch information
@mmurooka
mmurooka authored Apr 13, 2024
2 parents cb0e411 + 9b78f9f commit 4c27024
Show file tree
Hide file tree
Showing 3 changed files with 143 additions and 37 deletions.
23 changes: 22 additions & 1 deletion include/ForceColl/Contact.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,9 @@ class Contact
return static_cast<int>(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
Expand Down Expand Up @@ -122,7 +125,10 @@ class Contact
//! Local grasp matrix
Eigen::Matrix<double, 6, Eigen::Dynamic> localGraspMat_;

//! List of vertex with ridges
//! Friction pyramid
std::shared_ptr<FrictionPyramid> fricPyramid_;

//! List of global vertex with ridges
std::vector<VertexWithRidge> vertexWithRidgeList_;

//! Maximum wrench in local frame that can be accepted by this contact
Expand Down Expand Up @@ -150,6 +156,9 @@ class EmptyContact : public Contact
{
return "Empty";
}

/** \brief Do nothing because EmptyContact does not have any vertices. */
inline virtual void updateGlobalVertices(const sva::PTransformd & pose) override {}
};

/** \brief Surface contact. */
Expand All @@ -164,6 +173,9 @@ class SurfaceContact : public Contact
//! Map of surface vertices in local coordinates
static inline std::unordered_map<std::string, std::vector<Eigen::Vector3d>> verticesMap;

//! List of local verticies
std::vector<Eigen::Vector3d> localVertices_;

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Expand Down Expand Up @@ -191,6 +203,9 @@ class SurfaceContact : public Contact
return "Surface";
}

/** \brief Update graspMat_ and vertexWithRidgeList_ according to the input pose. */
virtual void updateGlobalVertices(const sva::PTransformd & pose) override;

/** \brief Add markers to GUI.
\param gui GUI
\param category category of GUI entries
Expand All @@ -217,6 +232,9 @@ class GraspContact : public Contact
//! Map of grasp vertices in local coordinates
static inline std::unordered_map<std::string, std::vector<sva::PTransformd>> verticesMap;

//! List of local verticies
std::vector<sva::PTransformd> localVertices_;

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Expand Down Expand Up @@ -244,6 +262,9 @@ class GraspContact : public Contact
return "Grasp";
}

/** \brief Update graspMat_ and vertexWithRidgeList_ according to the input pose. */
virtual void updateGlobalVertices(const sva::PTransformd & pose) override;

/** \brief Add markers to GUI.
\param gui GUI
\param category category of GUI entries
Expand Down
113 changes: 77 additions & 36 deletions src/Contact.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,32 +183,27 @@ SurfaceContact::SurfaceContact(const std::string & name,
std::optional<sva::ForceVecd> maxWrench)
: Contact(name, std::move(maxWrench))
{
// Set graspMat_ and vertexWithRidgeList_
FrictionPyramid fricPyramid(fricCoeff);

graspMat_.resize(6, static_cast<Eigen::DenseIndex>(localVertices.size()) * fricPyramid.ridgeNum());
localGraspMat_.resize(6, static_cast<Eigen::DenseIndex>(localVertices.size()) * fricPyramid.ridgeNum());
// Set graspMat_ and localRidgeList_
fricPyramid_ = std::make_shared<FrictionPyramid>(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<Eigen::DenseIndex>(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];
auto colIdx =
static_cast<Eigen::DenseIndex>(vertexIdx) * fricPyramid.ridgeNum() + static_cast<Eigen::DenseIndex>(ridgeIdx);
const auto & localRidge = fricPyramid_->localRidgeList_[ridgeIdx];
auto colIdx = static_cast<Eigen::DenseIndex>(vertexIdx) * fricPyramid_->ridgeNum()
+ static_cast<Eigen::DenseIndex>(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)
Expand All @@ -220,6 +215,31 @@ SurfaceContact::SurfaceContact(const mc_rtc::Configuration & mcRtcConfig)
{
}

void SurfaceContact::updateGlobalVertices(const sva::PTransformd & pose)
{
graspMat_.resize(6, static_cast<Eigen::DenseIndex>(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<Eigen::DenseIndex>(vertexIdx) * fricPyramid_->ridgeNum()
+ static_cast<Eigen::DenseIndex>(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<std::string> & category,
double forceScale,
Expand Down Expand Up @@ -255,34 +275,30 @@ GraspContact::GraspContact(const std::string & name,
std::optional<sva::ForceVecd> maxWrench)
: Contact(name, std::move(maxWrench))
{
// Set graspMat_ and vertexWithRidgeList_
FrictionPyramid fricPyramid(fricCoeff);
// Set graspMat_ and localRidgeList_
fricPyramid_ = std::make_shared<FrictionPyramid>(fricCoeff);

localVertices_.resize(localVertices.size());
std::copy(localVertices.begin(), localVertices.end(), localVertices_.begin());

graspMat_.resize(6, static_cast<Eigen::DenseIndex>(localVertices.size()) * fricPyramid.ridgeNum());
localGraspMat_.resize(6, static_cast<Eigen::DenseIndex>(localVertices.size()) * fricPyramid.ridgeNum());
localGraspMat_.resize(6, static_cast<Eigen::DenseIndex>(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<Eigen::DenseIndex>(vertexIdx) * fricPyramid.ridgeNum() + static_cast<Eigen::DenseIndex>(ridgeIdx);
auto colIdx = static_cast<Eigen::DenseIndex>(vertexIdx) * fricPyramid_->ridgeNum()
+ static_cast<Eigen::DenseIndex>(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)
Expand All @@ -294,6 +310,31 @@ GraspContact::GraspContact(const mc_rtc::Configuration & mcRtcConfig)
{
}

void GraspContact::updateGlobalVertices(const sva::PTransformd & pose)
{
graspMat_.resize(6, static_cast<Eigen::DenseIndex>(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<Eigen::DenseIndex>(vertexIdx) * fricPyramid_->ridgeNum()
+ static_cast<Eigen::DenseIndex>(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<std::string> & category,
double forceScale,
Expand Down
44 changes: 44 additions & 0 deletions tests/src/TestContact.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,6 +139,50 @@ 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);
Expand Down

0 comments on commit 4c27024

Please sign in to comment.