From fb64a466263a1c3304de1bd0016df4a08a6801c7 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 8 Dec 2015 16:07:18 +0100
Subject: [PATCH] Fix bug in Handle and AxialHandle
---
src/axial-handle.cc | 19 +++++++++----------
src/handle.cc | 5 ++---
2 files changed, 11 insertions(+), 13 deletions(-)
diff --git a/src/axial-handle.cc b/src/axial-handle.cc
index a16baef0..945e3173 100644
--- a/src/axial-handle.cc
+++ b/src/axial-handle.cc
@@ -40,7 +40,7 @@ namespace hpp {
std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(false);
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Transformation_(1,1,1,1,1,1)_" + name ()
+ ("Transformation_(1,1,1,1,1,0)_" + name ()
+ "_" + gripper->name (),
gripper->joint()->robot(),
gripper->joint (), joint (),
@@ -52,11 +52,11 @@ namespace hpp {
(const GripperPtr_t& gripper) const
{
using boost::assign::list_of;
- std::vector <bool> mask = list_of (false)(false)(false)(false)(true)
- (false);
+ std::vector <bool> mask = list_of (false)(false)(false)(false)(false)
+ (true);
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Transformation_(0,0,0,0,0,0)_" + name ()
+ ("Transformation_(0,0,0,0,0,1)_" + name ()
+ "_" + gripper->name (),
gripper->joint()->robot(),
gripper->joint (), joint (),
@@ -71,7 +71,7 @@ namespace hpp {
std::vector <bool> mask = list_of (false)(true)(true)(true)(true)(false);
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Transformation_(0,1,1,1,1,1)_" + name ()
+ ("Transformation_(0,1,1,1,1,0)_" + name ()
+ "_" + gripper->name (),
gripper->joint()->robot(),
gripper->joint (), joint (),
@@ -85,11 +85,10 @@ namespace hpp {
{
using boost::assign::list_of;
using core::DoubleInequality;
- std::vector <bool> mask = list_of (true)(false)(false)(false)(true)
- (false);
- Transform3f transform (gripper->objectPositionInJoint ().getRotation (),
- gripper->objectPositionInJoint ().getTranslation ()
- + fcl::Vec3f (shift,0,0));
+ std::vector <bool> mask = list_of (true)(false)(false)(false)(false)
+ (false);
+ Transform3f transform = gripper->objectPositionInJoint ()
+ * Transform3f (fcl::Vec3f (shift,0,0));
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
("Transformation_(1,0,0,0,0,0)_" + name ()
diff --git a/src/handle.cc b/src/handle.cc
index e0f33d0d..6122a5dd 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -115,9 +115,8 @@ namespace hpp {
using core::DoubleInequality;
std::vector <bool> mask = list_of (true)(false)(false)(false)(false)
(false);
- Transform3f transform (gripper->objectPositionInJoint ().getRotation (),
- gripper->objectPositionInJoint ().getTranslation ()
- + fcl::Vec3f (shift,0,0));
+ Transform3f transform = gripper->objectPositionInJoint ()
+ * Transform3f (fcl::Vec3f (shift,0,0));
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
("Transformation_(1,0,0,0,0,0)_" + name ()
--
GitLab