From 9c3ea5d8ab805d4a28d50ef6f8deba4625991a68 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 19 Aug 2016 01:41:24 +0200
Subject: [PATCH] Fix AxialHandle::createPreGrasp
---
include/hpp/manipulation/axial-handle.hh | 2 +-
src/axial-handle.cc | 10 ++++++----
2 files changed, 7 insertions(+), 5 deletions(-)
diff --git a/include/hpp/manipulation/axial-handle.hh b/include/hpp/manipulation/axial-handle.hh
index cb0c8374..7e8de973 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 baa84d88..025e3758 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)));
}
--
GitLab