Beispiel #1
0
    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));
}