Skip to content
GitLab
Commit 38948878 authored by gabrielebndn's avatar gabrielebndn Committed by Justin Carpentier
Browse files

Correct macro naming (#661) 

* Added PINOCCHIO_ prefix to Pinocchio EIGEN_ macros

* Converted (most) SE3_ macros to PINOCCHIO_

* Removed assert.hpp
parent 0d9b449b
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 38948878
......@@ -197,7 +197,6 @@ ENDIF(LUA5_FOUND)
LIST(APPEND HEADERS
exception.hpp
assert.hpp
macros.hpp
)
......
benchmark/timings-derivatives.cpp
View file @ 38948878
......@@ -28,9 +28,9 @@ void rnea_fd(const pinocchio::Model & model, pinocchio::Data & data_fd,
const Eigen::MatrixBase<Matrix2> & _drnea_dv,
const Eigen::MatrixBase<Matrix3> & _drnea_da)
{
Matrix1 & drnea_dq = EIGEN_CONST_CAST(Matrix1,_drnea_dq);
Matrix2 & drnea_dv = EIGEN_CONST_CAST(Matrix2,_drnea_dv);
Matrix3 & drnea_da = EIGEN_CONST_CAST(Matrix3,_drnea_da);
Matrix1 & drnea_dq = PINOCCHIO_EIGEN_CONST_CAST(Matrix1,_drnea_dq);
Matrix2 & drnea_dv = PINOCCHIO_EIGEN_CONST_CAST(Matrix2,_drnea_dv);
Matrix3 & drnea_da = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,_drnea_da);
using namespace Eigen;
VectorXd v_eps(VectorXd::Zero(model.nv));
......@@ -167,8 +167,8 @@ int main(int argc, const char ** argv)
taus[i] = Eigen::VectorXd::Random(model.nv);
}
EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd) drnea_dq(MatrixXd::Zero(model.nv,model.nv));
EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd) drnea_dv(MatrixXd::Zero(model.nv,model.nv));
PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd) drnea_dq(MatrixXd::Zero(model.nv,model.nv));
PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd) drnea_dv(MatrixXd::Zero(model.nv,model.nv));
MatrixXd drnea_da(MatrixXd::Zero(model.nv,model.nv));
MatrixXd daba_dq(MatrixXd::Zero(model.nv,model.nv));
......
bindings/python/spatial/explog.hpp
View file @ 38948878
......@@ -14,26 +14,26 @@ namespace pinocchio
{
template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,EIGEN_PLAIN_TYPE(Vector3Like)::Options>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
exp3_proxy(const Vector3Like & v)
{
return exp3(v);
}
template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,EIGEN_PLAIN_TYPE(Vector3Like)::Options>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
Jexp3_proxy(const Vector3Like & v)
{
typedef Eigen::Matrix<typename Vector3Like::Scalar,3,3,EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
typedef Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
ReturnType res; Jexp3(v,res);
return res;
}
template<typename Matrix3Like>
typename EIGEN_PLAIN_TYPE(Matrix3Like)
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)
Jlog3_proxy(const Matrix3Like & M)
{
typedef typename EIGEN_PLAIN_TYPE(Matrix3Like) ReturnType;
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) ReturnType;
ReturnType res; Jlog3(M,res);
return res;
}
......@@ -62,14 +62,14 @@ namespace pinocchio
}
template<typename Matrix3Like>
Eigen::Matrix<typename Matrix3Like::Scalar,3,1,EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
log3_proxy(const Matrix3Like & R)
{
return log3(R);
}
template<typename Matrix4Like>
MotionTpl<typename Matrix4Like::Scalar,EIGEN_PLAIN_TYPE(Matrix4Like)::Options>
MotionTpl<typename Matrix4Like::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix4Like)::Options>
log6_proxy(const Matrix4Like & homegenous_matrix)
{
return log6(homegenous_matrix);
......
src/algorithm/aba-derivatives.hxx
View file @ 38948878
......@@ -102,7 +102,7 @@ namespace pinocchio
ColsBlock U_cols = jmodel.jointCols(data.IS);
forceSet::se3Action(data.oMi[i],jdata.U(),U_cols); // expressed in the world frame
MatrixType & Minv_ = EIGEN_CONST_CAST(MatrixType,Minv);
MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType,Minv);
Minv_.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),jmodel.nv()) = jdata.Dinv();
const int nv_children = data.nvSubtree[i] - jmodel.nv();
......@@ -183,7 +183,7 @@ namespace pinocchio
ColsBlock UDinv_cols = jmodel.jointCols(data.UDinv);
forceSet::se3Action(data.oMi[i],jdata.UDinv(),UDinv_cols); // expressed in the world frame
MatrixType & Minv_ = EIGEN_CONST_CAST(MatrixType,Minv);
MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType,Minv);
if(parent > 0)
{
......@@ -225,7 +225,7 @@ namespace pinocchio
static void addForceCrossMatrix(const ForceDense<ForceDerived> & f,
const Eigen::MatrixBase<M6> & mout)
{
M6 & mout_ = EIGEN_CONST_CAST(M6,mout);
M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,mout);
addSkew(-f.linear(),mout_.template block<3,3>(ForceDerived::LINEAR,ForceDerived::ANGULAR));
addSkew(-f.linear(),mout_.template block<3,3>(ForceDerived::ANGULAR,ForceDerived::LINEAR));
addSkew(-f.angular(),mout_.template block<3,3>(ForceDerived::ANGULAR,ForceDerived::ANGULAR));
......@@ -354,7 +354,7 @@ namespace pinocchio
data.oa[0] = -model.gravity;
data.u = tau;
MatrixType3 & Minv_ = EIGEN_CONST_CAST(MatrixType3,aba_partial_dtau);
MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3,aba_partial_dtau);
Minv_.template triangularView<Eigen::Upper>().setZero();
/// First, compute Minv and a, the joint acceleration vector
......@@ -390,8 +390,8 @@ namespace pinocchio
Minv_.template triangularView<Eigen::StrictlyLower>()
= Minv_.transpose().template triangularView<Eigen::StrictlyLower>();
EIGEN_CONST_CAST(MatrixType1,aba_partial_dq).noalias() = -Minv_*data.dtau_dq;
EIGEN_CONST_CAST(MatrixType2,aba_partial_dv).noalias() = -Minv_*data.dtau_dv;
PINOCCHIO_EIGEN_CONST_CAST(MatrixType1,aba_partial_dq).noalias() = -Minv_*data.dtau_dq;
PINOCCHIO_EIGEN_CONST_CAST(MatrixType2,aba_partial_dv).noalias() = -Minv_*data.dtau_dv;
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
......@@ -424,7 +424,7 @@ namespace pinocchio
data.oa[0] = -model.gravity;
data.u = tau;
MatrixType3 & Minv_ = EIGEN_CONST_CAST(MatrixType3,aba_partial_dtau);
MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3,aba_partial_dtau);
Minv_.template triangularView<Eigen::Upper>().setZero();
/// First, compute Minv and a, the joint acceleration vector
......@@ -462,8 +462,8 @@ namespace pinocchio
Minv_.template triangularView<Eigen::StrictlyLower>()
= Minv_.transpose().template triangularView<Eigen::StrictlyLower>();
EIGEN_CONST_CAST(MatrixType1,aba_partial_dq).noalias() = -Minv_*data.dtau_dq;
EIGEN_CONST_CAST(MatrixType2,aba_partial_dv).noalias() = -Minv_*data.dtau_dv;
PINOCCHIO_EIGEN_CONST_CAST(MatrixType1,aba_partial_dq).noalias() = -Minv_*data.dtau_dq;
PINOCCHIO_EIGEN_CONST_CAST(MatrixType2,aba_partial_dv).noalias() = -Minv_*data.dtau_dv;
}
......
src/algorithm/aba.hxx
View file @ 38948878
......@@ -61,7 +61,7 @@ namespace pinocchio
struct SE3actOn
{
template<int Options, typename Matrix6Type>
static typename EIGEN_PLAIN_TYPE(Matrix6Type)
static typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type)
run(const SE3Tpl<Scalar,Options> & M,
const Eigen::MatrixBase<Matrix6Type> & I)
{
......@@ -71,10 +71,10 @@ namespace pinocchio
typedef const Eigen::Block<Matrix6Type,3,3> constBlock3;
typedef typename EIGEN_PLAIN_TYPE(Matrix6Type) ReturnType;
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type) ReturnType;
typedef Eigen::Block<ReturnType,3,3> Block3;
Matrix6Type & I_ = EIGEN_CONST_CAST(Matrix6Type,I);
Matrix6Type & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I);
const constBlock3 & Ai = I_.template block<3,3>(Inertia::LINEAR, Inertia::LINEAR);
const constBlock3 & Bi = I_.template block<3,3>(Inertia::LINEAR, Inertia::ANGULAR);
const constBlock3 & Di = I_.template block<3,3>(Inertia::ANGULAR, Inertia::ANGULAR);
......@@ -123,13 +123,13 @@ namespace pinocchio
typedef CppAD::AD<_Scalar> Scalar;
template<int Options, typename Matrix6Type>
static typename EIGEN_PLAIN_TYPE(Matrix6Type)
static typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type)
run(const SE3Tpl<Scalar,Options> & M,
const Eigen::MatrixBase<Matrix6Type> & I)
{
typedef SE3Tpl<Scalar,Options> SE3;
typedef typename EIGEN_PLAIN_TYPE(Matrix6Type) ReturnType;
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type) ReturnType;
const SE3 Minv = M.inverse();
typename SE3::ActionMatrixType dual_action_matrix(M.toDualActionMatrix());
......
src/algorithm/centroidal-derivatives.hxx
View file @ 38948878
......@@ -108,7 +108,7 @@ namespace pinocchio
static void addForceCrossMatrix(const ForceDense<ForceDerived> & f,
const Eigen::MatrixBase<M6> & mout)
{
M6 & mout_ = EIGEN_CONST_CAST(M6,mout);
M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,mout);
addSkew(-f.linear(),mout_.template block<3,3>(ForceDerived::LINEAR,ForceDerived::ANGULAR));
addSkew(-f.linear(),mout_.template block<3,3>(ForceDerived::ANGULAR,ForceDerived::LINEAR));
addSkew(-f.angular(),mout_.template block<3,3>(ForceDerived::ANGULAR,ForceDerived::ANGULAR));
......@@ -179,7 +179,7 @@ namespace pinocchio
const Eigen::MatrixBase<Min> & J,
const Eigen::MatrixBase<Mout> & F)
{
Mout & F_ = EIGEN_CONST_CAST(Mout,F);
Mout & F_ = PINOCCHIO_EIGEN_CONST_CAST(Mout,F);
motionSet::inertiaAction(Y,J.derived().transpose(),F_.transpose());
}
}; // struct CentroidalDynDerivativesBackwardStep
......@@ -202,8 +202,8 @@ namespace pinocchio
{
typedef ForceRef<typename Matrix6xLikeIn::ColXpr> ForceTypeIn;
typedef ForceRef<typename Matrix6xLikeOut::ColXpr> ForceTypeOut;
ForceTypeOut fout(EIGEN_CONST_CAST(Matrix6xLikeOut,Fout).col(k));
const ForceTypeIn fin(EIGEN_CONST_CAST(Matrix6xLikeIn,Fin).col(k));
ForceTypeOut fout(PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut,Fout).col(k));
const ForceTypeIn fin(PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeIn,Fin).col(k));
fout.linear() = fin.linear();
fout.angular().noalias() = fin.angular() - v3.cross(fin.linear());
}
......@@ -279,9 +279,9 @@ namespace pinocchio
data.Ig.inertia() = Ytot.inertia();
// Compute the partial derivatives
translateForceSet(data.dFdq,com,EIGEN_CONST_CAST(Matrix6xLike1,dhdot_dq));
translateForceSet(data.dFdv,com,EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv));
translateForceSet(data.dFda,com,EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da));
translateForceSet(data.dFdq,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike1,dhdot_dq));
translateForceSet(data.dFdv,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv));
translateForceSet(data.dFda,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da));
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
......@@ -389,9 +389,9 @@ namespace pinocchio
data.Ig.inertia() = Ytot.inertia();
// Retrieve the partial derivatives from RNEA derivatives
translateForceSet(data.Fcrb[0],com,EIGEN_CONST_CAST(Matrix6xLike1,dhdot_dq));
translateForceSet(data.dFdv,com,EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv));
translateForceSet(data.dFda,com,EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da));
translateForceSet(data.Fcrb[0],com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike1,dhdot_dq));
translateForceSet(data.dFdv,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv));
translateForceSet(data.dFda,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da));
}
} // namespace pinocchio
......
src/algorithm/cholesky.hpp
View file @ 38948878
......@@ -67,7 +67,7 @@ namespace pinocchio
/// \return A the result of \f$ Mv \f$.
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
typename EIGEN_PLAIN_TYPE(Mat)
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat)
Mv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & min);
......
src/algorithm/cholesky.hxx
View file @ 38948878
......@@ -70,7 +70,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & m)
{
Mat & m_ = EIGEN_CONST_CAST(Mat,m);
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
for(int k = 0; k < m_.cols(); ++k)
cholesky::Uv(model,data,m_.col(k));
}
......@@ -91,7 +91,7 @@ namespace pinocchio
assert(model.check(data) && "data is not consistent with model.");
#endif
assert(v.size() == model.nv);
Mat & v_ = EIGEN_CONST_CAST(Mat,v);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
const typename Data::MatrixXs & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow;
......@@ -110,7 +110,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & m)
{
Mat & m_ = EIGEN_CONST_CAST(Mat,m);
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
internal::Uv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
return m_.derived();
}
......@@ -125,7 +125,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & m)
{
Mat & m_ = EIGEN_CONST_CAST(Mat,m);
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
for(int k = 0; k < m_.cols(); ++k)
cholesky::Utv(model,data,m_.col(k));
}
......@@ -146,7 +146,7 @@ namespace pinocchio
assert(model.check(data) && "data is not consistent with model.");
#endif
assert(v.size() == model.nv);
Mat & v_ = EIGEN_CONST_CAST(Mat,v);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
const typename Data::MatrixXs & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow;
......@@ -165,7 +165,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & m)
{
Mat & m_ = EIGEN_CONST_CAST(Mat,m);
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
internal::Utv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
return m_.derived();
}
......@@ -180,7 +180,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & m)
{
Mat & m_ = EIGEN_CONST_CAST(Mat,m);
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
for(int k = 0; k < m_.cols(); ++k)
cholesky::Uiv(model,data,m_.col(k));
}
......@@ -201,7 +201,7 @@ namespace pinocchio
assert(model.check(data) && "data is not consistent with model.");
#endif
assert(v.size() == model.nv);
Mat & v_ = EIGEN_CONST_CAST(Mat,v);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
const typename Data::MatrixXs & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow;
......@@ -221,7 +221,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & m)
{
Mat & m_ = EIGEN_CONST_CAST(Mat,m);
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
internal::Uiv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
return m_.derived();
}
......@@ -236,7 +236,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & m)
{
Mat & m_ = EIGEN_CONST_CAST(Mat,m);
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
for(int k = 0; k < m_.cols(); ++k)
cholesky::Utiv(model,data,m_.col(k));
}
......@@ -257,7 +257,7 @@ namespace pinocchio
assert(model.check(data) && "data is not consistent with model.");
#endif
assert(v.size() == model.nv);
Mat & v_ = EIGEN_CONST_CAST(Mat,v);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
const typename Data::MatrixXs & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow;
......@@ -292,7 +292,7 @@ namespace pinocchio
const Eigen::MatrixBase<MatRes> & mout
)
{
MatRes & mout_ = EIGEN_CONST_CAST(MatRes,mout);
MatRes & mout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,mout);
for(int k = 0; k < min.cols(); ++k)
cholesky::Mv(model,data,min.col(k),mout_.col(k));
}
......@@ -316,7 +316,7 @@ namespace pinocchio
#endif
assert(vin.size() == model.nv);
assert(vout.size() == model.nv);
MatRes & vout_ = EIGEN_CONST_CAST(MatRes,vout);
MatRes & vout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,vout);
const typename Data::MatrixXs & M = data.M;
const std::vector<int> & nvt = data.nvSubtree_fromRow;
......@@ -345,11 +345,11 @@ namespace pinocchio
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat>
typename EIGEN_PLAIN_TYPE(Mat) Mv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) Mv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & min)
{
typedef typename EIGEN_PLAIN_TYPE(Mat) ReturnType;
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) ReturnType;
ReturnType res(model.nv,min.cols());
return Mv(model,data,min,res);
}
......@@ -380,7 +380,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & m)
{
Mat & m_ = EIGEN_CONST_CAST(Mat,m);
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
for(int k = 0; k < m_.cols(); ++k)
cholesky::UDUtv(model,data,m_.col(k));
}
......@@ -400,7 +400,7 @@ namespace pinocchio
#endif
assert(v.size() == model.nv);
Mat & v_ = EIGEN_CONST_CAST(Mat,v);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
cholesky::Utv(model,data,v_);
v_.array() *= data.D.array();
......@@ -415,7 +415,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & m)
{
Mat & m_ = EIGEN_CONST_CAST(Mat,m);
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
internal::UDUtv<Mat>::run(model,data,m_);
return m_;
......@@ -431,7 +431,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & m)
{
Mat & m_ = EIGEN_CONST_CAST(Mat,m);
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
for(int k = 0; k < m_.cols(); ++k)
cholesky::solve(model,data,m_.col(k));
}
......@@ -450,7 +450,7 @@ namespace pinocchio
assert(model.check(data) && "data is not consistent with model.");
#endif
Mat & v_ = EIGEN_CONST_CAST(Mat,v);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
cholesky::Uiv(model,data,v_);
v_.array() *= data.Dinv.array();
......@@ -465,7 +465,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & m)
{
Mat & m_ = EIGEN_CONST_CAST(Mat,m);
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m);
internal::solve<Mat,Mat::ColsAtCompileTime>::run(model,data,m_);
return m_.derived();
}
......@@ -490,7 +490,7 @@ namespace pinocchio
const typename Data::MatrixXs & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow;
Mat & v_ = EIGEN_CONST_CAST(Mat,v);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
const int last_col = std::min(col-1,model.nv-2); // You can start from nv-2 (no child in nv-1)
v_[col] = 1.;
......@@ -524,7 +524,7 @@ namespace pinocchio
assert(model.check(data) && "data is not consistent with model.");
#endif
Mat & Minv_ = EIGEN_CONST_CAST(Mat,Minv);
Mat & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,Minv);
for(int k = 0; k < model.nv; ++k)
internal::Miunit(model,data,k,Minv_.col(k));
......
src/algorithm/finite-differences.hxx
View file @ 38948878
......@@ -23,7 +23,7 @@ namespace pinocchio
static void algo(const JointModelBase<JointModel> & jmodel,
const Eigen::MatrixBase<TangentVectorType> & fd_increment)
{
jmodel.jointVelocitySelector(EIGEN_CONST_CAST(TangentVectorType,fd_increment))
jmodel.jointVelocitySelector(PINOCCHIO_EIGEN_CONST_CAST(TangentVectorType,fd_increment))
.fill(jmodel.finiteDifferenceIncrement());
}
......
src/algorithm/frames.hxx
View file @ 38948878
......@@ -124,13 +124,13 @@ namespace pinocchio
const JointIndex & joint_id = frame.parent;
if(rf == WORLD)
{
getJointJacobian(model,data,joint_id,WORLD,EIGEN_CONST_CAST(Matrix6xLike,J));
getJointJacobian(model,data,joint_id,WORLD,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J));
return;
}
if(rf == LOCAL)
{
Matrix6xLike & J_ = EIGEN_CONST_CAST(Matrix6xLike,J);
Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
const typename Data::SE3 & oMframe = data.oMf[frame_id];
const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
......@@ -148,7 +148,7 @@ namespace pinocchio
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const Eigen::MatrixBase<Matrix6xLike> & J)
{
getFrameJacobian(model,data,frame_id,LOCAL,EIGEN_CONST_CAST(Matrix6xLike,J));
getFrameJacobian(model,data,frame_id,LOCAL,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J));
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
......@@ -170,13 +170,13 @@ namespace pinocchio
const typename Model::JointIndex & joint_id = frame.parent;
if(rf == WORLD)
{
getJointJacobianTimeVariation(model,data,joint_id,WORLD,EIGEN_CONST_CAST(Matrix6xLike,dJ));
getJointJacobianTimeVariation(model,data,joint_id,WORLD,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ));
return;
}
if (rf == LOCAL)
{
Matrix6xLike & dJ_ = EIGEN_CONST_CAST(Matrix6xLike,dJ);
Matrix6xLike & dJ_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ);
const SE3 & oMframe = data.oMf[frame_id];
const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
......
src/algorithm/geometry.hxx
View file @ 38948878
......@@ -229,7 +229,7 @@ namespace pinocchio
/// Given p1..3 being either "min" or "max", return one of the corners of the
/// AABB cub of the FCL object.
#define SE3_GEOM_AABB(FCL,p1,p2,p3) \
#define PINOCCHIO_GEOM_AABB(FCL,p1,p2,p3) \
SE3::Vector3( \
FCL->aabb_local.p1##_ [0], \
FCL->aabb_local.p2##_ [1], \
......@@ -256,21 +256,21 @@ namespace pinocchio
// The radius is simply the one of the 8 corners of the AABB cube, expressed
// in the joint frame, whose norm is the highest.
radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,min,min,min)).squaredNorm(),radius);
radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,min,min,max)).squaredNorm(),radius);
radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,min,max,min)).squaredNorm(),radius);
radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,min,max,max)).squaredNorm(),radius);
radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,max,min,min)).squaredNorm(),radius);
radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,max,min,max)).squaredNorm(),radius);
radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,max,max,min)).squaredNorm(),radius);
radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,max,max,max)).squaredNorm(),radius);
radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(fcl,min,min,min)).squaredNorm(),radius);
radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(fcl,min,min,max)).squaredNorm(),radius);
radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(fcl,min,max,min)).squaredNorm(),radius);
radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(fcl,min,max,max)).squaredNorm(),radius);
radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(fcl,max,min,min)).squaredNorm(),radius);
radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(fcl,max,min,max)).squaredNorm(),radius);
radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(fcl,max,max,min)).squaredNorm(),radius);
radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(fcl,max,max,max)).squaredNorm(),radius);
// Don't forget to sqroot the squared norm before storing it.
geomData.radius[i] = sqrt(radius);
}
}
#undef SE3_GEOM_AABB
#undef PINOCCHIO_GEOM_AABB
#endif // PINOCCHIO_WITH_HPP_FCL
/* --- APPEND GEOMETRY MODEL ----------------------------------------------------------- */
......
src/algorithm/jacobian.hxx
View file @ 38948878
......@@ -128,7 +128,7 @@ namespace pinocchio
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef typename Data::Matrix6x::ConstColXpr M6xColXpr;
Matrix6Like & J_ = EIGEN_CONST_CAST(Matrix6Like,J);
Matrix6Like & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,J);
const typename Data::SE3 & oMjoint = data.oMi[jointId];
int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
......@@ -173,7 +173,7 @@ namespace pinocchio
data.liMi[i] = model.jointPlacements[i]*jdata.M();
data.iMf[parent] = data.liMi[i]*data.iMf[i];
Matrix6Like & J_ = EIGEN_CONST_CAST(Matrix6Like,J);
Matrix6Like & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,J);
jmodel.jointCols(J_) = data.iMf[i].inverse().act(jdata.S()); // TODO: use MotionRef
}
......@@ -197,7 +197,7 @@ namespace pinocchio
for(JointIndex i=jointId; i>0; i=model.parents[i])
{
Pass::run(model.joints[i],data.joints[i],
typename Pass::ArgsType(model,data,q,EIGEN_CONST_CAST(Matrix6Like,J)));
typename Pass::ArgsType(model,data,q,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,J)));
}
}
......@@ -297,7 +297,7 @@ namespace pinocchio
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
Matrix6Like & dJ_ = EIGEN_CONST_CAST(Matrix6Like,dJ);
Matrix6Like & dJ_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,dJ);
const typename Data::SE3 & oMjoint = data.oMi[jointId];
int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
......
src/algorithm/joint-configuration.hpp
View file @ 38948878
......@@ -20,7 +20,7 @@ namespace pinocchio
* @return The integrated configuration (size model.nq)
*/
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
inline typename EIGEN_PLAIN_TYPE(ConfigVectorType)
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v);
......@@ -53,7 +53,7 @@ namespace pinocchio
* @return The interpolated configuration (q0 if u = 0, q1 if u = 1)
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>