Add Inegality constraint for pre-grasp task

include/hpp/manipulation/handle.hh

@@ -105,6 +105,14 @@ namespace hpp {
       virtual DifferentiableFunctionPtr_t createPreGrasp
       (const GripperPtr_t& gripper) const;
 
+      /// Create a constraint define on the line of approach of the object
+      /// This is useful for inequality constraints.
+      /// \name gripper The hpp::model::Gripper
+      /// \name shift a position on the line wrt the axis of the handle joint.
+      /// \return A hpp::constraints::RelativePosition constraint.
+      virtual DifferentiableFunctionPtr_t createPreGraspOnLine
+        (const GripperPtr_t& gripper, const value_type& shift) const;
+
       static DifferentiableFunctionPtr_t createGrasp
       (const GripperPtr_t& gripper,const HandlePtr_t& handle)
       {

src/handle.cc

@@ -20,6 +20,7 @@
 #include <boost/assign/list_of.hpp>
 #include <fcl/math/transform.h>
 #include <hpp/model/joint.hh>
+#include <hpp/constraints/relative-position.hh>
 #include <hpp/constraints/relative-transformation.hh>
 #include <hpp/manipulation/handle.hh>
 #include <hpp/model/gripper.hh>
@@ -48,6 +49,17 @@ namespace hpp {
 	(gripper->joint()->robot(), joint(), gripper->joint (), transform, mask);
     }
 
+    DifferentiableFunctionPtr_t Handle::createPreGraspOnLine
+    (const GripperPtr_t& gripper, const value_type& shift) const
+    {
+      using boost::assign::list_of;
+      std::vector <bool> mask = list_of (false)(true)(false);
+      Transform3f transform = inverse (gripper->objectPositionInJoint ()) * localPosition();
+      fcl::Vec3f target = transform.getTranslation () + fcl::Vec3f (0,shift,0);
+      return RelativePosition::create
+        (gripper->joint()->robot(), joint(), gripper->joint (), target, fcl::Vec3f (0,0,0), mask);
+    }
+
     HandlePtr_t Handle::clone () const
     {
       HandlePtr_t self = weakPtr_.lock ();