Skip to content
GitLab
Verified Commit c0969180 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

joints/consraint: set axis as protected member

parent 0a8689e1
Hide whitespace changes
Inline Side-by-side
src/multibody/joint/joint-prismatic-unaligned.hpp
View file @ c0969180
......@@ -238,14 +238,14 @@ namespace pinocchio
template<typename Vector3Like>
ConstraintPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
: axis(axis)
: m_axis(axis)
{ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
template<typename Vector1Like>
JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
return JointMotion(axis,v[0]);
return JointMotion(m_axis,v[0]);
}
template<typename S1, int O1>
......@@ -254,7 +254,7 @@ namespace pinocchio
{
typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType res;
MotionRef<DenseBase> v(res);
v.linear().noalias() = m.rotation()*axis;
v.linear().noalias() = m.rotation()*m_axis;
v.angular().setZero();
return res;
}
......@@ -265,7 +265,7 @@ namespace pinocchio
{
typename SE3GroupAction<ConstraintPrismaticUnalignedTpl>::ReturnType res;
MotionRef<DenseBase> v(res);
v.linear().noalias() = m.rotation().transpose()*axis;
v.linear().noalias() = m.rotation().transpose()*m_axis;
v.angular().setZero();
return res;
}
......@@ -283,7 +283,7 @@ namespace pinocchio
{
typedef typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType ReturnType;
ReturnType res;
res[0] = ref.axis.dot(f.linear());
res[0] = ref.axis().dot(f.linear());
return res;
}
......@@ -294,12 +294,12 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
/* Return ax.T * F[1:3,:] */
return ref.axis.transpose() * F.template middleRows<3>(LINEAR);
return ref.axis().transpose() * F.template middleRows<3>(LINEAR);
}
};
TransposeConst transpose() const { return TransposeConst(*this); }
TransposeConst transpose() const { return TransposeConst(*this); }
/* CRBA joint operators
* - ForceSet::Block = ForceSet
......@@ -310,7 +310,8 @@ namespace pinocchio
DenseBase matrix_impl() const
{
DenseBase S;
S << axis, Vector3::Zero();
S.template segment<3>(LINEAR) = m_axis;
S.template segment<3>(ANGULAR).setZero();
return S;
}
......@@ -318,13 +319,18 @@ namespace pinocchio
DenseBase motionAction(const MotionDense<MotionDerived> & v) const
{
DenseBase res;
res << v.angular().cross(axis), Vector3::Zero();
res.template segment<3>(LINEAR).noalias() = v.angular().cross(m_axis);
res.template segment<3>(ANGULAR).setZero();
return res;
}
const Vector3 & axis() const { return m_axis; }
Vector3 & axis() { return m_axis; }
// data
Vector3 axis;
protected:
Vector3 m_axis;
}; // struct ConstraintPrismaticUnalignedTpl
......@@ -352,8 +358,8 @@ namespace pinocchio
const S1 & m = Y.mass();
const typename Inertia::Vector3 & c = Y.lever();
res.template head<3>().noalias() = m*cpu.axis;
res.template tail<3>().noalias() = c.cross(res.template head<3>());
res.template segment<3>(Constraint::LINEAR).noalias() = m*cpu.axis();
res.template segment<3>(Constraint::ANGULAR).noalias() = c.cross(res.template segment<3>(Constraint::LINEAR));
return res;
}
......@@ -383,7 +389,7 @@ namespace pinocchio
const Constraint & cru)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
return Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis;
return Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis();
}
};
} // namespace impl
......
src/multibody/joint/joint-revolute-unaligned.hpp
View file @ c0969180
......@@ -249,14 +249,14 @@ namespace pinocchio
template<typename Vector3Like>
ConstraintRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
: axis(axis)
: m_axis(axis)
{}
template<typename Vector1Like>
JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
return JointMotion(axis,v[0]);
return JointMotion(m_axis,v[0]);
}
template<typename S1, int O1>
......@@ -267,7 +267,7 @@ namespace pinocchio
/* X*S = [ R pxR ; 0 R ] [ 0 ; a ] = [ px(Ra) ; Ra ] */
ReturnType res;
res.template segment<3>(ANGULAR).noalias() = m.rotation() * axis;
res.template segment<3>(ANGULAR).noalias() = m.rotation() * m_axis;
res.template segment<3>(LINEAR).noalias() = m.translation().cross(res.template segment<3>(ANGULAR));
return res;
}
......@@ -279,8 +279,8 @@ namespace pinocchio
typedef typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType ReturnType;
ReturnType res;
res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * axis;
res.template segment<3>(LINEAR).noalias() = -m.rotation().transpose() * m.translation().cross(axis);
res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * m_axis;
res.template segment<3>(LINEAR).noalias() = -m.rotation().transpose() * m.translation().cross(m_axis);
return res;
}
......@@ -297,7 +297,7 @@ namespace pinocchio
{
typedef typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType ReturnType;
ReturnType res;
res[0] = ref.axis.dot(f.angular());
res[0] = ref.axis().dot(f.angular());
return res;
}
......@@ -308,7 +308,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
/* Return ax.T * F[3:end,:] */
return ref.axis.transpose() * F.template middleRows<3>(ANGULAR);
return ref.axis().transpose() * F.template middleRows<3>(ANGULAR);
}
};
......@@ -324,7 +324,8 @@ namespace pinocchio
DenseBase matrix_impl() const
{
DenseBase S;
S << Vector3::Zero(), axis;
S.template segment<3>(LINEAR).setZero();
S.template segment<3>(ANGULAR) = m_axis;
return S;
}
......@@ -336,13 +337,18 @@ namespace pinocchio
const typename MotionDerived::ConstAngularType w = m.angular();
DenseBase res;
res << v.cross(axis), w.cross(axis);
res.template segment<3>(LINEAR).noalias() = v.cross(m_axis);
res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis);
return res;
}
// data
Vector3 axis;
const Vector3 & axis() const { return m_axis; }
Vector3 & axis() { return m_axis; }
protected:
Vector3 m_axis;
}; // struct ConstraintRevoluteUnalignedTpl
......@@ -371,8 +377,8 @@ namespace pinocchio
const typename Inertia::Vector3 & c = Y.lever();
const typename Inertia::Symmetric3 & I = Y.inertia();
res.template segment<3>(Inertia::LINEAR) = -m*c.cross(cru.axis);
res.template segment<3>(Inertia::ANGULAR).noalias() = I*cru.axis;
res.template segment<3>(Inertia::LINEAR) = -m*c.cross(cru.axis());
res.template segment<3>(Inertia::ANGULAR).noalias() = I*cru.axis();
res.template segment<3>(Inertia::ANGULAR) += c.cross(res.template segment<3>(Inertia::LINEAR));
return res;
......@@ -404,7 +410,7 @@ namespace pinocchio
const Constraint & cru)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis;
return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis();
}
};
} // namespace impl
......
Supports Markdown
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