Merge pull request #75 from robotology/fix/RetargetingClient_reset

Bugfix in the retargeting client class
robotology · Dec 10, 2020 · 5119f80 · 5119f80
2 parents 44d2d4c + 028ae36
commit 5119f80
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 2 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -5,6 +5,7 @@ All notable changes to this project are documented in this file.
 ### Added
 
 ### Changed
+- Bugfix while resetting the hand smoother in the `RetargetingClient` (https://github.com/robotology/walking-controllers/pull/75)
 
 ## [0.4.0] - 2020-12-01
 ### Added

diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp
@@ -216,8 +216,8 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config,
 
 bool RetargetingClient::reset(WalkingFK& kinDynWrapper)
 {
-    m_leftHand.data = kinDynWrapper.getLeftHandToWorldTransform();
-    m_rightHand.data = kinDynWrapper.getRightHandToWorldTransform();
+    m_leftHand.data = kinDynWrapper.getHeadToWorldTransform().inverse() * kinDynWrapper.getLeftHandToWorldTransform();
+    m_rightHand.data = kinDynWrapper.getHeadToWorldTransform().inverse() * kinDynWrapper.getRightHandToWorldTransform();
 
     if(m_useHandRetargeting)
     {