improve ref and des yaw calculations in rot pid

ami-iit · Feb 7, 2024 · e7eb950 · e7eb950
1 parent edbcf6d
commit e7eb950
Showing 1 changed file with 5 additions and 24 deletions.
diff --git a/src/ML/src/velMANNAutoregressive.cpp b/src/ML/src/velMANNAutoregressive.cpp
@@ -129,31 +129,11 @@ struct velMANNAutoregressive::Impl
     Math::SchmittTrigger leftFootSchmittTrigger;
     Math::SchmittTrigger rightFootSchmittTrigger;
 
-    static double extractYawAngle(const Eigen::Ref<const Eigen::Matrix3d>& R);
-
     bool updateContact(const double referenceHeight,
                        Math::SchmittTrigger& trigger,
                        Contacts::EstimatedContact& contact);
 };
 
-double velMANNAutoregressive::Impl::extractYawAngle(const Eigen::Ref<const Eigen::Matrix3d>& R)
-{
-    if (R(2, 0) < 1.0)
-    {
-        if (R(2, 0) > -1.0)
-        {
-            return atan2(R(1, 0), R(0, 0));
-        } else
-        {
-            // Not a unique solution
-            return -atan2(-R(1, 2), R(1, 1));
-        }
-    }
-
-    // Not a unique solution
-    return atan2(-R(1, 2), R(1, 1));
-}
-
 bool velMANNAutoregressive::Impl::updateContact(const double referenceHeight,
                                              Math::SchmittTrigger& trigger,
                                              Contacts::EstimatedContact& contact)
@@ -646,16 +626,17 @@ bool velMANNAutoregressive::setInput(const Input& input)
         m_pimpl->state.I_H_ref = m_pimpl->state.I_H_B;
     }
 
-    const double refYaw = Impl::extractYawAngle(m_pimpl->state.I_H_ref.quat().toRotationMatrix());
+    const double refYaw = (iDynTree::Rotation(m_pimpl->state.I_H_ref.quat().toRotationMatrix()).asRPY())(2);
 
     Eigen::Matrix3Xd desiredFutureBaseDirections3d = (Eigen::Matrix3Xd(input.desiredFutureBaseDirections.rows() + 1, input.desiredFutureBaseDirections.cols()) << input.desiredFutureBaseDirections, Eigen::RowVectorXd::Zero(input.desiredFutureBaseDirections.cols())).finished();
+    Eigen::Vector3d forwardDir(1, 0, 0);
     const double desYaw =
-        std::atan2((desiredFutureBaseDirections3d.col(desiredFutureBaseDirections3d.cols() - 1).cross(desiredFutureBaseDirections3d.col(1)))(2),
-        desiredFutureBaseDirections3d.col(desiredFutureBaseDirections3d.cols() - 1).dot(desiredFutureBaseDirections3d.col(1)));
+        std::atan2((forwardDir.cross(desiredFutureBaseDirections3d.col(0)))(2),
+        forwardDir.dot(desiredFutureBaseDirections3d.col(0)));
 
     // Construct desired angle term of rotational PID equation
     manif::SE3d R_d = manif::SE3d(0, 0, 0, //xyz translation is unimportant
-                      0, 0, desYaw + refYaw);
+                      0, -0.09, desYaw + refYaw);
 
     Eigen::Matrix3d R_mult = (R_d.inverse().compose(m_pimpl->state.I_H_B).quat().toRotationMatrix());