From 2f1d52d10fe2deccf6c72a30ac74ab1819b4f506 Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Fri, 11 Sep 2015 10:27:06 +0200
Subject: [PATCH] [C++] All joints now use spatial CRTP classes
---
src/multibody/joint/joint-planar.hpp | 249 ++++++++++--------
src/multibody/joint/joint-prismatic.hpp | 232 ++++++++++------
.../joint/joint-revolute-unaligned.hpp | 227 +++++++++-------
src/multibody/joint/joint-spherical.hpp | 192 +++++++++-----
src/multibody/joint/joint-translation.hpp | 210 +++++++++------
5 files changed, 677 insertions(+), 433 deletions(-)
diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp
index fb6d8b9cf..52d74bff5 100644
--- a/src/multibody/joint/joint-planar.hpp
+++ b/src/multibody/joint/joint-planar.hpp
@@ -30,126 +30,170 @@ namespace se3
struct JointDataPlanar;
struct JointModelPlanar;
- struct JointPlanar
+ struct MotionPlanar;
+ template <>
+ struct traits< MotionPlanar >
{
- struct BiasZero
- {
- operator Motion () const { return Motion::Zero(); }
- }; // struct BiasZero
+ typedef double Scalar_t;
+ typedef Eigen::Matrix<double,3,1,0> Vector3;
+ typedef Eigen::Matrix<double,4,1,0> Vector4;
+ typedef Eigen::Matrix<double,6,1,0> Vector6;
+ typedef Eigen::Matrix<double,3,3,0> Matrix3;
+ typedef Eigen::Matrix<double,4,4,0> Matrix4;
+ typedef Eigen::Matrix<double,6,6,0> Matrix6;
+ typedef Vector3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Eigen::Quaternion<double,0> Quaternion_t;
+ typedef SE3Tpl<double,0> SE3;
+ typedef ForceTpl<double,0> Force;
+ typedef MotionTpl<double,0> Motion;
+ typedef Symmetric3Tpl<double,0> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
+ };
+ }; // traits MotionPlanar
+
+ struct MotionPlanar : MotionBase < MotionPlanar >
+ {
+ SPATIAL_TYPEDEF_NO_TEMPLATE(MotionPlanar);
- friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
- friend const Motion & operator+ ( const BiasZero&,const Motion& v) { return v; }
+ MotionPlanar () : x_dot_(NAN), y_dot_(NAN), theta_dot_(NAN) {}
+ MotionPlanar (Scalar_t x_dot, Scalar_t y_dot, Scalar_t theta_dot) : x_dot_(x_dot), y_dot_(y_dot), theta_dot_(theta_dot) {}
+ Scalar_t x_dot_;
+ Scalar_t y_dot_;
+ Scalar_t theta_dot_;
- struct MotionPlanar
+ operator Motion() const
{
- typedef double Scalar_t;
- typedef MotionTpl<Scalar_t> Motion;
+ return Motion (Motion::Vector3(x_dot_, y_dot_, 0.), Motion::Vector3(0., 0., theta_dot_));
+ }
- Scalar_t x_dot_, y_dot_, theta_dot_;
+ }; // struct MotionPlanar
- MotionPlanar () : x_dot_(NAN), y_dot_(NAN), theta_dot_(NAN) {}
- MotionPlanar (Scalar_t x_dot, Scalar_t y_dot, Scalar_t theta_dot) : x_dot_(x_dot), y_dot_(y_dot), theta_dot_(theta_dot) {}
+ const MotionPlanar operator+ (const MotionPlanar & m, const BiasZero &)
+ { return m; }
- operator Motion() const
- {
- return Motion (Motion::Vector3(x_dot_, y_dot_, 0.), Motion::Vector3(0., 0., theta_dot_));
- }
- }; // struct MotionPlanar
+
+ Motion operator+ (const MotionPlanar & m1, const Motion & m2)
+ {
+ Motion result (m2);
+ result.linear ()[0] += m1.x_dot_;
+ result.linear ()[1] += m1.y_dot_;
- friend const MotionPlanar operator+ (const MotionPlanar & m, const BiasZero &)
- { return m; }
+ result.angular ()[2] += m1.theta_dot_;
- friend Motion operator+ (const MotionPlanar & m1, const Motion & m2)
- {
- Motion result (m2);
- result.linear ()[0] += m1.x_dot_;
- result.linear ()[1] += m1.y_dot_;
+ return result;
+ }
- result.angular ()[2] += m1.theta_dot_;
- return result;
- }
+ struct ConstraintPlanar;
+ template <>
+ struct traits < ConstraintPlanar >
+ {
+ typedef double Scalar_t;
+ typedef Eigen::Matrix<double,3,1,0> Vector3;
+ typedef Eigen::Matrix<double,4,1,0> Vector4;
+ typedef Eigen::Matrix<double,6,1,0> Vector6;
+ typedef Eigen::Matrix<double,3,3,0> Matrix3;
+ typedef Eigen::Matrix<double,4,4,0> Matrix4;
+ typedef Eigen::Matrix<double,6,6,0> Matrix6;
+ typedef Matrix3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Eigen::Quaternion<double,0> Quaternion_t;
+ typedef SE3Tpl<double,0> SE3;
+ typedef ForceTpl<double,0> Force;
+ typedef MotionTpl<double,0> Motion;
+ typedef Symmetric3Tpl<double,0> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
+ };
+ typedef Eigen::Matrix<Scalar_t,1,1,0> JointMotion;
+ typedef Eigen::Matrix<Scalar_t,1,1,0> JointForce;
+ typedef Eigen::Matrix<Scalar_t,6,1> DenseBase;
+ }; // struct traits ConstraintPlanar
- struct ConstraintPlanar
- {
- public:
- typedef double Scalar_t;
- typedef MotionTpl<Scalar_t> Motion;
- typedef ForceTpl<Scalar_t> Force;
- typedef Motion::Matrix3 Matrix3;
- typedef Motion::Vector3 Vector3;
- public:
+ struct ConstraintPlanar : ConstraintBase < ConstraintPlanar >
+ {
- Motion operator* (const MotionPlanar & vj) const
- { return vj; }
+ SPATIAL_TYPEDEF_NO_TEMPLATE(ConstraintPlanar);
+ typedef traits<ConstraintPlanar>::JointMotion JointMotion;
+ typedef traits<ConstraintPlanar>::JointForce JointForce;
+ typedef traits<ConstraintPlanar>::DenseBase DenseBase;
- struct ConstraintTranspose
- {
- const ConstraintPlanar & ref;
- ConstraintTranspose(const ConstraintPlanar & ref) : ref(ref) {}
-
- Force::Vector3 operator* (const Force & phi)
- {
- return Force::Vector3 (phi.linear()[0], phi.linear()[1], phi.angular()[2]);
- }
-
- /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
- template<typename D>
- friend typename Eigen::Matrix <typename Eigen::MatrixBase<D>::Scalar, 3, -1>
- operator*( const ConstraintTranspose &, const Eigen::MatrixBase<D> & F )
- {
- typedef Eigen::Matrix<typename Eigen::MatrixBase<D>::Scalar, 3, -1> MatrixReturnType;
- assert(F.rows()==6);
-
- MatrixReturnType result (3, F.cols ());
- result.template topRows <2> () = F.template topRows <2> ();
- result.template bottomRows <1> () = F.template bottomRows <1> ();
- return result;
- }
- }; // struct ConstraintTranspose
-
- ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
-
- operator ConstraintXd () const
+ Motion operator* (const MotionPlanar & vj) const
+ { return vj; }
+
+
+ struct ConstraintTranspose
+ {
+ const ConstraintPlanar & ref;
+ ConstraintTranspose(const ConstraintPlanar & ref) : ref(ref) {}
+
+ Force::Vector3 operator* (const Force & phi)
{
- Eigen::Matrix<Scalar_t,6,3> S;
- S.block <3,3> (Inertia::LINEAR, 0).setZero ();
- S.block <3,3> (Inertia::ANGULAR, 0).setZero ();
- S(Inertia::LINEAR + 0,0) = 1.;
- S(Inertia::LINEAR + 1,1) = 1.;
- S(Inertia::ANGULAR + 2,2) = 1.;
- return ConstraintXd(S);
+ return Force::Vector3 (phi.linear()[0], phi.linear()[1], phi.angular()[2]);
}
- Eigen::Matrix <Scalar_t,6,3> se3Action (const SE3 & m) const
+ /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
+ template<typename D>
+ friend typename Eigen::Matrix <typename Eigen::MatrixBase<D>::Scalar, 3, -1>
+ operator*( const ConstraintTranspose &, const Eigen::MatrixBase<D> & F )
{
- Eigen::Matrix <double,6,3> X_subspace;
- X_subspace.block <3,2> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ().leftCols <2> ();
- X_subspace.block <3,1> (Motion::LINEAR, 2) = m.rotation ().rightCols <1> ();
-
- X_subspace.block <3,2> (Motion::ANGULAR, 0) = m.rotation ().leftCols <2> ();
- X_subspace.block <3,1> (Motion::ANGULAR, 2).setZero ();
+ typedef Eigen::Matrix<typename Eigen::MatrixBase<D>::Scalar, 3, -1> MatrixReturnType;
+ assert(F.rows()==6);
- return X_subspace;
+ MatrixReturnType result (3, F.cols ());
+ result.template topRows <2> () = F.template topRows <2> ();
+ result.template bottomRows <1> () = F.template bottomRows <1> ();
+ return result;
}
+ }; // struct ConstraintTranspose
- }; // struct ConstraintPlanar
+ ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
- template<typename D>
- friend Motion operator* (const ConstraintPlanar &, const Eigen::MatrixBase<D> & v)
+ operator ConstraintXd () const
{
- EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
- Motion result (Motion::Zero ());
- result.linear ().template head<2> () = v.template topRows<2> ();
- result.angular ().template tail<1> () = v.template bottomRows<1> ();
- return result;
+ Eigen::Matrix<Scalar_t,6,3> S;
+ S.block <3,3> (Inertia::LINEAR, 0).setZero ();
+ S.block <3,3> (Inertia::ANGULAR, 0).setZero ();
+ S(Inertia::LINEAR + 0,0) = 1.;
+ S(Inertia::LINEAR + 1,1) = 1.;
+ S(Inertia::ANGULAR + 2,2) = 1.;
+ return ConstraintXd(S);
+ }
+
+ Eigen::Matrix <Scalar_t,6,3> se3Action (const SE3 & m) const
+ {
+ Eigen::Matrix <double,6,3> X_subspace;
+ X_subspace.block <3,2> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ().leftCols <2> ();
+ X_subspace.block <3,1> (Motion::LINEAR, 2) = m.rotation ().rightCols <1> ();
+
+ X_subspace.block <3,2> (Motion::ANGULAR, 0) = m.rotation ().leftCols <2> ();
+ X_subspace.block <3,1> (Motion::ANGULAR, 2).setZero ();
+
+ return X_subspace;
}
- }; // struct JointPlanar
+ }; // struct ConstraintPlanar
- Motion operator^ (const Motion & m1, const JointPlanar::MotionPlanar & m2)
+ template<typename D>
+ Motion operator* (const ConstraintPlanar &, const Eigen::MatrixBase<D> & v)
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
+ Motion result (Motion::Zero ());
+ result.linear ().template head<2> () = v.template topRows<2> ();
+ result.angular ().template tail<1> () = v.template bottomRows<1> ();
+ return result;
+ }
+
+
+ Motion operator^ (const Motion & m1, const MotionPlanar & m2)
{
Motion result;
@@ -163,7 +207,7 @@ namespace se3
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
- Eigen::Matrix <Inertia::Scalar_t, 6, 3> operator* (const Inertia & Y, const JointPlanar::ConstraintPlanar &)
+ Eigen::Matrix <Inertia::Scalar_t, 6, 3> operator* (const Inertia & Y, const ConstraintPlanar &)
{
Eigen::Matrix <Inertia::Scalar_t, 6, 3> M;
const double mass = Y.mass ();
@@ -187,19 +231,20 @@ namespace se3
namespace internal
{
template<>
- struct ActionReturn<JointPlanar::ConstraintPlanar >
+ struct ActionReturn<ConstraintPlanar >
{ typedef Eigen::Matrix<double,6,3> Type; };
}
+ struct JointPlanar;
template<>
struct traits<JointPlanar>
{
typedef JointDataPlanar JointData;
typedef JointModelPlanar JointModel;
- typedef JointPlanar::ConstraintPlanar Constraint_t;
+ typedef ConstraintPlanar Constraint_t;
typedef SE3 Transformation_t;
- typedef JointPlanar::MotionPlanar Motion_t;
- typedef JointPlanar::BiasZero Bias_t;
+ typedef MotionPlanar Motion_t;
+ typedef BiasZero Bias_t;
typedef Eigen::Matrix<double,6,3> F_t;
enum {
NQ = 3,
@@ -213,12 +258,6 @@ namespace se3
{
typedef JointPlanar Joint;
SE3_JOINT_TYPEDEF;
-
- typedef Motion::Scalar_t Scalar;
-
- typedef Eigen::Matrix<Scalar,6,6> Matrix6;
- typedef Eigen::Matrix<Scalar,3,3> Matrix3;
- typedef Eigen::Matrix<Scalar,3,1> Vector3;
Constraint_t S;
Transformation_t M;
@@ -226,7 +265,7 @@ namespace se3
Bias_t c;
JointDataPlanar () : M(1) {}
- };
+ }; // struct JointDataPlanar
struct JointModelPlanar : public JointModelBase<JointModelPlanar>
{
@@ -263,7 +302,7 @@ namespace se3
data.v.y_dot_ = q_dot(1);
data.v.theta_dot_ = q_dot(2);
}
- };
+ }; // struct JointModelPlanar
} // namespace se3
diff --git a/src/multibody/joint/joint-prismatic.hpp b/src/multibody/joint/joint-prismatic.hpp
index 207e9fbeb..abba76968 100644
--- a/src/multibody/joint/joint-prismatic.hpp
+++ b/src/multibody/joint/joint-prismatic.hpp
@@ -36,11 +36,12 @@ namespace se3
double v;
CartesianVector3(const double & v) : v(v) {}
CartesianVector3() : v(NAN) {}
- operator Eigen::Vector3d () const; // { return Eigen::Vector3d(w,0,0); }
- };
+ operator Eigen::Vector3d () const;
+ }; // struct CartesianVector3
template<>CartesianVector3<0>::operator Eigen::Vector3d () const { return Eigen::Vector3d(v,0,0); }
template<>CartesianVector3<1>::operator Eigen::Vector3d () const { return Eigen::Vector3d(0,v,0); }
template<>CartesianVector3<2>::operator Eigen::Vector3d () const { return Eigen::Vector3d(0,0,v); }
+
Eigen::Vector3d operator+ (const Eigen::Vector3d & v1,const CartesianVector3<0> & vx)
{ return Eigen::Vector3d(v1[0]+vx.v,v1[1],v1[2]); }
Eigen::Vector3d operator+ (const Eigen::Vector3d & v1,const CartesianVector3<1> & vy)
@@ -49,74 +50,130 @@ namespace se3
{ return Eigen::Vector3d(v1[0],v1[1],v1[2]+vz.v); }
} // namespace prismatic
- template<int axis>
- struct JointPrismatic
+ template <int axis> struct MotionPrismatic;
+ template<int axis>
+ struct traits <MotionPrismatic < axis > >
{
- struct BiasZero
- {
- operator Motion () const { return Motion::Zero(); }
+ typedef double Scalar_t;
+ typedef Eigen::Matrix<double,3,1,0> Vector3;
+ typedef Eigen::Matrix<double,4,1,0> Vector4;
+ typedef Eigen::Matrix<double,6,1,0> Vector6;
+ typedef Eigen::Matrix<double,3,3,0> Matrix3;
+ typedef Eigen::Matrix<double,4,4,0> Matrix4;
+ typedef Eigen::Matrix<double,6,6,0> Matrix6;
+ typedef Vector3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Eigen::Quaternion<double,0> Quaternion_t;
+ typedef SE3Tpl<double,0> SE3;
+ typedef ForceTpl<double,0> Force;
+ typedef MotionTpl<double,0> Motion;
+ typedef Symmetric3Tpl<double,0> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
};
- friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
- friend const Motion & operator+ ( const BiasZero&,const Motion& v) { return v; }
+ }; // struct traits MotionPrismatic
- struct MotionPrismatic
- {
- MotionPrismatic() : v(NAN) {}
- MotionPrismatic( const double & v ) : v(v) {}
- double v;
-
- operator Motion() const
- {
- return Motion((Motion::Vector3)typename prismatic::CartesianVector3<axis>(v),
- Motion::Vector3::Zero());
- }
- }; // struct MotionPrismatic
+ template<int axis>
+ struct MotionPrismatic : MotionBase < MotionPrismatic < axis > >
+ {
+ SPATIAL_TYPEDEF_TEMPLATE(MotionPrismatic);
- friend const MotionPrismatic& operator+ (const MotionPrismatic& m, const BiasZero&)
- { return m; }
+ MotionPrismatic() : v(NAN) {}
+ MotionPrismatic( const double & v ) : v(v) {}
+ double v;
- friend Motion operator+( const MotionPrismatic& m1, const Motion& m2)
- {
- return Motion( m2.linear()+typename prismatic::CartesianVector3<axis>(m1.v),m2.angular());
- }
- struct ConstraintPrismatic
+ operator Motion() const
{
- template<typename D>
- MotionPrismatic operator*( const Eigen::MatrixBase<D> & v ) const
- {
-// EIGEN_STATIC_ASSERT_SIZE_1x1(D); // There is actually a bug in Eigen with such a macro
- return MotionPrismatic(v[0]);
- }
+ return Motion((Vector3)typename prismatic::CartesianVector3<axis>(v),
+ Motion::Vector3::Zero()
+ );
+ }
+ }; // struct MotionPrismatic
- Eigen::Matrix<double,6,1> se3Action(const SE3 & m) const
- {
- Eigen::Matrix<double,6,1> res;
- res.head<3>() = m.rotation().col(axis);
- res.tail<3>() = Motion::Vector3::Zero(); // Eigen::Vector3d::Zero() ?
- return res;
- }
+ template <int axis>
+ const MotionPrismatic<axis>& operator+ (const MotionPrismatic<axis>& m, const BiasZero&)
+ { return m; }
- struct TransposeConst
- {
- const ConstraintPrismatic & ref;
- TransposeConst(const ConstraintPrismatic & ref) : ref(ref) {}
+ template <int axis>
+ Motion operator+( const MotionPrismatic<axis>& m1, const Motion& m2)
+ {
+ return Motion( m2.linear()+typename prismatic::CartesianVector3<axis>(m1.v),m2.angular());
+ }
- Force::Vector3::ConstFixedSegmentReturnType<1>::Type
- operator*( const Force& f ) const
- { return f.linear().segment<1>(axis); }
+ template<int axis> struct ConstraintPrismatic;
+ template<int axis>
+ struct traits< ConstraintPrismatic<axis> >
+ {
+ typedef double Scalar_t;
+ typedef Eigen::Matrix<double,3,1,0> Vector3;
+ typedef Eigen::Matrix<double,4,1,0> Vector4;
+ typedef Eigen::Matrix<double,6,1,0> Vector6;
+ typedef Eigen::Matrix<double,3,3,0> Matrix3;
+ typedef Eigen::Matrix<double,4,4,0> Matrix4;
+ typedef Eigen::Matrix<double,6,6,0> Matrix6;
+ typedef Matrix3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Eigen::Quaternion<double,0> Quaternion_t;
+ typedef SE3Tpl<double,0> SE3;
+ typedef ForceTpl<double,0> Force;
+ typedef MotionTpl<double,0> Motion;
+ typedef Symmetric3Tpl<double,0> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
+ };
+ typedef Eigen::Matrix<Scalar_t,1,1,0> JointMotion;
+ typedef Eigen::Matrix<Scalar_t,1,1,0> JointForce;
+ typedef Eigen::Matrix<Scalar_t,6,1> DenseBase;
+ }; // traits ConstraintRevolute
- /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
- template<typename D>
- friend typename Eigen::MatrixBase<D>::ConstRowXpr
- operator*( const TransposeConst &, const Eigen::MatrixBase<D> & F )
- {
- assert(F.rows()==6);
- return F.row(axis);
- }
+ template <int axis>
+ struct ConstraintPrismatic : ConstraintBase < ConstraintPrismatic <axis > >
+ {
+ SPATIAL_TYPEDEF_TEMPLATE(ConstraintPrismatic);
+ enum { NV = 1, Options = 0 };
+ typedef typename traits<ConstraintPrismatic>::JointMotion JointMotion;
+ typedef typename traits<ConstraintPrismatic>::JointForce JointForce;
+ typedef typename traits<ConstraintPrismatic>::DenseBase DenseBase;
+
+ template<typename D>
+ MotionPrismatic<axis> operator*( const Eigen::MatrixBase<D> & v ) const
+ {
+// EIGEN_STATIC_ASSERT_SIZE_1x1(D); // There is actually a bug in Eigen with such a macro
+ return MotionPrismatic<axis>(v[0]);
+ }
- };
- TransposeConst transpose() const { return TransposeConst(*this); }
+ Eigen::Matrix<double,6,1> se3Action(const SE3 & m) const
+ {
+ Eigen::Matrix<double,6,1> res;
+ res.head<3>() = m.rotation().col(axis);
+ res.tail<3>() = Motion::Vector3::Zero();
+ return res;
+ }
+
+ struct TransposeConst
+ {
+ const ConstraintPrismatic & ref;
+ TransposeConst(const ConstraintPrismatic<axis> & ref) : ref(ref) {}
+ typename Force::Vector3::template ConstFixedSegmentReturnType<1>::Type
+ operator*( const Force& f ) const
+ { return f.linear().template segment<1>(axis); }
+
+ /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
+ template<typename D>
+ friend typename Eigen::MatrixBase<D>::ConstRowXpr
+ operator*( const TransposeConst &, const Eigen::MatrixBase<D> & F )
+ {
+ assert(F.rows()==6);
+ return F.row(axis);
+ }
+
+ }; // struct TransposeConst
+ TransposeConst transpose() const { return TransposeConst(*this); }
/* CRBA joint operators
* - ForceSet::Block = ForceSet
@@ -124,18 +181,26 @@ namespace se3
* - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
* - SE3::act(ForceSet::Block)
*/
- operator ConstraintXd () const
- {
- Eigen::Matrix<double,6,1> S;
- S << (Eigen::Vector3d)prismatic::CartesianVector3<axis>(), Eigen::Vector3d::Zero() ;
- return ConstraintXd(S);
- }
- }; // struct ConstraintPrismatic
+ operator ConstraintXd () const
+ {
+ Eigen::Matrix<double,6,1> S;
+ S << (Eigen::Vector3d)prismatic::CartesianVector3<axis>(), Eigen::Vector3d::Zero() ;
+ return ConstraintXd(S);
+ }
+
+ }; // struct ConstraintPrismatic
+
+
+
+
+ template<int axis>
+ struct JointPrismatic
+ {
static Eigen::Vector3d cartesianTranslation(const double & shift);
};
- Motion operator^( const Motion& m1, const JointPrismatic<0>::MotionPrismatic& m2)
+ Motion operator^( const Motion& m1, const MotionPrismatic<0>& m2)
{
/* nu1^nu2 = ( v1^w2+w1^v2, w1^w2 )
* nu1^(v2,0) = ( w1^v2 , 0 )
@@ -148,7 +213,7 @@ namespace se3
Motion::Vector3::Zero());
}
- Motion operator^( const Motion& m1, const JointPrismatic<1>::MotionPrismatic& m2)
+ Motion operator^( const Motion& m1, const MotionPrismatic<1>& m2)
{
/* nu1^nu2 = ( v1^w2+w1^v2, w1^w2 )
* nu1^(v2,0) = ( w1^v2 , 0 )
@@ -161,7 +226,7 @@ namespace se3
Motion::Vector3::Zero());
}
- Motion operator^( const Motion& m1, const JointPrismatic<2>::MotionPrismatic& m2)
+ Motion operator^( const Motion& m1, const MotionPrismatic<2>& m2)
{
/* nu1^nu2 = ( v1^w2+w1^v2, w1^w2 )
* nu1^(v2,0) = ( w1^v2 , 0 )
@@ -192,7 +257,7 @@ namespace se3
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
Eigen::Matrix<double,6,1>
- operator*( const Inertia& Y,const JointPrismatic<0>::ConstraintPrismatic & )
+ operator*( const Inertia& Y,const ConstraintPrismatic<0> & )
{
/* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */
const double
@@ -207,7 +272,7 @@ namespace se3
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
Eigen::Matrix<double,6,1>
- operator*( const Inertia& Y,const JointPrismatic<1>::ConstraintPrismatic & )
+ operator*( const Inertia& Y,const ConstraintPrismatic<1> & )
{
/* Y(:,1) = ( 0,1, 0, -z , 0 , x) */
const double
@@ -222,7 +287,7 @@ namespace se3
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
Eigen::Matrix<double,6,1>
- operator*( const Inertia& Y,const JointPrismatic<2>::ConstraintPrismatic & )
+ operator*( const Inertia& Y,const ConstraintPrismatic<2> & )
{
/* Y(:,2) = ( 0,0, 1, y , -x , 0) */
const double
@@ -238,15 +303,8 @@ namespace se3
namespace internal
{
- // TODO: I am not able to write the next three lines as a template. Why?
- template<>
- struct ActionReturn<JointPrismatic<0>::ConstraintPrismatic >
- { typedef Eigen::Matrix<double,6,1> Type; };
- template<>
- struct ActionReturn<JointPrismatic<1>::ConstraintPrismatic >
- { typedef Eigen::Matrix<double,6,1> Type; };
- template<>
- struct ActionReturn<JointPrismatic<2>::ConstraintPrismatic >
+ template<int axis>
+ struct ActionReturn<ConstraintPrismatic<axis> >
{ typedef Eigen::Matrix<double,6,1> Type; };
}
@@ -257,10 +315,10 @@ namespace se3
{
typedef JointDataPrismatic<axis> JointData;
typedef JointModelPrismatic<axis> JointModel;
- typedef typename JointPrismatic<axis>::ConstraintPrismatic Constraint_t;
+ typedef ConstraintPrismatic<axis> Constraint_t;
typedef SE3 Transformation_t;
- typedef typename JointPrismatic<axis>::MotionPrismatic Motion_t;
- typedef typename JointPrismatic<axis>::BiasZero Bias_t;
+ typedef MotionPrismatic<axis> Motion_t;
+ typedef BiasZero Bias_t;
typedef Eigen::Matrix<double,6,1> F_t;
enum {
NQ = 1,
@@ -273,7 +331,7 @@ namespace se3
template<int axis>
struct JointDataPrismatic : public JointDataBase< JointDataPrismatic<axis> >
- { //TODO : check.
+ {
typedef JointPrismatic<axis> Joint;
SE3_JOINT_TYPEDEF_TEMPLATE;
@@ -288,11 +346,11 @@ namespace se3
{
M.rotation(SE3::Matrix3::Identity());
}
- };
+ }; // struct JointDataPrismatic
template<int axis>
struct JointModelPrismatic : public JointModelBase< JointModelPrismatic<axis> >
- { //TODO
+ {
typedef JointPrismatic<axis> Joint;
SE3_JOINT_TYPEDEF_TEMPLATE;
@@ -318,7 +376,7 @@ namespace se3
data.M.translation(JointPrismatic<axis>::cartesianTranslation(q));
data.v.v = v;
}
- };
+ }; // struct JointModelPrismatic
typedef JointPrismatic<0> JointPX;
typedef JointDataPrismatic<0> JointDataPX;
diff --git a/src/multibody/joint/joint-revolute-unaligned.hpp b/src/multibody/joint/joint-revolute-unaligned.hpp
index 60bd910c3..770cd2b3b 100644
--- a/src/multibody/joint/joint-revolute-unaligned.hpp
+++ b/src/multibody/joint/joint-revolute-unaligned.hpp
@@ -28,50 +28,101 @@ namespace se3
struct JointDataRevoluteUnaligned;
struct JointModelRevoluteUnaligned;
-
- struct JointRevoluteUnaligned {
- struct BiasZero
- {
- operator Motion () const { return Motion::Zero(); }
+
+ struct MotionRevoluteUnaligned;
+ template <>
+ struct traits < MotionRevoluteUnaligned >
+ {
+ typedef double Scalar_t;
+ typedef Eigen::Matrix<double,3,1,0> Vector3;
+ typedef Eigen::Matrix<double,4,1,0> Vector4;
+ typedef Eigen::Matrix<double,6,1,0> Vector6;
+ typedef Eigen::Matrix<double,3,3,0> Matrix3;
+ typedef Eigen::Matrix<double,4,4,0> Matrix4;
+ typedef Eigen::Matrix<double,6,6,0> Matrix6;
+ typedef Vector3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Eigen::Quaternion<double,0> Quaternion_t;
+ typedef SE3Tpl<double,0> SE3;
+ typedef ForceTpl<double,0> Force;
+ typedef MotionTpl<double,0> Motion;
+ typedef Symmetric3Tpl<double,0> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
};
- friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
- friend const Motion & operator+ ( const BiasZero&,const Motion& v) { return v; }
+ }; // traits MotionRevoluteUnaligned
- struct MotionRevoluteUnaligned
- {
- // Empty constructor needed for now to avoid compilation errors
- MotionRevoluteUnaligned() : axis(Motion::Vector3::Constant(NAN)), w(NAN) {}
- MotionRevoluteUnaligned( const Motion::Vector3 & axis, const double & w ) : axis(axis), w(w) {}
+ struct MotionRevoluteUnaligned : MotionBase < MotionRevoluteUnaligned >
+ {
+ SPATIAL_TYPEDEF_NO_TEMPLATE(MotionRevoluteUnaligned);
- operator Motion() const
- {
- return Motion(Motion::Vector3::Zero(),
- axis*w);
- }
+ MotionRevoluteUnaligned() : axis(Motion::Vector3::Constant(NAN)), w(NAN) {}
+ MotionRevoluteUnaligned( const Motion::Vector3 & axis, const double & w ) : axis(axis), w(w) {}
- Motion::Vector3 axis;
- double w;
- }; // struct MotionRevoluteUnaligned
+ Motion::Vector3 axis;
+ double w;
- friend const MotionRevoluteUnaligned& operator+ (const MotionRevoluteUnaligned& m, const BiasZero&)
- { return m; }
+ operator Motion() const
+ {
+ return Motion(Motion::Vector3::Zero(),
+ axis*w);
+ }
+ }; // struct MotionRevoluteUnaligned
+
+ const MotionRevoluteUnaligned& operator+ (const MotionRevoluteUnaligned& m, const BiasZero&)
+ { return m; }
- friend Motion operator+ (const MotionRevoluteUnaligned& m1, const Motion& m2)
+ Motion operator+ (const MotionRevoluteUnaligned& m1, const Motion& m2)
+ {
+ return Motion( m2.linear(), m1.w*m1.axis+m2.angular() );
+ }
+
+ struct ConstraintRevoluteUnaligned;
+ template <>
+ struct traits < ConstraintRevoluteUnaligned >
+ {
+ typedef double Scalar_t;
+ typedef Eigen::Matrix<double,3,1,0> Vector3;
+ typedef Eigen::Matrix<double,4,1,0> Vector4;
+ typedef Eigen::Matrix<double,6,1,0> Vector6;
+ typedef Eigen::Matrix<double,3,3,0> Matrix3;
+ typedef Eigen::Matrix<double,4,4,0> Matrix4;
+ typedef Eigen::Matrix<double,6,6,0> Matrix6;
+ typedef Matrix3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Eigen::Quaternion<double,0> Quaternion_t;
+ typedef SE3Tpl<double,0> SE3;
+ typedef ForceTpl<double,0> Force;
+ typedef MotionTpl<double,0> Motion;
+ typedef Symmetric3Tpl<double,0> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
+ };
+ typedef Eigen::Matrix<Scalar_t,1,1,0> JointMotion;
+ typedef Eigen::Matrix<Scalar_t,1,1,0> JointForce;
+ typedef Eigen::Matrix<Scalar_t,6,1> DenseBase;
+ }; // traits ConstraintRevoluteUnaligned
+
+
+
+
+ struct ConstraintRevoluteUnaligned : ConstraintBase < ConstraintRevoluteUnaligned >
{
- return Motion( m2.linear(), m1.w*m1.axis+m2.angular() );
- }
+ SPATIAL_TYPEDEF_NO_TEMPLATE(ConstraintRevoluteUnaligned);
- struct ConstraintRevoluteUnaligned
- {
- // Empty constructor needed to avoid compilation error for now.
ConstraintRevoluteUnaligned() : axis(Eigen::Vector3d::Constant(NAN)) {}
ConstraintRevoluteUnaligned(const Motion::Vector3 & _axis) : axis(_axis) {}
-
- template<typename D> //todo : check operator is good
+ Motion::Vector3 axis;
+
+ template<typename D>
MotionRevoluteUnaligned operator*( const Eigen::MatrixBase<D> & v ) const
{
- EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,1);
- return MotionRevoluteUnaligned(axis,v[0]);
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,1);
+ return MotionRevoluteUnaligned(axis,v[0]);
}
Eigen::Matrix<double,6,1> se3Action(const SE3 & m) const
@@ -85,14 +136,14 @@ namespace se3
struct TransposeConst
{
- const ConstraintRevoluteUnaligned & ref;
- TransposeConst(const ConstraintRevoluteUnaligned & ref) : ref(ref) {}
+ const ConstraintRevoluteUnaligned & ref;
+ TransposeConst(const ConstraintRevoluteUnaligned & ref) : ref(ref) {}
- const Eigen::Matrix<double, 1, 1>
- operator*( const Force& f ) const
- {
- return ref.axis.transpose()*f.angular();
- }
+ const Eigen::Matrix<double, 1, 1>
+ operator*( const Force& f ) const
+ {
+ return ref.axis.transpose()*f.angular();
+ }
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
template<typename D>
@@ -119,62 +170,61 @@ namespace se3
*/
operator ConstraintXd () const
{
- Eigen::Matrix<double,6,1> S;
- S << Eigen::Vector3d::Zero(), axis;
- return ConstraintXd(S);
+ Eigen::Matrix<double,6,1> S;
+ S << Eigen::Vector3d::Zero(), axis;
+ return ConstraintXd(S);
}
- Motion::Vector3 axis;
+
}; // struct ConstraintRevoluteUnaligned
- }; // struct JoinRevoluteUnaligned
- Motion operator^( const Motion& m1, const JointRevoluteUnaligned::MotionRevoluteUnaligned & m2)
- {
- /* m1xm2 = [ v1xw2 + w1xv2; w1xw2 ] = [ v1xw2; w1xw2 ] */
- const Motion::Vector3& v1 = m1.linear();
- const Motion::Vector3& w1 = m1.angular();
- const Motion::Vector3& w2 = m2.axis * m2.w ;
- return Motion( v1.cross(w2),w1.cross(w2));
- }
+ Motion operator^( const Motion& m1, const MotionRevoluteUnaligned & m2)
+ {
+ /* m1xm2 = [ v1xw2 + w1xv2; w1xw2 ] = [ v1xw2; w1xw2 ] */
+ const Motion::Vector3& v1 = m1.linear();
+ const Motion::Vector3& w1 = m1.angular();
+ const Motion::Vector3& w2 = m2.axis * m2.w ;
+ return Motion( v1.cross(w2),w1.cross(w2));
+ }
- /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
- Eigen::Matrix<double,6,1>
- operator*( const Inertia& Y,const JointRevoluteUnaligned::ConstraintRevoluteUnaligned & cru)
- {
- /* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
- const double &m = Y.mass();
- const Inertia::Vector3 & c = Y.lever();
- const Inertia::Symmetric3 & I = Y.inertia();
-
- Eigen::Matrix<double,6,1> res;
- res.head<3>() = -m*c.cross(cru.axis);
- res.tail<3>() = I*cru.axis + c.cross(res.head<3>());
- return res;
- }
+ /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
+ Eigen::Matrix<double,6,1>
+ operator*( const Inertia& Y,const ConstraintRevoluteUnaligned & cru)
+ {
+ /* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
+ const double &m = Y.mass();
+ const Inertia::Vector3 & c = Y.lever();
+ const Inertia::Symmetric3 & I = Y.inertia();
+
+ Eigen::Matrix<double,6,1> res;
+ res.head<3>() = -m*c.cross(cru.axis);
+ res.tail<3>() = I*cru.axis + c.cross(res.head<3>());
+ return res;
+ }
- namespace internal
- {
- template<>
- struct ActionReturn<JointRevoluteUnaligned::ConstraintRevoluteUnaligned >
- { typedef Eigen::Matrix<double,6,1> Type; };
- }
-
+ namespace internal
+ {
+ template<>
+ struct ActionReturn<ConstraintRevoluteUnaligned >
+ { typedef Eigen::Matrix<double,6,1> Type; };
+ }
- template<>
- struct traits< JointRevoluteUnaligned >
- {
- typedef JointDataRevoluteUnaligned JointData;
- typedef JointModelRevoluteUnaligned JointModel;
- typedef JointRevoluteUnaligned::ConstraintRevoluteUnaligned Constraint_t;
- typedef SE3 Transformation_t;
- typedef JointRevoluteUnaligned::MotionRevoluteUnaligned Motion_t;
- typedef JointRevoluteUnaligned::BiasZero Bias_t;
- typedef Eigen::Matrix<double,6,1> F_t;
- enum {
- NQ = 1,
- NV = 1
+ struct JointRevoluteUnaligned;
+ template<>
+ struct traits< JointRevoluteUnaligned >
+ {
+ typedef JointDataRevoluteUnaligned JointData;
+ typedef JointModelRevoluteUnaligned JointModel;
+ typedef ConstraintRevoluteUnaligned Constraint_t;
+ typedef SE3 Transformation_t;
+ typedef MotionRevoluteUnaligned Motion_t;
+ typedef BiasZero Bias_t;
+ typedef Eigen::Matrix<double,6,1> F_t;
+ enum {
+ NQ = 1,
+ NV = 1
+ };
};
- };
template<> struct traits<JointDataRevoluteUnaligned> { typedef JointRevoluteUnaligned Joint; };
template<> struct traits<JointModelRevoluteUnaligned> { typedef JointRevoluteUnaligned Joint; };
@@ -192,7 +242,6 @@ namespace se3
F_t F;
Eigen::AngleAxisd angleaxis;
- /* The empty constructor is needed for the variant. */
JointDataRevoluteUnaligned()
: M(1),S(Eigen::Vector3d::Constant(NAN)),v(Eigen::Vector3d::Constant(NAN),NAN)
, angleaxis( NAN,Eigen::Vector3d::Constant(NAN))
@@ -212,7 +261,6 @@ namespace se3
using JointModelBase<JointModelRevoluteUnaligned>::idx_v;
using JointModelBase<JointModelRevoluteUnaligned>::setIndexes;
- /* The empty constructor is needed for the variant. */
JointModelRevoluteUnaligned() : axis(Eigen::Vector3d::Constant(NAN)) {}
JointModelRevoluteUnaligned( const Motion::Vector3 & axis ) : axis(axis)
{
@@ -247,7 +295,6 @@ namespace se3
data.v.w = v;
}
- //protected: TODO
Eigen::Vector3d axis;
};
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index f8d5a3ad4..f74dcf0e3 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -30,94 +30,146 @@ namespace se3
struct JointDataSpherical;
struct JointModelSpherical;
- struct JointSpherical
+ struct MotionSpherical;
+ template <>
+ struct traits< MotionSpherical >
{
- struct BiasZero
- {
- operator Motion () const { return Motion::Zero(); }
- }; // struct BiasZero
+ typedef double Scalar_t;
+ typedef Eigen::Matrix<double,3,1,0> Vector3;
+ typedef Eigen::Matrix<double,4,1,0> Vector4;
+ typedef Eigen::Matrix<double,6,1,0> Vector6;
+ typedef Eigen::Matrix<double,3,3,0> Matrix3;
+ typedef Eigen::Matrix<double,4,4,0> Matrix4;
+ typedef Eigen::Matrix<double,6,6,0> Matrix6;
+ typedef Vector3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Eigen::Quaternion<double,0> Quaternion_t;
+ typedef SE3Tpl<double,0> SE3;
+ typedef ForceTpl<double,0> Force;
+ typedef MotionTpl<double,0> Motion;
+ typedef Symmetric3Tpl<double,0> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
+ };
+ }; // traits MotionSpherical
+
+ struct MotionSpherical : MotionBase < MotionSpherical >
+ {
+ SPATIAL_TYPEDEF_NO_TEMPLATE(MotionSpherical);
- friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
- friend const Motion & operator+ ( const BiasZero&,const Motion& v) { return v; }
+ MotionSpherical () : w (Motion::Vector3(NAN, NAN, NAN)) {}
+ MotionSpherical (const Motion::Vector3 & w) : w (w) {}
+ Motion::Vector3 w;
- struct MotionSpherical
+ Motion::Vector3 & operator() () { return w; }
+ const Motion::Vector3 & operator() () const { return w; }
+
+ operator Motion() const
{
- MotionSpherical () : w (Motion::Vector3(NAN, NAN, NAN)) {}
- MotionSpherical (const Motion::Vector3 & w) : w (w) {}
- Motion::Vector3 w;
+ return Motion (Motion::Vector3::Zero (), w);
+ }
+ }; // struct MotionSpherical
- Motion::Vector3 & operator() () { return w; }
- const Motion::Vector3 & operator() () const { return w; }
+ const MotionSpherical operator+ (const MotionSpherical & m, const BiasZero & )
+ { return m; }
- operator Motion() const
- {
- return Motion (Motion::Vector3::Zero (), w);
- }
- }; // struct MotionSpherical
+ Motion operator+ (const MotionSpherical & m1, const Motion & m2)
+ {
+ return Motion( m2.linear(), m2.angular() + m1.w);
+ }
- friend const MotionSpherical operator+ (const MotionSpherical & m, const BiasZero & )
- { return m; }
+ struct ConstraintRotationalSubspace;
+ template <>
+ struct traits < struct ConstraintRotationalSubspace >
+ {
+ typedef double Scalar_t;
+ typedef Eigen::Matrix<double,3,1,0> Vector3;
+ typedef Eigen::Matrix<double,4,1,0> Vector4;
+ typedef Eigen::Matrix<double,6,1,0> Vector6;
+ typedef Eigen::Matrix<double,3,3,0> Matrix3;
+ typedef Eigen::Matrix<double,4,4,0> Matrix4;
+ typedef Eigen::Matrix<double,6,6,0> Matrix6;
+ typedef Matrix3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Eigen::Quaternion<double,0> Quaternion_t;
+ typedef SE3Tpl<double,0> SE3;
+ typedef ForceTpl<double,0> Force;
+ typedef MotionTpl<double,0> Motion;
+ typedef Symmetric3Tpl<double,0> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
+ };
+ typedef Eigen::Matrix<Scalar_t,1,1,0> JointMotion;
+ typedef Eigen::Matrix<Scalar_t,1,1,0> JointForce;
+ typedef Eigen::Matrix<Scalar_t,6,1> DenseBase;
+ }; // struct traits struct ConstraintRotationalSubspace
- friend Motion operator+ (const MotionSpherical & m1, const Motion & m2)
- {
- return Motion( m2.linear(), m2.angular() + m1.w);
- }
+ struct ConstraintRotationalSubspace
+ {
+ SPATIAL_TYPEDEF_NO_TEMPLATE(ConstraintRotationalSubspace);
+ typedef traits<ConstraintRotationalSubspace>::JointMotion JointMotion;
+ typedef traits<ConstraintRotationalSubspace>::JointForce JointForce;
+ typedef traits<ConstraintRotationalSubspace>::DenseBase DenseBase;
- struct ConstraintRotationalSubspace
+ /// Missing operator*
+ // Motion operator* (const MotionSpherical & vj) const
+ // { return ??; }
+
+ struct TransposeConst
{
+ Force::Vector3 operator* (const Force & phi)
+ { return phi.angular (); }
- struct TransposeConst
- {
- Force::Vector3 operator* (const Force & phi)
- { return phi.angular (); }
-
- /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
- template<typename D>
- friend typename Eigen::Matrix <typename Eigen::MatrixBase<D>::Scalar, 3, -1>
- operator*( const TransposeConst &, const Eigen::MatrixBase<D> & F )
- {
- assert(F.rows()==6);
- return F.template middleRows <3> (Inertia::ANGULAR);
- }
- };
-
- TransposeConst transpose() const { return TransposeConst(); }
-
- operator ConstraintXd () const
+ /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
+ template<typename D>
+ friend typename Eigen::Matrix <typename Eigen::MatrixBase<D>::Scalar, 3, -1>
+ operator*( const TransposeConst &, const Eigen::MatrixBase<D> & F )
{
- Eigen::Matrix<double,6,3> S;
- S.block <3,3> (Inertia::LINEAR, 0).setZero ();
- S.block <3,3> (Inertia::ANGULAR, 0).setIdentity ();
- return ConstraintXd(S);
+ assert(F.rows()==6);
+ return F.template middleRows <3> (Inertia::ANGULAR);
}
+ };
- Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const
- {
- Eigen::Matrix <double,6,3> X_subspace;
- X_subspace.block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ();
- X_subspace.block <3,3> (Motion::ANGULAR, 0) = m.rotation ();
-
- return X_subspace;
- }
+ TransposeConst transpose() const { return TransposeConst(); }
- }; // struct ConstraintRotationalSubspace
+ operator ConstraintXd () const
+ {
+ Eigen::Matrix<double,6,3> S;
+ S.block <3,3> (Inertia::LINEAR, 0).setZero ();
+ S.block <3,3> (Inertia::ANGULAR, 0).setIdentity ();
+ return ConstraintXd(S);
+ }
- template<typename D>
- friend Motion operator* (const ConstraintRotationalSubspace&, const Eigen::MatrixBase<D>& v)
+ Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const
{
- EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
- return Motion (Motion::Vector3::Zero (), v);
+ Eigen::Matrix <double,6,3> X_subspace;
+ X_subspace.block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ();
+ X_subspace.block <3,3> (Motion::ANGULAR, 0) = m.rotation ();
+
+ return X_subspace;
}
- }; // struct JointSpherical
+ }; // struct ConstraintRotationalSubspace
+
+ template<typename D>
+ Motion operator* (const ConstraintRotationalSubspace&, const Eigen::MatrixBase<D>& v)
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
+ return Motion (Motion::Vector3::Zero (), v);
+ }
+
- Motion operator^ (const Motion & m1, const JointSpherical::MotionSpherical & m2)
+ Motion operator^ (const Motion & m1, const MotionSpherical & m2)
{
return Motion(m1.linear ().cross (m2.w), m1.angular ().cross (m2.w));
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
- Eigen::Matrix <double, 6, 3> operator* (const Inertia & Y, const JointSpherical::ConstraintRotationalSubspace & )
+ Eigen::Matrix <double, 6, 3> operator* (const Inertia & Y, const ConstraintRotationalSubspace & )
{
Eigen::Matrix <double, 6, 3> M;
// M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
@@ -129,20 +181,20 @@ namespace se3
namespace internal
{
template<>
- struct ActionReturn<JointSpherical::ConstraintRotationalSubspace >
+ struct ActionReturn<ConstraintRotationalSubspace >
{ typedef Eigen::Matrix<double,6,3> Type; };
}
-
+ struct JointSpherical;
template<>
struct traits<JointSpherical>
{
typedef JointDataSpherical JointData;
typedef JointModelSpherical JointModel;
- typedef JointSpherical::ConstraintRotationalSubspace Constraint_t;
+ typedef ConstraintRotationalSubspace Constraint_t;
typedef SE3 Transformation_t;
- typedef JointSpherical::MotionSpherical Motion_t;
- typedef JointSpherical::BiasZero Bias_t;
+ typedef MotionSpherical Motion_t;
+ typedef BiasZero Bias_t;
typedef Eigen::Matrix<double,6,3> F_t;
enum {
NQ = 4,
@@ -168,7 +220,7 @@ namespace se3
JointDataSpherical () : M(1)
{}
- };
+ }; // struct JointDataSpherical
struct JointModelSpherical : public JointModelBase<JointModelSpherical>
{
@@ -197,7 +249,7 @@ namespace se3
const JointData::Quaternion quat(Eigen::Matrix<double,4,1> (q.tail <4> ())); // TODO
data.M.rotation (quat.matrix ());
}
- };
+ }; // struct JointModelSpherical
} // namespace se3
diff --git a/src/multibody/joint/joint-translation.hpp b/src/multibody/joint/joint-translation.hpp
index beb77e08b..0ac6209bd 100644
--- a/src/multibody/joint/joint-translation.hpp
+++ b/src/multibody/joint/joint-translation.hpp
@@ -29,113 +29,160 @@ namespace se3
struct JointDataTranslation;
struct JointModelTranslation;
- struct JointTranslation
+ struct MotionTranslation;
+ template <>
+ struct traits < MotionTranslation >
{
- struct BiasZero
- {
- operator Motion () const { return Motion::Zero(); }
- }; // struct BiasZero
-
- friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
- friend const Motion & operator+ ( const BiasZero&,const Motion& v) { return v; }
-
- struct MotionTranslation
- {
- MotionTranslation () : v (Motion::Vector3 (NAN, NAN, NAN)) {}
- MotionTranslation (const Motion::Vector3 & v) : v (v) {}
- MotionTranslation (const MotionTranslation & other) : v (other.v) {}
- Motion::Vector3 v;
-
- Motion::Vector3 & operator() () { return v; }
- const Motion::Vector3 & operator() () const { return v; }
+ typedef double Scalar_t;
+ typedef Eigen::Matrix<double,3,1,0> Vector3;
+ typedef Eigen::Matrix<double,4,1,0> Vector4;
+ typedef Eigen::Matrix<double,6,1,0> Vector6;
+ typedef Eigen::Matrix<double,3,3,0> Matrix3;
+ typedef Eigen::Matrix<double,4,4,0> Matrix4;
+ typedef Eigen::Matrix<double,6,6,0> Matrix6;
+ typedef Vector3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Eigen::Quaternion<double,0> Quaternion_t;
+ typedef SE3Tpl<double,0> SE3;
+ typedef ForceTpl<double,0> Force;
+ typedef MotionTpl<double,0> Motion;
+ typedef Symmetric3Tpl<double,0> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
+ };
+ }; // traits MotionTranslation
- operator Motion() const
- {
- return Motion (v, Motion::Vector3::Zero ());
- }
+ struct MotionTranslation : MotionBase < MotionTranslation >
+ {
+ SPATIAL_TYPEDEF_NO_TEMPLATE(MotionTranslation);
- MotionTranslation & operator= (const MotionTranslation & other)
- {
- v = other.v;
- return *this;
- }
- }; // struct MotionTranslation
+ MotionTranslation () : v (Motion::Vector3 (NAN, NAN, NAN)) {}
+ MotionTranslation (const Motion::Vector3 & v) : v (v) {}
+ MotionTranslation (const MotionTranslation & other) : v (other.v) {}
+ Vector3 v;
- friend const MotionTranslation operator+ (const MotionTranslation & m, const BiasZero &)
- { return m; }
+ Vector3 & operator() () { return v; }
+ const Vector3 & operator() () const { return v; }
- friend Motion operator+ (const MotionTranslation & m1, const Motion & m2)
+ operator Motion() const
{
- return Motion (m2.linear () + m1.v, m2.angular ());
+ return Motion (v, Motion::Vector3::Zero ());
}
- struct ConstraintTranslationSubspace
+ MotionTranslation & operator= (const MotionTranslation & other)
{
- public:
+ v = other.v;
+ return *this;
+ }
+ }; // struct MotionTranslation
- Motion operator* (const MotionTranslation & vj) const
- { return Motion (vj (), Motion::Vector3::Zero ()); }
+ const MotionTranslation operator+ (const MotionTranslation & m, const BiasZero &)
+ { return m; }
- ConstraintTranslationSubspace () {}
+ Motion operator+ (const MotionTranslation & m1, const Motion & m2)
+ {
+ return Motion (m2.linear () + m1.v, m2.angular ());
+ }
- struct ConstraintTranspose
- {
- const ConstraintTranslationSubspace & ref;
- ConstraintTranspose(const ConstraintTranslationSubspace & ref) : ref(ref) {}
+ struct ConstraintTranslationSubspace;
+ template <>
+ struct traits < ConstraintTranslationSubspace>
+ {
+ typedef double Scalar_t;
+ typedef Eigen::Matrix<double,3,1,0> Vector3;
+ typedef Eigen::Matrix<double,4,1,0> Vector4;
+ typedef Eigen::Matrix<double,6,1,0> Vector6;
+ typedef Eigen::Matrix<double,3,3,0> Matrix3;
+ typedef Eigen::Matrix<double,4,4,0> Matrix4;
+ typedef Eigen::Matrix<double,6,6,0> Matrix6;
+ typedef Matrix3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Eigen::Quaternion<double,0> Quaternion_t;
+ typedef SE3Tpl<double,0> SE3;
+ typedef ForceTpl<double,0> Force;
+ typedef MotionTpl<double,0> Motion;
+ typedef Symmetric3Tpl<double,0> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
+ };
+ typedef Eigen::Matrix<Scalar_t,1,1,0> JointMotion;
+ typedef Eigen::Matrix<Scalar_t,1,1,0> JointForce;
+ typedef Eigen::Matrix<Scalar_t,6,1> DenseBase;
+ }; // traits ConstraintTranslationSubspace
- Force::Vector3 operator* (const Force & phi)
- {
- return phi.linear ();
- }
+ struct ConstraintTranslationSubspace : ConstraintBase < ConstraintTranslationSubspace >
+ {
+ SPATIAL_TYPEDEF_NO_TEMPLATE(ConstraintTranslationSubspace);
+ typedef traits<ConstraintTranslationSubspace>::JointMotion JointMotion;
+ typedef traits<ConstraintTranslationSubspace>::JointForce JointForce;
+ typedef traits<ConstraintTranslationSubspace>::DenseBase DenseBase;
+ ConstraintTranslationSubspace () {}
+ Motion operator* (const MotionTranslation & vj) const
+ { return Motion (vj (), Motion::Vector3::Zero ()); }
- /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
- template<typename D>
- friend typename Eigen::Matrix <typename Eigen::MatrixBase<D>::Scalar, 3, -1>
- operator*( const ConstraintTranspose &, const Eigen::MatrixBase<D> & F )
- {
- assert(F.rows()==6);
- return F.template middleRows <3> (Inertia::LINEAR);
- }
- }; // struct ConstraintTranspose
- ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
+ struct ConstraintTranspose
+ {
+ const ConstraintTranslationSubspace & ref;
+ ConstraintTranspose(const ConstraintTranslationSubspace & ref) : ref(ref) {}
- operator ConstraintXd () const
+ Force::Vector3 operator* (const Force & phi)
{
- Eigen::Matrix<double,6,3> S;
- S.block <3,3> (Inertia::LINEAR, 0).setIdentity ();
- S.block <3,3> (Inertia::ANGULAR, 0).setZero ();
- return ConstraintXd(S);
+ return phi.linear ();
}
- Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const
- {
- Eigen::Matrix <double,6,3> M;
- M.block <3,3> (Motion::LINEAR, 0) = m.rotation ();
- M.block <3,3> (Motion::ANGULAR, 0).setZero ();
- return M;
+ /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
+ template<typename D>
+ friend typename Eigen::Matrix <typename Eigen::MatrixBase<D>::Scalar, 3, -1>
+ operator*( const ConstraintTranspose &, const Eigen::MatrixBase<D> & F )
+ {
+ assert(F.rows()==6);
+ return F.template middleRows <3> (Inertia::LINEAR);
}
+ }; // struct ConstraintTranspose
- }; // struct ConstraintTranslationSubspace
+ ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
- template<typename D>
- friend Motion operator* (const ConstraintTranslationSubspace &, const Eigen::MatrixBase<D> & v)
+ operator ConstraintXd () const
{
- EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
- return Motion (v, Motion::Vector3::Zero ());
+ Eigen::Matrix<double,6,3> S;
+ S.block <3,3> (Inertia::LINEAR, 0).setIdentity ();
+ S.block <3,3> (Inertia::ANGULAR, 0).setZero ();
+ return ConstraintXd(S);
}
- }; // struct JointTranslation
+ Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const
+ {
+ Eigen::Matrix <double,6,3> M;
+ M.block <3,3> (Motion::LINEAR, 0) = m.rotation ();
+ M.block <3,3> (Motion::ANGULAR, 0).setZero ();
+
+ return M;
+ }
+
+ }; // struct ConstraintTranslationSubspace
+
+ template<typename D>
+ Motion operator* (const ConstraintTranslationSubspace &, const Eigen::MatrixBase<D> & v)
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
+ return Motion (v, Motion::Vector3::Zero ());
+ }
+
- Motion operator^ (const Motion & m1, const JointTranslation::MotionTranslation & m2)
+ Motion operator^ (const Motion & m1, const MotionTranslation & m2)
{
return Motion (m1.angular ().cross (m2.v), Motion::Vector3::Zero ());
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
- Eigen::Matrix <double, 6, 3> operator* (const Inertia & Y, const JointTranslation::ConstraintTranslationSubspace &)
+ Eigen::Matrix <double, 6, 3> operator* (const Inertia & Y, const ConstraintTranslationSubspace &)
{
Eigen::Matrix <double, 6, 3> M;
M.block <3,3> (Inertia::ANGULAR, 0) = alphaSkew(Y.mass (), Y.lever ());
@@ -149,26 +196,27 @@ namespace se3
namespace internal
{
template<>
- struct ActionReturn<JointTranslation::ConstraintTranslationSubspace >
+ struct ActionReturn<ConstraintTranslationSubspace >
{ typedef Eigen::Matrix<double,6,3> Type; };
}
+ struct JointTranslation;
template<>
struct traits<JointTranslation>
{
typedef JointDataTranslation JointData;
typedef JointModelTranslation JointModel;
- typedef JointTranslation::ConstraintTranslationSubspace Constraint_t;
+ typedef ConstraintTranslationSubspace Constraint_t;
typedef SE3 Transformation_t;
- typedef JointTranslation::MotionTranslation Motion_t;
- typedef JointTranslation::BiasZero Bias_t;
+ typedef MotionTranslation Motion_t;
+ typedef BiasZero Bias_t;
typedef Eigen::Matrix<double,6,3> F_t;
enum {
NQ = 3,
NV = 3
};
- };
+ }; // traits JointTranslation
template<> struct traits<JointDataTranslation> { typedef JointTranslation Joint; };
template<> struct traits<JointModelTranslation> { typedef JointTranslation Joint; };
@@ -188,7 +236,7 @@ namespace se3
Bias_t c;
JointDataTranslation () : M(1) {}
- };
+ }; // struct JointDataTranslation
struct JointModelTranslation : public JointModelBase<JointModelTranslation>
{
@@ -209,7 +257,7 @@ namespace se3
data.M.translation (qs.segment<NQ> (idx_q ()));
data.v () = vs.segment<NQ> (idx_v ());
}
- };
+ }; // struct JointModelTranslation
} // namespace se3
--
GitLab