Add methods to Handle to generate constraints.

include/hpp/manipulation/axial-handle.hh

@@ -52,6 +52,32 @@ namespace hpp {
       virtual DifferentiableFunctionPtr_t createGrasp
       (const GripperPtr_t& gripper) const;
 
+      /// Create constraint that acts on the non-constrained axis of the
+      /// constraint generated by Handle::createGrasp.
+      /// \param gripper object containing the gripper information
+      /// \return a relative orientation constraint between the handle and
+      ///         the gripper. Only the rotation around the x-axis is constrained.
+      virtual DifferentiableFunctionPtr_t createGraspComplement
+      (const GripperPtr_t& gripper) const;
+
+      /// Create constraint corresponding to a pregrasping task.
+      /// \param gripper object containing the gripper information
+      /// \return the constraint of relative transformation between the handle and
+      ///         the gripper.
+      /// \note The translation along x-axis and the rotation around z-axis are not constrained.
+      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
+      /// \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.
+      virtual DifferentiableFunctionPtr_t createPreGraspComplement
+      (const GripperPtr_t& gripper, const value_type& shift) const;
+
       virtual std::ostream& print (std::ostream& os) const;
     protected:
       /// Constructor

include/hpp/manipulation/handle.hh

@@ -91,9 +91,16 @@ namespace hpp {
       virtual DifferentiableFunctionPtr_t createGrasp
       (const GripperPtr_t& gripper) const;
 
-      /// Create constraint corresponding to a gripper grasping this object
+      /// Create constraint that acts on the non-constrained axis of the
+      /// constraint generated by Handle::createGrasp.
       /// \param gripper object containing the gripper information
-      /// \return the constraint of relative position between the handle and
+      /// \return a constraints that is not doing anything.
+      virtual DifferentiableFunctionPtr_t createGraspComplement
+      (const GripperPtr_t& gripper) const;
+
+      /// Create constraint corresponding to a pregrasping task.
+      /// \param gripper object containing the gripper information
+      /// \return the constraint of relative transformation between the handle and
       ///         the gripper.
       /// \note Only 5 DOFs of the relative transformation between the handle and the gripper
       ///       are constrained. The translation along x-axis is not constrained.

src/axial-handle.cc

@@ -38,6 +38,44 @@ namespace hpp {
 	(gripper->joint()->robot(), gripper->joint (), joint(),
 	 inverse (localPosition()) * gripper->objectPositionInJoint (), mask);
     }
+
+    DifferentiableFunctionPtr_t AxialHandle::createGraspComplement
+    (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 ();
+      return RelativeOrientation::create
+	(gripper->joint()->robot(), gripper->joint (), joint(), transform.getRotation (), mask);
+    }
+
+    DifferentiableFunctionPtr_t AxialHandle::createPreGrasp
+    (const GripperPtr_t& gripper) const
+    {
+      using boost::assign::list_of;
+      std::vector <bool> mask = list_of (false)(true)(true)(false)(true)(true);
+      return RelativeTransformation::create
+	(gripper->joint()->robot(), gripper->joint (), joint(),
+	 inverse (localPosition()) * gripper->objectPositionInJoint (), mask);
+    }
+
+    DifferentiableFunctionPtr_t AxialHandle::createPreGraspComplement
+      (const GripperPtr_t& gripper, const value_type& shift) const
+    {
+      //using boost::assign::list_of;
+      //std::vector <bool> mask = list_of (true)(false)(false)(true)(false)(false);
+      //Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint ();
+      //transform.setTranslation (transform.getTranslation () + fcl::Vec3f (shift,0,0));
+      //return RelativeTransformation::create
+	//(gripper->joint()->robot(), gripper->joint (), joint(), transform, mask);
+      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 (shift,0,0);
+      return RelativePosition::create
+        (gripper->joint()->robot(), gripper->joint (), joint(), target, fcl::Vec3f (0,0,0), mask);
+    }
+
     HandlePtr_t AxialHandle::clone () const
     {
       AxialHandlePtr_t self = weakPtr_.lock ();

src/handle.cc

@@ -35,8 +35,18 @@ namespace hpp {
       using boost::assign::list_of;
       std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(true);
       return RelativeTransformation::create
-	(gripper->joint()->robot(), gripper->joint (), joint(), inverse (localPosition()) *
-	 gripper->objectPositionInJoint (), mask);
+	(gripper->joint()->robot(), gripper->joint (), joint(),
+         inverse (localPosition()) * gripper->objectPositionInJoint (), mask);
+    }
+
+    DifferentiableFunctionPtr_t Handle::createGraspComplement
+    (const GripperPtr_t& gripper) const
+    {
+      using boost::assign::list_of;
+      std::vector <bool> mask = list_of (false)(false)(false)(false)(false)(false);
+      return RelativeTransformation::create
+	(gripper->joint()->robot(), gripper->joint (), joint(),
+         inverse (localPosition()) * gripper->objectPositionInJoint (), mask);
     }
 
     DifferentiableFunctionPtr_t Handle::createPreGrasp