fix reference frame when setting link vel

Signed-off-by: Ian Chen <ichen@osrfoundation.org>
gazebosim · Feb 19, 2021 · ba45184 · ba45184
1 parent fcdd420
commit ba45184
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/tpe/plugin/src/FreeGroupFeatures.cc b/tpe/plugin/src/FreeGroupFeatures.cc
@@ -153,8 +153,10 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity(
     auto linkIt = this->links.find(_groupID.id);
     if (linkIt != this->links.end() && linkIt->second != nullptr)
     {
+      math::Pose3d linkWorldPose = linkIt->second->link->GetWorldPose();
       linkIt->second->link->SetLinearVelocity(
-        math::eigen3::convert(_linearVelocity));
+        linkWorldPose.Rot().Inverse() *
+        math::eigen3::convert( _linearVelocity));
     }
   }
 }
@@ -175,7 +177,9 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(
     auto linkIt = this->links.find(_groupID.id);
     if (linkIt != this->links.end() && linkIt->second != nullptr)
     {
+      math::Pose3d linkWorldPose = linkIt->second->link->GetWorldPose();
       linkIt->second->link->SetAngularVelocity(
+        linkWorldPose.Rot().Inverse() *
         math::eigen3::convert(_angularVelocity));
     }
   }