Fix AxialHandle::createPreGrasp

include/hpp/manipulation/axial-handle.hh

@@ -69,7 +69,7 @@ namespace hpp {
       /// \todo this function is never called. It should follow changes of
       ///       Handle::createPreGrasp prototype.
       virtual NumericalConstraintPtr_t createPreGrasp
-      (const GripperPtr_t& gripper) const;
+      (const GripperPtr_t& gripper, const value_type& shift) const;
 
       /// Create constraint that acts on the non-constrained axis of the
       /// constraint generated by Handle::createPreGrasp.

src/axial-handle.cc

@@ -65,17 +65,19 @@ namespace hpp {
     }
 
     NumericalConstraintPtr_t AxialHandle::createPreGrasp
-    (const GripperPtr_t& gripper) const
+    (const GripperPtr_t& gripper, const value_type& shift) const
     {
       using boost::assign::list_of;
-      std::vector <bool> mask = list_of (false)(true)(true)(true)(true)(false);
+      std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(false);
+      Transform3f transform = gripper->objectPositionInJoint ()
+        * Transform3f (fcl::Vec3f (shift,0,0));
       return NumericalConstraintPtr_t
 	(NumericalConstraint::create (RelativeTransformation::create
-				      ("Transformation_(0,1,1,1,1,0)_" + name ()
+				      ("Transformation_(1,1,1,1,1,0)_" + name ()
 				       + "_" + gripper->name (),
 				       gripper->joint()->robot(),
 				       gripper->joint (), joint (),
-				       gripper->objectPositionInJoint (),
+				       transform,
 				       localPosition(), mask)));
     }