diff --git a/cmake/DARTMacros.cmake b/cmake/DARTMacros.cmake
index a5c30ea6b1bcd..8504177b3e2f1 100644
--- a/cmake/DARTMacros.cmake
+++ b/cmake/DARTMacros.cmake
@@ -371,7 +371,7 @@ function(dart_build_tests)
     )
 
     # Link libraries
-    target_link_libraries(${target_name} PRIVATE gtest gtest_main)
+    target_link_libraries(${target_name} PRIVATE GTest::gtest GTest::gtest_main)
     target_link_libraries(
       ${target_name} PRIVATE ${_ARG_LINK_LIBRARIES}
     )
diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp
index c51e37ddf08f5..c455b3416af62 100644
--- a/dart/constraint/BoxedLcpConstraintSolver.cpp
+++ b/dart/constraint/BoxedLcpConstraintSolver.cpp
@@ -203,7 +203,7 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group)
           if (mFIndex[mOffset[i] + j] >= 0)
             mFIndex[mOffset[i] + j] += mOffset[i];
 
-          // Apply impulse for mipulse test
+          // Apply impulse for impulse test
           {
             DART_PROFILE_SCOPED_N("Unit impulse test");
             constraint->applyUnitImpulse(j);
@@ -220,34 +220,21 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group)
                   mA.data() + index, false);
             }
           }
-
-          // Filling symmetric part of A matrix
-          {
-            DART_PROFILE_SCOPED_N("Fill lower triangle of A");
-            for (std::size_t k = 0; k < i; ++k) {
-              const int indexI = mOffset[i] + j;
-              for (std::size_t l = 0;
-                   l < group.getConstraint(k)->getDimension();
-                   ++l) {
-                const int indexJ = mOffset[k] + l;
-                mA(indexI, indexJ) = mA(indexJ, indexI);
-              }
-            }
-          }
         }
       }
 
-      assert(isSymmetric(
-          n,
-          mA.data(),
-          mOffset[i],
-          mOffset[i] + constraint->getDimension() - 1));
-
       {
         DART_PROFILE_SCOPED_N("Unexcite");
         constraint->unexcite();
       }
     }
+
+    {
+      // Fill lower triangle blocks of A matrix
+      DART_PROFILE_SCOPED_N("Fill lower triangle of A");
+      mA.leftCols(n).triangularView<Eigen::Lower>()
+          = mA.leftCols(n).triangularView<Eigen::Upper>().transpose();
+    }
   }
 
   assert(isSymmetric(n, mA.data()));
@@ -258,7 +245,7 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group)
   //  std::cout << std::endl;
 
   // Solve LCP using the primary solver and fallback to secondary solver when
-  // the parimary solver failed.
+  // the primary solver failed.
   if (mSecondaryBoxedLcpSolver) {
     // Make backups for the secondary LCP solver because the primary solver
     // modifies the original terms.
@@ -283,7 +270,7 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group)
       earlyTermination);
 
   // Sanity check. LCP solvers should not report success with nan values, but
-  // it could happen. So we set the sucees to false for nan values.
+  // it could happen. So we set the success to false for nan values.
   if (success && mX.hasNaN())
     success = false;
 
diff --git a/dart/constraint/DantzigLCPSolver.cpp b/dart/constraint/DantzigLCPSolver.cpp
index 952759f1c4282..fde0dd166b278 100644
--- a/dart/constraint/DantzigLCPSolver.cpp
+++ b/dart/constraint/DantzigLCPSolver.cpp
@@ -119,7 +119,7 @@ void DantzigLCPSolver::solve(ConstrainedGroup* _group)
       if (findex[offset[i] + j] >= 0)
         findex[offset[i] + j] += offset[i];
 
-      // Apply impulse for mipulse test
+      // Apply impulse for impulse test
       constraint->applyUnitImpulse(j);
 
       // Fill upper triangle blocks of A matrix
diff --git a/dart/constraint/PGSLCPSolver.cpp b/dart/constraint/PGSLCPSolver.cpp
index 58a5ff377eae3..1a5a446f39cb5 100644
--- a/dart/constraint/PGSLCPSolver.cpp
+++ b/dart/constraint/PGSLCPSolver.cpp
@@ -114,7 +114,7 @@ void PGSLCPSolver::solve(ConstrainedGroup* _group)
       if (findex[offset[i] + j] >= 0)
         findex[offset[i] + j] += offset[i];
 
-      // Apply impulse for mipulse test
+      // Apply impulse for impulse test
       constraint->applyUnitImpulse(j);
 
       // Fill upper triangle blocks of A matrix