From 391bd2d5fd669c872ffe57dd25428999d88d97ad Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Thu, 27 Sep 2018 17:58:14 +0200
Subject: [PATCH] [Handle] Fix size of comparison type.
---
src/handle.cc | 15 ++++++++++++---
1 file changed, 12 insertions(+), 3 deletions(-)
diff --git a/src/handle.cc b/src/handle.cc
index 0a37c5ba..1022ad1b 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -69,6 +69,15 @@ namespace hpp {
return false;
}
+ inline std::size_t maskSize (const std::vector<bool>& mask)
+ {
+ std::size_t res (0);
+ for (std::size_t i = 0; i < 6; ++i) {
+ if (mask[i]) ++res;
+ }
+ return res;
+ }
+
inline bool is6Dmask (const std::vector<bool>& mask)
{
for (std::size_t i = 0; i < 6; ++i) if (!mask[i]) return false;
@@ -123,7 +132,7 @@ namespace hpp {
return constraints::implicit::RelativePose::create
(n, gripper->joint ()->robot (), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition(),
- mask_, ComparisonTypes_t (mask_.size (), constraints::EqualToZero));
+ mask_, ComparisonTypes_t (maskSize (mask_), constraints::EqualToZero));
}
ImplicitPtr_t Handle::createGraspComplement
@@ -145,7 +154,7 @@ namespace hpp {
return constraints::implicit::RelativePose::create
(n, gripper->joint ()->robot (), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition(),
- Cmask, ComparisonTypes_t (Cmask.size (), constraints::Equality));
+ Cmask, ComparisonTypes_t (maskSize (Cmask), constraints::Equality));
}
}
@@ -192,7 +201,7 @@ namespace hpp {
(constraints::implicit::RelativePose::create
(n, gripper->joint()->robot(), gripper->joint (), joint (),
transform, localPosition(), mask_, ComparisonTypes_t
- (mask_.size (), constraints::EqualToZero)));
+ (maskSize (mask_), constraints::EqualToZero)));
return result;
}
--
GitLab