From 13b9f0dcd96e749638d2c249326afb3f34aabf1f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 16 Sep 2014 11:28:25 +0200
Subject: [PATCH] Add Inegality constraint for pre-grasp task
---
include/hpp/manipulation/handle.hh | 8 ++++++++
src/handle.cc | 12 ++++++++++++
2 files changed, 20 insertions(+)
diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh
index b87f459..86f1908 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 4d3fb29..b943b8d 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 ();
--
GitLab