diff --git a/include/hpp/manipulation/axial-handle.hh b/include/hpp/manipulation/axial-handle.hh index 34e4634b46396b123dd100b780bf3373857ffdff..1f220e31d23e9f98814dd24fda57fa6758405ae5 100644 --- a/include/hpp/manipulation/axial-handle.hh +++ b/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 diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh index 8dc122280e06f78e33afa814259bb1720d8adfc9..78019b8a98f727eb1563f6fcec0a23dcbdd379dc 100644 --- a/include/hpp/manipulation/handle.hh +++ b/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. diff --git a/src/axial-handle.cc b/src/axial-handle.cc index 8e37c4156c17e6b2be113d22b35ad16777b06fc7..8d52183cef7503e78988559a102ea2e3420cc087 100644 --- a/src/axial-handle.cc +++ b/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 (); diff --git a/src/handle.cc b/src/handle.cc index b2ae72c4f3886b9f6787bee8d74119ffbbd449c7..f783812753070d54305b314ef9cd5cb36de8ef66 100644 --- a/src/handle.cc +++ b/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