spatial: Add SE3Expr unit test

stack-of-tasks · Jan 9, 2025 · 5fc8b44 · 5fc8b44
1 parent c680da2
commit 5fc8b44
Showing 1 changed file with 19 additions and 0 deletions.
diff --git a/unittest/spatial.cpp b/unittest/spatial.cpp
@@ -116,6 +116,25 @@ BOOST_AUTO_TEST_CASE(test_SE3)
   }
 }
 
+BOOST_AUTO_TEST_CASE(test_SE3_expr)
+{
+  using namespace pinocchio;
+
+  SE3 amb = SE3::Random();
+  SE3 bmc = SE3::Random();
+
+  SE3 amc_expected;
+  SE3 amc_expr;
+  SE3 amc_expr_noalias;
+
+  amc_expected = amb * bmc;
+  amc_expr.expr() = amb.const_expr() * bmc.const_expr();
+  amc_expr_noalias.expr().noalias() = amb.const_expr() * bmc.const_expr();
+
+  BOOST_CHECK(amc_expected.toActionMatrix().isApprox(amc_expr.toActionMatrix()));
+  BOOST_CHECK(amc_expected.toActionMatrix().isApprox(amc_expr_noalias.toActionMatrix()));
+}
+
 BOOST_AUTO_TEST_CASE(test_Motion)
 {
   using namespace pinocchio;