From 2e2d766fa47f28008cca58b854dbbd53f9225ce0 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Sat, 19 Feb 2022 17:38:19 +0000
Subject: [PATCH] [Handle] Allow to customize mask complement.
---
include/hpp/manipulation/handle.hh | 27 +++++++++++++++++++++++-
src/handle.cc | 33 +++++++++++++++++++++++-------
2 files changed, 52 insertions(+), 8 deletions(-)
diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh
index 6574da17..206082ec 100644
--- a/include/hpp/manipulation/handle.hh
+++ b/include/hpp/manipulation/handle.hh
@@ -28,7 +28,23 @@
namespace hpp {
namespace manipulation {
typedef constraints::ImplicitPtr_t ImplicitPtr_t;
- /// Part of an object that is aimed at being grasped
+ /// Frame attached to an object that is aimed at being grasped
+ ///
+ /// Together with a hpp::pinocchio::Gripper, a handle defines a grasp.
+ /// A vector of 6 Boolean values called a mask can be passed to the
+ /// constructor to define the symmetries of the handle. For example,
+ /// {True,True,True,False,True,True} means that the handle can be
+ /// grasped with free orientation around x-axis.
+ /// See https://hal.laas.fr/hal-02995125v2 for details.
+ /// The setter method \c mask allows users to define the mask.
+ ///
+ /// Along motions where the handle is grasped by a gripper, an additional
+ /// constraint is enforced, called the complement constraint. This latter
+ /// constraint ensures that the object is rigidly fixed to the gripper.
+ ///
+ /// However, for some applications, the complement constraint can be
+ /// customized using setter \c maskComp. Note that calling setter method
+ /// \c mask reinitializes the mask complement.
class HPP_MANIPULATION_DLLAPI Handle
{
public:
@@ -102,6 +118,13 @@ namespace hpp {
const std::vector<bool>& mask () const
{ return mask_; }
+ /// Set mask of complement constraint
+ void maskComp (const std::vector<bool>& mask);
+
+ /// Get mask of complement constraint
+ const std::vector<bool>& maskComp () const
+ { return maskComp_; }
+
/// Create constraint corresponding to a gripper grasping this handle
/// \param gripper object containing the gripper information
/// \return the constraint of relative transformation between the handle
@@ -188,6 +211,8 @@ namespace hpp {
value_type clearance_;
/// Mask
std::vector<bool> mask_;
+ /// Mask of complement constraint
+ std::vector<bool> maskComp_;
/// Weak pointer to itself
HandleWkPtr_t weakPtr_;
diff --git a/src/handle.cc b/src/handle.cc
index 7a5db0bb..5cb6283a 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -78,6 +78,17 @@ namespace hpp {
return (int)res;
}
+ inline std::vector<bool> boolOr(std::vector<bool> mask1,
+ std::vector<bool> mask2)
+ {
+ assert(mask1.size() == mask2.size() == 6);
+ std::vector<bool> res(mask1.size());
+ for (std::size_t i = 0; i < 6; ++i) {
+ res[i] = mask1[i] || mask2[i];
+ }
+ return res;
+ }
+
inline bool is6Dmask (const std::vector<bool>& mask)
{
for (std::size_t i = 0; i < 6; ++i) if (!mask[i]) return false;
@@ -114,6 +125,13 @@ namespace hpp {
break;
}
mask_ = mask;
+ maskComp_ = complementMask(mask);
+ }
+
+ void Handle::maskComp (const std::vector<bool>& mask)
+ {
+ assert(maskComp.size() == 6);
+ maskComp_ = mask;
}
ImplicitPtr_t Handle::createGrasp
@@ -139,21 +157,19 @@ namespace hpp {
(const GripperPtr_t& gripper, std::string n) const
{
if (n.empty()) {
- std::vector<bool> Cmask = complementMask(mask_);
n = gripper->name() + "_grasps_" + name() + "/complement_" +
- maskToStr (Cmask);
+ maskToStr (maskComp_);
}
core::DevicePtr_t r = robot();
- if (is6Dmask(mask_)) {
+ if (maskSize(maskComp_) == 0) {
return Implicit::create (
shared_ptr <ZeroDiffFunc> (new ZeroDiffFunc (
r->configSize(), r->numberDof (), n)), ComparisonTypes_t());
} else {
- std::vector<bool> Cmask = complementMask(mask_);
return Implicit::create (RelativeTransformationR3xSO3::create
(n, r, gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition()),
- 6 * constraints::Equality, Cmask);
+ 6 * constraints::Equality, maskComp_);
}
}
@@ -174,7 +190,8 @@ namespace hpp {
}
}
// If handle is on a freeflying object, create an explicit constraint
- if (isHandleOnFreeflyer (*this)) {
+ if (isHandleOnFreeflyer (*this) &&
+ maskSize(boolOr(mask_, maskComp_)) == 6) {
return constraints::explicit_::RelativePose::create
(n, robot (), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition(), comp);
@@ -182,7 +199,7 @@ namespace hpp {
return Implicit::create (RelativeTransformationR3xSO3::create
(n, robot (), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition()),
- comp, std::vector <bool> (6, true));
+ comp, boolOr(mask_, maskComp_));
}
ImplicitPtr_t Handle::createPreGrasp
@@ -205,6 +222,7 @@ namespace hpp {
{
HandlePtr_t other = Handle::create (name (), localPosition (), robot(), joint ());
other->mask(mask_);
+ other->mask(maskComp_);
other->clearance(clearance_);
return other;
}
@@ -215,6 +233,7 @@ namespace hpp {
os << "local position :" << localPosition () << std::endl;
os << "joint :" << joint ()->name () << std::endl;
os << "mask :" << maskToStr (mask()) << std::endl;
+ os << "mask complement:" << maskToStr (maskComp_) << std::endl;
return os;
}
--
GitLab