Skip to content

Commit 6bb1d16

Browse files
jorisvjorisv
committed
spatial: Add SE3ExprBase and SE3TplExpr to use Eigen evaluation to compute SE3 transformation
1 parent 69f8b2e commit 6bb1d16

File tree

4 files changed

+268
-0
lines changed

4 files changed

+268
-0
lines changed

‎include/pinocchio/spatial/fwd.hpp‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,21 @@ namespace pinocchio
3838
template<typename Scalar, int Options = context::Options>
3939
struct SE3Tpl;
4040

41+
template<typename Derived>
42+
struct SE3ExprBase;
43+
template<typename Derived>
44+
struct SE3ExprNoalias;
45+
template<typename RotProduct, typename TransProduct>
46+
struct SE3ExprProduct;
47+
template<typename RotProduct, typename TransProduct>
48+
SE3ExprProduct<RotProduct, TransProduct>
49+
make_se3_expr_product(RotProduct rot_prod, TransProduct trans_prod);
50+
51+
template<typename _Scalar, int _Options>
52+
struct SE3TplExpr;
53+
template<typename _Scalar, int _Options>
54+
struct SE3TplConstExpr;
55+
4156
template<typename Derived>
4257
class MotionBase;
4358
template<typename Derived>

‎include/pinocchio/spatial/se3-expr-base.hpp‎

Lines changed: 156 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,156 @@
1+
//
2+
// Copyright (c) 2025 INRIA
3+
//
4+
5+
#ifndef __pinocchio_spatial_se3_expr_base_hpp__
6+
#define __pinocchio_spatial_se3_expr_base_hpp__
7+
8+
namespace pinocchio
9+
{
10+
11+
// Forward traits typedef
12+
#define PINOCCHIO_SE3_EXPR_TYPEDEF_TPL(Derived) \
13+
typedef typename traits<Derived>::Scalar Scalar; \
14+
typedef typename traits<Derived>::AngularType AngularType; \
15+
typedef typename traits<Derived>::LinearType LinearType; \
16+
typedef typename traits<Derived>::AngularRef AngularRef; \
17+
typedef typename traits<Derived>::LinearRef LinearRef; \
18+
typedef typename traits<Derived>::ConstAngularRef ConstAngularRef; \
19+
typedef typename traits<Derived>::ConstLinearRef ConstLinearRef; \
20+
typedef typename traits<Derived>::PlainType PlainType; \
21+
enum \
22+
{ \
23+
Options = traits<Derived>::Options, \
24+
}
25+
26+
// Use Eigen3 expression to optimize SE3 operations.
27+
template<typename Derived>
28+
struct SE3ExprBase
29+
{
30+
PINOCCHIO_SE3_EXPR_TYPEDEF_TPL(Derived);
31+
32+
Derived & derived()
33+
{
34+
return *static_cast<Derived *>(this);
35+
}
36+
const Derived & derived() const
37+
{
38+
return *static_cast<const Derived *>(this);
39+
}
40+
41+
ConstAngularRef rotation() const
42+
{
43+
return derived().rotation_impl();
44+
}
45+
ConstLinearRef translation() const
46+
{
47+
return derived().translation_impl();
48+
}
49+
AngularRef rotation()
50+
{
51+
return derived().rotation_impl();
52+
}
53+
LinearRef translation()
54+
{
55+
return derived().translation_impl();
56+
}
57+
58+
template<typename OtherDerived>
59+
auto operator*(const SE3ExprBase<OtherDerived> & other) const
60+
{
61+
return make_se3_expr_product(
62+
rotation() * other.rotation(), translation() + rotation() * other.translation());
63+
}
64+
65+
template<typename OtherDerived>
66+
Derived & operator=(const SE3ExprBase<OtherDerived> & other)
67+
{
68+
rotation() = other.rotation();
69+
translation() = other.translation();
70+
return derived();
71+
}
72+
73+
SE3ExprNoalias<Derived> noalias()
74+
{
75+
return SE3ExprNoalias<Derived>(derived());
76+
}
77+
};
78+
79+
// SE3Noalias use noalias in operator=
80+
template<typename Derived>
81+
struct SE3ExprNoalias
82+
{
83+
SE3ExprNoalias(Derived & expr)
84+
: expr(expr)
85+
{
86+
}
87+
88+
template<typename OtherDerived>
89+
Derived & operator=(const SE3ExprBase<OtherDerived> & other)
90+
{
91+
expr.rotation().noalias() = other.rotation();
92+
expr.translation().noalias() = other.translation();
93+
return expr;
94+
}
95+
96+
Derived & expr;
97+
};
98+
99+
template<typename RotProduct, typename TransProduct>
100+
struct traits<SE3ExprProduct<RotProduct, TransProduct>>
101+
{
102+
enum
103+
{
104+
Options = 0
105+
};
106+
typedef double Scalar;
107+
typedef RotProduct AngularType;
108+
typedef RotProduct & AngularRef;
109+
typedef const RotProduct & ConstAngularRef;
110+
typedef TransProduct LinearType;
111+
typedef TransProduct & LinearRef;
112+
typedef const TransProduct & ConstLinearRef;
113+
typedef SE3ExprProduct<RotProduct, TransProduct> PlainType;
114+
};
115+
116+
template<typename RotProduct, typename TransProduct>
117+
struct SE3ExprProduct : SE3ExprBase<SE3ExprProduct<RotProduct, TransProduct>>
118+
{
119+
PINOCCHIO_SE3_EXPR_TYPEDEF_TPL(SE3ExprProduct);
120+
121+
SE3ExprProduct(RotProduct rot_prod, TransProduct trans_prod)
122+
: rot_prod(rot_prod)
123+
, trans_prod(trans_prod)
124+
{
125+
}
126+
127+
ConstAngularRef rotation_impl() const
128+
{
129+
return rot_prod;
130+
}
131+
ConstLinearRef translation_impl() const
132+
{
133+
return trans_prod;
134+
}
135+
AngularRef rotation_impl()
136+
{
137+
return rot_prod;
138+
}
139+
LinearRef translation_impl()
140+
{
141+
return trans_prod;
142+
}
143+
144+
RotProduct rot_prod;
145+
TransProduct trans_prod;
146+
};
147+
148+
template<typename RotProduct, typename TransProduct>
149+
inline SE3ExprProduct<RotProduct, TransProduct>
150+
make_se3_expr_product(RotProduct rot_prod, TransProduct trans_prod)
151+
{
152+
return SE3ExprProduct<RotProduct, TransProduct>(rot_prod, trans_prod);
153+
}
154+
} // namespace pinocchio
155+
156+
#endif // __pinocchio_spatial_se3_expr_base_hpp__

