Commit 456fe702 authored by Justin Carpentier's avatar Justin Carpentier

joints/all: unirformize accessor to JointMotion

parent 9de0a8cf
......@@ -67,44 +67,44 @@ namespace pinocchio
MotionPlanarTpl(const Scalar & x_dot, const Scalar & y_dot,
const Scalar & theta_dot)
: m_x_dot(x_dot), m_y_dot(y_dot), m_theta_dot(theta_dot)
{}
{
m_data << x_dot, y_dot, theta_dot;
}
template<typename Vector3Like>
MotionPlanarTpl(const Eigen::MatrixBase<Vector3Like> & vj)
: m_x_dot(vj[0]), m_y_dot(vj[1]), m_theta_dot(vj[2])
: m_data(vj)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
}
inline PlainReturnType plain() const
{
return PlainReturnType(typename PlainReturnType::Vector3(m_x_dot,m_y_dot,Scalar(0)),
typename PlainReturnType::Vector3(Scalar(0),Scalar(0),m_theta_dot));
return PlainReturnType(typename PlainReturnType::Vector3(vx(),vy(),Scalar(0)),
typename PlainReturnType::Vector3(Scalar(0),Scalar(0),wz()));
}
template<typename Derived>
void addTo(MotionDense<Derived> & other) const
{
other.linear()[0] += m_x_dot;
other.linear()[1] += m_y_dot;
other.angular()[2] += m_theta_dot;
other.linear()[0] += vx();
other.linear()[1] += vy();
other.angular()[2] += wz();
}
template<typename MotionDerived>
void setTo(MotionDense<MotionDerived> & other) const
{
other.linear() << m_x_dot, m_y_dot, (Scalar)0;
other.angular() << (Scalar)0, (Scalar)0, m_theta_dot;
other.linear() << vx(), vy(), (Scalar)0;
other.angular() << (Scalar)0, (Scalar)0, wz();
}
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;
v.angular().noalias() = m.rotation().col(2) * wz();
v.linear().noalias() = m.rotation().template leftCols<2>() * m_data.template head<2>();
v.linear() += m.translation().cross(v.angular());
}
template<typename S2, int O2>
......@@ -121,12 +121,12 @@ namespace pinocchio
// Linear
// TODO: use v.angular() as temporary variable
Vector3 v3_tmp;
AxisZ::alphaCross(m_theta_dot,m.translation(),v3_tmp);
v3_tmp[0] += m_x_dot; v3_tmp[1] += m_y_dot;
AxisZ::alphaCross(wz(),m.translation(),v3_tmp);
v3_tmp[0] += vx(); v3_tmp[1] += vy();
v.linear().noalias() = m.rotation().transpose() * v3_tmp;
// Angular
v.angular().noalias() = m.rotation().transpose().col(2) * m_theta_dot;
v.angular().noalias() = m.rotation().transpose().col(2) * wz();
}
template<typename S2, int O2>
......@@ -141,17 +141,17 @@ namespace pinocchio
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
AxisZ::alphaCross(-m_theta_dot,v.linear(),mout.linear());
AxisZ::alphaCross(-wz(),v.linear(),mout.linear());
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 ;
v_out[0] -= w_in[2] * vy();
v_out[1] += w_in[2] * vx();
v_out[2] += -w_in[1] * vx() + w_in[0] * vy() ;
// Angular
AxisZ::alphaCross(-m_theta_dot,v.angular(),mout.angular());
AxisZ::alphaCross(-wz(),v.angular(),mout.angular());
}
template<typename M1>
......@@ -162,10 +162,26 @@ namespace pinocchio
return res;
}
// data
Scalar m_x_dot;
Scalar m_y_dot;
Scalar m_theta_dot;
const Scalar & vx() const { return m_data[0]; }
Scalar & vx() { return m_data[0]; }
const Scalar & vy() const { return m_data[1]; }
Scalar & vy() { return m_data[1]; }
const Scalar & wz() const { return m_data[2]; }
Scalar & wz() { return m_data[2]; }
const Vector3 & data() const { return m_data; }
Vector3 & data() { return m_data; }
bool isEqual_impl(const MotionPlanarTpl & other) const
{
return m_data == other.m_data;
}
protected:
Vector3 m_data;
}; // struct MotionPlanarTpl
......@@ -174,10 +190,10 @@ namespace pinocchio
operator+(const MotionPlanarTpl<Scalar,Options> & m1, const MotionDense<MotionDerived> & m2)
{
typename MotionDerived::MotionPlain result(m2);
result.linear()[0] += m1.m_x_dot;
result.linear()[1] += m1.m_y_dot;
result.linear()[0] += m1.vx();
result.linear()[1] += m1.vy();
result.angular()[2] += m1.m_theta_dot;
result.angular()[2] += m1.wz();
return result;
}
......@@ -269,7 +285,8 @@ namespace pinocchio
// LINEAR
X_subspace.template block <3,2>(Motion::LINEAR, 0) = m.rotation ().template leftCols <2> ();
X_subspace.template block <3,1>(Motion::LINEAR, 2) = m.translation().cross(m.rotation ().template rightCols<1>());
X_subspace.template block <3,1>(Motion::LINEAR, 2).noalias()
= m.translation().cross(m.rotation ().template rightCols<1>());
// ANGULAR
X_subspace.template block <3,2>(Motion::ANGULAR, 0).setZero ();
......@@ -285,8 +302,8 @@ namespace pinocchio
// LINEAR
X_subspace.template block <3,2>(Motion::LINEAR, 0) = m.rotation().transpose().template leftCols <2>();
X_subspace.template block <3,1>(Motion::ANGULAR,2) = m.rotation().transpose() * m.translation(); // tmp variable
X_subspace.template block <3,1>(Motion::LINEAR, 2) = -X_subspace.template block <3,1>(Motion::ANGULAR,2).cross(m.rotation().transpose().template rightCols<1>());
X_subspace.template block <3,1>(Motion::ANGULAR,2).noalias() = m.rotation().transpose() * m.translation(); // tmp variable
X_subspace.template block <3,1>(Motion::LINEAR, 2).noalias() = -X_subspace.template block <3,1>(Motion::ANGULAR,2).cross(m.rotation().transpose().template rightCols<1>());
// ANGULAR
X_subspace.template block <3,2>(Motion::ANGULAR, 0).setZero();
......@@ -488,9 +505,9 @@ namespace pinocchio
typename TangentVector::template ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.template segment<NV>(idx_v ());
data.v.m_x_dot = q_dot(0);
data.v.m_y_dot = q_dot(1);
data.v.m_theta_dot = q_dot(2);
data.v.vx() = q_dot(0);
data.v.vy() = q_dot(1);
data.v.wz() = q_dot(2);
}
template<typename Matrix6Like>
......
......@@ -65,39 +65,39 @@ namespace pinocchio
template<typename Vector3Like, typename S2>
MotionPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
const S2 & rate)
: axis(axis), rate(rate)
const S2 & m_v)
: m_axis(axis), m_v(m_v)
{ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
inline PlainReturnType plain() const
{
return PlainReturnType(axis*rate,
return PlainReturnType(m_axis*m_v,
PlainReturnType::Vector3::Zero());
}
template<typename OtherScalar>
MotionPrismaticUnalignedTpl __mult__(const OtherScalar & alpha) const
{
return MotionPrismaticUnalignedTpl(axis,alpha*rate);
return MotionPrismaticUnalignedTpl(m_axis,alpha*m_v);
}
template<typename Derived>
void addTo(MotionDense<Derived> & other) const
{
other.linear() += axis * rate;
other.linear() += m_axis * m_v;
}
template<typename Derived>
void setTo(MotionDense<Derived> & other) const
{
other.linear().noalias() = axis*rate;
other.linear().noalias() = m_axis*m_v;
other.angular().setZero();
}
template<typename S2, int O2, typename D2>
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
v.linear().noalias() = rate * (m.rotation() * axis); // TODO: check efficiency
v.linear().noalias() = m_v * (m.rotation() * m_axis); // TODO: check efficiency
v.angular().setZero();
}
......@@ -113,7 +113,7 @@ namespace pinocchio
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
// Linear
v.linear().noalias() = rate * (m.rotation().transpose() * axis);
v.linear().noalias() = m_v * (m.rotation().transpose() * m_axis);
// Angular
v.angular().setZero();
......@@ -131,8 +131,8 @@ namespace pinocchio
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
mout.linear().noalias() = v.angular().cross(axis);
mout.linear() *= rate;
mout.linear().noalias() = v.angular().cross(m_axis);
mout.linear() *= m_v;
// Angular
mout.angular().setZero();
......@@ -146,9 +146,21 @@ namespace pinocchio
return res;
}
// data
Vector3 axis;
Scalar rate;
bool isEqual_impl(const MotionPrismaticUnalignedTpl & other) const
{
return m_axis == other.m_axis && m_v == other.m_v;
}
const Scalar & linearRate() const { return m_v; }
Scalar & linearRate() { return m_v; }
const Vector3 & axis() const { return m_axis; }
Vector3 & axis() { return m_axis; }
protected:
Vector3 m_axis;
Scalar m_v;
}; // struct MotionPrismaticUnalignedTpl
template<typename Scalar, int Options, typename MotionDerived>
......@@ -156,7 +168,7 @@ namespace pinocchio
operator+(const MotionPrismaticUnalignedTpl<Scalar,Options> & m1, const MotionDense<MotionDerived> & m2)
{
typedef typename MotionDerived::MotionPlain ReturnType;
return ReturnType(m1.rate*m1.axis + m2.linear(), m2.angular());
return ReturnType(m1.m_v*m1.axis + m2.linear(), m2.angular());
}
template<typename MotionDerived, typename S2, int O2>
......@@ -509,7 +521,7 @@ namespace pinocchio
typedef typename TangentVector::Scalar S2;
const S2 & v = vs[idx_v()];
data.v.rate = v;
data.v.linearRate() = v;
}
template<typename Matrix6Like>
......
......@@ -66,28 +66,28 @@ namespace pinocchio
typedef typename Axis::CartesianAxis3 CartesianAxis3;
MotionPrismaticTpl() {}
MotionPrismaticTpl(const Scalar & v) : rate(v) {}
MotionPrismaticTpl(const Scalar & v) : m_v(v) {}
inline PlainReturnType plain() const { return Axis() * rate; }
inline PlainReturnType plain() const { return Axis() * m_v; }
template<typename OtherScalar>
MotionPrismaticTpl __mult__(const OtherScalar & alpha) const
{
return MotionPrismaticTpl(alpha*rate);
return MotionPrismaticTpl(alpha*m_v);
}
template<typename Derived>
void addTo(MotionDense<Derived> & other) const
{
typedef typename MotionDense<Derived>::Scalar OtherScalar;
other.linear()[_axis] += (OtherScalar) rate;
other.linear()[_axis] += (OtherScalar) m_v;
}
template<typename MotionDerived>
void setTo(MotionDense<MotionDerived> & other) const
{
for(Eigen::DenseIndex k = 0; k < 3; ++k)
other.linear()[k] = k == axis ? rate : (Scalar)0;
other.linear()[k] = k == axis ? m_v : (Scalar)0;
other.angular().setZero();
}
......@@ -95,7 +95,7 @@ namespace pinocchio
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
v.angular().setZero();
v.linear().noalias() = rate * (m.rotation().col(axis));
v.linear().noalias() = m_v * (m.rotation().col(axis));
}
template<typename S2, int O2>
......@@ -110,7 +110,7 @@ namespace pinocchio
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
// Linear
v.linear().noalias() = rate * (m.rotation().transpose().col(axis));
v.linear().noalias() = m_v * (m.rotation().transpose().col(axis));
// Angular
v.angular().setZero();
......@@ -128,7 +128,7 @@ namespace pinocchio
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
CartesianAxis3::alphaCross(-rate,v.angular(),mout.linear());
CartesianAxis3::alphaCross(-m_v,v.angular(),mout.linear());
// Angular
mout.angular().setZero();
......@@ -142,8 +142,17 @@ namespace pinocchio
return res;
}
//data
Scalar rate;
Scalar & linearRate() { return m_v; }
const Scalar & linearRate() const { return m_v; }
bool isEqual_impl(const MotionPrismaticTpl & other) const
{
return m_v == other.m_v;
}
protected:
Scalar m_v;
}; // struct MotionPrismaticTpl
template<typename Scalar, int Options, int axis, typename MotionDerived>
......@@ -585,7 +594,7 @@ namespace pinocchio
typedef typename TangentVector::Scalar S2;
const S2 & v = vs[idx_v()];
data.v.rate = v;
data.v.linearRate() = v;
}
template<typename Matrix6Like>
......
......@@ -65,40 +65,40 @@ namespace pinocchio
template<typename Vector3Like, typename OtherScalar>
MotionRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
const OtherScalar & w)
: axis(axis)
, w(w)
: m_axis(axis)
, m_w(w)
{}
inline PlainReturnType plain() const
{
return PlainReturnType(PlainReturnType::Vector3::Zero(),
axis*w);
m_axis*m_w);
}
template<typename OtherScalar>
MotionRevoluteUnalignedTpl __mult__(const OtherScalar & alpha) const
{
return MotionRevoluteUnalignedTpl(axis,alpha*w);
return MotionRevoluteUnalignedTpl(m_axis,alpha*m_w);
}
template<typename MotionDerived>
inline void addTo(MotionDense<MotionDerived> & v) const
{
v.angular() += axis*w;
v.angular() += m_axis*m_w;
}
template<typename Derived>
void setTo(MotionDense<Derived> & other) const
{
other.linear().setZero();
other.angular().noalias() = axis*w;
other.angular().noalias() = m_axis*m_w;
}
template<typename S2, int O2, typename D2>
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
{
// Angular
v.angular().noalias() = w * m.rotation() * axis;
v.angular().noalias() = m_w * m.rotation() * m_axis;
// Linear
v.linear().noalias() = m.translation().cross(v.angular());
......@@ -118,13 +118,13 @@ namespace pinocchio
// Linear
// TODO: use v.angular() as temporary variable
Vector3 v3_tmp;
v3_tmp.noalias() = axis.cross(m.translation());
v3_tmp *= w;
v3_tmp.noalias() = m_axis.cross(m.translation());
v3_tmp *= m_w;
v.linear().noalias() = m.rotation().transpose() * v3_tmp;
// Angular
v.angular().noalias() = m.rotation().transpose() * axis;
v.angular() *= w;
v.angular().noalias() = m.rotation().transpose() * m_axis;
v.angular() *= m_w;
}
template<typename S2, int O2>
......@@ -139,12 +139,12 @@ namespace pinocchio
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
mout.linear().noalias() = v.linear().cross(axis);
mout.linear() *= w;
mout.linear().noalias() = v.linear().cross(m_axis);
mout.linear() *= m_w;
// Angular
mout.angular().noalias() = v.angular().cross(axis);
mout.angular() *= w;
mout.angular().noalias() = v.angular().cross(m_axis);
mout.angular() *= m_w;
}
template<typename M1>
......@@ -155,9 +155,20 @@ namespace pinocchio
return res;
}
// data
Vector3 axis;
Scalar w;
bool isEqual_impl(const MotionRevoluteUnalignedTpl & other) const
{
return m_axis == other.m_axis && m_w == other.m_w;
}
const Scalar & angularRate() const { return m_w; }
Scalar & angularRate() { return m_w; }
const Vector3 & axis() const { return m_axis; }
Vector3 & axis() { return m_axis; }
protected:
Vector3 m_axis;
Scalar m_w;
}; // struct MotionRevoluteUnalignedTpl
......@@ -532,7 +543,7 @@ namespace pinocchio
{
calc(data,qs.derived());
data.v.w = (Scalar)vs[idx_v()];
data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
}
template<typename Matrix6Like>
......
......@@ -152,7 +152,7 @@ namespace pinocchio
const typename Eigen::MatrixBase<TangentVector> & vs) const
{
calc(data,qs.derived());
data.v.w = (Scalar)vs[idx_v()];
data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
}
template<typename Matrix6Like>
......
......@@ -300,7 +300,11 @@ namespace pinocchio
Scalar & angularRate() { return m_w; }
const Scalar & angularRate() const { return m_w; }
// data
bool isEqual_impl(const MotionRevoluteTpl & other) const
{
return m_w == other.m_w;
}
protected:
Scalar m_w;
......
......@@ -66,51 +66,51 @@ namespace pinocchio
template<typename Vector3Like>
MotionSphericalTpl(const Eigen::MatrixBase<Vector3Like> & w)
: w(w)
: m_w(w)
{}
Vector3 & operator() () { return w; }
const Vector3 & operator() () const { return w; }
Vector3 & operator() () { return m_w; }
const Vector3 & operator() () const { return m_w; }
inline PlainReturnType plain() const
{
return PlainReturnType(PlainReturnType::Vector3::Zero(), w);
return PlainReturnType(PlainReturnType::Vector3::Zero(), m_w);
}
template<typename MotionDerived>
void addTo(MotionDense<MotionDerived> & other) const
{
other.angular() += w;
other.angular() += m_w;
}
template<typename Derived>
void setTo(MotionDense<Derived> & other) const
{
other.linear().setZero();
other.angular() = w;
other.angular() = m_w;
}
MotionSphericalTpl __plus__(const MotionSphericalTpl & other) const
{
return MotionSphericalTpl(w + other.w);
return MotionSphericalTpl(m_w + other.m_w);
}
bool isEqual_impl(const MotionSphericalTpl & other) const
{
return w == other.w;
return m_w == other.m_w;
}
template<typename MotionDerived>
bool isEqual_impl(const MotionDense<MotionDerived> & other) const
{
return other.angular() == w && other.linear().isZero(0);
return other.angular() == m_w && other.linear().isZero(0);
}
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() * w;
v.angular().noalias() = m.rotation() * m_w;
// Linear
v.linear().noalias() = m.translation().cross(v.angular());
......@@ -130,11 +130,11 @@ namespace pinocchio
// Linear
// TODO: use v.angular() as temporary variable
Vector3 v3_tmp;
v3_tmp.noalias() = w.cross(m.translation());
v3_tmp.noalias() = m_w.cross(m.translation());
v.linear().noalias() = m.rotation().transpose() * v3_tmp;
// Angular
v.angular().noalias() = m.rotation().transpose() * w;
v.angular().noalias() = m.rotation().transpose() * m_w;
}
template<typename S2, int O2>
......@@ -149,10 +149,10 @@ namespace pinocchio
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
// Linear
mout.linear().noalias() = v.linear().cross(w);
mout.linear().noalias() = v.linear().cross(m_w);
// Angular
mout.angular().noalias() = v.angular().cross(w);
mout.angular().noalias() = v.angular().cross(m_w);
}
template<typename M1>
......@@ -162,16 +162,20 @@ namespace pinocchio
motionAction(v,res);
return res;
}
// data
Vector3 w;
const Vector3 & angular() const { return m_w; }
Vector3 & angular() { return m_w; }
protected:
Vector3 m_w;
}; // struct MotionSphericalTpl
template<typename S1, int O1, typename MotionDerived>
inline typename MotionDerived::MotionPlain
operator+(const MotionSphericalTpl<S1,O1> & m1, const MotionDense<MotionDerived> & m2)
{
return typename MotionDerived::MotionPlain(m2.linear(),m2.angular() + m1.w);
return typename MotionDerived::MotionPlain(m2.linear(),m2.angular() + m1.angular());
}
template<typename Scalar, int Options> struct ConstraintSphericalTpl;
......@@ -454,7 +458,7 @@ namespace pinocchio
{
calc(data,qs.derived());
data.v() = vs.template segment<NV>(idx_v());
data.v.angular() = vs.template segment<NV>(idx_v());
}
template<typename Matrix6Like>
......@@ -495,7 +499,6 @@ namespace pinocchio
return res;
}
}; // struct JointModelSphericalTpl
} // namespace pinocchio
......
......@@ -65,37 +65,42 @@ namespace pinocchio
template<typename Vector3Like>
MotionTranslationTpl(const Eigen::MatrixBase<Vector3Like> & v)
: rate(v)
: m_v(v)
{}
MotionTranslationTpl(const MotionTranslationTpl & other)
: rate(other.rate)
: m_v(other.m_v)
{}
Vector3 & operator()() { return rate; }
const Vector3 & operator()() const { return rate; }
Vector3 & operator()() { return m_v; }
const Vector3 & operator()() const { return m_v; }
inline PlainReturnType plain() const
{
return PlainReturnType(rate,PlainReturnType::Vector3::Zero());
return PlainReturnType(m_v,PlainReturnType::Vector3::Zero());
}
bool isEqual_impl(const MotionTranslationTpl & other) const
{
return m_v == other.m_v;
}
MotionTranslationTpl & operator=(const MotionTranslationTpl & other)
{
rate = other.rate;
m_v = other.m_v;
return *this;
}
template<typename Derived>
void addTo(MotionDense<Derived> & other) const
{
other.linear() += rate;
other.linear() += m_v;
}