From 4c931d1f6a8ef4f9ba367cad39f9f5383ccc8b22 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Sat, 25 Nov 2017 11:52:53 +0100
Subject: [PATCH] Add a method createGraspAndComplement in Handle classes
- create a constraint merging a grasp constraint and its complement.
---
include/hpp/manipulation/axial-handle.hh | 7 ++++
include/hpp/manipulation/handle.hh | 21 +++++++----
src/axial-handle.cc | 6 ++++
src/handle.cc | 45 ++++++++++++++++++------
4 files changed, 62 insertions(+), 17 deletions(-)
diff --git a/include/hpp/manipulation/axial-handle.hh b/include/hpp/manipulation/axial-handle.hh
index 6a947de3..28de06e9 100644
--- a/include/hpp/manipulation/axial-handle.hh
+++ b/include/hpp/manipulation/axial-handle.hh
@@ -63,6 +63,13 @@ namespace hpp {
virtual NumericalConstraintPtr_t createGraspComplement
(const GripperPtr_t& gripper, std::string name) const;
+ /// Create constraint composed of grasp constraint and its complement
+ /// \param gripper object containing the gripper information
+ /// \return the composition of grasp constraint and its complement.
+ /// \note the 6 degrees of freedom are constrained
+ virtual NumericalConstraintPtr_t createGraspAndComplement
+ (const GripperPtr_t& gripper, std::string name) 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
diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh
index 7dc2c249..a0e7ebbe 100644
--- a/include/hpp/manipulation/handle.hh
+++ b/include/hpp/manipulation/handle.hh
@@ -94,21 +94,30 @@ namespace hpp {
const std::vector<bool>& mask () const
{ return mask_; }
- /// Create constraint corresponding to a gripper grasping this object
+ /// Create constraint corresponding to a gripper grasping this handle
/// \param gripper object containing the gripper information
- /// \return the constraint of relative transformation between the handle and
- /// the gripper.
+ /// \return the constraint of relative transformation between the handle
+ /// and the gripper.
/// \note The 6 DOFs of the relative transformation are constrained.
virtual NumericalConstraintPtr_t createGrasp
(const GripperPtr_t& gripper, std::string name) const;
- /// Create constraint that acts on the non-constrained axis of the
- /// constraint generated by Handle::createGrasp.
+ /// Create complement constraint of gripper grasping this handle
/// \param gripper object containing the gripper information
- /// \return a constraints that is not doing anything.
+ /// \return trivial constraint
+ /// \note for this base class, the complement constraint is trivial, i.e.
+ /// it is of dimension 0.
virtual NumericalConstraintPtr_t createGraspComplement
(const GripperPtr_t& gripper, std::string name) const;
+ /// Create constraint composed of grasp constraint and its complement
+ /// \param gripper object containing the gripper information
+ /// \return the composition of grasp constraint and its complement.
+ /// \note for this base class, this constraint is the same as the grasp
+ /// constraint.
+ virtual NumericalConstraintPtr_t createGraspAndComplement
+ (const GripperPtr_t& gripper, std::string name) 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
diff --git a/src/axial-handle.cc b/src/axial-handle.cc
index ee652d88..1c5af7e5 100644
--- a/src/axial-handle.cc
+++ b/src/axial-handle.cc
@@ -115,6 +115,12 @@ namespace hpp {
DoubleInequality::create (width)));
}
+ NumericalConstraintPtr_t AxialHandle::createGraspAndComplement
+ (const GripperPtr_t& gripper, std::string n) const
+ {
+ return Handle::createGraspAndComplement (gripper, n);
+ }
+
HandlePtr_t AxialHandle::clone () const
{
AxialHandlePtr_t self = weakPtr_.lock ();
diff --git a/src/handle.cc b/src/handle.cc
index d9e79e31..9ed70574 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -107,16 +107,15 @@ namespace hpp {
(const GripperPtr_t& gripper, std::string n) const
{
using core::ExplicitRelativeTransformation;
+ if (n.empty()) {
+ n = gripper->name() + "_grasps_" + name() + "_" + maskToStr (mask_);
+ }
// If handle is on a freeflying object, create an explicit constraint
if (is6Dmask(mask_) && isHandleOnFreeflyer (*this)) {
- if (n.empty())
- n = "Explicit_relative_transform_" + name() + "_" + gripper->name();
return ExplicitRelativeTransformation::create
(n, gripper->joint ()->robot (), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition())->createNumericalConstraint();
}
- if (n.empty())
- n = "Grasp_" + maskToStr(mask_) + "_" + name() + "_" + gripper->name();
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
(n,
@@ -129,20 +128,19 @@ namespace hpp {
NumericalConstraintPtr_t Handle::createGraspComplement
(const GripperPtr_t& gripper, std::string n) const
{
+ if (n.empty()) {
+ std::vector<bool> Cmask = complementMask(mask_);
+ n = gripper->name() + "_grasps_" + name() + "/complement_" +
+ maskToStr (Cmask);
+ }
core::DevicePtr_t robot = gripper->joint()->robot();
if (is6Dmask(mask_)) {
- if (n.empty())
- n = "GraspComp_(0,0,0,0,0,0)_" + name () + "_" + gripper->name ();
return NumericalConstraint::create (
boost::shared_ptr <ZeroDiffFunc> (new ZeroDiffFunc (
robot->configSize(), robot->numberDof (), n))
);
} else {
- // TODO handle cases where rotations or translation are allowed.
std::vector<bool> Cmask = complementMask(mask_);
- if (n.empty())
- n = "Transformation_" + maskToStr(Cmask) + "_" + name ()
- + "_" + gripper->name ();
RelativeTransformationPtr_t function = RelativeTransformation::create
(n,
gripper->joint()->robot(),
@@ -157,10 +155,35 @@ namespace hpp {
}
}
+ NumericalConstraintPtr_t Handle::createGraspAndComplement
+ (const GripperPtr_t& gripper, std::string n) const
+ {
+ using boost::assign::list_of;
+ using core::ExplicitRelativeTransformation;
+ if (n.empty()) {
+ n = gripper->name() + "_holds_" + name();
+ }
+ // If handle is on a freeflying object, create an explicit constraint
+ if (isHandleOnFreeflyer (*this)) {
+ return ExplicitRelativeTransformation::create
+ (n, gripper->joint ()->robot (), gripper->joint (), joint (),
+ gripper->objectPositionInJoint (),
+ localPosition())->createNumericalConstraint();
+ }
+ return NumericalConstraintPtr_t
+ (NumericalConstraint::create (RelativeTransformation::create
+ (n,
+ gripper->joint()->robot(),
+ gripper->joint (), joint (),
+ gripper->objectPositionInJoint (),
+ localPosition(),
+ list_of (true)(true)(true)(true)(true)
+ (true))));
+ }
+
NumericalConstraintPtr_t Handle::createPreGrasp
(const GripperPtr_t& gripper, const value_type& shift, std::string n) const
{
- using boost::assign::list_of;
Transform3f transform = gripper->objectPositionInJoint ()
* Transform3f (I3, vector3_t (shift,0,0));
if (n.empty())
--
GitLab