diff --git a/src/handle.cc b/src/handle.cc index c5a8c13e81b6b611c75130f302e56ce1cc4fdff4..130b84d59fc6a81b72615e8de48a3a2a855d2d7f 100644 --- a/src/handle.cc +++ b/src/handle.cc @@ -32,8 +32,7 @@ #include <hpp/constraints/generic-transformation.hh> #include <hpp/constraints/implicit.hh> -#include <hpp/constraints/explicit.hh> -#include <hpp/core/explicit-relative-transformation.hh> +#include <hpp/constraints/explicit/relative-pose.hh> namespace hpp { namespace manipulation { @@ -111,23 +110,20 @@ namespace hpp { ImplicitPtr_t Handle::createGrasp (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)) { - return ExplicitRelativeTransformation::create + return constraints::explicit_::RelativePose::create (n, gripper->joint ()->robot (), gripper->joint (), joint (), - gripper->objectPositionInJoint (), localPosition())->createNumericalConstraint(); + gripper->objectPositionInJoint (), localPosition(), + mask_, ComparisonTypes_t (6, constraints::EqualToZero)); } - return ImplicitPtr_t - (Implicit::create (RelativeTransformation::create - (n, - gripper->joint()->robot(), - gripper->joint (), joint (), - gripper->objectPositionInJoint (), - localPosition(), mask_))); + return constraints::implicit::RelativePose::create + (n, gripper->joint ()->robot (), gripper->joint (), joint (), + gripper->objectPositionInJoint (), localPosition(), + mask_, ComparisonTypes_t (mask_.size (), constraints::EqualToZero)); } ImplicitPtr_t Handle::createGraspComplement @@ -146,14 +142,10 @@ namespace hpp { ); } else { std::vector<bool> Cmask = complementMask(mask_); - RelativeTransformationPtr_t function = RelativeTransformation::create - (n, - gripper->joint()->robot(), - gripper->joint (), joint (), - gripper->objectPositionInJoint (), - localPosition(), Cmask); - return Implicit::create (function, - ComparisonTypes_t(function->outputSize(), constraints::Equality)); + return constraints::implicit::RelativePose::create + (n, gripper->joint ()->robot (), gripper->joint (), joint (), + gripper->objectPositionInJoint (), localPosition(), + Cmask, ComparisonTypes_t (Cmask.size (), constraints::Equality)); } } @@ -177,23 +169,15 @@ namespace hpp { } // If handle is on a freeflying object, create an explicit constraint if (isHandleOnFreeflyer (*this)) { - ExplicitPtr_t enc - (ExplicitRelativeTransformation::create - (n, gripper->joint ()->robot (), gripper->joint (), joint (), - gripper->objectPositionInJoint (), - localPosition())->createNumericalConstraint()); - enc->comparisonType (comp); - return enc; + return constraints::explicit_::RelativePose::create + (n, gripper->joint ()->robot (), gripper->joint (), joint (), + gripper->objectPositionInJoint (), localPosition(), + std::vector <bool> (6, true), comp); } - return ImplicitPtr_t - (Implicit::create (RelativeTransformation::create - (n, - gripper->joint()->robot(), - gripper->joint (), joint (), - gripper->objectPositionInJoint (), - localPosition(), - list_of (true)(true)(true)(true)(true) - (true)), comp)); + return constraints::implicit::RelativePose::create + (n, gripper->joint ()->robot (), gripper->joint (), joint (), + gripper->objectPositionInJoint (), localPosition(), + std::vector <bool> (true, 6), comp); } ImplicitPtr_t Handle::createPreGrasp @@ -204,12 +188,11 @@ namespace hpp { if (n.empty()) n = "Pregrasp_ " + maskToStr(mask_) + "_" + name () + "_" + gripper->name (); - return ImplicitPtr_t - (Implicit::create (RelativeTransformation::create - (n, - gripper->joint()->robot(), - gripper->joint (), joint (), - transform, localPosition(), mask_))); + ImplicitPtr_t result + (constraints::implicit::RelativePose::create + (n, gripper->joint()->robot(), gripper->joint (), joint (), + transform, localPosition(), mask_)); + return result; } HandlePtr_t Handle::clone () const