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));
}