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;
}
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 ;
return Motion( v1.cross(w2),w1.cross(w2));
}
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;
}
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;
}
inline Motion operator^( const Motion& m1, const MotionPrismatic<2>& m2)
{
/* nu1^nu2 = ( v1^w2+w1^v2, w1^w2 )
* nu1^(v2,0) = ( w1^v2 , 0 )
* (x,y,z)^(0,0,v) = ( yv,-xv,0 )
* nu1^(0,vx) = ( vy1 vx,-vx1 vx, 0, 0, 0, 0 )
*/
const Motion::Vector3& w = m1.angular();
const double & vx = m2.v;
return Motion( Motion::Vector3(w[1]*vx,-w[0]*vx,0),
Motion::Vector3::Zero());
}
inline Motion operator^( const Motion& m1, const MotionPrismatic<1>& m2)
{
/* nu1^nu2 = ( v1^w2+w1^v2, w1^w2 )
* nu1^(v2,0) = ( w1^v2 , 0 )
* (x,y,z)^(0,v,0) = ( -zv,0,xv )
* nu1^(0,vx) = ( -vz1 vx,0,vx1 vx, 0, 0, 0)
*/
const Motion::Vector3& w = m1.angular();
const double & vx = m2.v;
return Motion( Motion::Vector3(-w[2]*vx,0,w[0]*vx),
Motion::Vector3::Zero());
}
inline Motion operator+ (const MotionSpherical & m1, const Motion & m2)
{
return Motion( m2.linear(), m2.angular() + m1.w);
}
inline Motion operator^ (const Motion & m1, const MotionSpherical & m2)
{
return Motion(m1.linear ().cross (m2.w), m1.angular ().cross (m2.w));
}
Motion operator+( const MotionPrismatic<axis>& m1, const Motion& m2)
{
return Motion( m2.linear()+typename prismatic::CartesianVector3<axis>(m1.v),m2.angular());
}
inline Motion operator+ (const MotionRevoluteUnaligned& m1, const Motion& m2)
{
return Motion( m2.linear(), m1.w*m1.axis+m2.angular() );
}
