Verified Commit 819049f8 authored by jcarpent's avatar jcarpent Committed by Justin Carpentier
Browse files

[Joint] Add {SE3,Motion} action on sparse JointMotion

parent 2d6c76d6
......@@ -21,6 +21,7 @@
#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/spatial/cartesian-axis.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/spatial/motion.hpp"
......@@ -30,6 +31,22 @@ namespace se3
{
template<typename Scalar, int Options = 0> struct MotionPlanarTpl;
typedef MotionPlanarTpl<double> MotionPlanar;
namespace internal
{
template<typename Scalar, int Options>
struct SE3GroupAction< MotionPlanarTpl<Scalar,Options> >
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< MotionPlanarTpl<Scalar,Options>, MotionDerived>
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
}
template<typename _Scalar, int _Options>
struct traits< MotionPlanarTpl<_Scalar,_Options> >
......@@ -57,6 +74,8 @@ namespace se3
struct MotionPlanarTpl : MotionBase< MotionPlanarTpl<_Scalar,_Options> >
{
MOTION_TYPEDEF_TPL(MotionPlanarTpl);
typedef CartesianAxis<2> AxisZ;
MotionPlanarTpl () : m_x_dot(NAN), m_y_dot(NAN), m_theta_dot(NAN) {}
......@@ -77,6 +96,74 @@ namespace se3
v.linear()[1] += m_y_dot;
v.angular()[2] += m_theta_dot;
}
template<typename S2, int O2, typename D2>
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
v.angular().noalias() = m.rotation().col(2) * m_theta_dot;
v.linear().noalias() = m.translation().cross(v.angular());
v.linear() += m.rotation().col(0) * m_x_dot;
v.linear() += m.rotation().col(1) * m_y_dot;
}
template<typename S2, int O2>
MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
{
MotionPlain res;
se3Action_impl(m,res);
return res;
}
template<typename S2, int O2, typename D2>
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
// Linear
// TODO: use v.angular() as temporary variable
Vector3 v3_tmp;
AxisZ::cross(m.translation(),v3_tmp);
v3_tmp *= m_theta_dot;
v3_tmp[0] += m_x_dot; v3_tmp[1] += m_y_dot;
v.linear().noalias() = m.rotation().transpose() * v3_tmp;
// Angular
v.angular().noalias() = m.rotation().transpose().col(2) * m_theta_dot;
}
template<typename S2, int O2>
MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
{
MotionPlain res;
se3ActionInverse_impl(m,res);
return res;
}
template<typename M1, typename M2>
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
// mout.linear() = v.linear().cross(angular())+v.angular().cross(linear());
AxisZ::cross(v.linear(),mout.linear());
mout.linear() *= -m_theta_dot;
typename M1::ConstAngularType w_in = v.angular();
typename M2::LinearType v_out = mout.linear();
v_out[0] -= w_in[2] * m_y_dot;
v_out[1] += w_in[2] * m_x_dot;
v_out[2] += -w_in[1] * m_x_dot + w_in[0] * m_y_dot ;
// Angular
AxisZ::cross(v.angular(),mout.angular());
mout.angular() *= -m_theta_dot;
}
template<typename M1>
MotionPlain motionAction(const MotionDense<M1> & v) const
{
MotionPlain res;
motionAction(v,res);
return res;
}
// data
Scalar m_x_dot;
......
......@@ -27,7 +27,23 @@
namespace se3
{
template<typename Scalar, int Options> struct MotionPrismaticUnalignedTpl;
template<typename Scalar, int Options=0> struct MotionPrismaticUnalignedTpl;
typedef MotionPrismaticUnalignedTpl<double> MotionPrismaticUnaligned;
namespace internal
{
template<typename Scalar, int Options>
struct SE3GroupAction< MotionPrismaticUnalignedTpl<Scalar,Options> >
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< MotionPrismaticUnalignedTpl<Scalar,Options>, MotionDerived>
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
}
template<typename _Scalar, int _Options>
struct traits< MotionPrismaticUnalignedTpl<_Scalar,_Options> >
......@@ -56,10 +72,13 @@ namespace se3
{
MOTION_TYPEDEF_TPL(MotionPrismaticUnalignedTpl);
MotionPrismaticUnalignedTpl () : axis(Vector3::Constant(NAN)), rate(NAN) {}
MotionPrismaticUnalignedTpl()
: axis(Vector3::Constant(NAN)), rate(NAN)
{}
template<typename Vector3Like, typename S2>
MotionPrismaticUnalignedTpl (const Eigen::MatrixBase<Vector3Like> & axis, const S2 rate)
MotionPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
const S2 rate)
: axis(axis), rate(rate)
{ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
......@@ -72,6 +91,58 @@ namespace se3
v.linear() += axis * rate;
}
template<typename S2, int O2, typename D2>
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
v.angular().setZero();
v.linear().noalias() = rate * (m.rotation() * axis); // TODO: check efficiency
}
template<typename S2, int O2>
MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
{
MotionPlain res;
se3Action_impl(m,res);
return res;
}
template<typename S2, int O2, typename D2>
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
// Linear
v.linear().noalias() = rate * (m.rotation().transpose() * axis);
// Angular
v.angular().setZero();
}
template<typename S2, int O2>
MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
{
MotionPlain res;
se3ActionInverse_impl(m,res);
return res;
}
template<typename M1, typename M2>
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
mout.linear().noalias() = v.angular().cross(axis);
mout.linear() *= rate;
// Angular
mout.angular().setZero();
}
template<typename M1>
MotionPlain motionAction(const MotionDense<M1> & v) const
{
MotionPlain res;
motionAction(v,res);
return res;
}
// data
Vector3 axis;
Scalar rate;
......
......@@ -31,6 +31,21 @@ namespace se3
template<typename Scalar, int Options, int _axis> struct MotionPrismaticTpl;
namespace internal
{
template<typename Scalar, int Options, int axis>
struct SE3GroupAction< MotionPrismaticTpl<Scalar,Options,axis> >
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename Scalar, int Options, int axis, typename MotionDerived>
struct MotionAlgebraAction< MotionPrismaticTpl<Scalar,Options,axis>, MotionDerived>
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
}
template<typename _Scalar, int _Options, int _axis>
struct traits < MotionPrismaticTpl<_Scalar,_Options,_axis> >
{
......@@ -57,21 +72,78 @@ namespace se3
struct MotionPrismaticTpl : MotionBase < MotionPrismaticTpl<_Scalar,_Options,_axis> >
{
MOTION_TYPEDEF_TPL(MotionPrismaticTpl);
enum { axis = _axis };
typedef SpatialAxis<_axis+LINEAR> Axis;
typedef typename Axis::CartesianAxis3 CartesianAxis3;
MotionPrismaticTpl() : v(NAN) {}
MotionPrismaticTpl( const Scalar & v ) : v(v) {}
MotionPrismaticTpl() : rate(NAN) {}
MotionPrismaticTpl( const Scalar & v ) : rate(v) {}
inline operator MotionPlain() const { return Axis() * v; }
inline operator MotionPlain() const { return Axis() * rate; }
template<typename Derived>
void addTo(MotionDense<Derived> & v_) const
{
typedef typename MotionDense<Derived>::Scalar OtherScalar;
v_.linear()[_axis] += (OtherScalar) v;
v_.linear()[_axis] += (OtherScalar) rate;
}
template<typename S2, int O2, typename D2>
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
v.angular().setZero();
v.linear().noalias() = rate * (m.rotation().col(axis));
}
template<typename S2, int O2>
MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
{
MotionPlain res;
se3Action_impl(m,res);
return res;
}
template<typename S2, int O2, typename D2>
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
// Linear
v.linear().noalias() = rate * (m.rotation().transpose().col(axis));
// Angular
v.angular().setZero();
}
template<typename S2, int O2>
MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
{
MotionPlain res;
se3ActionInverse_impl(m,res);
return res;
}
template<typename M1, typename M2>
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
CartesianAxis3::cross(v.angular(),mout.linear());
mout.linear() *= -rate;
// Angular
mout.angular().setZero();
}
template<typename M1>
MotionPlain motionAction(const MotionDense<M1> & v) const
{
MotionPlain res;
motionAction(v,res);
return res;
}
//data
Scalar v;
Scalar rate;
}; // struct MotionPrismaticTpl
template<typename Scalar, int Options, int axis, typename MotionDerived>
......@@ -207,7 +279,7 @@ namespace se3
*/
typedef typename MotionDerived::MotionPlain MotionPlain;
const typename MotionDerived::ConstAngularType & w = m1.angular();
const S1 & vx = m2.v;
const S1 & vx = m2.rate;
return MotionPlain(typename MotionPlain::Vector3(0,w[2]*vx,-w[1]*vx),
MotionPlain::Vector3::Zero());
}
......@@ -224,7 +296,7 @@ namespace se3
*/
typedef typename MotionDerived::MotionPlain MotionPlain;
const typename MotionDerived::ConstAngularType & w = m1.angular();
const S1 & vy = m2.v;
const S1 & vy = m2.rate;
return MotionPlain(typename MotionPlain::Vector3(-w[2]*vy,0,w[0]*vy),
MotionPlain::Vector3::Zero());
}
......@@ -241,7 +313,7 @@ namespace se3
*/
typedef typename MotionDerived::MotionPlain MotionPlain;
const typename MotionDerived::ConstAngularType & w = m1.angular();
const S1 & vz = m2.v;
const S1 & vz = m2.rate;
return MotionPlain(typename Motion::Vector3(w[1]*vz,-w[0]*vz,0),
MotionPlain::Vector3::Zero());
}
......@@ -433,7 +505,7 @@ namespace se3
typedef typename TangentVector::Scalar S2;
const S2 & v = vs[idx_v()];
data.v.v = v;
data.v.rate = v;
}
template<typename S2, int O2>
......
......@@ -27,8 +27,24 @@
namespace se3
{
template<typename Scalar, int Options> struct MotionRevoluteUnalignedTpl;
template<typename Scalar, int Options=0> struct MotionRevoluteUnalignedTpl;
typedef MotionRevoluteUnalignedTpl<double> MotionRevoluteUnaligned;
namespace internal
{
template<typename Scalar, int Options>
struct SE3GroupAction< MotionRevoluteUnalignedTpl<Scalar,Options> >
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< MotionRevoluteUnalignedTpl<Scalar,Options>, MotionDerived>
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
}
template<typename _Scalar, int _Options>
struct traits< MotionRevoluteUnalignedTpl<_Scalar,_Options> >
{
......@@ -76,6 +92,68 @@ namespace se3
{
v.angular() += axis*w;
}
template<typename S2, int O2, typename D2>
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
// Angular
v.angular().noalias() = m.rotation() * axis;
v.angular() *= w;
// Linear
v.linear().noalias() = m.translation().cross(v.angular());
}
template<typename S2, int O2>
MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
{
MotionPlain res;
se3Action_impl(m,res);
return res;
}
template<typename S2, int O2, typename D2>
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
// Linear
// TODO: use v.angular() as temporary variable
Vector3 v3_tmp;
v3_tmp.noalias() = axis.cross(m.translation());
v3_tmp *= w;
v.linear().noalias() = m.rotation().transpose() * v3_tmp;
// Angular
v.angular().noalias() = m.rotation().transpose() * axis;
v.angular() *= w;
}
template<typename S2, int O2>
MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
{
MotionPlain res;
se3ActionInverse_impl(m,res);
return res;
}
template<typename M1, typename M2>
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
mout.linear().noalias() = v.linear().cross(axis);
mout.linear() *= w;
// Angular
mout.angular().noalias() = v.angular().cross(axis);
mout.angular() *= w;
}
template<typename M1>
MotionPlain motionAction(const MotionDense<M1> & v) const
{
MotionPlain res;
motionAction(v,res);
return res;
}
// data
Vector3 axis;
......
......@@ -28,7 +28,7 @@ namespace se3
{
template<typename Scalar, int Options, int axis> struct JointRevoluteUnboundedTpl;
template<typename _Scalar, int _Options, int axis>
struct traits< JointRevoluteUnboundedTpl<_Scalar,_Options,axis> >
{
......@@ -92,7 +92,6 @@ namespace se3
SE3_JOINT_TYPEDEF_TEMPLATE;
typedef JointRevoluteTpl<Scalar,_Options,axis> JointDerivedBase;
using JointModelBase<JointModelRevoluteUnboundedTpl>::id;
using JointModelBase<JointModelRevoluteUnboundedTpl>::idx_q;
using JointModelBase<JointModelRevoluteUnboundedTpl>::idx_v;
......
......@@ -30,6 +30,21 @@ namespace se3
{
template<typename Scalar, int Options, int axis> struct MotionRevoluteTpl;
namespace internal
{
template<typename Scalar, int Options, int axis>
struct SE3GroupAction< MotionRevoluteTpl<Scalar,Options,axis> >
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename Scalar, int Options, int axis, typename MotionDerived>
struct MotionAlgebraAction< MotionRevoluteTpl<Scalar,Options,axis>, MotionDerived>
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
}
template<typename _Scalar, int _Options, int axis>
struct traits< MotionRevoluteTpl<_Scalar,_Options,axis> >
......@@ -58,6 +73,7 @@ namespace se3
{
MOTION_TYPEDEF_TPL(MotionRevoluteTpl);
typedef SpatialAxis<axis+ANGULAR> Axis;
typedef typename Axis::CartesianAxis3 CartesianAxis3;
MotionRevoluteTpl() : w(NAN) {}
......@@ -73,6 +89,63 @@ namespace se3
v.angular()[axis] += (OtherScalar)w;
}
template<typename S2, int O2, typename D2>
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
v.angular().noalias() = m.rotation().col(axis) * w;
v.linear().noalias() = m.translation().cross(v.angular());
}
template<typename S2, int O2>
MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
{
MotionPlain res;
se3Action_impl(m,res);
return res;
}
template<typename S2, int O2, typename D2>
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
// Linear
// TODO: use v.angular() as temporary variable
Vector3 v3_tmp;
CartesianAxis3::cross(m.translation(),v3_tmp);
v3_tmp *= w;
v.linear().noalias() = m.rotation().transpose() * v3_tmp;
// Angular
v.angular().noalias() = m.rotation().transpose().col(axis) * w;
}
template<typename S2, int O2>
MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
{
MotionPlain res;
se3ActionInverse_impl(m,res);
return res;
}
template<typename M1, typename M2>
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
CartesianAxis3::cross(v.linear(),mout.linear());
mout.linear() *= -w;
// Angular
CartesianAxis3::cross(v.angular(),mout.angular());
mout.angular() *= -w;
}
template<typename M1>
MotionPlain motionAction(const MotionDense<M1> & v) const
{
MotionPlain res;
motionAction(v,res);
return res;
}
// data
Scalar w;
}; // struct MotionRevoluteTpl
......
......@@ -31,6 +31,21 @@ namespace se3
template<typename Scalar, int Options> struct BiasSphericalZYXTpl;
namespace internal
{
template<typename Scalar, int Options>
struct SE3GroupAction< BiasSphericalZYXTpl<Scalar,Options> >
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< BiasSphericalZYXTpl<Scalar,Options>, MotionDerived>
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
}
template<typename _Scalar, int _Options>
struct traits< BiasSphericalZYXTpl<_Scalar,_Options> >
{
......@@ -74,6 +89,64 @@ namespace se3
void addTo(MotionDense<D2> & other) const
{ other.angular() += c_J; }
template<typename S2, int O2, typename D2>
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
// Angular
v.angular().noalias() = m.rotation() * c_J;
// Linear
v.linear().noalias() = m.translation().cross(v.angular());
}