Fixed to be compatible with contact point formulation

ADVRHumanoids · Apr 17, 2024 · 62dbe51 · 62dbe51
1 parent b908cc1
commit 62dbe51
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 5 deletions.
diff --git a/src/constraints/acceleration/TorqueLimits.cpp b/src/constraints/acceleration/TorqueLimits.cpp
@@ -36,7 +36,7 @@ void TorqueLimits::update()
         }
         else {
             _robot.getJacobian(_contact_links[i], _Jtmp);
-            _dyn_constraint = _dyn_constraint + (-_Jtmp.transpose()) * _wrenches[i];
+            _dyn_constraint = _dyn_constraint + (-_Jtmp.block(0,0,_wrenches[i].getM().rows(),_Jtmp.cols()).transpose()) * _wrenches[i];
         }
     }
 

diff --git a/src/tasks/acceleration/DynamicFeasibility.cpp b/src/tasks/acceleration/DynamicFeasibility.cpp
@@ -36,7 +36,7 @@ void OpenSoT::tasks::acceleration::DynamicFeasibility::_update()
         else {
             _robot.getJacobian(_contact_links[i], _Jtmp);
             _Jf = _Jtmp.block<6,6>(0,0).transpose();
-            _dyn_constraint = _dyn_constraint + (-_Jf) * _wrenches[i];
+            _dyn_constraint = _dyn_constraint + (-_Jf.block(0,0,6,_wrenches[i].getM().rows())) * _wrenches[i];
         }
     }
 

diff --git a/src/variables/Torque.cpp b/src/variables/Torque.cpp
@@ -42,11 +42,11 @@ void OpenSoT::variables::Torque::update()
 
         _model->getJacobian(_contact_links[i], _Jc[i]);
 
-        self() =  self() + (-_S)*_Jc[i]*_force_vars.at(i);
+        self() =  self() + (-_S)*_Jc[i].block(0,0,_force_vars.at(i).getM().rows(), _Jc[i].cols()).transpose()*_force_vars.at(i);
 
         // previous line is equivalent to the following two lines
-        // _C -= _S * _Jc[i] * _force_vars.at(i).getC();
-        // _d -= _S * _Jc[i] * _force_vars.at(i).getd();
+        // _C -= _S * _Jc[i].transpose() * _force_vars.at(i).getC();
+        // _d -= _S * _Jc[i].transpose() * _force_vars.at(i).getd();
     }
 
     _model->computeNonlinearTerm(_h);