‎include/pinocchio/spatial/se3-tpl.hpp‎

Lines changed: 96 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -451,6 +451,102 @@ namespace pinocchio
451451

452452
} // namespace internal
453453

454+
template<typename _Scalar, int _Options>
455+
struct traits<SE3TplExpr<_Scalar, _Options>>
456+
{
457+
enum
458+
{
459+
Options = 0
460+
};
461+
typedef _Scalar Scalar;
462+
typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
463+
typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
464+
typedef Matrix3 AngularType;
465+
typedef Matrix3 & AngularRef;
466+
typedef const Matrix3 & ConstAngularRef;
467+
typedef Vector3 LinearType;
468+
typedef Vector3 & LinearRef;
469+
typedef const Vector3 & ConstLinearRef;
470+
typedef SE3Tpl<Scalar, Options> PlainType;
471+
};
472+
473+
template<typename _Scalar, int _Options>
474+
struct SE3TplExpr : SE3ExprBase<SE3TplExpr<_Scalar, _Options>>
475+
{
476+
PINOCCHIO_SE3_EXPR_TYPEDEF_TPL(SE3TplExpr);
477+
478+
SE3TplExpr(PlainType & se3)
479+
: se3(se3)
480+
{
481+
}
482+
483+
ConstAngularRef rotation_impl() const
484+
{
485+
return se3.rotation();
486+
}
487+
ConstLinearRef translation_impl() const
488+
{
489+
return se3.translation();
490+
}
491+
AngularRef rotation_impl()
492+
{
493+
return se3.rotation();
494+
}
495+
LinearRef translation_impl()
496+
{
497+
return se3.translation();
498+
}
499+
500+
template<typename OtherDerived>
501+
SE3TplExpr & operator=(const SE3ExprBase<OtherDerived> & other)
502+
{
503+
return SE3ExprBase<SE3TplExpr>::operator=(other);
504+
}
505+
506+
PlainType & se3;
507+
};
508+
509+
template<typename _Scalar, int _Options>
510+
struct traits<SE3TplConstExpr<_Scalar, _Options>>
511+
{
512+
enum
513+
{
514+
Options = 0
515+
};
516+
typedef _Scalar Scalar;
517+
typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
518+
typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
519+
typedef Matrix3 AngularType;
520+
typedef Matrix3 & AngularRef;
521+
typedef const Matrix3 & ConstAngularRef;
522+
typedef Vector3 LinearType;
523+
typedef Vector3 & LinearRef;
524+
typedef const Vector3 & ConstLinearRef;
525+
typedef SE3Tpl<Scalar, Options> PlainType;
526+
};
527+
528+
template<typename _Scalar, int _Options>
529+
struct SE3TplConstExpr : SE3ExprBase<SE3TplConstExpr<_Scalar, _Options>>
530+
{
531+
PINOCCHIO_SE3_EXPR_TYPEDEF_TPL(SE3TplConstExpr);
532+
533+
SE3TplConstExpr(const PlainType & se3)
534+
: se3(se3)
535+
{
536+
}
537+
538+
ConstAngularRef rotation_impl() const
539+
{
540+
return se3.rotation();
541+
}
542+
ConstLinearRef translation_impl() const
543+
{
544+
return se3.translation();
545+
}
546+
547+
const PlainType & se3;
548+
};
549+
454550
} // namespace pinocchio
455551

456552
#endif // ifndef __pinocchio_spatial_se3_tpl_hpp__

‎include/pinocchio/spatial/se3.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,7 @@ namespace pinocchio
4444
} // namespace pinocchio
4545

4646
#include "pinocchio/spatial/se3-base.hpp"
47+
#include "pinocchio/spatial/se3-expr-base.hpp"
4748
#include "pinocchio/spatial/se3-tpl.hpp"
4849

4950
#endif // ifndef __pinocchio_spatial_se3_hpp__

0 commit comments

Comments
 (0)