Verified Commit e1e2eeff authored by Justin Carpentier's avatar Justin Carpentier
Browse files

joints: fix operations

parent 50b407b6
...@@ -65,8 +65,8 @@ namespace pinocchio ...@@ -65,8 +65,8 @@ namespace pinocchio
template<typename Vector3Like, typename S2> template<typename Vector3Like, typename S2>
MotionPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis, MotionPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
const S2 & m_v) const S2 & v)
: m_axis(axis), m_v(m_v) : m_axis(axis), m_v(v)
{ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); } { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
inline PlainReturnType plain() const inline PlainReturnType plain() const
...@@ -168,7 +168,7 @@ namespace pinocchio ...@@ -168,7 +168,7 @@ namespace pinocchio
operator+(const MotionPrismaticUnalignedTpl<Scalar,Options> & m1, const MotionDense<MotionDerived> & m2) operator+(const MotionPrismaticUnalignedTpl<Scalar,Options> & m1, const MotionDense<MotionDerived> & m2)
{ {
typedef typename MotionDerived::MotionPlain ReturnType; typedef typename MotionDerived::MotionPlain ReturnType;
return ReturnType(m1.m_v*m1.axis + m2.linear(), m2.angular()); return ReturnType(m1.linearRate() * m1.axis() + m2.linear(), m2.angular());
} }
template<typename MotionDerived, typename S2, int O2> template<typename MotionDerived, typename S2, int O2>
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment