From d8cf85f07f2e09ead9d79df690fc85df7230a5a0 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Tue, 19 May 2020 14:31:24 +0200
Subject: [PATCH] Update to modifications in hpp-constraints
- Specify comparison types in implicit constraint constructors.
---
src/handle.cc | 16 +++++++---------
src/problem-solver.cc | 3 ++-
2 files changed, 9 insertions(+), 10 deletions(-)
diff --git a/src/handle.cc b/src/handle.cc
index 9d512a25..fe5c9174 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -72,13 +72,13 @@ namespace hpp {
return false;
}
- inline std::size_t maskSize (const std::vector<bool>& mask)
+ inline int 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;
+ return (int)res;
}
inline bool is6Dmask (const std::vector<bool>& mask)
@@ -130,7 +130,7 @@ namespace hpp {
return constraints::explicit_::RelativePose::create
(n, robot (), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition(),
- mask_, ComparisonTypes_t (6, constraints::EqualToZero));
+ 6 * constraints::EqualToZero);
}
return Implicit::create (RelativeTransformation::create
(n, robot (), gripper->joint (), joint (),
@@ -150,15 +150,14 @@ namespace hpp {
if (is6Dmask(mask_)) {
return Implicit::create (
boost::shared_ptr <ZeroDiffFunc> (new ZeroDiffFunc (
- r->configSize(), r->numberDof (), n))
- );
+ r->configSize(), r->numberDof (), n)), ComparisonTypes_t());
} else {
std::vector<bool> Cmask = complementMask(mask_);
return Implicit::create (RelativeTransformation::create
(n, r, gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition(),
Cmask),
- ComparisonTypes_t (maskSize (Cmask), constraints::Equality));
+ maskSize(Cmask) * constraints::Equality);
}
}
@@ -184,8 +183,7 @@ namespace hpp {
if (isHandleOnFreeflyer (*this)) {
return constraints::explicit_::RelativePose::create
(n, robot (), gripper->joint (), joint (),
- gripper->objectPositionInJoint (), localPosition(),
- std::vector <bool> (6, true), comp);
+ gripper->objectPositionInJoint (), localPosition(), comp);
}
return Implicit::create (RelativeTransformation::create
(n, robot (), gripper->joint (), joint (),
@@ -206,7 +204,7 @@ namespace hpp {
(RelativeTransformation::create
(n, robot(), gripper->joint (), joint (),
M, localPosition(), mask_),
- ComparisonTypes_t(maskSize (mask_), constraints::EqualToZero)));
+ maskSize(mask_) * constraints::EqualToZero));
return result;
}
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 43e64d85..381549a6 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -324,7 +324,8 @@ namespace hpp {
(hpp::constraints::ConvexShapeContact::create
(name, robot_, floorSurfaces, objectSurfaces));
cvxShape->setNormalMargin(margin + width);
- addNumericalConstraint (name, Implicit::create (cvxShape));
+ addNumericalConstraint (name, Implicit::create
+ (cvxShape, 5 * constraints::EqualToZero));
}
void ProblemSolver::createGraspConstraint
--
GitLab