From dd1e12f0588d55808d213030c85c0e16b9837ab1 Mon Sep 17 00:00:00 2001
From: pFernbach <pierre.fernbach@gmail.com>
Date: Wed, 5 Jun 2019 18:19:58 +0200
Subject: [PATCH] computeCenterOfContactAtStateForLimb correctly apply the
offset when the rotation is not Identity
---
src/rbprmbuilder.impl.cc | 5 ++---
1 file changed, 2 insertions(+), 3 deletions(-)
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 2d969882..1606f049 100644
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -2134,9 +2134,8 @@ namespace hpp {
res->length ((_CORBA_ULong)2);
-
- fcl::Vec3f& position = state.contactPositions_.at(limbName);
- position += fullBody()->GetLimb(limb)->offset_;
+ fcl::Transform3f jointT( state.contactRotation_.at(limbName), state.contactPositions_.at(limbName));
+ fcl::Vec3f position = jointT.transform(fullBody()->GetLimb(limb)->offset_);
_CORBA_ULong size = (_CORBA_ULong) 3;
double* dofArray = hpp::floatSeq::allocbuf(size);
hpp::floatSeq floats (size, size, dofArray, true);
--
GitLab