Commit 0241aeaa authored by jcarpent's avatar jcarpent
Browse files

[All] Introduce MOTION_TYPEDEF macro to replace the generic SPATIAL_TYPEDEF

parent 735c7a04
......@@ -23,6 +23,7 @@
#include <boost/python/tuple.hpp>
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/bindings/python/utils/copyable.hpp"
#include "pinocchio/bindings/python/utils/printable.hpp"
......@@ -38,10 +39,10 @@ namespace se3
struct MotionPythonVisitor
: public boost::python::def_visitor< MotionPythonVisitor<Motion> >
{
typedef typename Motion::Force Force;
typedef typename Motion::Scalar Scalar;
typedef ForceTpl<Scalar,traits<Motion>::Options> Force;
typedef typename Motion::Vector6 Vector6;
typedef typename Motion::Vector3 Vector3;
typedef typename Motion::Scalar Scalar;
public:
......
......@@ -95,7 +95,7 @@ namespace se3
assert(model.check(data) && "data is not consistent with model.");
data.potential_energy = 0.;
const Motion::ConstLinear_t & g = model.gravity.linear();
const Motion::ConstLinearType & g = model.gravity.linear();
SE3::Vector3 com_global;
if (update_kinematics)
......
......@@ -21,9 +21,9 @@
#include "pinocchio/macros.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/explog.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/explog.hpp"
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/quaternion.hpp"
......@@ -78,14 +78,14 @@ namespace se3
struct TransposeConst
{
Force::Vector6 operator* (const Force & phi)
const Force::Vector6 & operator* (const Force & phi)
{ return phi.toVector(); }
};
TransposeConst transpose() const { return TransposeConst(); }
operator ConstraintXd () const { return ConstraintXd(SE3::Matrix6::Identity()); }
DenseBase motionAction(const Motion & m) const { return m.toActionMatrix(); }
DenseBase motionAction(const Motion & v) const { return v.toActionMatrix(); }
}; // struct ConstraintIdentity
template<typename D>
......@@ -209,7 +209,7 @@ namespace se3
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,V);
//using std::sqrt;
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
typedef Eigen::Map<const SE3::Quaternion_t> ConstQuaternionMap_t;
ConstQuaternionMap_t quat(q_joint.template tail<4>().data());
//assert(std::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
......@@ -222,7 +222,7 @@ namespace se3
void calc(JointDataDerived & data,
const Eigen::VectorXd & qs) const
{
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
typedef Eigen::Map<const SE3::Quaternion_t> ConstQuaternionMap_t;
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type q = qs.segment<NQ>(idx_q());
ConstQuaternionMap_t quat(q.tail<4> ().data());
......@@ -235,7 +235,7 @@ namespace se3
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
{
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
typedef Eigen::Map<const SE3::Quaternion_t> ConstQuaternionMap_t;
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type q = qs.segment<NQ>(idx_q());
data.v = vs.segment<NV>(idx_v());
......@@ -263,7 +263,7 @@ namespace se3
ConfigVector_t integrate_impl(const Eigen::VectorXd & qs, const Eigen::VectorXd & vs) const
{
typedef Eigen::Map<Motion_t::Quaternion_t> QuaternionMap_t;
typedef Eigen::Map<SE3::Quaternion_t> QuaternionMap_t;
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.segment<NV> (idx_v ());
......@@ -284,7 +284,7 @@ namespace se3
ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0, const Eigen::VectorXd & q1, const double u) const
{
typedef Eigen::Map<Motion_t::Quaternion_t> QuaternionMap_t;
typedef Eigen::Map<SE3::Quaternion_t> QuaternionMap_t;
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_0 = q0.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
......@@ -315,7 +315,7 @@ namespace se3
{
ConfigVector_t q(ConfigVector_t::Random());
typedef Eigen::Map<Motion_t::Quaternion_t> QuaternionMap_t;
typedef Eigen::Map<SE3::Quaternion_t> QuaternionMap_t;
uniformRandom(QuaternionMap_t(q.segment<4>(3).data()));
return q;
......@@ -338,7 +338,7 @@ namespace se3
result[i] = lower_pos_limit[i] + (upper_pos_limit[i] - lower_pos_limit[i]) * (Scalar)(rand())/RAND_MAX;
}
typedef Eigen::Map<Motion_t::Quaternion_t> QuaternionMap_t;
typedef Eigen::Map<SE3::Quaternion_t> QuaternionMap_t;
uniformRandom(QuaternionMap_t(result.segment<4>(3).data()));
return result;
......@@ -372,7 +372,7 @@ namespace se3
bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
typedef Eigen::Map<const SE3::Quaternion_t> ConstQuaternionMap_t;
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.segment<NQ> (idx_q ());
......
......@@ -42,16 +42,12 @@ namespace se3
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 const Vector3 ConstAngular_t;
typedef const Vector3 ConstLinear_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;
typedef Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<double,0> MotionPlain;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -60,7 +56,7 @@ namespace se3
struct MotionPlanar : MotionBase < MotionPlanar >
{
SPATIAL_TYPEDEF_NO_TEMPLATE(MotionPlanar);
MOTION_TYPEDEF(MotionPlanar);
MotionPlanar () : x_dot_(NAN), y_dot_(NAN), theta_dot_(NAN) {}
MotionPlanar (Scalar x_dot, Scalar y_dot, Scalar theta_dot) : x_dot_(x_dot), y_dot_(y_dot), theta_dot_(theta_dot) {}
......@@ -191,8 +187,8 @@ namespace se3
DenseBase motionAction(const Motion & m) const
{
const Motion::ConstLinear_t v = m.linear();
const Motion::ConstAngular_t w = m.angular();
const Motion::ConstLinearType v = m.linear();
const Motion::ConstAngularType w = m.angular();
DenseBase res(DenseBase::Zero());
res(0,1) = -w[2]; res(0,2) = v[1];
......
......@@ -36,21 +36,14 @@ namespace se3
{
typedef double 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;
typedef Vector3 Angular_t;
typedef Vector3 Linear_t;
typedef const Vector3 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 Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<Scalar,0> MotionPlain;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -59,7 +52,7 @@ namespace se3
struct MotionPrismaticUnaligned : MotionBase <MotionPrismaticUnaligned>
{
SPATIAL_TYPEDEF_NO_TEMPLATE(MotionPrismaticUnaligned);
MOTION_TYPEDEF(MotionPrismaticUnaligned);
MotionPrismaticUnaligned () : axis(Vector3::Constant(NAN)), v(NAN) {}
MotionPrismaticUnaligned (const Vector3 & axis, const Scalar v) : axis(axis), v(v) {}
......
......@@ -59,21 +59,14 @@ namespace se3
{
typedef double Scalar;
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 const Vector3 ConstAngular_t;
typedef const Vector3 ConstLinear_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;
typedef Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<double,0> MotionPlain;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -83,7 +76,7 @@ namespace se3
template<int axis>
struct MotionPrismatic : MotionBase < MotionPrismatic < axis > >
{
SPATIAL_TYPEDEF_TEMPLATE(MotionPrismatic);
MOTION_TYPEDEF_TPL(MotionPrismatic);
MotionPrismatic() : v(NAN) {}
MotionPrismatic( const double & v ) : v(v) {}
......
......@@ -35,21 +35,14 @@ namespace se3
{
typedef double Scalar;
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 const Vector3 ConstAngular_t;
typedef const Vector3 ConstLinear_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;
typedef Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<double,0> MotionPlain;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -58,7 +51,7 @@ namespace se3
struct MotionRevoluteUnaligned : MotionBase < MotionRevoluteUnaligned >
{
SPATIAL_TYPEDEF_NO_TEMPLATE(MotionRevoluteUnaligned);
MOTION_TYPEDEF(MotionRevoluteUnaligned);
MotionRevoluteUnaligned() : axis(Motion::Vector3::Constant(NAN)), w(NAN) {}
MotionRevoluteUnaligned( const Motion::Vector3 & axis, const double & w ) : axis(axis), w(w) {}
......@@ -194,8 +187,8 @@ namespace se3
DenseBase motionAction(const Motion & m) const
{
const Motion::ConstLinear_t v = m.linear();
const Motion::ConstAngular_t w = m.angular();
const Motion::ConstLinearType v = m.linear();
const Motion::ConstAngularType w = m.angular();
DenseBase res;
res << v.cross(axis), w.cross(axis);
......@@ -209,9 +202,9 @@ namespace se3
inline 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 ;
const Motion::ConstLinearType v1 = m1.linear();
const Motion::ConstAngularType w1 = m1.angular();
const Motion::Vector3 w2(m2.axis * m2.w);
return Motion( v1.cross(w2),w1.cross(w2));
}
......
......@@ -62,21 +62,14 @@ namespace se3
{
typedef double Scalar;
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 const Vector3 ConstAngular_t;
typedef const Vector3 ConstLinear_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;
typedef Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<double,0> MotionPlain;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -86,7 +79,7 @@ namespace se3
template<int axis>
struct MotionRevolute : MotionBase < MotionRevolute <axis > >
{
SPATIAL_TYPEDEF_TEMPLATE(MotionRevolute);
MOTION_TYPEDEF_TPL(MotionRevolute);
MotionRevolute() : w(NAN) {}
MotionRevolute( const double & w ) : w(w) {}
......@@ -212,8 +205,8 @@ namespace se3
DenseBase motionAction(const Motion & m) const
{
const typename Motion::ConstLinear_t v = m.linear();
const typename Motion::ConstAngular_t w = m.angular();
const typename Motion::ConstLinearType v = m.linear();
const typename Motion::ConstAngularType w = m.angular();
const Vector3 a(revolute::CartesianVector3<axis>(1).vector());
DenseBase res;
......
......@@ -191,8 +191,8 @@ namespace se3
DenseBase motionAction(const Motion & m) const
{
const typename Motion::ConstLinear_t v = m.linear();
const typename Motion::ConstAngular_t w = m.angular();
const typename Motion::ConstLinearType v = m.linear();
const typename Motion::ConstAngularType w = m.angular();
DenseBase res;
cross(v,S_minimal,res.template middleRows<3>(Motion::LINEAR));
......
......@@ -33,26 +33,20 @@ namespace se3
{
struct MotionSpherical;
template <>
struct traits< MotionSpherical >
{
typedef double Scalar;
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 const Vector3 ConstAngular_t;
typedef const Vector3 ConstLinear_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;
typedef Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<double,0> MotionPlain;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -61,7 +55,7 @@ namespace se3
struct MotionSpherical : MotionBase < MotionSpherical >
{
SPATIAL_TYPEDEF_NO_TEMPLATE(MotionSpherical);
MOTION_TYPEDEF(MotionSpherical);
MotionSpherical () : w (Motion::Vector3(NAN, NAN, NAN)) {}
MotionSpherical (const Motion::Vector3 & w) : w (w) {}
......@@ -164,8 +158,8 @@ namespace se3
DenseBase motionAction(const Motion & m) const
{
const Motion::ConstLinear_t v = m.linear();
const Motion::ConstAngular_t w = m.angular();
const Motion::ConstLinearType v = m.linear();
const Motion::ConstAngularType w = m.angular();
DenseBase res;
skew(v,res.middleRows<3>(LINEAR));
......@@ -279,7 +273,7 @@ namespace se3
using JointModelBase<JointModelSpherical>::idx_v;
using JointModelBase<JointModelSpherical>::setIndexes;
typedef Motion::Vector3 Vector3;
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
typedef Eigen::Map<const SE3::Quaternion_t> ConstQuaternionMap_t;
JointDataDerived createData() const { return JointDataDerived(); }
......@@ -288,7 +282,7 @@ namespace se3
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,V);
//using std::sqrt;
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
typedef Eigen::Map<const SE3::Quaternion_t> ConstQuaternionMap_t;
ConstQuaternionMap_t quat(q_joint.derived().data());
//assert(std::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
......@@ -301,7 +295,7 @@ namespace se3
void calc (JointDataDerived & data,
const Eigen::VectorXd & qs) const
{
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
typedef Eigen::Map<const SE3::Quaternion_t> ConstQuaternionMap_t;
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ>(idx_q ());
......@@ -313,7 +307,7 @@ namespace se3
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
{
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
typedef Eigen::Map<const SE3::Quaternion_t> ConstQuaternionMap_t;
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ> (idx_q ());
data.v () = vs.segment<NV> (idx_v ());
......@@ -345,13 +339,13 @@ namespace se3
ConfigVector_t integrate_impl(const Eigen::VectorXd & qs,const Eigen::VectorXd & vs) const
{
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
typedef Eigen::Map<const SE3::Quaternion_t> ConstQuaternionMap_t;
ConstQuaternionMap_t q(qs.segment<NQ>(idx_q()).data());
Eigen::VectorXd::ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.segment<NV> (idx_v ());
Motion_t::Quaternion_t pOmega(se3::exp3(q_dot));
Motion_t::Quaternion_t quaternion_result(q*pOmega);
SE3::Quaternion_t pOmega(se3::exp3(q_dot));
SE3::Quaternion_t quaternion_result(q*pOmega);
firstOrderNormalize(quaternion_result);
return quaternion_result.coeffs();
......@@ -359,14 +353,14 @@ namespace se3
ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) const
{
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
typedef Eigen::Map<const SE3::Quaternion_t> ConstQuaternionMap_t;
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_0 = q0.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
ConstQuaternionMap_t p0 (q_0.data());
ConstQuaternionMap_t p1 (q_1.data());
Motion_t::Quaternion_t quaternion_result(p0.slerp(u, p1));
SE3::Quaternion_t quaternion_result(p0.slerp(u, p1));
return quaternion_result.coeffs();
}
......@@ -374,7 +368,7 @@ namespace se3
ConfigVector_t random_impl() const
{
ConfigVector_t q;
typedef Eigen::Map<Motion_t::Quaternion_t> QuaternionMap_t;
typedef Eigen::Map<SE3::Quaternion_t> QuaternionMap_t;
uniformRandom(QuaternionMap_t(q.data()));
return q;
}
......@@ -382,7 +376,7 @@ namespace se3
ConfigVector_t randomConfiguration_impl(const ConfigVector_t &, const ConfigVector_t &) const
{
ConfigVector_t result;
typedef Eigen::Map<Motion_t::Quaternion_t> QuaternionMap_t;
typedef Eigen::Map<SE3::Quaternion_t> QuaternionMap_t;
uniformRandom(QuaternionMap_t(result.data()));
return result;
}
......@@ -415,7 +409,7 @@ namespace se3
bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
typedef Eigen::Map<const SE3::Quaternion_t> ConstQuaternionMap_t;
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.segment<NQ> (idx_q ());
......
......@@ -36,21 +36,14 @@ namespace se3
{
typedef double Scalar;
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 const Vector3 ConstAngular_t;
typedef const Vector3 ConstLinear_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;
typedef Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<double,0> MotionPlain;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -59,7 +52,7 @@ namespace se3
struct MotionTranslation : MotionBase < MotionTranslation >
{
SPATIAL_TYPEDEF_NO_TEMPLATE(MotionTranslation);
MOTION_TYPEDEF(MotionTranslation);
MotionTranslation () : v (Motion::Vector3 (NAN, NAN, NAN)) {}
MotionTranslation (const Motion::Vector3 & v) : v (v) {}
......@@ -68,27 +61,27 @@ namespace se3
Vector3 & operator() () { return v; }
const Vector3 & operator() () const { return v; }
operator Motion() const
{
return Motion (v, Motion::Vector3::Zero ());
}
MotionTranslation & operator= (const MotionTranslation & other)
{
v = other.v;
return *this;
}
}; // struct MotionTranslation
inline const MotionTranslation operator+ (const MotionTranslation & m, const BiasZero &)
{ return m; }
inline Motion operator+ (const MotionTranslation & m1, const Motion & m2)
{
return Motion (m2.linear () + m1.v, m2.angular ());
}
struct ConstraintTranslationSubspace;
template <>
struct traits < ConstraintTranslationSubspace>
......@@ -118,7 +111,7 @@ namespace se3
typedef Eigen::Matrix<Scalar,3,1,0> JointForce;
typedef Eigen::Matrix<Scalar,6,3> DenseBase;
}; // traits ConstraintTranslationSubspace
struct ConstraintTranslationSubspace : ConstraintBase < ConstraintTranslationSubspace >