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

joints/constraints: enforce the output consistency

parent 899f5772
......@@ -173,7 +173,33 @@ namespace pinocchio
typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
typedef DenseBase MatrixReturnType;
typedef const DenseBase ConstMatrixReturnType;
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
}; // traits ConstraintPrismaticUnaligned
template<typename Scalar, int Options>
struct SE3GroupAction< ConstraintPrismaticUnaligned<Scalar,Options> >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< ConstraintPrismaticUnaligned<Scalar,Options>,MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, typename ForceDerived>
struct ConstraintForceOp< ConstraintPrismaticUnaligned<Scalar,Options>, ForceDerived>
{
typedef typename traits< ConstraintRevoluteUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
};
template<typename Scalar, int Options, typename ForceSet>
struct ConstraintForceSetOp< ConstraintPrismaticUnaligned<Scalar,Options>, ForceSet>
{
typedef typename traits< ConstraintPrismaticUnaligned<Scalar,Options> >::Vector3 Vector3;
typedef typename MatrixMatrixProduct<Eigen::Transpose<const Vector3>,
typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
>::type ReturnType;
};
template<typename _Scalar, int _Options>
struct ConstraintPrismaticUnaligned
......@@ -184,7 +210,7 @@ namespace pinocchio
enum { NV = 1 };
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
typedef typename traits<ConstraintPrismaticUnaligned>::Vector3 Vector3;
ConstraintPrismaticUnaligned() {}
......@@ -216,33 +242,22 @@ namespace pinocchio
const ConstraintPrismaticUnaligned & ref;
TransposeConst(const ConstraintPrismaticUnaligned & ref) : ref(ref) {}
template<typename Derived>
Eigen::Matrix<
typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<Derived>::ConstLinearType),
1,1>
operator* (const ForceDense<Derived> & f) const
template<typename ForceDerived>
typename ConstraintForceOp<ConstraintPrismaticUnaligned,ForceDerived>::ReturnType
operator* (const ForceDense<ForceDerived> & f) const
{
typedef Eigen::Matrix<
typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<Derived>::ConstLinearType),
1,1> ReturnType;
ReturnType res; res[0] = ref.axis.dot(f.linear());
return res;
typedef typename ConstraintForceOp<ConstraintPrismaticUnaligned,ForceDerived>::ReturnType ReturnType;
return ReturnType(ref.axis.dot(f.linear()));
}
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
template<typename Derived>
friend
const typename MatrixMatrixProduct<
Eigen::Transpose<const Vector3>,
typename Eigen::MatrixBase<const Derived>::template NRowsBlockXpr<3>::Type
>::type
operator*(const TransposeConst & tc,
const Eigen::MatrixBase<Derived> & F)
template<typename ForceSet>
typename ConstraintForceSetOp<ConstraintPrismaticUnaligned,ForceSet>::ReturnType
operator*(const Eigen::MatrixBase<ForceSet> & F)
{
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
/* Return ax.T * F[1:3,:] */
return tc.ref.axis.transpose () * F.template topRows<3> ();
return ref.axis.transpose() * F.template middleRows<3>(LINEAR);
}
};
......@@ -313,15 +328,6 @@ namespace pinocchio
return Y.template block<6,3> (0,Inertia::LINEAR) * cpu.axis;
}
template<typename Scalar, int Options>
struct SE3GroupAction< ConstraintPrismaticUnaligned<Scalar,Options> >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< ConstraintPrismaticUnaligned<Scalar,Options>,MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options> struct JointPrismaticUnalignedTpl;
template<typename _Scalar, int _Options>
......
......@@ -257,6 +257,22 @@ namespace pinocchio
typedef DenseBase MatrixReturnType;
typedef const DenseBase ConstMatrixReturnType;
}; // traits ConstraintRevolute
template<typename Scalar, int Options, int axis>
struct SE3GroupAction< ConstraintPrismatic<Scalar,Options,axis> >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, int axis, typename MotionDerived>
struct MotionAlgebraAction< ConstraintPrismatic<Scalar,Options,axis>, MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, int axis, typename ForceDerived>
struct ConstraintForceOp< ConstraintPrismatic<Scalar,Options,axis>, ForceDerived>
{ typedef typename ForceDense<ForceDerived>::ConstLinearType::template ConstFixedBlockXpr<1,1>::Type ReturnType; };
template<typename Scalar, int Options, int axis, typename ForceSet>
struct ConstraintForceSetOp< ConstraintPrismatic<Scalar,Options,axis>, ForceSet>
{ typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
template<typename _Scalar, int _Options, int axis>
struct ConstraintPrismatic
......@@ -277,9 +293,10 @@ namespace pinocchio
}
template<typename S2, int O2>
DenseBase se3Action(const SE3Tpl<S2,O2> & m) const
typename SE3GroupAction<ConstraintPrismatic>::ReturnType
se3Action(const SE3Tpl<S2,O2> & m) const
{
DenseBase res;
typename SE3GroupAction<ConstraintPrismatic>::ReturnType res;
MotionRef<DenseBase> v(res);
v.linear() = m.rotation().col(axis);
v.angular().setZero();
......@@ -293,18 +310,18 @@ namespace pinocchio
const ConstraintPrismatic & ref;
TransposeConst(const ConstraintPrismatic & ref) : ref(ref) {}
template<typename Derived>
typename ForceDense<Derived>::ConstLinearType::template ConstFixedSegmentReturnType<1>::Type
operator* (const ForceDense<Derived> & f) const
template<typename ForceDerived>
typename ConstraintForceOp<ConstraintPrismatic,ForceDerived>::ReturnType
operator* (const ForceDense<ForceDerived> & f) const
{ return f.linear().template segment<1>(axis); }
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
template<typename D>
friend typename Eigen::MatrixBase<D>::ConstRowXpr
operator*( const TransposeConst &, const Eigen::MatrixBase<D> & F )
template<typename Derived>
typename ConstraintForceSetOp<ConstraintPrismatic,Derived>::ReturnType
operator*(const Eigen::MatrixBase<Derived> & F )
{
assert(F.rows()==6);
return F.row(axis);
return F.row(LINEAR+axis);
}
}; // struct TransposeConst
......@@ -325,9 +342,10 @@ namespace pinocchio
}
template<typename MotionDerived>
DenseBase motionAction(const MotionDense<MotionDerived> & m) const
typename MotionAlgebraAction<ConstraintPrismatic,MotionDerived>::ReturnType
motionAction(const MotionDense<MotionDerived> & m) const
{
DenseBase res;
typename MotionAlgebraAction<ConstraintPrismatic,MotionDerived>::ReturnType res;
MotionRef<DenseBase> v(res);
v = m.cross(Axis());
return res;
......@@ -387,14 +405,6 @@ namespace pinocchio
return Y.derived().col(Inertia::LINEAR + axis);
}
template<typename Scalar, int Options, int axis>
struct SE3GroupAction< ConstraintPrismatic<Scalar,Options,axis> >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, int axis, typename MotionDerived>
struct MotionAlgebraAction< ConstraintPrismatic<Scalar,Options,axis>, MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename _Scalar, int _Options, int _axis>
struct JointPrismaticTpl
{
......
......@@ -80,7 +80,7 @@ namespace pinocchio
}
template<typename MotionDerived>
void addTo(MotionDense<MotionDerived> & v) const
inline void addTo(MotionDense<MotionDerived> & v) const
{
v.angular() += axis*w;
}
......@@ -184,7 +184,33 @@ namespace pinocchio
typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
typedef DenseBase MatrixReturnType;
typedef const DenseBase ConstMatrixReturnType;
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
}; // traits ConstraintRevoluteUnalignedTpl
template<typename Scalar, int Options>
struct SE3GroupAction< ConstraintRevoluteUnalignedTpl<Scalar,Options> >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< ConstraintRevoluteUnalignedTpl<Scalar,Options>, MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, typename ForceDerived>
struct ConstraintForceOp< ConstraintRevoluteUnalignedTpl<Scalar,Options>, ForceDerived>
{
typedef typename traits< ConstraintRevoluteUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
};
template<typename Scalar, int Options, typename ForceSet>
struct ConstraintForceSetOp< ConstraintRevoluteUnalignedTpl<Scalar,Options>, ForceSet>
{
typedef typename traits< ConstraintRevoluteUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
typedef typename MatrixMatrixProduct<Eigen::Transpose<const Vector3>,
typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
>::type ReturnType;
};
template<typename _Scalar, int _Options>
struct ConstraintRevoluteUnalignedTpl
......@@ -195,7 +221,7 @@ namespace pinocchio
enum { NV = 1 };
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
typedef typename traits<ConstraintRevoluteUnalignedTpl>::Vector3 Vector3;
ConstraintRevoluteUnalignedTpl() {}
......@@ -212,10 +238,13 @@ namespace pinocchio
}
template<typename S1, int O1>
Eigen::Matrix<Scalar,6,1,Options> se3Action(const SE3Tpl<S1,O1> & m) const
typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType
se3Action(const SE3Tpl<S1,O1> & m) const
{
typedef typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType ReturnType;
/* X*S = [ R pxR ; 0 R ] [ 0 ; a ] = [ px(Ra) ; Ra ] */
Eigen::Matrix<Scalar,6,1,Options> res;
ReturnType res;
res.template segment<3>(ANGULAR).noalias() = m.rotation() * axis;
res.template segment<3>(LINEAR).noalias() = m.translation().cross(res.template segment<3>(ANGULAR));
return res;
......@@ -228,29 +257,20 @@ namespace pinocchio
const ConstraintRevoluteUnalignedTpl & ref;
TransposeConst(const ConstraintRevoluteUnalignedTpl & ref) : ref(ref) {}
template<typename Derived>
Eigen::Matrix<
typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<Derived>::ConstAngularType),
1,1>
operator*(const ForceDense<Derived> & f) const
template<typename ForceDerived>
typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType
operator*(const ForceDense<ForceDerived> & f) const
{
typedef Eigen::Matrix<
typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<Derived>::ConstAngularType),
1,1> ReturnType;
ReturnType res; res[0] = ref.axis.dot(f.angular());
return res;
typedef typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType ReturnType;
return ReturnType(ref.axis.dot(f.angular()));
}
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
template<typename Derived>
const typename MatrixMatrixProduct<
Eigen::Transpose<const Vector3>,
typename Eigen::MatrixBase<const Derived>::template NRowsBlockXpr<3>::Type
>::type
operator*(const Eigen::MatrixBase<Derived> & F)
template<typename ForceSet>
typename ConstraintForceSetOp<ConstraintRevoluteUnalignedTpl,ForceSet>::ReturnType
operator*(const Eigen::MatrixBase<ForceSet> & F)
{
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
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);
}
......@@ -273,7 +293,8 @@ namespace pinocchio
}
template<typename MotionDerived>
DenseBase motionAction(const MotionDense<MotionDerived> & m) const
typename MotionAlgebraAction<ConstraintRevoluteUnalignedTpl,MotionDerived>::ReturnType
motionAction(const MotionDense<MotionDerived> & m) const
{
const typename MotionDerived::ConstLinearType v = m.linear();
const typename MotionDerived::ConstAngularType w = m.angular();
......@@ -330,14 +351,6 @@ namespace pinocchio
return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis;
}
template<typename Scalar, int Options>
struct SE3GroupAction< ConstraintRevoluteUnalignedTpl<Scalar,Options> >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< ConstraintRevoluteUnalignedTpl<Scalar,Options>,MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options> struct JointRevoluteUnalignedTpl;
template<typename _Scalar, int _Options>
......
......@@ -318,6 +318,14 @@ namespace pinocchio
template<typename Scalar, int Options, int axis, typename MotionDerived>
struct MotionAlgebraAction< ConstraintRevoluteTpl<Scalar,Options,axis>, MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, int axis, typename ForceDerived>
struct ConstraintForceOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceDerived>
{ typedef typename ForceDense<ForceDerived>::ConstAngularType::template ConstFixedBlockXpr<1,1>::Type ReturnType; };
template<typename Scalar, int Options, int axis, typename ForceSet>
struct ConstraintForceSetOp< ConstraintRevoluteTpl<Scalar,Options,axis>, ForceSet>
{ typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
template<typename _Scalar, int _Options, int axis>
struct traits< ConstraintRevoluteTpl<_Scalar,_Options,axis> >
......@@ -368,15 +376,15 @@ namespace pinocchio
const ConstraintRevoluteTpl & ref;
TransposeConst(const ConstraintRevoluteTpl & ref) : ref(ref) {}
template<typename Derived>
typename ForceDense<Derived>::ConstAngularType::template ConstFixedSegmentReturnType<1>::Type
operator* (const ForceDense<Derived> & f) const
template<typename ForceDerived>
typename ConstraintForceOp<ConstraintRevoluteTpl,ForceDerived>::ReturnType
operator*(const ForceDense<ForceDerived> & f) const
{ return f.angular().template segment<1>(axis); }
/// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
template<typename D>
typename Eigen::MatrixBase<D>::ConstRowXpr
operator*(const Eigen::MatrixBase<D> & F) const
/// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
template<typename Derived>
typename ConstraintForceSetOp<ConstraintRevoluteTpl,Derived>::ReturnType
operator*(const Eigen::MatrixBase<Derived> & F) const
{
assert(F.rows()==6);
return F.row(ANGULAR + axis);
......
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