spatial: Add expr and const_expr method in SE3Tpl

stack-of-tasks · Jan 9, 2025 · c680da2 · c680da2
1 parent 8a2641d
commit c680da2
Show file tree

Hide file tree

Showing 7 changed files with 32 additions and 0 deletions.
diff --git a/include/pinocchio/multibody/joint/joint-helical.hpp b/include/pinocchio/multibody/joint/joint-helical.hpp
@@ -84,6 +84,8 @@ namespace pinocchio
     typedef const typename Vector3::ConstantReturnType ConstLinearRef;
     typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
     typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
+    typedef typename traits<PlainType>::ExprType ExprType;
+    typedef typename traits<PlainType>::ConstExprType ConstExprType;
   }; // traits TransformHelicalTpl
 
   template<typename Scalar, int Options, int axis>

diff --git a/include/pinocchio/multibody/joint/joint-prismatic.hpp b/include/pinocchio/multibody/joint/joint-prismatic.hpp
@@ -220,6 +220,8 @@ namespace pinocchio
     typedef const Vector3 ConstLinearRef;
     typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
     typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
+    typedef typename traits<PlainType>::ExprType ExprType;
+    typedef typename traits<PlainType>::ConstExprType ConstExprType;
   }; // traits TransformPrismaticTpl
 
   template<typename Scalar, int Options, int axis>

diff --git a/include/pinocchio/multibody/joint/joint-revolute.hpp b/include/pinocchio/multibody/joint/joint-revolute.hpp
@@ -85,6 +85,8 @@ namespace pinocchio
     typedef const typename Vector3::ConstantReturnType ConstLinearRef;
     typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
     typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
+    typedef typename traits<PlainType>::ExprType ExprType;
+    typedef typename traits<PlainType>::ConstExprType ConstExprType;
   }; // traits TransformRevoluteTpl
 
   template<typename Scalar, int Options, int axis>

diff --git a/include/pinocchio/multibody/joint/joint-translation.hpp b/include/pinocchio/multibody/joint/joint-translation.hpp
@@ -216,6 +216,8 @@ namespace pinocchio
     typedef const LinearType & ConstLinearRef;
     typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
     typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
+    typedef typename traits<PlainType>::ExprType ExprType;
+    typedef typename traits<PlainType>::ConstExprType ConstExprType;
   }; // traits TransformTranslationTpl
 
   template<typename Scalar, int Options>

diff --git a/include/pinocchio/spatial/se3-base.hpp b/include/pinocchio/spatial/se3-base.hpp
@@ -214,6 +214,16 @@ namespace pinocchio
       derived().normalized();
     }
 
+    ExprType expr()
+    {
+      return derived().expr_impl();
+    }
+
+    ConstExprType const_expr() const
+    {
+      return derived().const_expr_impl();
+    }
+
   }; // struct SE3Base
 
 } // namespace pinocchio

diff --git a/include/pinocchio/spatial/se3-tpl.hpp b/include/pinocchio/spatial/se3-tpl.hpp
@@ -42,6 +42,8 @@ namespace pinocchio
     typedef Matrix6 ActionMatrixType;
     typedef Matrix4 HomogeneousMatrixType;
     typedef SE3Tpl<Scalar, Options> PlainType;
+    typedef SE3TplExpr<Scalar, Options> ExprType;
+    typedef SE3TplConstExpr<Scalar, Options> ConstExprType;
   }; // traits SE3Tpl
 
   template<typename _Scalar, int _Options>
@@ -404,6 +406,16 @@ namespace pinocchio
       return res;
     }
 
+    ExprType expr_impl()
+    {
+      return ExprType(*this);
+    }
+
+    ConstExprType const_expr_impl() const
+    {
+      return ConstExprType(*this);
+    }
+
     ///
     /// \brief Linear interpolation on the SE3 manifold.
     ///

diff --git a/include/pinocchio/spatial/se3.hpp b/include/pinocchio/spatial/se3.hpp
@@ -19,6 +19,8 @@
   typedef TYPENAME traits<Derived>::ActionMatrixType ActionMatrixType;                             \
   typedef TYPENAME traits<Derived>::HomogeneousMatrixType HomogeneousMatrixType;                   \
   typedef TYPENAME traits<Derived>::PlainType PlainType;                                           \
+  typedef TYPENAME traits<Derived>::ExprType ExprType;                                             \
+  typedef TYPENAME traits<Derived>::ConstExprType ConstExprType;                                   \
   enum                                                                                             \
   {                                                                                                \
     Options = traits<Derived>::Options,                                                            \