diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh index bbe58d292ebe9b4b1e74b078635e40fe99e9d4a5..b87f4593bd0345e9475fbe5fb5d55a684adc6aeb 100644 --- a/include/hpp/manipulation/handle.hh +++ b/include/hpp/manipulation/handle.hh @@ -94,6 +94,17 @@ namespace hpp { virtual DifferentiableFunctionPtr_t createGrasp (const GripperPtr_t& gripper) const; + /// Create constraint corresponding to a gripper grasping this object + /// \param gripper object containing the gripper information + /// \return the constraint of relative position between the handle and + /// the gripper. + /// \note the constraint may constrain less than 6 degrees of freedom + /// if the handle can be grasped in different positions. In case of + /// a handle symmetric with respect to its x-axis for instance, the + /// rotation around x is not constrained. + virtual DifferentiableFunctionPtr_t createPreGrasp + (const GripperPtr_t& gripper) const; + static DifferentiableFunctionPtr_t createGrasp (const GripperPtr_t& gripper,const HandlePtr_t& handle) { diff --git a/src/handle.cc b/src/handle.cc index c9e1599cc2d955bc1e0791707385a49ab7155c03..4d3fb291f55427ef21172b821e1db1ed8c232275 100644 --- a/src/handle.cc +++ b/src/handle.cc @@ -38,6 +38,16 @@ namespace hpp { gripper->objectPositionInJoint (), mask); } + DifferentiableFunctionPtr_t Handle::createPreGrasp + (const GripperPtr_t& gripper) const + { + using boost::assign::list_of; + std::vector <bool> mask = list_of (true)(false)(true)(true)(true)(true); + Transform3f transform = inverse (gripper->objectPositionInJoint ()) * localPosition(); + return RelativeTransformation::create + (gripper->joint()->robot(), joint(), gripper->joint (), transform, mask); + } + HandlePtr_t Handle::clone () const { HandlePtr_t self = weakPtr_.lock ();