diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh index b87f4593bd0345e9475fbe5fb5d55a684adc6aeb..86f1908fa18ab421929b767ad135bf096913ebc6 100644 --- a/include/hpp/manipulation/handle.hh +++ b/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) { diff --git a/src/handle.cc b/src/handle.cc index 4d3fb291f55427ef21172b821e1db1ed8c232275..b943b8d5d92aa150c82f50f6dff967460d463b0d 100644 --- a/src/handle.cc +++ b/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 ();