From c94e2afb86a9b5c50141135b5718593718e89eaa Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 10 Sep 2014 13:11:50 +0200
Subject: [PATCH] Add Handle::createPreGrasp to generate a pregrasping
constraint
---
include/hpp/manipulation/handle.hh | 11 +++++++++++
src/handle.cc | 10 ++++++++++
2 files changed, 21 insertions(+)
diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh
index bbe58d2..b87f459 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 c9e1599..4d3fb29 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 ();
--
GitLab