diff --git a/src/constraints/acceleration/TorqueLimits.cpp b/src/constraints/acceleration/TorqueLimits.cpp index 27cc9a43..6ad8bcfd 100644 --- 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 index abadb084..4275e8bf 100644 --- 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 index 76784fbc..d5574ee3 100644 --- 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);