Commit 64cb43fe authored by jcarpent's avatar jcarpent
Browse files

[Spatial] Add method addTo for all non MotionDense types

parent e90358cc
......@@ -68,6 +68,14 @@ namespace se3
{
return Motion (Motion::Vector3(x_dot_, y_dot_, 0.), Motion::Vector3(0., 0., theta_dot_));
}
template<typename Derived>
void addTo(MotionDense<Derived> & v) const
{
v.linear()[0] += x_dot_;
v.linear()[1] += y_dot_;
v.angular()[2] += theta_dot_;
}
}; // struct MotionPlanar
......
......@@ -54,13 +54,19 @@ namespace se3
{
MOTION_TYPEDEF(MotionPrismaticUnaligned);
MotionPrismaticUnaligned () : axis(Vector3::Constant(NAN)), v(NAN) {}
MotionPrismaticUnaligned (const Vector3 & axis, const Scalar v) : axis(axis), v(v) {}
MotionPrismaticUnaligned () : axis(Vector3::Constant(NAN)), rate(NAN) {}
MotionPrismaticUnaligned (const Vector3 & axis, const Scalar rate) : axis(axis), rate(rate) {}
Vector3 axis;
Scalar v;
Scalar rate;
operator Motion() const { return Motion(axis*v, Vector3::Zero());}
operator Motion() const { return Motion(axis*rate, Vector3::Zero());}
template<typename Derived>
void addTo(MotionDense<Derived> & v) const
{
v.linear() += axis * rate;
}
}; // struct MotionPrismaticUnaligned
inline const MotionPrismaticUnaligned & operator+ (const MotionPrismaticUnaligned & m, const BiasZero &)
......@@ -68,7 +74,7 @@ namespace se3
inline Motion operator+ (const MotionPrismaticUnaligned & m1, const Motion & m2)
{
return Motion(m1.v*m1.axis + m2.linear(), m2.angular());
return Motion(m1.rate*m1.axis + m2.linear(), m2.angular());
}
struct ConstraintPrismaticUnaligned;
......@@ -199,7 +205,7 @@ namespace se3
{
/* m1xm2 = [ v1xw2 + w1xv2; w1xw2 ] = [ v1xw2; w1xw2 ] */
const Motion::Vector3 & w1 = m1.angular();
const Motion::Vector3 & v2 = m2.axis * m2.v;
const Motion::Vector3 & v2 = m2.axis * m2.rate;
return Motion (w1.cross(v2), Motion::Vector3::Zero());
}
......@@ -350,7 +356,7 @@ namespace se3
const Scalar & v = vs[idx_v()];
data.M.translation() = axis * q;
data.v.v = v;
data.v.rate = v;
}
void calc_aba(JointDataDerived & data, Inertia::Matrix6 & I, const bool update_I) const
......
......@@ -88,6 +88,12 @@ namespace se3
Motion::Vector3::Zero()
);
}
template<typename Derived>
void addTo(MotionDense<Derived> & v_) const
{
v_.linear()[axis] += v;
}
}; // struct MotionPrismatic
template <int axis>
......
......@@ -64,6 +64,12 @@ namespace se3
return Motion(Motion::Vector3::Zero(),
axis*w);
}
template<typename Derived>
void addTo(MotionDense<Derived> & v) const
{
v.angular() += axis*w;
}
}; // struct MotionRevoluteUnaligned
inline const MotionRevoluteUnaligned& operator+ (const MotionRevoluteUnaligned& m, const BiasZero&)
......
......@@ -91,6 +91,12 @@ namespace se3
typename revolute::CartesianVector3<axis>(w).vector()
);
}
template<typename Derived>
void addTo(MotionDense<Derived> & v) const
{
v.angular()[axis] += w;
}
}; // struct MotionRevolute
template <int axis >
......
......@@ -68,6 +68,12 @@ namespace se3
{
return Motion (Motion::Vector3::Zero (), w);
}
template<typename Derived>
void addTo(MotionDense<Derived> & v) const
{
v.angular() += w;
}
}; // struct MotionSpherical
inline const MotionSpherical operator+ (const MotionSpherical & m, const BiasZero & )
......
......@@ -72,6 +72,13 @@ namespace se3
v = other.v;
return *this;
}
template<typename Derived>
void addTo(MotionDense<Derived> & v_) const
{
v_.linear() += v;
}
}; // struct MotionTranslation
inline const MotionTranslation operator+ (const MotionTranslation & m, const BiasZero &)
......
......@@ -89,6 +89,10 @@ namespace se3
template<typename M1>
Derived & operator+=(const MotionDense<M1> & v) { return derived().__pequ__(v); }
template<typename M1>
Derived & operator+=(const MotionBase<M1> & v)
{ v.derived().addTo(derived()); return derived(); }
template<typename M1>
Derived & operator-=(const MotionDense<M1> & v) { return derived().__mequ__(v); }
MotionPlain operator*(const Scalar alpha) const { return derived().__mult__(alpha); }
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment