diff --git a/include/hpp/manipulation/axial-handle.hh b/include/hpp/manipulation/axial-handle.hh index cb0c837465a8af81fd9fab5b45665038ef0edfd5..7e8de9732381b50db1b37cd161727926b802734d 100644 --- a/include/hpp/manipulation/axial-handle.hh +++ b/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. diff --git a/src/axial-handle.cc b/src/axial-handle.cc index baa84d8854867ade701dc29883928d12956d636e..025e3758d5fa8abd2acfb8795762628c6702204f 100644 --- a/src/axial-handle.cc +++ b/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))); }