Verified Commit 2cdd2050 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

all: use RowMatrixXs instead of RowMatrixXd

parent caf8530c
...@@ -23,7 +23,7 @@ namespace se3 ...@@ -23,7 +23,7 @@ namespace se3
namespace python namespace python
{ {
const Data::RowMatrixXd & const Data::RowMatrixXs &
computeMinverse_proxy(const Model & model, Data & data, const Eigen::VectorXd & q) computeMinverse_proxy(const Model & model, Data & data, const Eigen::VectorXd & q)
{ {
computeMinverse(model,data,q); computeMinverse(model,data,q);
......
...@@ -76,7 +76,7 @@ namespace se3 ...@@ -76,7 +76,7 @@ namespace se3
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,ddq,"Joint accelerations (output of ABA)") .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,ddq,"Joint accelerations (output of ABA)")
.ADD_DATA_PROPERTY(container::aligned_vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body") .ADD_DATA_PROPERTY(container::aligned_vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,M,"The joint space inertia matrix") .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,M,"The joint space inertia matrix")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Data::RowMatrixXd,Minv,"The inverse of the joint space inertia matrix") .ADD_DATA_PROPERTY_READONLY_BYVALUE(Data::RowMatrixXs,Minv,"The inverse of the joint space inertia matrix")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,C,"The Coriolis C(q,v) matrix such that the Coriolis effects are given by c(q,v) = C(q,v)v") .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,C,"The Coriolis C(q,v) matrix such that the Coriolis effects are given by c(q,v) = C(q,v)v")
.ADD_DATA_PROPERTY(container::aligned_vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA") .ADD_DATA_PROPERTY(container::aligned_vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
.ADD_DATA_PROPERTY(std::vector<int>,lastChild,"Index of the last child (for CRBA)") .ADD_DATA_PROPERTY(std::vector<int>,lastChild,"Index of the last child (for CRBA)")
...@@ -134,7 +134,7 @@ namespace se3 ...@@ -134,7 +134,7 @@ namespace se3
bp::class_< std::vector<int> >("StdVec_int") bp::class_< std::vector<int> >("StdVec_int")
.def(bp::vector_indexing_suite< std::vector<int> >()); .def(bp::vector_indexing_suite< std::vector<int> >());
eigenpy::enableEigenPySpecific<Data::RowMatrixXd>(); eigenpy::enableEigenPySpecific<Data::RowMatrixXs>();
} }
}; };
......
...@@ -88,7 +88,7 @@ namespace se3 ...@@ -88,7 +88,7 @@ namespace se3
/// \return The inverse of the joint space inertia matrix stored in data.ddq. /// \return The inverse of the joint space inertia matrix stored in data.ddq.
/// ///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::RowMatrixXd & inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::RowMatrixXs &
computeMinverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, computeMinverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data, DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q); const Eigen::MatrixBase<ConfigVectorType> & q);
......
...@@ -377,7 +377,7 @@ namespace se3 ...@@ -377,7 +377,7 @@ namespace se3
const JointIndex & parent = model.parents[i]; const JointIndex & parent = model.parents[i];
typename Inertia::Matrix6 & Ia = data.Yaba[i]; typename Inertia::Matrix6 & Ia = data.Yaba[i];
typename Data::RowMatrixXd & Minv = data.Minv; typename Data::RowMatrixXs & Minv = data.Minv;
typename Data::Matrix6x & Fcrb = data.Fcrb[0]; typename Data::Matrix6x & Fcrb = data.Fcrb[0];
typename Data::Matrix6x & FcrbTmp = data.Fcrb.back(); typename Data::Matrix6x & FcrbTmp = data.Fcrb.back();
...@@ -437,7 +437,7 @@ namespace se3 ...@@ -437,7 +437,7 @@ namespace se3
const JointIndex & i = jmodel.id(); const JointIndex & i = jmodel.id();
const JointIndex & parent = model.parents[i]; const JointIndex & parent = model.parents[i];
typename Data::RowMatrixXd & Minv = data.Minv; typename Data::RowMatrixXs & Minv = data.Minv;
typename Data::Matrix6x & FcrbTmp = data.Fcrb.back(); typename Data::Matrix6x & FcrbTmp = data.Fcrb.back();
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
...@@ -461,7 +461,7 @@ namespace se3 ...@@ -461,7 +461,7 @@ namespace se3
}; };
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::RowMatrixXd & inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::RowMatrixXs &
computeMinverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, computeMinverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data, DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q) const Eigen::MatrixBase<ConfigVectorType> & q)
......
...@@ -82,7 +82,7 @@ namespace se3 ...@@ -82,7 +82,7 @@ namespace se3
typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x; typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6; typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXd; typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXs;
/// \brief Vector of se3::JointData associated to the se3::JointModel stored in model, /// \brief Vector of se3::JointData associated to the se3::JointModel stored in model,
/// encapsulated in JointDataAccessor. /// encapsulated in JointDataAccessor.
...@@ -150,7 +150,7 @@ namespace se3 ...@@ -150,7 +150,7 @@ namespace se3
MatrixXs M; MatrixXs M;
/// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv). /// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
RowMatrixXd Minv; RowMatrixXs Minv;
/// \brief The Coriolis matrix (a square matrix of dim model.nv). /// \brief The Coriolis matrix (a square matrix of dim model.nv).
MatrixXs C; MatrixXs C;
......
...@@ -54,7 +54,7 @@ BOOST_AUTO_TEST_CASE(test_aba_derivatives) ...@@ -54,7 +54,7 @@ BOOST_AUTO_TEST_CASE(test_aba_derivatives)
MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero(); MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero(); MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
Data::RowMatrixXd aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero(); Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
computeABADerivatives(model, data, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau); computeABADerivatives(model, data, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
computeRNEADerivatives(model,data_ref,q,v,a); computeRNEADerivatives(model,data_ref,q,v,a);
...@@ -152,7 +152,7 @@ BOOST_AUTO_TEST_CASE(test_aba_minimal_argument) ...@@ -152,7 +152,7 @@ BOOST_AUTO_TEST_CASE(test_aba_minimal_argument)
MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero(); MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero(); MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
Data::RowMatrixXd aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero(); Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
computeABADerivatives(model, data_ref, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau); computeABADerivatives(model, data_ref, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
...@@ -194,7 +194,7 @@ BOOST_AUTO_TEST_CASE(test_aba_derivatives_fext) ...@@ -194,7 +194,7 @@ BOOST_AUTO_TEST_CASE(test_aba_derivatives_fext)
MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero(); MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero(); MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
Data::RowMatrixXd aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero(); Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();
computeABADerivatives(model, data, q, v, tau, fext, computeABADerivatives(model, data, q, v, tau, fext,
aba_partial_dq, aba_partial_dv, aba_partial_dtau); aba_partial_dq, aba_partial_dv, aba_partial_dtau);
......
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