Merge pull request #109 from florent-lamiraux/devel

[Handle] Allow to customize mask complement.

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_;
 

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;
     }