示例#1
0
 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;
 }
示例#2
0
    EIGEN_STRONG_INLINE
    void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
    {
      // Linear
      CartesianAxis3::alphaCross(-w,v.linear(),mout.linear());

      // Angular
      CartesianAxis3::alphaCross(-w,v.angular(),mout.angular());
    }
示例#3
0
 void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
 {
   // Linear
   // TODO: use v.angular() as temporary variable
   Vector3 v3_tmp;
   CartesianAxis3::alphaCross(w,m.translation(),v3_tmp);
   v.linear().noalias() = m.rotation().transpose() * v3_tmp;
   
   // Angular
   v.angular().noalias() = m.rotation().transpose().col(axis) * w;
 }
示例#4
0
 void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
 {
   // 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;
   v.linear().noalias() = m.rotation().transpose() * v3_tmp;
   
   // Angular
   v.angular().noalias() = m.rotation().transpose().col(2) * m_theta_dot;
 }
示例#5
0
 DenseBase motionAction(const MotionDense<MotionDerived> & m) const
 {
   const typename MotionDerived::ConstLinearType v = m.linear();
   const typename MotionDerived::ConstAngularType w = m.angular();
   DenseBase res(DenseBase::Zero());
   
   res(0,1) = -w[2]; res(0,2) = v[1];
   res(1,0) = w[2]; res(1,2) = -v[0];
   res(2,0) = -w[1]; res(2,1) = w[0];
   res(3,2) = w[1];
   res(4,2) = -w[0];
   
   return res;
 }
示例#6
0
 DenseBase motionAction(const MotionDense<MotionDerived> & m) const
 {
   DenseBase res;
   MotionRef<DenseBase> v(res);
   v = m.cross(Axis());
   return res;
 }
示例#7
0
 void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
 {
   // Linear
   AxisZ::alphaCross(-m_theta_dot,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 ;
   
   // Angular
   AxisZ::alphaCross(-m_theta_dot,v.angular(),mout.angular());
 }
示例#8
0
 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;
 }
示例#9
0
 void addTo(MotionDense<Derived> & other) const
 {
   other.linear()[0] += m_x_dot;
   other.linear()[1] += m_y_dot;
   other.angular()[2] += m_theta_dot;
 }
示例#10
0
 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());
 }
示例#11
0
 void addTo(MotionDense<MotionDerived> & v) const
 {
   typedef typename MotionDense<MotionDerived>::Scalar OtherScalar;
   v.angular()[axis] += (OtherScalar)w;
 }
示例#12
0
 void setTo(MotionDense<MotionDerived> & m) const
 {
   m.linear().setZero();
   for(Eigen::DenseIndex k = 0; k < 3; ++k)
     m.angular()[k] = k == axis ? w : (Scalar)0;
 }