Skip to content

GitLab

Verified Commit b20f0039 authored by Justin Carpentier's avatar Justin Carpentier Committed by Justin Carpentier
Browse files

[All] Improve templatization of Model and Data struct

parent cd0b8c5a
Hide whitespace changes
Inline Side-by-side
bindings/python/algorithm/expose-aba.cpp
View file @ b20f0039
......@@ -37,7 +37,7 @@ namespace se3
using namespace Eigen;
bp::def("aba",
&aba<JointCollectionDefault,VectorXd,VectorXd,VectorXd>,
&aba<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd>,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)",
......@@ -46,7 +46,7 @@ namespace se3
bp::return_value_policy<bp::return_by_value>());
bp::def("aba",
&aba<JointCollectionDefault,VectorXd,VectorXd,VectorXd,Force>,
&aba<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd,Force>,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)",
......
bindings/python/algorithm/expose-centroidal.cpp
View file @ b20f0039
......@@ -28,7 +28,7 @@ namespace se3
using namespace Eigen;
bp::def("ccrba",
&ccrba<JointCollectionDefault,VectorXd,VectorXd>,
&ccrba<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)"),
......@@ -36,7 +36,7 @@ namespace se3
bp::return_value_policy<bp::return_by_value>());
bp::def("dccrba",
dccrba<JointCollectionDefault,VectorXd,VectorXd>,
dccrba<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)"),
......
bindings/python/algorithm/expose-com.cpp
View file @ b20f0039
......@@ -78,7 +78,7 @@ namespace se3
"Computes the center of mass position, velocity and acceleration by storing the result in Data"
"and returns the center of mass position of the full model expressed in the world frame.");
bp::def("jacobianCenterOfMass",&jacobianCenterOfMass<JointCollectionDefault,VectorXd>,
bp::def("jacobianCenterOfMass",&jacobianCenterOfMass<double,0,JointCollectionDefaultTpl,VectorXd>,
jacobianCenterOfMass_overload(bp::args("Model","Data",
"Joint configuration q (size Model::nq)"),
"Computes the jacobian of the center of mass, puts the result in Data and return it.")[
......
bindings/python/algorithm/expose-dynamics.cpp
View file @ b20f0039
......@@ -28,7 +28,7 @@ namespace se3
using namespace Eigen;
bp::def("forwardDynamics",
&forwardDynamics<JointCollectionDefault,VectorXd,VectorXd,VectorXd,MatrixXd,VectorXd>,
&forwardDynamics<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd,MatrixXd,VectorXd>,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)",
......@@ -41,7 +41,7 @@ namespace se3
bp::return_value_policy<bp::return_by_value>());
bp::def("impactDynamics",
&impulseDynamics<JointCollectionDefault,VectorXd,VectorXd,MatrixXd>,
&impulseDynamics<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,MatrixXd>,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity before impact v_before (size Model::nv)",
......
bindings/python/algorithm/expose-energy.cpp
View file @ b20f0039
......@@ -28,7 +28,7 @@ namespace se3
using namespace Eigen;
bp::def("kineticEnergy",
&kineticEnergy<JointCollectionDefault,VectorXd,VectorXd>,
&kineticEnergy<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)",
......@@ -38,7 +38,7 @@ namespace se3
"in data.kinetic_energy. By default, the first order kinematic quantities of the model are updated.");
bp::def("potentialEnergy",
&potentialEnergy<JointCollectionDefault,VectorXd>,
&potentialEnergy<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Update kinematics (bool)"),
......
bindings/python/algorithm/expose-geometry.cpp
View file @ b20f0039
......@@ -28,14 +28,14 @@ namespace se3
using namespace Eigen;
bp::def("updateGeometryPlacements",
&updateGeometryPlacements<JointCollectionDefault,VectorXd>,
&updateGeometryPlacements<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("model", "data", "geometry model", "geometry data", "Configuration vector q (size Model::nq)"),
"Update the placement of the collision objects according to the current configuration."
"The algorithm also updates the current placement of the joint in Data."
);
bp::def("updateGeometryPlacements",
&updateGeometryPlacements<JointCollectionDefault>,
&updateGeometryPlacements<double,0,JointCollectionDefaultTpl>,
bp::args("model", "data", "geometry model", "geometry data"),
"Update the placement of the collision objects according to the current joint placement stored in data."
);
......@@ -54,7 +54,7 @@ namespace se3
);
bp::def("computeCollisions",
&computeCollisions<JointCollectionDefault,VectorXd>,
&computeCollisions<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("model","data","geometry model","geometry data","Configuration q (size Model::nq)", "bool"),
"Update the geometry for a given configuration and"
"determine if all collision pairs are effectively in collision or not."
......@@ -74,7 +74,7 @@ namespace se3
);
bp::def("computeDistances",
&computeDistances<JointCollectionDefault,VectorXd>,
&computeDistances<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("model","data","geometry model","geometry data","Configuration q (size Model::nq)"),
"Update the geometry for a given configuration and"
"compute the distance between each collision pair"
......
bindings/python/algorithm/expose-joints.cpp
View file @ b20f0039
......@@ -41,14 +41,14 @@ namespace se3
using namespace Eigen;
bp::def("integrate",
&integrate<JointCollectionDefault,VectorXd,VectorXd>,
&integrate<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model",
"Configuration q (size Model::nq)",
"Velocity v (size Model::nv)"),
"Integrate the model for a tangent vector during one unit time .");
bp::def("interpolate",
&interpolate<JointCollectionDefault,VectorXd,VectorXd,double>,
&interpolate<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model",
"Configuration q1 (size Model::nq)",
"Configuration q2 (size Model::nq)",
......@@ -56,7 +56,7 @@ namespace se3
"Interpolate the model between two configurations.");
bp::def("difference",
&difference<JointCollectionDefault,VectorXd,VectorXd>,
&difference<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model",
"Configuration q1 (size Model::nq)",
"Configuration q2 (size Model::nq)"),
......@@ -64,14 +64,14 @@ namespace se3
"to go from q1 to q2");
bp::def("squaredDistance",
&squaredDistance<JointCollectionDefault,VectorXd,VectorXd>,
&squaredDistance<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model",
"Configuration q1 (size Model::nq)",
"Configuration q2 (size Model::nq)"),
"Squared distance vector between two configurations.");
bp::def("distance",
&distance<JointCollectionDefault,VectorXd,VectorXd>,
&distance<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model",
"Configuration q1 (size Model::nq)",
"Configuration q2 (size Model::nq)"),
......@@ -83,7 +83,7 @@ namespace se3
"Generate a random configuration in the bounds given by the lower and upper limits contained in model.");
bp::def("randomConfiguration",
&randomConfiguration<JointCollectionDefault,VectorXd,VectorXd>,
&randomConfiguration<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model",
"Joint lower limits (size Model::nq)",
"Joint upper limits (size Model::nq)"),
......@@ -95,7 +95,7 @@ namespace se3
"return the configuration normalized ");
bp::def("isSameConfiguration",
&isSameConfiguration<JointCollectionDefault,VectorXd,VectorXd>,
&isSameConfiguration<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model",
"Configuration q1 (size Model::nq)",
"Configuration q2 (size Model::nq)",
......
bindings/python/algorithm/expose-kinematics-derivatives.cpp
View file @ b20f0039
......@@ -66,7 +66,7 @@ namespace se3
using namespace Eigen;
bp::def("computeForwardKinematicsDerivatives",
&computeForwardKinematicsDerivatives<JointCollectionDefault,VectorXd,VectorXd,VectorXd>,
&computeForwardKinematicsDerivatives<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd>,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity v (size Model::nv)",
......
bindings/python/algorithm/expose-kinematics.cpp
View file @ b20f0039
......@@ -27,20 +27,20 @@ namespace se3
{
using namespace Eigen;
bp::def("updateGlobalPlacements",
&updateGlobalPlacements<JointCollectionDefault>,
&updateGlobalPlacements<double,0,JointCollectionDefaultTpl>,
bp::args("Model","Data"),
"Updates the global placements of all the frames of the kinematic "
"tree and put the results in data according to the relative placements of the joints.");
bp::def("forwardKinematics",
&forwardKinematics<JointCollectionDefault,VectorXd>,
&forwardKinematics<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
"Compute the global placements of all the joints of the kinematic "
"tree and put the results in data.");
bp::def("forwardKinematics",
&forwardKinematics<JointCollectionDefault,VectorXd,VectorXd>,
&forwardKinematics<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity v (size Model::nv)"),
......@@ -48,7 +48,7 @@ namespace se3
"tree and put the results in data.");
bp::def("forwardKinematics",
&forwardKinematics<JointCollectionDefault,VectorXd,VectorXd,VectorXd>,
&forwardKinematics<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd>,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity v (size Model::nv)",
......
bindings/python/algorithm/expose-regressor.cpp
View file @ b20f0039
......@@ -28,7 +28,7 @@ namespace se3
using namespace Eigen;
bp::def("computeStaticRegressor",
&regressor::computeStaticRegressor<JointCollectionDefault,VectorXd>,
&regressor::computeStaticRegressor<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
"Compute the static regressor that links the inertia parameters of the system to its center of mass position\n"
......
bindings/python/algorithm/expose-rnea.cpp
View file @ b20f0039
......@@ -28,7 +28,7 @@ namespace se3
using namespace Eigen;
bp::def("rnea",
&rnea<JointCollectionDefault,VectorXd,VectorXd,VectorXd>,
&rnea<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd>,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity v (size Model::nv)",
......@@ -37,7 +37,7 @@ namespace se3
bp::return_value_policy<bp::return_by_value>());
bp::def("rnea",
&rnea<JointCollectionDefault,VectorXd,VectorXd,VectorXd,Force>,
&rnea<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd,Force>,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity v (size Model::nv)",
......@@ -48,7 +48,7 @@ namespace se3
bp::def("nle",
&nonLinearEffects<JointCollectionDefault,VectorXd,VectorXd>,
&nonLinearEffects<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity v (size Model::nv)"),
......@@ -57,7 +57,7 @@ namespace se3
bp::def("computeGeneralizedGravity",
&computeGeneralizedGravity<JointCollectionDefault,VectorXd>,
&computeGeneralizedGravity<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
"Computes the generalized gravity contribution g(q) of the Lagrangian dynamics.",
......@@ -65,7 +65,7 @@ namespace se3
bp::def("computeCoriolisMatrix",
&computeCoriolisMatrix<JointCollectionDefault,VectorXd,VectorXd>,
&computeCoriolisMatrix<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity v (size Model::nv)"),
......
src/algorithm/aba-derivatives.hpp
View file @ b20f0039
......@@ -47,10 +47,10 @@ namespace se3
///
/// \sa se3::aba
///
template<typename JointCollection, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
typename MatrixType1, typename MatrixType2, typename MatrixType3>
inline void computeABADerivatives(const ModelTpl<JointCollection> & model,
DataTpl<JointCollection> & data,
inline void computeABADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & tau,
......@@ -78,9 +78,9 @@ namespace se3
///
/// \sa se3::aba and \sa se3::computeABADerivatives.
///
template<typename JointCollection, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
inline void computeABADerivatives(const ModelTpl<JointCollection> & model,
DataTpl<JointCollection> & data,
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
inline void computeABADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & tau)
......
src/algorithm/aba-derivatives.hxx
View file @ b20f0039
......@@ -25,12 +25,12 @@
namespace se3
{
template<typename JointCollection, typename ConfigVectorType, typename TangentVectorType>
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
struct ComputeABADerivativesForwardStep1
: public fusion::JointVisitorBase< ComputeABADerivativesForwardStep1<JointCollection,ConfigVectorType,TangentVectorType> >
: public fusion::JointVisitorBase< ComputeABADerivativesForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> >
{
typedef ModelTpl<JointCollection> Model;
typedef DataTpl<JointCollection> Data;
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef boost::fusion::vector<const Model &,
Data &,
......@@ -82,12 +82,12 @@ namespace se3
};
template<typename JointCollection, typename MatrixType>
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MatrixType>
struct ComputeABADerivativesBackwardStep1
: public fusion::JointVisitorBase< ComputeABADerivativesBackwardStep1<JointCollection,MatrixType> >
: public fusion::JointVisitorBase< ComputeABADerivativesBackwardStep1<Scalar,Options,JointCollectionTpl,MatrixType> >
{
typedef ModelTpl<JointCollection> Model;
typedef DataTpl<JointCollection> Data;
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef boost::fusion::vector<const Model &,
Data &,
......@@ -148,7 +148,7 @@ namespace se3
{
typename Data::Force & pa = data.f[i];
pa.toVector() += Ia * data.a[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u);
data.Yaba[parent] += AbaBackwardStep<JointCollection>::SE3actOn(data.liMi[i], Ia);
data.Yaba[parent] += AbaBackwardStep<Scalar,Options,JointCollectionTpl>::SE3actOn(data.liMi[i], Ia);
data.f[parent] += data.liMi[i].act(pa);
}
......@@ -156,12 +156,12 @@ namespace se3
};
template<typename JointCollection, typename MatrixType>
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MatrixType>
struct ComputeABADerivativesForwardStep2
: public fusion::JointVisitorBase< ComputeABADerivativesForwardStep2<JointCollection,MatrixType> >
: public fusion::JointVisitorBase< ComputeABADerivativesForwardStep2<Scalar,Options,JointCollectionTpl,MatrixType> >
{
typedef ModelTpl<JointCollection> Model;
typedef DataTpl<JointCollection> Data;
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef boost::fusion::vector<const Model &,
Data &,
......@@ -248,12 +248,12 @@ namespace se3
};
template<typename JointCollection>
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
struct ComputeABADerivativesBackwardStep2
: public fusion::JointVisitorBase< ComputeABADerivativesBackwardStep2<JointCollection> >
: public fusion::JointVisitorBase< ComputeABADerivativesBackwardStep2<Scalar,Options,JointCollectionTpl> >
{
typedef ModelTpl<JointCollection> Model;
typedef DataTpl<JointCollection> Data;
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef boost::fusion::vector<const Model &,
Data &> ArgsType;
......@@ -332,10 +332,10 @@ namespace se3
}
};
template<typename JointCollection, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
typename MatrixType1, typename MatrixType2, typename MatrixType3>
inline void computeABADerivatives(const ModelTpl<JointCollection> & model,
DataTpl<JointCollection> & data,
inline void computeABADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & tau,
......@@ -354,7 +354,7 @@ namespace se3
assert(aba_partial_dtau.rows() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<JointCollection>::JointIndex JointIndex;
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
data.a[0] = -model.gravity;
data.oa[0] = -model.gravity;
......@@ -363,7 +363,7 @@ namespace se3
MatrixType3 & Minv_ = EIGEN_CONST_CAST(MatrixType3,aba_partial_dtau);
/// First, compute Minv and a, the joint acceleration vector
typedef ComputeABADerivativesForwardStep1<JointCollection,ConfigVectorType,TangentVectorType1> Pass1;
typedef ComputeABADerivativesForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1> Pass1;
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
{
Pass1::run(model.joints[i],data.joints[i],
......@@ -371,21 +371,21 @@ namespace se3
}
data.Fcrb[0].setZero();
typedef ComputeABADerivativesBackwardStep1<JointCollection,MatrixType3> Pass2;
typedef ComputeABADerivativesBackwardStep1<Scalar,Options,JointCollectionTpl,MatrixType3> Pass2;
for(JointIndex i=(JointIndex)(model.njoints-1);i>0;--i)
{
Pass2::run(model.joints[i],data.joints[i],
typename Pass2::ArgsType(model,data,Minv_));
}
typedef ComputeABADerivativesForwardStep2<JointCollection,MatrixType3> Pass3;
typedef ComputeABADerivativesForwardStep2<Scalar,Options,JointCollectionTpl,MatrixType3> Pass3;
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
{
Pass3::run(model.joints[i],data.joints[i],
typename Pass3::ArgsType(model,data,Minv_));
}
typedef ComputeABADerivativesBackwardStep2<JointCollection> Pass4;
typedef ComputeABADerivativesBackwardStep2<Scalar,Options,JointCollectionTpl> Pass4;
for(JointIndex i=(JointIndex)(model.njoints-1);i>0;--i)
{
Pass4::run(model.joints[i],
......
src/algorithm/aba.hpp
View file @ b20f0039
......@@ -40,10 +40,10 @@ namespace se3
///
/// \return The current joint acceleration stored in data.ddq.
///
template<typename JointCollection, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
inline const typename DataTpl<JointCollection>::TangentVectorType &
aba(const ModelTpl<JointCollection> & model,
DataTpl<JointCollection> & data,
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
aba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & tau);
......@@ -66,10 +66,10 @@ namespace se3
///
/// \return The current joint acceleration stored in data.ddq.
///
template<typename JointCollection, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, typename ForceDerived>
inline const typename DataTpl<JointCollection>::TangentVectorType &
aba(const ModelTpl<JointCollection> & model,
DataTpl<JointCollection> & data,
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, typename ForceDerived>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
aba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & tau,
......@@ -87,10 +87,10 @@ namespace se3
///
/// \return The inverse of the joint space inertia matrix stored in data.ddq.
///
template<typename JointCollection, typename ConfigVectorType>
inline const typename DataTpl<JointCollection>::RowMatrixXd &
computeMinverse(const ModelTpl<JointCollection> & model,
DataTpl<JointCollection> & data,
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::RowMatrixXd &
computeMinverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q);
......
src/algorithm/aba.hxx
View file @ b20f0039
......@@ -26,12 +26,12 @@
namespace se3
{
template<typename JointCollection, typename ConfigVectorType, typename TangentVectorType>
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
struct AbaForwardStep1
: public fusion::JointVisitorBase< AbaForwardStep1<JointCollection,ConfigVectorType,TangentVectorType> >
: public fusion::JointVisitorBase< AbaForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> >
{
typedef ModelTpl<JointCollection> Model;
typedef DataTpl<JointCollection> Data;
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef boost::fusion::vector<const Model &,
Data &,
......@@ -67,12 +67,12 @@ namespace se3
};
template<typename JointCollection>
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
struct AbaBackwardStep
: public fusion::JointVisitorBase< AbaBackwardStep<JointCollection> >
: public fusion::JointVisitorBase< AbaBackwardStep<Scalar,Options,JointCollectionTpl> >
{
typedef ModelTpl<JointCollection> Model;
typedef DataTpl<JointCollection> Data;
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef boost::fusion::vector<const Model &,
Data &> ArgsType;
......@@ -103,12 +103,12 @@ namespace se3
}
}
template<typename Scalar, int Options, typename Matrix6Type>
template<typename S2, int O2, typename Matrix6Type>
inline static typename EIGEN_PLAIN_TYPE(Matrix6Type)
SE3actOn(const SE3Tpl<Scalar,Options> & M,
SE3actOn(const SE3Tpl<S2,O2> & M,
const Eigen::MatrixBase<Matrix6Type> & I)
{
typedef SE3Tpl<Scalar,Options> SE3;
typedef SE3Tpl<S2,O2> SE3;
typedef typename SE3::Matrix3 Matrix3;
typedef typename SE3::Vector3 Vector3;
......@@ -158,12 +158,12 @@ namespace se3
}
};
template<typename JointCollection>
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
struct AbaForwardStep2
: public fusion::JointVisitorBase< AbaForwardStep2<JointCollection> >
: public fusion::JointVisitorBase< AbaForwardStep2<Scalar,Options,JointCollectionTpl> >
{
typedef ModelTpl<JointCollection> Model;
typedef DataTpl<JointCollection> Data;
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef boost::fusion::vector<const Model &,
Data &> ArgsType;
......@@ -188,10 +188,10 @@ namespace se3
};
template<typename JointCollection, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
inline const typename DataTpl<JointCollection>::TangentVectorType &
aba(const ModelTpl<JointCollection> & model,
DataTpl<JointCollection> & data,
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
aba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & tau)
......@@ -201,27 +201,27 @@ namespace se3
assert(v.size() == model.nv && "The joint velocity vector is not of right size");
assert(tau.size() == model.nv && "The joint acceleration vector is not of right size");
typedef typename ModelTpl<JointCollection>::JointIndex JointIndex;
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
data.v[0].setZero();
data.a[0] = -model.gravity;
data.u = tau;
typedef AbaForwardStep1<JointCollection,ConfigVectorType,TangentVectorType1> Pass1;
typedef AbaForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1> Pass1;
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
{