finished testing dimensions

Author: adubredu

BUILD

@@ -1,4 +1,4 @@
-load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library")
+load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library", "cc_test")
 
 cc_library(
     name = "mpcinterface",
@@ -11,6 +11,15 @@ cc_library(
     ],
 )
 
+cc_test(
+    name = "mpcinterface_test",
+    srcs = ["test/mpcinterface_test.cpp"],
+    deps = [
+        ":mpcinterface",
+        "@com_google_googletest//:gtest_main",
+    ],
+)
+
 cc_binary(
     name = "main",
     srcs = ["main.cpp"],

WORKSPACE

@@ -6,3 +6,9 @@ http_archive(
     strip_prefix = "eigen-3.4.0", 
     build_file = "//third_party/eigen:BUILD",
 )
+
+http_archive(
+    name = "com_google_googletest",
+    urls = ["https://github.com/google/googletest/archive/refs/tags/v1.14.0.tar.gz"],
+    strip_prefix = "googletest-1.14.0",
+)

include/mpcinterface.h

@@ -15,13 +15,13 @@ class MPCInterface
 
     // Inputs:
     // State cost matrix
-    Eigen::VectorXd Q;
+    Eigen::MatrixXd Q;
 
     // Input cost matrix
-    Eigen::VectorXd R;
+    Eigen::MatrixXd R;
 
     // Terminal state cost matrix
-    Eigen::VectorXd P;
+    Eigen::MatrixXd P;
 
     // State lower bound
     Eigen::VectorXd x_min;
@@ -42,10 +42,10 @@ class MPCInterface
     Eigen::MatrixXd B;
 
     // State reference
-    Eigen::VectorXd x_ref;
+    Eigen::MatrixXd x_ref;
 
     // Input reference
-    Eigen::VectorXd u_ref;
+    Eigen::MatrixXd u_ref;
 
     // Outputs:
     struct CanonicalForm {

src/mpcinterface.cpp

@@ -11,13 +11,13 @@ MPCInterface::MPCInterface (int H, int n, int m) {
     this->m = m;
 
     // Initialize state cost matrix
-    this->Q = Eigen::VectorXd::Zero(n, n);
+    this->Q = Eigen::MatrixXd::Zero(n, n);
 
     // Initialize input cost matrix
-    this->R = Eigen::VectorXd::Zero(m, m);
+    this->R = Eigen::MatrixXd::Zero(m, m);
 
     // Initialize terminal state cost matrix
-    this->P = Eigen::VectorXd::Zero(n, n);
+    this->P = Eigen::MatrixXd::Zero(n, n);
 
     // Initialize state lower bound
     this->x_min = Eigen::VectorXd::Zero(n);
@@ -38,18 +38,18 @@ MPCInterface::MPCInterface (int H, int n, int m) {
     this->B = Eigen::MatrixXd::Zero(n, m);
 
     // Initialize state reference
-    this->x_ref = Eigen::VectorXd::Zero(n);
+    this->x_ref = Eigen::MatrixXd::Zero(n, H);
 
     // Initialize input reference
-    this->u_ref = Eigen::VectorXd::Zero(m);
+    this->u_ref = Eigen::MatrixXd::Zero(m, H);
 }
 
 MPCInterface::~MPCInterface() {}
 
 MPCInterface::CanonicalForm MPCInterface::getCanonicalForm() {
 
     // Create P_bar
-    Eigen::MatrixXd P_bar((H * n + H * m), (H * n + H * m));
+    Eigen::MatrixXd P_bar(H * (n + m), H * (n + m));
     P_bar.setZero();  // Initialize to zeros
     for (int i = 0; i < H; i++) {
         P_bar.block(i * n, i * n, n, n) = Q;

test/mpcinterface_test.cpp

@@ -0,0 +1,65 @@
+#include <Eigen/Core>
+#include "mpcinterface.h"
+#include "gtest/gtest.h"
+
+TEST(MPCInterfaceTest, TestInitializedMatrixDimensions) {
+    // Initialize horizon length
+    int H = 5;
+
+    // Initialize state dimension
+    int n = 3;
+
+    // Initialize input dimension
+    int m = 2;
+
+    // Initialize MPCInterface object
+    MPCInterface mpc(H, n, m);
+
+    EXPECT_EQ(mpc.Q.rows(), n);
+    EXPECT_EQ(mpc.Q.cols(), n);
+    EXPECT_EQ(mpc.R.rows(), m);
+    EXPECT_EQ(mpc.R.cols(), m);
+    EXPECT_EQ(mpc.P.rows(), n);
+    EXPECT_EQ(mpc.P.cols(), n);
+    EXPECT_EQ(mpc.x_min.size(), n);
+    EXPECT_EQ(mpc.x_max.size(), n);
+    EXPECT_EQ(mpc.u_min.size(), m);
+    EXPECT_EQ(mpc.u_max.size(), m);
+    EXPECT_EQ(mpc.A.rows(), n);
+    EXPECT_EQ(mpc.A.cols(), n);
+    EXPECT_EQ(mpc.B.rows(), n);
+    EXPECT_EQ(mpc.B.cols(), m);
+    EXPECT_EQ(mpc.x_ref.cols(), H);
+    EXPECT_EQ(mpc.x_ref.rows(), n);
+    EXPECT_EQ(mpc.u_ref.cols(), H);
+    EXPECT_EQ(mpc.u_ref.rows(), m);
+}
+
+TEST(MPCInterfaceTest, TestCanonicalMatrixDimensions) {
+    // Initialize horizon length
+    int H = 5;
+
+    // Initialize state dimension
+    int n = 3;
+
+    // Initialize input dimension
+    int m = 2;
+
+    // Initialize MPCInterface object
+    MPCInterface mpc(H, n, m);
+
+    // Generate Canonical Form
+    MPCInterface::CanonicalForm canonical_form = mpc.getCanonicalForm();
+
+    EXPECT_EQ(canonical_form.P_bar.rows(), H * (n + m));
+    EXPECT_EQ(canonical_form.P_bar.cols(), H * (n + m));
+    EXPECT_EQ(canonical_form.q_bar.size(), H * (n + m)); 
+    EXPECT_EQ(canonical_form.A_bar.rows(), 2 * H * (n + m) + H * n);
+    EXPECT_EQ(canonical_form.A_bar.cols(), H * (n + m));
+    EXPECT_EQ(canonical_form.l_bar.size(), 2 * H * (n + m)); 
+    EXPECT_EQ(canonical_form.u_bar.size(), 2 * H * (n + m));
+}
+
+TEST(MPCInterfaceTest, TestCanonicalForm) {
+    
+}