createPreGraspComplement makes use of the clearance parameters.

* old createPreGraspComplement is now deprecated.
* a new createPreGraspComplement without the shift argument is introduced.

include/hpp/manipulation/axial-handle.hh

@@ -70,13 +70,28 @@ namespace hpp {
 
       /// Create constraint that acts on the non-constrained axis of the
       /// constraint generated by Handle::createPreGrasp.
+      /// \warning {currently, the only constrained axis is x. The rotation
+      ///           around the z-axis is not constrained.}
+      /// \param gripper object containing the gripper information
+      /// \return the constraint of relative position between the handle and
+      ///         the gripper.
+      /// \note The translation along x-axis is constrained.
+      virtual DifferentiableFunctionPtr_t createPreGraspComplement
+      (const GripperPtr_t& gripper) const;
+
+      /// Create constraint that acts on the non-constrained axis of the
+      /// constraint generated by Handle::createPreGrasp.
+      /// \warning {currently, the only constrained axis is x. The rotation
+      ///           around the z-axis is not constrained.}
       /// \param gripper object containing the gripper information
       /// \param shift the target value along the x-axis
       /// \return the constraint of relative position between the handle and
       ///         the gripper.
-      /// \note The translation along x-axis and the rotation around z-axis are constrained.
+      /// \note The translation along x-axis is constrained.
+      /// \deprecated Use createPreGraspComplement(const GripperPtr_t&) instead.
       virtual DifferentiableFunctionPtr_t createPreGraspComplement
-      (const GripperPtr_t& gripper, const value_type& shift) const;
+      (const GripperPtr_t& gripper, const value_type& shift) const
+      HPP_MANIPULATION_DEPRECATED;
 
       virtual std::ostream& print (std::ostream& os) const;
     protected:

include/hpp/manipulation/handle.hh

@@ -21,6 +21,7 @@
 # define HPP_MANIPULATION_HANDLE_HH
 
 # include <hpp/fcl/math/transform.h>
+# include <hpp/manipulation/deprecated.hh>
 # include <hpp/manipulation/config.hh>
 # include <hpp/manipulation/fwd.hh>
 
@@ -107,6 +108,17 @@ namespace hpp {
       virtual DifferentiableFunctionPtr_t createPreGrasp
       (const GripperPtr_t& gripper) const;
 
+      /// Create constraint that acts on the non-constrained axis of the
+      /// constraint generated by Handle::createPreGrasp.
+      /// \param gripper object containing the gripper information
+      /// \return the constraint of relative position between the handle and
+      ///         the gripper.
+      /// \note Only the x-axis of the relative transformation between the handle and the gripper
+      ///       is constrained. The centers are shifted of
+      ///       \f clearance () + gripper->clearance ()\f along the x-axis.
+      virtual DifferentiableFunctionPtr_t createPreGraspComplement
+        (const GripperPtr_t& gripper) const;
+
       /// Create constraint that acts on the non-constrained axis of the
       /// constraint generated by Handle::createPreGrasp.
       /// \param gripper object containing the gripper information
@@ -115,8 +127,10 @@ namespace hpp {
       ///         the gripper.
       /// \note Only the x-axis of the relative transformation between the handle and the gripper
       ///       is constrained.
+      /// \deprecated Use createPreGraspComplement(const GripperPtr_t&) instead.
       virtual DifferentiableFunctionPtr_t createPreGraspComplement
-        (const GripperPtr_t& gripper, const value_type& shift) const;
+        (const GripperPtr_t& gripper, const value_type& shift) const
+        HPP_MANIPULATION_DEPRECATED;
 
       static DifferentiableFunctionPtr_t createGrasp
       (const GripperPtr_t& gripper,const HandlePtr_t& handle)

src/axial-handle.cc

@@ -62,6 +62,19 @@ namespace hpp {
           inverse (localPosition()) * gripper->objectPositionInJoint (), mask);
     }
 
+    DifferentiableFunctionPtr_t AxialHandle::createPreGraspComplement
+      (const GripperPtr_t& gripper) const
+    {
+      using boost::assign::list_of;
+      std::vector <bool> mask = list_of (true)(false)(false);
+      Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint ();
+      fcl::Vec3f target = transform.getTranslation ()
+                        + fcl::Vec3f (clearance () + gripper->clearance (),0,0);
+      return RelativePosition::create ("Position_(1,0,0)_" + name () + "_" + gripper->name (),
+          gripper->joint()->robot(), gripper->joint (), joint(), target, fcl::Vec3f (0,0,0), mask);
+    }
+
+
     DifferentiableFunctionPtr_t AxialHandle::createPreGraspComplement
       (const GripperPtr_t& gripper, const value_type& shift) const
     {

src/handle.cc

@@ -62,6 +62,18 @@ namespace hpp {
           gripper->joint()->robot(), gripper->joint (), joint(), transform, mask);
     }
 
+    DifferentiableFunctionPtr_t Handle::createPreGraspComplement
+    (const GripperPtr_t& gripper) const
+    {
+      using boost::assign::list_of;
+      std::vector <bool> mask = list_of (true)(false)(false);
+      Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint ();
+      fcl::Vec3f target = transform.getTranslation ()
+        + fcl::Vec3f (clearance () + gripper->clearance (),0,0);
+      return RelativePosition::create ("Position_(1,0,0)_" + name () + "_" + gripper->name (),
+          gripper->joint()->robot(), gripper->joint (), joint(), target, fcl::Vec3f (0,0,0), mask);
+    }
+
     DifferentiableFunctionPtr_t Handle::createPreGraspComplement
     (const GripperPtr_t& gripper, const value_type& shift) const
     {