EIGEN_DONT_INLINE void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs, const typename Eigen::MatrixBase<TangentVector> & vs) const { calc(data,qs.derived()); data.v.w = (Scalar)vs[idx_v()]; }
void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs, const typename Eigen::MatrixBase<TangentVector> & vs) const { calc(data,qs.derived()); 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); }
inline const typename Eigen::CwiseUnaryOp< typename Eigen::internal::scalar_add_op< typename Eigen::internal::traits<D>::Scalar>, const D > operator+(const typename Eigen::internal::traits<D>::Scalar &s, const typename Eigen::MatrixBase<D> &m) { return Eigen::CwiseUnaryOp< typename Eigen::internal::scalar_add_op< typename Eigen::internal::traits<D>::Scalar>, const D >(m.derived(),Eigen::internal::scalar_add_op< typename Eigen::internal::traits<D>::Scalar>(s)); }