Verified Commit 51d0061d authored by Justin Carpentier's avatar Justin Carpentier
Browse files

all: fix rebase

parent e9b49b32
......@@ -30,24 +30,20 @@
namespace pinocchio
{
namespace internal
/// \brief Return type of the Constraint::Transpose * Force operation
template<class ConstraintDerived, typename Force>
struct ConstraintForceOp
{
/// \brief Return type of the Constraint::Transpose * Force operation
template<class ConstraintDerived, typename Force>
struct ConstraintForceOp
{
typedef ReturnTypeNotDefined ReturnType;
};
/// \brief Return type of the Constraint::Transpose * ForceSet operation
template<class ConstraintDerived, typename ForceSet>
struct ConstraintForceSetOp
{
typedef ReturnTypeNotDefined ReturnType;
};
} // namespace internal
typedef ReturnTypeNotDefined ReturnType;
};
/// \brief Return type of the Constraint::Transpose * ForceSet operation
template<class ConstraintDerived, typename ForceSet>
struct ConstraintForceSetOp
{
typedef ReturnTypeNotDefined ReturnType;
};
template<class Derived>
class ConstraintBase
{
......
......@@ -29,36 +29,32 @@ namespace pinocchio
typedef typename traits<Constraint>::ConstMatrixReturnType ConstMatrixReturnType;
}; // traits ScaledConstraint
namespace internal
template<class Constraint>
struct SE3GroupAction< ScaledConstraint<Constraint> >
{ typedef typename SE3GroupAction<Constraint>::ReturnType ReturnType; };
template<class Constraint, typename MotionDerived>
struct MotionAlgebraAction< ScaledConstraint<Constraint>, MotionDerived >
{ typedef typename MotionAlgebraAction<Constraint,MotionDerived>::ReturnType ReturnType; };
template<class Constraint, typename ForceDerived>
struct ConstraintForceOp< ScaledConstraint<Constraint>, ForceDerived>
{
template<class Constraint>
struct SE3GroupAction< ScaledConstraint<Constraint> >
{ typedef typename SE3GroupAction<Constraint>::ReturnType ReturnType; };
template<class Constraint, typename MotionDerived>
struct MotionAlgebraAction< ScaledConstraint<Constraint>, MotionDerived >
{ typedef typename MotionAlgebraAction<Constraint,MotionDerived>::ReturnType ReturnType; };
template<class Constraint, typename ForceDerived>
struct ConstraintForceOp< ScaledConstraint<Constraint>, ForceDerived>
{
typedef typename Constraint::Scalar Scalar;
typedef typename ConstraintForceOp<Constraint,ForceDerived>::ReturnType OriginalReturnType;
typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type IdealReturnType;
typedef Eigen::Matrix<Scalar,IdealReturnType::RowsAtCompileTime,IdealReturnType::ColsAtCompileTime,Constraint::Options> ReturnType;
};
template<class Constraint, typename ForceSet>
struct ConstraintForceSetOp< ScaledConstraint<Constraint>, ForceSet>
{
typedef typename Constraint::Scalar Scalar;
typedef typename ConstraintForceSetOp<Constraint,ForceSet>::ReturnType OriginalReturnType;
typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type ReturnType;
};
typedef typename Constraint::Scalar Scalar;
typedef typename ConstraintForceOp<Constraint,ForceDerived>::ReturnType OriginalReturnType;
}
typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type IdealReturnType;
typedef Eigen::Matrix<Scalar,IdealReturnType::RowsAtCompileTime,IdealReturnType::ColsAtCompileTime,Constraint::Options> ReturnType;
};
template<class Constraint, typename ForceSet>
struct ConstraintForceSetOp< ScaledConstraint<Constraint>, ForceSet>
{
typedef typename Constraint::Scalar Scalar;
typedef typename ConstraintForceSetOp<Constraint,ForceSet>::ReturnType OriginalReturnType;
typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type ReturnType;
};
template<class Constraint>
struct ScaledConstraint
: ConstraintBase< ScaledConstraint<Constraint> >
......@@ -70,7 +66,7 @@ namespace pinocchio
typedef ConstraintBase<ScaledConstraint> Base;
using Base::nv;
typedef typename internal::SE3GroupAction<Constraint>::ReturnType SE3ActionReturnType;
typedef typename SE3GroupAction<Constraint>::ReturnType SE3ActionReturnType;
ScaledConstraint() {}
......@@ -111,17 +107,17 @@ namespace pinocchio
TransposeConst(const ScaledConstraint & ref) : ref(ref) {}
template<typename Derived>
typename internal::ConstraintForceOp<ScaledConstraint,Derived>::ReturnType
typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType
operator*(const ForceDense<Derived> & f) const
{
// TODO: I don't know why, but we should a dense a return type, otherwise it failes at the evaluation level;
typedef typename internal::ConstraintForceOp<ScaledConstraint,Derived>::ReturnType ReturnType;
typedef typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType ReturnType;
return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f));
}
/// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
template<typename Derived>
typename internal::ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType
typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType
operator*(const Eigen::MatrixBase<Derived> & F) const
{
return ref.m_scaling_factor * (ref.m_constraint.transpose() * F);
......@@ -138,10 +134,10 @@ namespace pinocchio
}
template<typename MotionDerived>
typename internal::MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType
typename MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType
motionAction(const MotionDense<MotionDerived> & m) const
{
typedef typename internal::MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType ReturnType;
typedef typename MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType ReturnType;
ReturnType res = m_scaling_factor * m_constraint.motionAction(m);
return res;
}
......
......@@ -188,24 +188,24 @@ namespace pinocchio
}; // traits ConstraintPrismaticUnalignedTpl
template<typename Scalar, int Options>
struct SE3GroupAction< ConstraintPrismaticUnaligned<Scalar,Options> >
struct SE3GroupAction< ConstraintPrismaticUnalignedTpl<Scalar,Options> >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< ConstraintPrismaticUnaligned<Scalar,Options>,MotionDerived >
struct MotionAlgebraAction< ConstraintPrismaticUnalignedTpl<Scalar,Options>,MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, typename ForceDerived>
struct ConstraintForceOp< ConstraintPrismaticUnaligned<Scalar,Options>, ForceDerived>
struct ConstraintForceOp< ConstraintPrismaticUnalignedTpl<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>
struct ConstraintForceSetOp< ConstraintPrismaticUnalignedTpl<Scalar,Options>, ForceSet>
{
typedef typename traits< ConstraintPrismaticUnaligned<Scalar,Options> >::Vector3 Vector3;
typedef typename traits< ConstraintPrismaticUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
typedef typename MatrixMatrixProduct<Eigen::Transpose<const Vector3>,
typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
>::type ReturnType;
......@@ -253,10 +253,10 @@ namespace pinocchio
TransposeConst(const ConstraintPrismaticUnalignedTpl & ref) : ref(ref) {}
template<typename ForceDerived>
typename ConstraintForceOp<ConstraintPrismaticUnaligned,ForceDerived>::ReturnType
typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType
operator* (const ForceDense<ForceDerived> & f) const
{
typedef typename ConstraintForceOp<ConstraintPrismaticUnaligned,ForceDerived>::ReturnType ReturnType;
typedef typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType ReturnType;
ReturnType res;
res[0] = ref.axis.dot(f.linear());
return res;
......@@ -264,7 +264,7 @@ namespace pinocchio
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
template<typename ForceSet>
typename ConstraintForceSetOp<ConstraintPrismaticUnaligned,ForceSet>::ReturnType
typename ConstraintForceSetOp<ConstraintPrismaticUnalignedTpl,ForceSet>::ReturnType
operator*(const Eigen::MatrixBase<ForceSet> & F)
{
EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
......
......@@ -259,21 +259,20 @@ namespace pinocchio
typedef const DenseBase ConstMatrixReturnType;
}; // traits ConstraintRevolute
<<<<<<< HEAD
template<typename Scalar, int Options, int axis>
struct SE3GroupAction< ConstraintPrismatic<Scalar,Options,axis> >
struct SE3GroupAction< ConstraintPrismaticTpl<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 >
struct MotionAlgebraAction< ConstraintPrismaticTpl<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>
struct ConstraintForceOp< ConstraintPrismaticTpl<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>
struct ConstraintForceSetOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceSet>
{ typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
template<typename _Scalar, int _Options, int axis>
......@@ -297,10 +296,10 @@ namespace pinocchio
}
template<typename S2, int O2>
typename SE3GroupAction<ConstraintPrismatic>::ReturnType
typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType
se3Action(const SE3Tpl<S2,O2> & m) const
{
typename SE3GroupAction<ConstraintPrismatic>::ReturnType res;
typename SE3GroupAction<ConstraintPrismaticTpl>::ReturnType res;
MotionRef<DenseBase> v(res);
v.linear() = m.rotation().col(axis);
v.angular().setZero();
......@@ -315,13 +314,13 @@ namespace pinocchio
TransposeConst(const ConstraintPrismaticTpl & ref) : ref(ref) {}
template<typename ForceDerived>
typename ConstraintForceOp<ConstraintPrismatic,ForceDerived>::ReturnType
typename ConstraintForceOp<ConstraintPrismaticTpl,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 Derived>
typename ConstraintForceSetOp<ConstraintPrismatic,Derived>::ReturnType
typename ConstraintForceSetOp<ConstraintPrismaticTpl,Derived>::ReturnType
operator*(const Eigen::MatrixBase<Derived> & F )
{
assert(F.rows()==6);
......@@ -346,10 +345,10 @@ namespace pinocchio
}
template<typename MotionDerived>
typename MotionAlgebraAction<ConstraintPrismatic,MotionDerived>::ReturnType
typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType
motionAction(const MotionDense<MotionDerived> & m) const
{
typename MotionAlgebraAction<ConstraintPrismatic,MotionDerived>::ReturnType res;
typename MotionAlgebraAction<ConstraintPrismaticTpl,MotionDerived>::ReturnType res;
MotionRef<DenseBase> v(res);
v = m.cross(Axis());
return res;
......
......@@ -41,34 +41,27 @@ namespace pinocchio
typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
};
template<typename Vector6ArgType>
struct SE3GroupAction< MotionRef<Vector6ArgType> >
{
typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
};
template<typename Vector6ArgType, typename MotionDerived>
struct MotionAlgebraAction< MotionRef<Vector6ArgType>, MotionDerived >
{
typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
};
template<typename Vector6ArgType, typename Scalar>
struct RHSScalarMultiplication< MotionRef<Vector6ArgType>, Scalar >
{
typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
};
template<typename Vector6ArgType, typename Scalar>
struct LHSScalarMultiplication< MotionRef<Vector6ArgType>, Scalar >
namespace internal
{
typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
};
template<typename Vector6ArgType, typename Scalar>
struct RHSScalarMultiplication< MotionRef<Vector6ArgType>, Scalar >
{
typedef typename pinocchio::traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
};
template<typename Vector6ArgType, typename Scalar>
struct LHSScalarMultiplication< MotionRef<Vector6ArgType>, Scalar >
{
typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
};
}
template<typename Vector6ArgType, typename MotionDerived>
struct MotionAlgebraAction< MotionRef<Vector6ArgType>, MotionDerived >
{ typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType; };
template<typename Vector6ArgType>
class MotionRef : public MotionDense< MotionRef<Vector6ArgType> >
{
......
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