Commit d7d7915d authored by jcarpent's avatar jcarpent
Browse files

[Joint] Add Options to JointPrismatic{.,Unaligned}

parent b83105b2
......@@ -43,11 +43,14 @@ namespace se3
template<typename Scalar, int Options = 0> struct JointDataSphericalZYXTpl;
typedef JointDataSphericalZYXTpl<double> JointDataSphericalZYX;
template<typename Scalar, int axis> struct JointModelPrismatic;
template<typename Scalar, int axis> struct JointDataPrismatic;
template<typename Scalar, int Options, int axis> struct JointModelPrismatic;
template<typename Scalar, int Options, int axis> struct JointDataPrismatic;
template<typename Scalar> struct JointModelPrismaticUnalignedTpl;
template<typename Scalar> struct JointDataPrismaticUnalignedTpl;
template<typename Scalar, int Options = 0> struct JointModelPrismaticUnalignedTpl;
typedef JointModelPrismaticUnalignedTpl<double> JointModelPrismaticUnaligned;
template<typename Scalar, int Options = 0> struct JointDataPrismaticUnalignedTpl;
typedef JointDataPrismaticUnalignedTpl<double> JointDataPrismaticUnaligned;
template<typename Scalar, int Options = 0> struct JointModelFreeFlyerTpl;
typedef JointModelFreeFlyerTpl<double> JointModelFreeFlyer;
......
......@@ -27,16 +27,16 @@
namespace se3
{
template<typename Scalar>
struct MotionPrismaticUnaligned;
template<typename Scalar, int Options> struct MotionPrismaticUnaligned;
template<typename _Scalar>
struct traits< MotionPrismaticUnaligned<_Scalar> >
template<typename _Scalar, int _Options>
struct traits< MotionPrismaticUnaligned<_Scalar,_Options> >
{
typedef _Scalar Scalar;
typedef Eigen::Matrix<Scalar,3,1,0> Vector3;
typedef Eigen::Matrix<Scalar,6,1,0> Vector6;
typedef Eigen::Matrix<Scalar,6,6,0> Matrix6;
enum { Options = _Options };
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
typedef typename EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef typename EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef Vector3 AngularType;
......@@ -44,15 +44,15 @@ namespace se3
typedef const Vector3 ConstAngularType;
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<Scalar,0> MotionPlain;
typedef MotionTpl<Scalar,Options> MotionPlain;
enum {
LINEAR = 0,
ANGULAR = 3
};
}; // traits MotionPrismaticUnaligned
template<typename _Scalar>
struct MotionPrismaticUnaligned : MotionBase < MotionPrismaticUnaligned<_Scalar> >
template<typename _Scalar, int _Options>
struct MotionPrismaticUnaligned : MotionBase < MotionPrismaticUnaligned<_Scalar,_Options> >
{
MOTION_TYPEDEF_TPL(MotionPrismaticUnaligned);
......@@ -72,77 +72,79 @@ namespace se3
v.linear() += axis * rate;
}
// data
Vector3 axis;
Scalar rate;
}; // struct MotionPrismaticUnaligned
template<typename Scalar, typename MotionDerived>
inline typename MotionDerived::MotionPlain operator+ (const MotionPrismaticUnaligned<Scalar> & m1, const MotionDense<MotionDerived> & m2)
template<typename Scalar, int Options, typename MotionDerived>
inline typename MotionDerived::MotionPlain
operator+(const MotionPrismaticUnaligned<Scalar,Options> & m1, const MotionDense<MotionDerived> & m2)
{
typedef typename MotionDerived::MotionPlain ReturnType;
return ReturnType(m1.rate*m1.axis + m2.linear(), m2.angular());
}
template<typename Scalar>
struct ConstraintPrismaticUnaligned;
template<typename Scalar, int Options> struct ConstraintPrismaticUnaligned;
template<typename _Scalar>
struct traits< ConstraintPrismaticUnaligned<_Scalar> >
template<typename _Scalar, int _Options>
struct traits< ConstraintPrismaticUnaligned<_Scalar,_Options> >
{
typedef _Scalar Scalar;
typedef Eigen::Matrix<Scalar,3,1,0> Vector3;
typedef Eigen::Matrix<Scalar,4,1,0> Vector4;
typedef Eigen::Matrix<Scalar,6,1,0> Vector6;
typedef Eigen::Matrix<Scalar,3,3,0> Matrix3;
typedef Eigen::Matrix<Scalar,4,4,0> Matrix4;
typedef Eigen::Matrix<Scalar,6,6,0> Matrix6;
enum { Options = _Options };
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
typedef Matrix3 Angular_t;
typedef Vector3 Linear_t;
typedef const Matrix3 ConstAngular_t;
typedef const Vector3 ConstLinear_t;
typedef Matrix6 ActionMatrix_t;
typedef Eigen::Quaternion<Scalar,0> Quaternion_t;
typedef SE3Tpl<Scalar,0> SE3;
typedef ForceTpl<Scalar,0> Force;
typedef MotionTpl<Scalar,0> Motion;
typedef Symmetric3Tpl<Scalar,0> Symmetric3;
typedef Eigen::Quaternion<Scalar,Options> Quaternion_t;
typedef SE3Tpl<Scalar,Options> SE3;
typedef ForceTpl<Scalar,Options> Force;
typedef MotionTpl<Scalar,Options> Motion;
typedef Symmetric3Tpl<Scalar,Options> Symmetric3;
enum {
LINEAR = 0,
ANGULAR = 3
};
typedef Eigen::Matrix<Scalar,1,1,0> JointMotion;
typedef Eigen::Matrix<Scalar,1,1,0> JointForce;
typedef Eigen::Matrix<Scalar,6,1> DenseBase;
typedef Eigen::Matrix<Scalar,1,1,Options> JointMotion;
typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
typedef DenseBase MatrixReturnType;
typedef const DenseBase ConstMatrixReturnType;
}; // traits ConstraintPrismaticUnaligned
template<typename _Scalar>
struct ConstraintPrismaticUnaligned : ConstraintBase < ConstraintPrismaticUnaligned<_Scalar> >
template<typename _Scalar, int _Options>
struct ConstraintPrismaticUnaligned : ConstraintBase< ConstraintPrismaticUnaligned<_Scalar,_Options> >
{
SPATIAL_TYPEDEF_TEMPLATE(ConstraintPrismaticUnaligned);
enum { NV = 1, Options = 0 };
enum { NV = 1, Options = _Options };
typedef typename traits<ConstraintPrismaticUnaligned>::JointMotion JointMotion;
typedef typename traits<ConstraintPrismaticUnaligned>::JointForce JointForce;
typedef typename traits<ConstraintPrismaticUnaligned>::DenseBase DenseBase;
ConstraintPrismaticUnaligned () : axis (Vector3::Constant(NAN)) {}
ConstraintPrismaticUnaligned() : axis(Vector3::Constant(NAN)) {}
template<typename Vector3Like>
ConstraintPrismaticUnaligned (const Eigen::MatrixBase<Vector3Like> & axis)
ConstraintPrismaticUnaligned(const Eigen::MatrixBase<Vector3Like> & axis)
: axis(axis)
{ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
template<typename Derived>
MotionPrismaticUnaligned<Scalar> operator* (const Eigen::MatrixBase<Derived> & v) const
MotionPrismaticUnaligned<Scalar,Options> operator*(const Eigen::MatrixBase<Derived> & v) const
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,1);
return MotionPrismaticUnaligned<Scalar>(axis,v[0]);
return MotionPrismaticUnaligned<Scalar,Options>(axis,v[0]);
}
template<typename S1, int O1>
Vector6 se3Action (const SE3Tpl<S1,O1> & m) const
Vector6 se3Action(const SE3Tpl<S1,O1> & m) const
{
Vector6 res;
res.template head<3>().noalias() = m.rotation()*axis;
......@@ -222,27 +224,28 @@ namespace se3
return res;
}
// data
Vector3 axis;
}; // struct ConstraintPrismaticUnaligned
template<typename MotionDerived, typename Scalar>
template<typename MotionDerived, typename S2, int O2>
inline typename MotionDerived::MotionPlain
operator^(const MotionDense<MotionDerived> & m1,
const MotionPrismaticUnaligned<Scalar> & m2)
const MotionPrismaticUnaligned<S2,O2> & m2)
{
typedef typename MotionDerived::MotionPlain ReturnType;
/* m1xm2 = [ v1xw2 + w1xv2; w1xw2 ] = [ v1xw2; w1xw2 ] */
const typename MotionDerived::ConstAngularType & w1 = m1.angular();
const typename MotionPrismaticUnaligned<Scalar>::Vector3 & v2 = m2.axis * m2.rate;
const typename MotionPrismaticUnaligned<S2,O2>::Vector3 & v2 = m2.axis * m2.rate;
return ReturnType(w1.cross(v2), ReturnType::Vector3::Zero());
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
template<typename S1, int O1, typename S2>
template<typename S1, int O1, typename S2, int O2>
inline Eigen::Matrix<S1,6,1>
operator* (const InertiaTpl<S1,O1> & Y, const ConstraintPrismaticUnaligned<S2> & cpu)
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPrismaticUnaligned<S2,O2> & cpu)
{
typedef InertiaTpl<S1,O1> Inertia;
/* YS = [ m -mcx ; mcx I-mcxcx ] [ v ; 0 ] = [ mv ; mcxv ] */
......@@ -256,19 +259,19 @@ namespace se3
}
/* [ABA] Y*S operator (Inertia Y,Constraint S) */
template<typename M6, typename S2>
template<typename M6, typename S2, int O2>
#if EIGEN_VERSION_AT_LEAST(3,2,90)
const typename Eigen::Product<
Eigen::Block<const M6,6,3>,
typename ConstraintPrismaticUnaligned<S2>::Vector3,
typename ConstraintPrismaticUnaligned<S2,O2>::Vector3,
Eigen::DefaultProduct>
#else
const typename Eigen::ProductReturnType<
Eigen::Block<const M6,6,3>,
const typename ConstraintPrismaticUnaligned<S2>::Vector3
const typename ConstraintPrismaticUnaligned<S2,O2>::Vector3
>::Type
#endif
operator*(const Eigen::MatrixBase<M6> & Y, const ConstraintPrismaticUnaligned<S2> & cpu)
operator*(const Eigen::MatrixBase<M6> & Y, const ConstraintPrismaticUnaligned<S2,O2> & cpu)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
return Y.template block<6,3> (0,Inertia::LINEAR) * cpu.axis;
......@@ -277,49 +280,51 @@ namespace se3
namespace internal
{
template<typename Scalar>
struct SE3GroupAction< ConstraintPrismaticUnaligned<Scalar> >
{ typedef Eigen::Matrix<Scalar,6,1> ReturnType; };
template<typename Scalar, int Options>
struct SE3GroupAction< ConstraintPrismaticUnaligned<Scalar,Options> >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, typename MotionDerived>
struct MotionAlgebraAction< ConstraintPrismaticUnaligned<Scalar>,MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1> ReturnType; };
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< ConstraintPrismaticUnaligned<Scalar,Options>,MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
}
template<typename Scalar> struct JointPrismaticUnalignedTpl;
template<typename Scalar, int Options> struct JointPrismaticUnalignedTpl;
template<typename _Scalar>
struct traits< JointPrismaticUnalignedTpl<_Scalar> >
template<typename _Scalar, int _Options>
struct traits< JointPrismaticUnalignedTpl<_Scalar,_Options> >
{
enum {
NQ = 1,
NV = 1
};
typedef _Scalar Scalar;
typedef JointDataPrismaticUnalignedTpl<Scalar> JointDataDerived;
typedef JointModelPrismaticUnalignedTpl<Scalar> JointModelDerived;
typedef ConstraintPrismaticUnaligned<Scalar> Constraint_t;
typedef SE3 Transformation_t;
typedef MotionPrismaticUnaligned<Scalar> Motion_t;
enum { Options = _Options };
typedef JointDataPrismaticUnalignedTpl<Scalar,Options> JointDataDerived;
typedef JointModelPrismaticUnalignedTpl<Scalar,Options> JointModelDerived;
typedef ConstraintPrismaticUnaligned<Scalar,Options> Constraint_t;
typedef SE3Tpl<Scalar,Options> Transformation_t;
typedef MotionPrismaticUnaligned<Scalar,Options> Motion_t;
typedef BiasZero Bias_t;
typedef Eigen::Matrix<Scalar,6,NV> F_t;
typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
// [ABA]
typedef Eigen::Matrix<Scalar,6,NV> U_t;
typedef Eigen::Matrix<Scalar,NV,NV> D_t;
typedef Eigen::Matrix<Scalar,6,NV> UD_t;
typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
typedef Eigen::Matrix<Scalar,NQ,1> ConfigVector_t;
typedef Eigen::Matrix<Scalar,NV,1> TangentVector_t;
typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
};
template<typename Scalar> struct traits< JointDataPrismaticUnalignedTpl<Scalar> >
{ typedef JointPrismaticUnalignedTpl<Scalar> JointDerived; };
template<typename Scalar, int Options>
struct traits< JointDataPrismaticUnalignedTpl<Scalar,Options> >
{ typedef JointPrismaticUnalignedTpl<Scalar,Options> JointDerived; };
template<typename _Scalar>
struct JointDataPrismaticUnalignedTpl : public JointDataBase< JointDataPrismaticUnalignedTpl<_Scalar> >
template<typename _Scalar, int _Options>
struct JointDataPrismaticUnalignedTpl : public JointDataBase< JointDataPrismaticUnalignedTpl<_Scalar,_Options> >
{
typedef JointPrismaticUnalignedTpl<_Scalar> JointDerived;
typedef JointPrismaticUnalignedTpl<_Scalar,_Options> JointDerived;
SE3_JOINT_TYPEDEF_TEMPLATE;
Transformation_t M;
......@@ -351,13 +356,14 @@ namespace se3
}; // struct JointDataPrismaticUnalignedTpl
template<typename Scalar> struct traits< JointModelPrismaticUnalignedTpl<Scalar> >
{ typedef JointPrismaticUnalignedTpl<Scalar> JointDerived; };
template<typename Scalar, int Options>
struct traits< JointModelPrismaticUnalignedTpl<Scalar,Options> >
{ typedef JointPrismaticUnalignedTpl<Scalar,Options> JointDerived; };
template<typename _Scalar>
struct JointModelPrismaticUnalignedTpl : public JointModelBase< JointModelPrismaticUnalignedTpl<_Scalar> >
template<typename _Scalar, int _Options>
struct JointModelPrismaticUnalignedTpl : public JointModelBase< JointModelPrismaticUnalignedTpl<_Scalar,_Options> >
{
typedef JointPrismaticUnalignedTpl<_Scalar> JointDerived;
typedef JointPrismaticUnalignedTpl<_Scalar,_Options> JointDerived;
SE3_JOINT_TYPEDEF_TEMPLATE;
using JointModelBase<JointModelPrismaticUnalignedTpl>::id;
......@@ -430,10 +436,7 @@ namespace se3
Vector3 axis;
}; // struct JointModelPrismaticUnalignedTpl
typedef JointPrismaticUnalignedTpl<double> JointPrismaticUnaligned;
typedef JointDataPrismaticUnalignedTpl<double> JointDataPrismaticUnaligned;
typedef JointModelPrismaticUnalignedTpl<double> JointModelPrismaticUnaligned;
typedef JointPrismaticUnalignedTpl<double,0> JointPrismaticUnaligned;
} //namespace se3
......
......@@ -29,14 +29,16 @@
namespace se3
{
template<typename _Scalar, int _axis> struct MotionPrismatic;
template<typename _Scalar, int _axis>
struct traits < MotionPrismatic<_Scalar,_axis> >
template<typename Scalar, int Options, int _axis> struct MotionPrismatic;
template<typename _Scalar, int _Options, int _axis>
struct traits < MotionPrismatic<_Scalar,_Options,_axis> >
{
typedef _Scalar Scalar;
typedef Eigen::Matrix<Scalar,3,1,0> Vector3;
typedef Eigen::Matrix<Scalar,6,1,0> Vector6;
typedef Eigen::Matrix<Scalar,6,6,0> Matrix6;
enum { Options = _Options };
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
typedef typename EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef typename EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef Vector3 AngularType;
......@@ -44,15 +46,15 @@ namespace se3
typedef const Vector3 ConstAngularType;
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<Scalar,0> MotionPlain;
typedef MotionTpl<Scalar,Options> MotionPlain;
enum {
LINEAR = 0,
ANGULAR = 3
};
}; // struct traits MotionPrismatic
template<typename _Scalar, int _axis>
struct MotionPrismatic : MotionBase < MotionPrismatic<_Scalar,_axis> >
template<typename _Scalar, int _Options, int _axis>
struct MotionPrismatic : MotionBase < MotionPrismatic<_Scalar,_Options,_axis> >
{
MOTION_TYPEDEF_TPL(MotionPrismatic);
typedef SpatialAxis<_axis+LINEAR> Axis;
......@@ -71,9 +73,9 @@ namespace se3
}
}; // struct MotionPrismatic
template<typename Scalar, int axis, typename MotionDerived>
template<typename Scalar, int Options, int axis, typename MotionDerived>
typename MotionDerived::MotionPlain
operator+(const MotionPrismatic<Scalar,axis> & m1,
operator+(const MotionPrismatic<Scalar,Options,axis> & m1,
const MotionDense<MotionDerived> & m2)
{
typename MotionDerived::MotionPlain res(m2);
......@@ -81,54 +83,56 @@ namespace se3
return res;
}
template<typename Scalar, int axis> struct ConstraintPrismatic;
template<typename _Scalar, int axis>
struct traits< ConstraintPrismatic<_Scalar,axis> >
template<typename Scalar, int Options, int axis> struct ConstraintPrismatic;
template<typename _Scalar, int _Options, int axis>
struct traits< ConstraintPrismatic<_Scalar,_Options,axis> >
{
typedef _Scalar Scalar;
typedef Eigen::Matrix<Scalar,3,1,0> Vector3;
typedef Eigen::Matrix<Scalar,4,1,0> Vector4;
typedef Eigen::Matrix<Scalar,6,1,0> Vector6;
typedef Eigen::Matrix<Scalar,3,3,0> Matrix3;
typedef Eigen::Matrix<Scalar,4,4,0> Matrix4;
typedef Eigen::Matrix<Scalar,6,6,0> Matrix6;
enum { Options = _Options };
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
typedef Matrix3 Angular_t;
typedef Vector3 Linear_t;
typedef const Matrix3 ConstAngular_t;
typedef const Vector3 ConstLinear_t;
typedef Matrix6 ActionMatrix_t;
typedef Eigen::Quaternion<Scalar,0> Quaternion_t;
typedef SE3Tpl<Scalar,0> SE3;
typedef ForceTpl<Scalar,0> Force;
typedef MotionTpl<Scalar,0> Motion;
typedef Symmetric3Tpl<Scalar,0> Symmetric3;
typedef Eigen::Quaternion<Scalar,Options> Quaternion_t;
typedef SE3Tpl<Scalar,Options> SE3;
typedef ForceTpl<Scalar,Options> Force;
typedef MotionTpl<Scalar,Options> Motion;
typedef Symmetric3Tpl<Scalar,Options> Symmetric3;
enum {
LINEAR = 0,
ANGULAR = 3
};
typedef Eigen::Matrix<Scalar,1,1,0> JointMotion;
typedef Eigen::Matrix<Scalar,1,1,0> JointForce;
typedef Eigen::Matrix<Scalar,6,1> DenseBase;
typedef Eigen::Matrix<Scalar,1,1,Options> JointMotion;
typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
typedef DenseBase MatrixReturnType;
typedef const DenseBase ConstMatrixReturnType;
}; // traits ConstraintRevolute
template<typename _Scalar, int axis>
struct ConstraintPrismatic : ConstraintBase < ConstraintPrismatic <_Scalar,axis> >
template<typename _Scalar, int _Options, int axis>
struct ConstraintPrismatic : ConstraintBase < ConstraintPrismatic <_Scalar,_Options,axis> >
{
SPATIAL_TYPEDEF_TEMPLATE(ConstraintPrismatic);
enum { NV = 1, Options = 0 };
enum { NV = 1, Options = _Options };
typedef typename traits<ConstraintPrismatic>::JointMotion JointMotion;
typedef typename traits<ConstraintPrismatic>::JointForce JointForce;
typedef typename traits<ConstraintPrismatic>::DenseBase DenseBase;
typedef SpatialAxis<LINEAR+axis> Axis;
template<typename D>
MotionPrismatic<Scalar,axis> operator*( const Eigen::MatrixBase<D> & v ) const
MotionPrismatic<Scalar,Options,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
assert(v.cols() == 1 && v.rows() == 1);
return MotionPrismatic<Scalar,axis>(v[0]);
return MotionPrismatic<Scalar,Options,axis>(v[0]);
}
template<typename S2, int O2>
......@@ -190,10 +194,10 @@ namespace se3
}; // struct ConstraintPrismatic
template<typename MotionDerived, typename Scalar>
template<typename MotionDerived, typename S1, int O1>
inline typename MotionDerived::MotionPlain
operator^(const MotionDense<MotionDerived> & m1,
const MotionPrismatic<Scalar,0>& m2)
const MotionPrismatic<S1,O1,0>& m2)
{
/* nu1^nu2 = ( v1^w2+w1^v2, w1^w2 )
* nu1^(v2,0) = ( w1^v2 , 0 )
......@@ -202,15 +206,15 @@ namespace se3
*/
typedef typename MotionDerived::MotionPlain MotionPlain;
const typename MotionDerived::ConstAngularType & w = m1.angular();
const Scalar & vx = m2.v;
const S1 & vx = m2.v;
return MotionPlain(typename MotionPlain::Vector3(0,w[2]*vx,-w[1]*vx),
MotionPlain::Vector3::Zero());
}
template<typename MotionDerived, typename Scalar>
template<typename MotionDerived, typename S1, int O1>
inline typename MotionDerived::MotionPlain
operator^(const MotionDense<MotionDerived> & m1,
const MotionPrismatic<Scalar,1>& m2)
const MotionPrismatic<S1,O1,1>& m2)
{
/* nu1^nu2 = ( v1^w2+w1^v2, w1^w2 )
* nu1^(v2,0) = ( w1^v2 , 0 )
......@@ -219,15 +223,15 @@ namespace se3
*/
typedef typename MotionDerived::MotionPlain MotionPlain;
const typename MotionDerived::ConstAngularType & w = m1.angular();
const Scalar & vx = m2.v;
return MotionPlain(typename MotionPlain::Vector3(-w[2]*vx,0,w[0]*vx),
const S1 & vy = m2.v;
return MotionPlain(typename MotionPlain::Vector3(-w[2]*vy,0,w[0]*vy),
MotionPlain::Vector3::Zero());
}
template<typename MotionDerived, typename Scalar>
template<typename MotionDerived, typename S1, int O1>
inline typename MotionDerived::MotionPlain
operator^(const MotionDense<MotionDerived> & m1,
const MotionPrismatic<Scalar,2>& m2)
const MotionPrismatic<S1,O1,2>& m2)
{
/* nu1^nu2 = ( v1^w2+w1^v2, w1^w2 )
* nu1^(v2,0) = ( w1^v2 , 0 )
......@@ -236,15 +240,15 @@ namespace se3
*/
typedef typename MotionDerived::MotionPlain MotionPlain;
const typename MotionDerived::ConstAngularType & w = m1.angular();
const Scalar & vx = m2.v;
return MotionPlain(typename Motion::Vector3(w[1]*vx,-w[0]*vx,0),
const S1 & vz = m2.v;
return MotionPlain(typename Motion::Vector3(w[1]*vz,-w[0]*vz,0),
MotionPlain::Vector3::Zero());
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
template<typename S1, int O1, typename S2>
template<typename S1, int O1, typename S2, int O2>
inline Eigen::Matrix<S1,6,1,O1>
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPrismatic<S2,0> &)
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPrismatic<S2,O2,0> &)
{
/* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */
const S1
......@@ -256,9 +260,9 @@ namespace se3
return res;
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
template<typename S1, int O1, typename S2>
template<typename S1, int O1, typename S2, int O2>
inline Eigen::Matrix<S1,6,1,O1>
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPrismatic<S2,1> & )
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPrismatic<S2,O2,1> & )
{
/* Y(:,1) = ( 0,1, 0, -z , 0 , x) */
const S1
......@@ -270,9 +274,9 @@ namespace se3
return res;
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
template<typename S1, int O1, typename S2>
template<typename S1, int O1, typename S2, int O2>
inline Eigen::Matrix<S1,6,1,O1>
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPrismatic<S2,2> & )
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPrismatic<S2,O2,2> & )
{
/* Y(:,2) = ( 0,0, 1, y , -x , 0) */
const S1
......@@ -285,9 +289,9 @@ namespace se3
}
/* [ABA] operator* (Inertia Y,Constraint S) */
template<typename M6Like, typename S2, int axis>
template<typename M6Like, typename S2, int O2, int axis>
inline const typename M6Like::ConstColXpr
operator*(const Eigen::MatrixBase<M6Like> & Y, const ConstraintPrismatic<S2,axis> &)
operator*(const Eigen::MatrixBase<M6Like> & Y, const ConstraintPrismatic<S2,O2,axis> &)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
return Y.derived().col(Inertia::LINEAR + axis);
......@@ -295,20 +299,20 @@ namespace se3