Merge pull request #300 from Simple-Robotics/topic/lqr-problem-is-approx

[gar] Add `LqrProblemTpl::isApprox()` and helper `lqrKnotsSameDim`, check dimensions in `LqrKnotTpl::isApprox()`

Author: ManifoldFR

CHANGELOG.md

@@ -10,6 +10,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Added
 
 - Add CMake macro `aligator_create_python_extension()` to export ([#298](https://github.com/Simple-Robotics/aligator/pull/298))
+- Add `LqrProblemTpl::isApprox()` and helper `lqrKnotsSameDim`, check dimensions in `LqrKnotTpl::isApprox()` ([#300](https://github.com/Simple-Robotics/aligator/pull/300))
 
 ## [0.13.0] - 2025-04-26
 

gar/include/aligator/gar/lqr-problem.hpp

@@ -94,6 +94,17 @@ template <typename Scalar> struct LqrProblemTpl {
 
   inline uint ntheta() const { return stages[0].nth; }
 
+  inline bool isApprox(const LqrProblemTpl &other) {
+    if (horizon() != other.horizon() || !G0.isApprox(other.G0) ||
+        !g0.isApprox(other.g0))
+      return false;
+    for (uint i = 0; i < uint(horizon()); i++) {
+      if (!stages[i].isApprox(other.stages[i]))
+        return false;
+    }
+    return true;
+  }
+
   /// Evaluate the quadratic objective.
   Scalar evaluate(const VectorOfVectors &xs, const VectorOfVectors &us,
                   const std::optional<ConstVectorRef> &theta_) const;
@@ -106,6 +117,13 @@ template <typename Scalar> struct LqrProblemTpl {
   }
 };
 
+template <typename Scalar>
+bool lqrKnotsSameDim(const LqrKnotTpl<Scalar> &lhs,
+                     const LqrKnotTpl<Scalar> &rhs) {
+  return (lhs.nx == rhs.nx) && (lhs.nu == rhs.nu) && (lhs.nc == rhs.nc) &&
+         (lhs.nx2 == rhs.nx2) && (lhs.nth == rhs.nth);
+}
+
 template <typename Scalar>
 std::ostream &operator<<(std::ostream &oss, const LqrKnotTpl<Scalar> &self) {
   oss << "LqrKnot {";

gar/include/aligator/gar/lqr-problem.hxx

@@ -45,17 +45,24 @@ void LqrKnotTpl<Scalar>::addParameterization(uint nth) {
 
 template <typename Scalar>
 bool LqrKnotTpl<Scalar>::isApprox(const LqrKnotTpl &other, Scalar prec) const {
-  bool cost = Q.isApprox(other.Q, prec) && S.isApprox(other.S, prec) &&
-              R.isApprox(other.R, prec) && q.isApprox(other.q, prec) &&
-              r.isApprox(other.r, prec);
-  bool dyn = A.isApprox(other.A, prec) && B.isApprox(other.B, prec) &&
-             E.isApprox(other.E, prec) && f.isApprox(other.f, prec);
-  bool cstr = C.isApprox(other.C, prec) && D.isApprox(other.D, prec) &&
-              d.isApprox(other.d, prec);
-  bool th = Gth.isApprox(other.Gth, prec) && Gx.isApprox(other.Gx, prec) &&
-            Gu.isApprox(other.Gu, prec) && Gv.isApprox(other.Gv, prec) &&
-            gamma.isApprox(other.gamma, prec);
-  return cost && dyn && cstr && th;
+  if (!lqrKnotsSameDim(*this, other))
+    return false;
+  if (!(Q.isApprox(other.Q, prec) && S.isApprox(other.S, prec) &&
+        R.isApprox(other.R, prec) && q.isApprox(other.q, prec) &&
+        r.isApprox(other.r, prec)))
+    return false;
+
+  if (!(A.isApprox(other.A, prec) && B.isApprox(other.B, prec) &&
+        E.isApprox(other.E, prec) && f.isApprox(other.f, prec)))
+    return false;
+
+  if (!(C.isApprox(other.C, prec) && D.isApprox(other.D, prec) &&
+        d.isApprox(other.d, prec)))
+    return false;
+
+  return Gth.isApprox(other.Gth, prec) && Gx.isApprox(other.Gx, prec) &&
+         Gu.isApprox(other.Gu, prec) && Gv.isApprox(other.Gv, prec) &&
+         gamma.isApprox(other.gamma, prec);
 }
 
 template <typename Scalar>