From 83ca50cf0ac57e8ef7131bbcc45e868d9c7c0532 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 13 Oct 2014 13:42:15 +0200
Subject: [PATCH] Add methods to Handle to generate constraints.
---
include/hpp/manipulation/axial-handle.hh | 26 ++++++++++++++++
include/hpp/manipulation/handle.hh | 11 +++++--
src/axial-handle.cc | 38 ++++++++++++++++++++++++
src/handle.cc | 14 +++++++--
4 files changed, 85 insertions(+), 4 deletions(-)
diff --git a/include/hpp/manipulation/axial-handle.hh b/include/hpp/manipulation/axial-handle.hh
index 34e4634..1f220e3 100644
--- a/include/hpp/manipulation/axial-handle.hh
+++ b/include/hpp/manipulation/axial-handle.hh
@@ -52,6 +52,32 @@ namespace hpp {
virtual DifferentiableFunctionPtr_t createGrasp
(const GripperPtr_t& gripper) const;
+ /// Create constraint that acts on the non-constrained axis of the
+ /// constraint generated by Handle::createGrasp.
+ /// \param gripper object containing the gripper information
+ /// \return a relative orientation constraint between the handle and
+ /// the gripper. Only the rotation around the x-axis is constrained.
+ virtual DifferentiableFunctionPtr_t createGraspComplement
+ (const GripperPtr_t& gripper) const;
+
+ /// Create constraint corresponding to a pregrasping task.
+ /// \param gripper object containing the gripper information
+ /// \return the constraint of relative transformation between the handle and
+ /// the gripper.
+ /// \note The translation along x-axis and the rotation around z-axis are not constrained.
+ virtual DifferentiableFunctionPtr_t createPreGrasp
+ (const GripperPtr_t& gripper) const;
+
+ /// Create constraint that acts on the non-constrained axis of the
+ /// constraint generated by Handle::createPreGrasp.
+ /// \param gripper object containing the gripper information
+ /// \param shift the target value along the x-axis
+ /// \return the constraint of relative position between the handle and
+ /// the gripper.
+ /// \note The translation along x-axis and the rotation around z-axis are constrained.
+ virtual DifferentiableFunctionPtr_t createPreGraspComplement
+ (const GripperPtr_t& gripper, const value_type& shift) const;
+
virtual std::ostream& print (std::ostream& os) const;
protected:
/// Constructor
diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh
index 8dc1222..78019b8 100644
--- a/include/hpp/manipulation/handle.hh
+++ b/include/hpp/manipulation/handle.hh
@@ -91,9 +91,16 @@ namespace hpp {
virtual DifferentiableFunctionPtr_t createGrasp
(const GripperPtr_t& gripper) const;
- /// Create constraint corresponding to a gripper grasping this object
+ /// Create constraint that acts on the non-constrained axis of the
+ /// constraint generated by Handle::createGrasp.
/// \param gripper object containing the gripper information
- /// \return the constraint of relative position between the handle and
+ /// \return a constraints that is not doing anything.
+ virtual DifferentiableFunctionPtr_t createGraspComplement
+ (const GripperPtr_t& gripper) const;
+
+ /// Create constraint corresponding to a pregrasping task.
+ /// \param gripper object containing the gripper information
+ /// \return the constraint of relative transformation between the handle and
/// the gripper.
/// \note Only 5 DOFs of the relative transformation between the handle and the gripper
/// are constrained. The translation along x-axis is not constrained.
diff --git a/src/axial-handle.cc b/src/axial-handle.cc
index 8e37c41..8d52183 100644
--- a/src/axial-handle.cc
+++ b/src/axial-handle.cc
@@ -38,6 +38,44 @@ namespace hpp {
(gripper->joint()->robot(), gripper->joint (), joint(),
inverse (localPosition()) * gripper->objectPositionInJoint (), mask);
}
+
+ DifferentiableFunctionPtr_t AxialHandle::createGraspComplement
+ (const GripperPtr_t& gripper) const
+ {
+ using boost::assign::list_of;
+ std::vector <bool> mask = list_of (true)(false)(false);
+ Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint ();
+ return RelativeOrientation::create
+ (gripper->joint()->robot(), gripper->joint (), joint(), transform.getRotation (), mask);
+ }
+
+ DifferentiableFunctionPtr_t AxialHandle::createPreGrasp
+ (const GripperPtr_t& gripper) const
+ {
+ using boost::assign::list_of;
+ std::vector <bool> mask = list_of (false)(true)(true)(false)(true)(true);
+ return RelativeTransformation::create
+ (gripper->joint()->robot(), gripper->joint (), joint(),
+ inverse (localPosition()) * gripper->objectPositionInJoint (), mask);
+ }
+
+ DifferentiableFunctionPtr_t AxialHandle::createPreGraspComplement
+ (const GripperPtr_t& gripper, const value_type& shift) const
+ {
+ //using boost::assign::list_of;
+ //std::vector <bool> mask = list_of (true)(false)(false)(true)(false)(false);
+ //Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint ();
+ //transform.setTranslation (transform.getTranslation () + fcl::Vec3f (shift,0,0));
+ //return RelativeTransformation::create
+ //(gripper->joint()->robot(), gripper->joint (), joint(), transform, mask);
+ using boost::assign::list_of;
+ std::vector <bool> mask = list_of (true)(false)(false);
+ Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint ();
+ fcl::Vec3f target = transform.getTranslation () + fcl::Vec3f (shift,0,0);
+ return RelativePosition::create
+ (gripper->joint()->robot(), gripper->joint (), joint(), target, fcl::Vec3f (0,0,0), mask);
+ }
+
HandlePtr_t AxialHandle::clone () const
{
AxialHandlePtr_t self = weakPtr_.lock ();
diff --git a/src/handle.cc b/src/handle.cc
index b2ae72c..f783812 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -35,8 +35,18 @@ namespace hpp {
using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(true);
return RelativeTransformation::create
- (gripper->joint()->robot(), gripper->joint (), joint(), inverse (localPosition()) *
- gripper->objectPositionInJoint (), mask);
+ (gripper->joint()->robot(), gripper->joint (), joint(),
+ inverse (localPosition()) * gripper->objectPositionInJoint (), mask);
+ }
+
+ DifferentiableFunctionPtr_t Handle::createGraspComplement
+ (const GripperPtr_t& gripper) const
+ {
+ using boost::assign::list_of;
+ std::vector <bool> mask = list_of (false)(false)(false)(false)(false)(false);
+ return RelativeTransformation::create
+ (gripper->joint()->robot(), gripper->joint (), joint(),
+ inverse (localPosition()) * gripper->objectPositionInJoint (), mask);
}
DifferentiableFunctionPtr_t Handle::createPreGrasp
--
GitLab