update base orientation with rotational PID output

ami-iit · Feb 5, 2024 · 41e2485 · 41e2485
1 parent 71f4ee9
commit 41e2485
Showing 1 changed file with 1 addition and 2 deletions.
diff --git a/src/ML/src/velMANNAutoregressive.cpp b/src/ML/src/velMANNAutoregressive.cpp
@@ -745,8 +745,7 @@ bool velMANNAutoregressive::advance()
     // if the robot is stopped (i.e, if the current velMANN input and the previous one are the same)
     // we set the yaw rate equal to zero
     //TODO need to add Baumgarte stabilization
-    //TODO once rotational PID is added, use omegaE to update the ang vel
-    const manif::SO3Tangentd baseAngularVelocity = m_pimpl->isRobotStopped ? Eigen::Vector3d{0, 0, 0} : velMannOutput.futureBaseAngularVelocityTrajectory.col(0);
+    const manif::SO3Tangentd baseAngularVelocity = m_pimpl->isRobotStopped ? Eigen::Vector3d{0, 0, 0} : m_pimpl->previousOmegaE;
     if (!m_pimpl->baseOrientationDynamics->setControlInput({baseAngularVelocity}))
     {
         log()->error("{} Unable to set the control input to the base orientation dynamics.",