From 7c92b15355ec9516a44c94d609ba22c29e77585b Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Fri, 27 Sep 2024 15:45:53 +0200 Subject: [PATCH] fixed constraint ominwheel working on local velocity --- src/constraints/velocity/OmniWheels4X.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/constraints/velocity/OmniWheels4X.cpp b/src/constraints/velocity/OmniWheels4X.cpp index 55f39d12..76b5d518 100644 --- a/src/constraints/velocity/OmniWheels4X.cpp +++ b/src/constraints/velocity/OmniWheels4X.cpp @@ -71,6 +71,8 @@ void OmniWheels4X::update() { // _robot.getPose(_base_link, _w_T_b); // _Aineq.rightCols(_x_size-6).noalias() = _w_T_b.linear() * _J.rightCols(_x_size-6); + + //todo: here we assumes velocity of the floating base are in local frame! _Aineq.rightCols(_x_size-6).noalias() = _J.rightCols(_x_size-6); }