From 5c3aa71ace7980bd3fda827dba51f9b0a3787ccc Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Tue, 12 Jan 2021 16:13:09 +0000
Subject: [PATCH] [Handle] Use constraints with values in R3xSO(3) for grasp
constraints
RelativeTransformation -> RelativeTransformationSE3 with mask.
---
include/hpp/manipulation/fwd.hh | 1 +
src/handle.cc | 28 ++++++++++++----------------
2 files changed, 13 insertions(+), 16 deletions(-)
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 35e0ade..80f7ee5 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -75,6 +75,7 @@ namespace hpp {
typedef constraints::RelativeOrientationPtr_t RelativeOrientationPtr_t;
typedef constraints::RelativePositionPtr_t RelativePositionPtr_t;
typedef constraints::RelativeTransformation RelativeTransformation;
+ typedef constraints::RelativeTransformationSE3 RelativeTransformationSE3;
typedef constraints::RelativeTransformationPtr_t
RelativeTransformationPtr_t;
typedef core::value_type value_type;
diff --git a/src/handle.cc b/src/handle.cc
index fe5c917..c559875 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -132,10 +132,10 @@ namespace hpp {
gripper->objectPositionInJoint (), localPosition(),
6 * constraints::EqualToZero);
}
- return Implicit::create (RelativeTransformation::create
+ return Implicit::create (RelativeTransformationSE3::create
(n, robot (), gripper->joint (), joint (),
- gripper->objectPositionInJoint (), localPosition(), mask_),
- ComparisonTypes_t (maskSize (mask_), constraints::EqualToZero));
+ gripper->objectPositionInJoint (), localPosition()),
+ 6 * constraints::EqualToZero, mask_);
}
ImplicitPtr_t Handle::createGraspComplement
@@ -153,19 +153,16 @@ namespace hpp {
r->configSize(), r->numberDof (), n)), ComparisonTypes_t());
} else {
std::vector<bool> Cmask = complementMask(mask_);
- return Implicit::create (RelativeTransformation::create
+ return Implicit::create (RelativeTransformationSE3::create
(n, r, gripper->joint (), joint (),
- gripper->objectPositionInJoint (), localPosition(),
- Cmask),
- maskSize(Cmask) * constraints::Equality);
+ gripper->objectPositionInJoint (), localPosition()),
+ 6 * constraints::Equality, Cmask);
}
}
ImplicitPtr_t Handle::createGraspAndComplement
(const GripperPtr_t& gripper, std::string n) const
{
- using boost::assign::list_of;
- using core::ExplicitRelativeTransformation;
if (n.empty()) {
n = gripper->name() + "_holds_" + name();
}
@@ -185,11 +182,10 @@ namespace hpp {
(n, robot (), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition(), comp);
}
- return Implicit::create (RelativeTransformation::create
+ return Implicit::create (RelativeTransformationSE3::create
(n, robot (), gripper->joint (), joint (),
- gripper->objectPositionInJoint (), localPosition(),
- std::vector <bool> (6, true)),
- comp);
+ gripper->objectPositionInJoint (), localPosition()),
+ comp, std::vector <bool> (6, true));
}
ImplicitPtr_t Handle::createPreGrasp
@@ -201,10 +197,10 @@ namespace hpp {
n = "Pregrasp_ " + maskToStr(mask_) + "_" + name ()
+ "_" + gripper->name ();
ImplicitPtr_t result (Implicit::create
- (RelativeTransformation::create
+ (RelativeTransformationSE3::create
(n, robot(), gripper->joint (), joint (),
- M, localPosition(), mask_),
- maskSize(mask_) * constraints::EqualToZero));
+ M, localPosition()),
+ 6 * constraints::EqualToZero, mask_));
return result;
}
--
GitLab