Commit 8faae27f authored by fvalenza's avatar fvalenza
Browse files

Merge pull request #20 from jcarpent/topic/joint

Fix bug in prismatic joint + add planar joint + add translational joint
parents 51c3c8e1 be3c965c
......@@ -76,6 +76,8 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint-spherical.hpp
multibody/joint/joint-spherical-ZYX.hpp
multibody/joint/joint-prismatic.hpp
multibody/joint/joint-planar.hpp
multibody/joint/joint-translation.hpp
multibody/joint/joint-free-flyer.hpp
multibody/joint/joint-variant.hpp
multibody/joint/joint-generic.hpp
......
#ifndef __se3_joint_planar_hpp__
#define __se3_joint_planar_hpp__
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/inertia.hpp"
namespace se3
{
struct JointDataPlanar;
struct JointModelPlanar;
struct JointPlanar
{
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 MotionPlanar
{
typedef double Scalar_t;
typedef MotionTpl<Scalar_t> Motion;
Scalar_t x_dot_, y_dot_, theta_dot_;
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) {}
operator Motion() const
{
return Motion (Motion::Vector3(x_dot_, y_dot_, 0.), Motion::Vector3(0., 0., theta_dot_));
}
}; // struct MotionPlanar
friend const MotionPlanar operator+ (const MotionPlanar & m, const BiasZero &)
{ return m; }
friend Motion operator+ (const MotionPlanar & m1, const Motion & m2)
{
Motion result (m2);
result.linear ()[0] += m1.x_dot_;
result.linear ()[1] += m1.y_dot_;
result.angular ()[2] += m1.theta_dot_;
return result;
}
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:
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)
{
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
{
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 ConstraintPlanar
template<typename D>
friend 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;
}
}; // struct JointPlanar
Motion operator^ (const Motion & m1, const JointPlanar::MotionPlanar & m2)
{
Motion result;
const Motion::Vector3 & m1_t = m1.linear();
const Motion::Vector3 & m1_w = m1.angular();
result.angular () << m1_w(1) * m2.theta_dot_, - m1_w(0) * m2.theta_dot_, 0.;
result.linear () << m1_t(1) * m2.theta_dot_ - m1_w(2) * m2.y_dot_, - m1_t(0) * m2.theta_dot_ + m1_w(2) * m2.x_dot_, m1_w(0) * m2.y_dot_ - m1_w(1) * m2.x_dot_;
return result;
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
Eigen::Matrix <Inertia::Scalar, 6, 3> operator* (const Inertia & Y, const JointPlanar::ConstraintPlanar &)
{
Eigen::Matrix <Inertia::Scalar, 6, 3> M;
const double mass = Y.mass ();
const Inertia::Vector3 & com = Y.lever ();
const Symmetric3 & inertia = Y.inertia ();
M.topLeftCorner <3,3> ().setZero ();
M.topLeftCorner <2,2> ().diagonal ().fill (mass);
Inertia::Vector3 mc (mass * com);
M.rightCols <1> ().head <2> () << mc(1), - mc(0);
M.bottomLeftCorner <3,2> () << 0., mc(2), - mc(1), 0., mc(1), -mc(0);
M.rightCols <1> ().tail <3> () = inertia.data ().tail <3> ();
M.rightCols <1> ().head <2> () = mc.head <2> () * com(2);
M.rightCols <1> ().tail <1> () = -mc.head <2> ().transpose () * com.head <2> ();
return M;
}
namespace internal
{
template<>
struct ActionReturn<JointPlanar::ConstraintPlanar >
{ typedef Eigen::Matrix<double,6,3> Type; };
}
template<>
struct traits<JointPlanar>
{
typedef JointDataPlanar JointData;
typedef JointModelPlanar JointModel;
typedef JointPlanar::ConstraintPlanar Constraint_t;
typedef SE3 Transformation_t;
typedef JointPlanar::MotionPlanar Motion_t;
typedef JointPlanar::BiasZero Bias_t;
typedef Eigen::Matrix<double,6,3> F_t;
enum {
NQ = 3,
NV = 3
};
};
template<> struct traits<JointDataPlanar> { typedef JointPlanar Joint; };
template<> struct traits<JointModelPlanar> { typedef JointPlanar Joint; };
struct JointDataPlanar : public JointDataBase<JointDataPlanar>
{
typedef JointPlanar Joint;
SE3_JOINT_TYPEDEF;
typedef Motion::Scalar 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;
Motion_t v;
Bias_t c;
JointDataPlanar () : M(1) {}
};
struct JointModelPlanar : public JointModelBase<JointModelPlanar>
{
typedef JointPlanar Joint;
SE3_JOINT_TYPEDEF;
JointData createData() const { return JointData(); }
void calc (JointData & data,
const Eigen::VectorXd & qs) const
{
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ>(idx_q ());
double c_theta,s_theta; SINCOS (q(2), &s_theta, &c_theta);
data.M.rotation ().topLeftCorner <2,2> () << c_theta, -s_theta, s_theta, c_theta;
data.M.translation ().head <2> () = q.head<2> ();
}
void calc (JointData & data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
{
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.segment<NQ> (idx_v ());
double c_theta,s_theta; SINCOS (q(2), &s_theta, &c_theta);
data.M.rotation ().topLeftCorner <2,2> () << c_theta, -s_theta, s_theta, c_theta;
data.M.translation ().head <2> () = q.head<2> ();
data.v.x_dot_ = q_dot(0);
data.v.y_dot_ = q_dot(1);
data.v.theta_dot_ = q_dot(2);
}
};
} // namespace se3
#endif // ifndef __se3_joint_planar_hpp__
......@@ -4,7 +4,6 @@
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/math/sincos.hpp"
namespace se3
{
......@@ -19,12 +18,12 @@ namespace se3
{
double v;
CartesianVector3(const double & v) : v(v) {}
CartesianVector3() : v(1) {}
operator Eigen::Vector3d (); // { return Eigen::Vector3d(w,0,0); }
CartesianVector3() : v(NAN) {}
operator Eigen::Vector3d () const; // { return Eigen::Vector3d(w,0,0); }
};
template<>CartesianVector3<0>::operator Eigen::Vector3d () { return Eigen::Vector3d(v,0,0); }
template<>CartesianVector3<1>::operator Eigen::Vector3d () { return Eigen::Vector3d(0,v,0); }
template<>CartesianVector3<2>::operator Eigen::Vector3d () { return Eigen::Vector3d(0,0,v); }
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)
......@@ -67,7 +66,10 @@ namespace se3
{
template<typename D>
MotionPrismatic operator*( const Eigen::MatrixBase<D> & v ) const
{ return MotionPrismatic(v[0]); }
{
// EIGEN_STATIC_ASSERT_SIZE_1x1(D); // There is actually a bug in Eigen with such a macro
return MotionPrismatic(v[0]);
}
Eigen::Matrix<double,6,1> se3Action(const SE3 & m) const
{
......
#ifndef __se3_joint_translation_hpp__
#define __se3_joint_translation_hpp__
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/skew.hpp"
namespace se3
{
struct JointDataTranslation;
struct JointModelTranslation;
struct JointTranslation
{
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; }
operator Motion() const
{
return Motion (v, Motion::Vector3::Zero ());
}
MotionTranslation & operator= (const MotionTranslation & other)
{
v = other.v;
return *this;
}
}; // struct MotionTranslation
friend const MotionTranslation operator+ (const MotionTranslation & m, const BiasZero &)
{ return m; }
friend Motion operator+ (const MotionTranslation & m1, const Motion & m2)
{
return Motion (m2.linear () + m1.v, m2.angular ());
}
struct ConstraintTranslationSubspace
{
public:
Motion operator* (const MotionTranslation & vj) const
{ return Motion (vj (), Motion::Vector3::Zero ()); }
ConstraintTranslationSubspace () {}
struct ConstraintTranspose
{
const ConstraintTranslationSubspace & ref;
ConstraintTranspose(const ConstraintTranslationSubspace & ref) : ref(ref) {}
Force::Vector3 operator* (const Force & phi)
{
return phi.linear ();
}
/* [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); }
operator ConstraintXd () const
{
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);
}
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>
friend Motion operator* (const ConstraintTranslationSubspace & S, const Eigen::MatrixBase<D> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
return Motion (v, Motion::Vector3::Zero ());
}
}; // struct JointTranslation
Motion operator^ (const Motion & m1, const JointTranslation::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 & S)
{
Eigen::Matrix <double, 6, 3> M;
M.block <3,3> (Inertia::ANGULAR, 0) = alphaSkew(Y.mass (), Y.lever ());
// M.block <3,3> (Inertia::LINEAR, 0) = Y.mass () * Inertia::Matrix3::Identity ();
M.block <3,3> (Inertia::LINEAR, 0).setZero ();
M.block <3,3> (Inertia::LINEAR, 0).diagonal ().fill (Y.mass ());
return M;
}
namespace internal
{
template<>
struct ActionReturn<JointTranslation::ConstraintTranslationSubspace >
{ typedef Eigen::Matrix<double,6,3> Type; };
}
template<>
struct traits<JointTranslation>
{
typedef JointDataTranslation JointData;
typedef JointModelTranslation JointModel;
typedef JointTranslation::ConstraintTranslationSubspace Constraint_t;
typedef SE3 Transformation_t;
typedef JointTranslation::MotionTranslation Motion_t;
typedef JointTranslation::BiasZero Bias_t;
typedef Eigen::Matrix<double,6,3> F_t;
enum {
NQ = 3,
NV = 3
};
};
template<> struct traits<JointDataTranslation> { typedef JointTranslation Joint; };
template<> struct traits<JointModelTranslation> { typedef JointTranslation Joint; };
struct JointDataTranslation : public JointDataBase<JointDataTranslation>
{
typedef JointTranslation Joint;
SE3_JOINT_TYPEDEF;
typedef Eigen::Matrix<double,6,6> Matrix6;
typedef Eigen::Matrix<double,3,3> Matrix3;
typedef Eigen::Matrix<double,3,1> Vector3;
Constraint_t S;
Transformation_t M;
Motion_t v;
Bias_t c;
JointDataTranslation () : M(1) {}
};
struct JointModelTranslation : public JointModelBase<JointModelTranslation>
{
typedef JointTranslation Joint;
SE3_JOINT_TYPEDEF;
JointData createData() const { return JointData(); }
void calc (JointData & data,
const Eigen::VectorXd & qs) const
{
data.M.translation (qs.segment<NQ>(idx_q ()));
}
void calc (JointData & data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
{
data.M.translation (qs.segment<NQ> (idx_q ()));
data.v () = vs.segment<NQ> (idx_v ());
}
};
} // namespace se3
#endif // ifndef __se3_joint_translation_hpp__
......@@ -6,12 +6,14 @@
#include "pinocchio/multibody/joint/joint-spherical.hpp"
#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp"
#include "pinocchio/multibody/joint/joint-prismatic.hpp"
#include "pinocchio/multibody/joint/joint-planar.hpp"
#include "pinocchio/multibody/joint/joint-translation.hpp"
#include "pinocchio/multibody/joint/joint-free-flyer.hpp"
namespace se3
{
typedef boost::variant< JointModelRX,JointModelRY,JointModelRZ , JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelFreeFlyer> JointModelVariant;
typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ , JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataFreeFlyer > JointDataVariant;
typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelFreeFlyer, JointModelPlanar, JointModelTranslation> JointModelVariant;
typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataFreeFlyer, JointDataPlanar, JointDataTranslation> JointDataVariant;
typedef std::vector<JointModelVariant> JointModelVector;
typedef std::vector<JointDataVariant> JointDataVector;
......
......@@ -32,34 +32,34 @@ namespace se3
typedef Eigen::Matrix<Scalar,3,2,Options> Matrix32;
public:
Symmetric3Tpl(): data() {}
Symmetric3Tpl(): data_() {}
template<typename Sc,int N,int Opt>
explicit Symmetric3Tpl(const Eigen::Matrix<Sc,N,N,Opt> & I)
{
assert( (I.rows()==3)&&(I.cols()==3) );
assert( (I-I.transpose()).isMuchSmallerThan(I) );
data(0) = I(0,0);
data(1) = I(1,0); data(2) = I(1,1);
data(3) = I(2,0); data(4) = I(2,1); data(5) = I(2,2);
data_(0) = I(0,0);
data_(1) = I(1,0); data_(2) = I(1,1);
data_(3) = I(2,0); data_(4) = I(2,1); data_(5) = I(2,2);
}
explicit Symmetric3Tpl(const Eigen::MatrixBase<Matrix3> &I)
{
assert( (I-I.transpose()).isMuchSmallerThan(I) );
data(0) = I(0,0);
data(1) = I(1,0); data(2) = I(1,1);
data(3) = I(2,0); data(4) = I(2,1); data(5) = I(2,2);
data_(0) = I(0,0);
data_(1) = I(1,0); data_(2) = I(1,1);
data_(3) = I(2,0); data_(4) = I(2,1); data_(5) = I(2,2);
}
explicit Symmetric3Tpl(const Vector6 &I) : data(I) {}
explicit Symmetric3Tpl(const Vector6 &I) : data_(I) {}
Symmetric3Tpl(const double & a0,const double & a1,const double & a2,
const double & a3,const double & a4,const double & a5)
{ data << a0,a1,a2,a3,a4,a5; }
{ data_ << a0,a1,a2,a3,a4,a5; }
static Symmetric3Tpl Zero() { return Symmetric3Tpl(Vector6::Zero() ); }
static Symmetric3Tpl Random() { return Symmetric3Tpl(Vector6::Random().eval()); }
static Symmetric3Tpl Identity() { return Symmetric3Tpl( 1, 0, 1, 0, 0, 1); }
/* Requiered by Inertia::operator== */
bool operator== (const Symmetric3Tpl & S2 ) { return data == S2.data; }
bool operator== (const Symmetric3Tpl & S2 ) { return data_ == S2.data_; }
struct SkewSquare
{
......@@ -76,16 +76,16 @@ namespace se3
Symmetric3Tpl operator- (const SkewSquare & v) const
{
const double & x = v.v[0], & y = v.v[1], & z = v.v[2];
return Symmetric3Tpl( data[0]+y*y+z*z,
data[1]-x*y , data[2]+x*x+z*z,
data[3]-x*z , data[4]-y*z , data[5]+x*x+y*y );
return Symmetric3Tpl( data_[0]+y*y+z*z,
data_[1]-x*y , data_[2]+x*x+z*z,
data_[3]-x*z , data_[4]-y*z , data_[5]+x*x+y*y );
}
Symmetric3Tpl& operator-= (const SkewSquare & v)
{
const double & x = v.v[0], & y = v.v[1], & z = v.v[2];
data[0]+=y*y+z*z;
data[1]-=x*y ; data[2]+=x*x+z*z;
data[3]-=x*z ; data[4]-=y*z ; data[5]+=x*x+y*y;
data_[0]+=y*y+z*z;