Verified Commit 08fb287d authored by Justin Carpentier's avatar Justin Carpentier
Browse files

data: init all the quantities

This will make easy the serialization of the Data struct
parent c8ada6ea
...@@ -142,7 +142,7 @@ namespace pinocchio ...@@ -142,7 +142,7 @@ namespace pinocchio
container::aligned_vector<Inertia> Ycrb; container::aligned_vector<Inertia> Ycrb;
/// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ \dot{Y}_{crb}\f$. See Data::Ycrb for more details. /// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ \dot{Y}_{crb}\f$. See Data::Ycrb for more details.
container::aligned_vector<typename Inertia::Matrix6> dYcrb; // TODO: change with dense symmetric matrix6 container::aligned_vector<Matrix6> dYcrb; // TODO: change with dense symmetric matrix6
/// \brief The joint space inertia matrix (a square matrix of dim model.nv). /// \brief The joint space inertia matrix (a square matrix of dim model.nv).
MatrixXs M; MatrixXs M;
...@@ -175,19 +175,19 @@ namespace pinocchio ...@@ -175,19 +175,19 @@ namespace pinocchio
Matrix6x IS; Matrix6x IS;
/// \brief Right variation of the inertia matrix /// \brief Right variation of the inertia matrix
container::aligned_vector<typename Inertia::Matrix6> vxI; container::aligned_vector<Matrix6> vxI;
/// \brief Left variation of the inertia matrix /// \brief Left variation of the inertia matrix
container::aligned_vector<typename Inertia::Matrix6> Ivx; container::aligned_vector<Matrix6> Ivx;
/// \brief Inertia quantities expressed in the world frame /// \brief Inertia quantities expressed in the world frame
container::aligned_vector<Inertia> oYcrb; container::aligned_vector<Inertia> oYcrb;
/// \brief Time variation of the inertia quantities expressed in the world frame /// \brief Time variation of the inertia quantities expressed in the world frame
container::aligned_vector<typename Inertia::Matrix6> doYcrb; container::aligned_vector<Matrix6> doYcrb;
/// \brief Temporary for derivative algorithms /// \brief Temporary for derivative algorithms
typename Inertia::Matrix6 Itmp; Matrix6 Itmp;
/// \brief Temporary for derivative algorithms /// \brief Temporary for derivative algorithms
Matrix6 M6tmp; Matrix6 M6tmp;
...@@ -199,7 +199,7 @@ namespace pinocchio ...@@ -199,7 +199,7 @@ namespace pinocchio
// ABA internal data // ABA internal data
/// \brief Inertia matrix of the subtree expressed as dense matrix [ABA] /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
container::aligned_vector<typename Inertia::Matrix6> Yaba; // TODO: change with dense symmetric matrix6 container::aligned_vector<Matrix6> Yaba; // TODO: change with dense symmetric matrix6
/// \brief Intermediate quantity corresponding to apparent torque [ABA] /// \brief Intermediate quantity corresponding to apparent torque [ABA]
TangentVectorType u; // Joint Inertia TangentVectorType u; // Joint Inertia
......
...@@ -23,79 +23,79 @@ namespace pinocchio ...@@ -23,79 +23,79 @@ namespace pinocchio
inline DataTpl<Scalar,Options,JointCollectionTpl>:: inline DataTpl<Scalar,Options,JointCollectionTpl>::
DataTpl(const Model & model) DataTpl(const Model & model)
: joints(0) : joints(0)
, a((std::size_t)model.njoints) , a((std::size_t)model.njoints,Motion::Zero())
, oa((std::size_t)model.njoints) , oa((std::size_t)model.njoints,Motion::Zero())
, a_gf((std::size_t)model.njoints) , a_gf((std::size_t)model.njoints,Motion::Zero())
, oa_gf((std::size_t)model.njoints) , oa_gf((std::size_t)model.njoints,Motion::Zero())
, v((std::size_t)model.njoints) , v((std::size_t)model.njoints,Motion::Zero())
, ov((std::size_t)model.njoints) , ov((std::size_t)model.njoints,Motion::Zero())
, f((std::size_t)model.njoints) , f((std::size_t)model.njoints,Force::Zero())
, of((std::size_t)model.njoints) , of((std::size_t)model.njoints,Force::Zero())
, h((std::size_t)model.njoints) , h((std::size_t)model.njoints,Force::Zero())
, oh((std::size_t)model.njoints) , oh((std::size_t)model.njoints,Force::Zero())
, oMi((std::size_t)model.njoints) , oMi((std::size_t)model.njoints,SE3::Identity())
, liMi((std::size_t)model.njoints) , liMi((std::size_t)model.njoints,SE3::Identity())
, tau(model.nv) , tau(VectorXs::Zero(model.nv))
, nle(model.nv) , nle(VectorXs::Zero(model.nv))
, g(model.nv) , g(VectorXs::Zero(model.nv))
, oMf((std::size_t)model.nframes) , oMf((std::size_t)model.nframes,SE3::Identity())
, Ycrb((std::size_t)model.njoints) , Ycrb((std::size_t)model.njoints,Inertia::Zero())
, dYcrb((std::size_t)model.njoints) , dYcrb((std::size_t)model.njoints,Inertia::Zero())
, M(model.nv,model.nv) , M(MatrixXs::Zero(model.nv,model.nv))
, Minv(model.nv,model.nv) , Minv(MatrixXs::Zero(model.nv,model.nv))
, C(model.nv,model.nv) , C(MatrixXs::Zero(model.nv,model.nv))
, dHdq(6,model.nv) , dHdq(Matrix6x::Zero(6,model.nv))
, dFdq(6,model.nv) , dFdq(Matrix6x::Zero(6,model.nv))
, dFdv(6,model.nv) , dFdv(Matrix6x::Zero(6,model.nv))
, dFda(6,model.nv) , dFda(Matrix6x::Zero(6,model.nv))
, SDinv(6,model.nv) , SDinv(Matrix6x::Zero(6,model.nv))
, UDinv(6,model.nv) , UDinv(Matrix6x::Zero(6,model.nv))
, IS(6,model.nv) , IS(MatrixXs::Zero(6,model.nv))
, vxI((std::size_t)model.njoints) , vxI((std::size_t)model.njoints,Inertia::Matrix6::Zero())
, Ivx((std::size_t)model.njoints) , Ivx((std::size_t)model.njoints,Inertia::Matrix6::Zero())
, oYcrb((std::size_t)model.njoints) , oYcrb((std::size_t)model.njoints,Inertia::Zero())
, doYcrb((std::size_t)model.njoints) , doYcrb((std::size_t)model.njoints,Inertia::Matrix6::Zero())
, ddq(model.nv) , ddq(VectorXs::Zero(model.nv))
, Yaba((std::size_t)model.njoints) , Yaba((std::size_t)model.njoints,Inertia::Matrix6::Zero())
, u(model.nv) , u(VectorXs::Zero(model.nv))
, Ag(6,model.nv) , Ag(Matrix6x::Zero(6,model.nv))
, dAg(6,model.nv) , dAg(Matrix6x::Zero(6,model.nv))
, Fcrb((std::size_t)model.njoints) , Fcrb((std::size_t)model.njoints,Matrix6x::Zero(6,model.nv))
, lastChild((std::size_t)model.njoints) , lastChild((std::size_t)model.njoints)
, nvSubtree((std::size_t)model.njoints) , nvSubtree((std::size_t)model.njoints)
, start_idx_v_fromRow((std::size_t)model.nv) , start_idx_v_fromRow((std::size_t)model.nv)
, end_idx_v_fromRow((std::size_t)model.nv) , end_idx_v_fromRow((std::size_t)model.nv)
, U(model.nv,model.nv) , U(MatrixXs::Identity(model.nv,model.nv))
, D(model.nv) , D(VectorXs::Zero(model.nv))
, Dinv(model.nv) , Dinv(VectorXs::Zero(model.nv))
, tmp(model.nv) , tmp(VectorXs::Zero(model.nv))
, parents_fromRow((std::size_t)model.nv) , parents_fromRow((std::size_t)model.nv)
, supports_fromRow((std::size_t)model.nv) , supports_fromRow((std::size_t)model.nv)
, nvSubtree_fromRow((std::size_t)model.nv) , nvSubtree_fromRow((std::size_t)model.nv)
, J(6,model.nv) , J(Matrix6x::Zero(6,model.nv))
, dJ(6,model.nv) , dJ(Matrix6x::Zero(6,model.nv))
, dVdq(6,model.nv) , dVdq(Matrix6x::Zero(6,model.nv))
, dAdq(6,model.nv) , dAdq(Matrix6x::Zero(6,model.nv))
, dAdv(6,model.nv) , dAdv(Matrix6x::Zero(6,model.nv))
, dtau_dq(MatrixXs::Zero(model.nv,model.nv)) , dtau_dq(MatrixXs::Zero(model.nv,model.nv))
, dtau_dv(MatrixXs::Zero(model.nv,model.nv)) , dtau_dv(MatrixXs::Zero(model.nv,model.nv))
, ddq_dq(MatrixXs::Zero(model.nv,model.nv)) , ddq_dq(MatrixXs::Zero(model.nv,model.nv))
, ddq_dv(MatrixXs::Zero(model.nv,model.nv)) , ddq_dv(MatrixXs::Zero(model.nv,model.nv))
, iMf((std::size_t)model.njoints) , iMf((std::size_t)model.njoints,SE3::Identity())
, com((std::size_t)model.njoints) , com((std::size_t)model.njoints,Vector3::Zero())
, vcom((std::size_t)model.njoints) , vcom((std::size_t)model.njoints,Vector3::Zero())
, acom((std::size_t)model.njoints) , acom((std::size_t)model.njoints,Vector3::Zero())
, mass((std::size_t)model.njoints) , mass((std::size_t)model.njoints,(Scalar)(-1))
, Jcom(3,model.nv) , Jcom(Matrix3x::Zero(3,model.nv))
, JMinvJt() , JMinvJt()
, llt_JMinvJt() , llt_JMinvJt()
, lambda_c() , lambda_c()
, sDUiJt(model.nv,model.nv) , sDUiJt(MatrixXs::Zero(model.nv,model.nv))
, torque_residual(model.nv) , torque_residual(VectorXs::Zero(model.nv))
, dq_after(model.nv) , dq_after(VectorXs::Zero(model.nv))
, impulse_c() , impulse_c()
, staticRegressor(3,4*(model.njoints-1)) , staticRegressor(Matrix3x::Zero(3,4*(model.njoints-1)))
, jointTorqueRegressor(model.nv,10*(model.njoints-1)) , jointTorqueRegressor(MatrixXs::Zero(model.nv,10*(model.njoints-1)))
#if EIGEN_VERSION_AT_LEAST(3,2,90) && !EIGEN_VERSION_AT_LEAST(3,2,93) #if EIGEN_VERSION_AT_LEAST(3,2,90) && !EIGEN_VERSION_AT_LEAST(3,2,93)
, kinematic_hessians(6,std::max(1,model.nv),std::max(1,model.nv)) // the minimum size should be 1 for compatibility reasons , kinematic_hessians(6,std::max(1,model.nv),std::max(1,model.nv)) // the minimum size should be 1 for compatibility reasons
#else #else
...@@ -116,32 +116,13 @@ namespace pinocchio ...@@ -116,32 +116,13 @@ namespace pinocchio
computeLastChild(model); computeLastChild(model);
/* Init for Coriolis */ /* Init for Coriolis */
C.setZero();
/* Init for Cholesky */ /* Init for Cholesky */
U.setIdentity();
computeParents_fromRow(model); computeParents_fromRow(model);
computeSupports_fromRow(model); computeSupports_fromRow(model);
/* Init Jacobian */
J.setZero();
Ag.setZero();
/* Init universe states relatively to itself */ /* Init universe states relatively to itself */
a[0].setZero();
oa[0].setZero();
v[0].setZero();
ov[0].setZero();
a_gf[0] = -model.gravity; a_gf[0] = -model.gravity;
f[0].setZero();
h[0].setZero();
oMi[0].setIdentity();
liMi[0].setIdentity();
oMf[0].setIdentity();
Yaba[0].setZero();
Ycrb[0].setZero();
kinematic_hessians.setZero(); kinematic_hessians.setZero();
} }
...@@ -225,6 +206,15 @@ namespace pinocchio ...@@ -225,6 +206,15 @@ namespace pinocchio
} }
} }
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
bool operator==(const DataTpl<Scalar,Options,JointCollectionTpl> & data1,
const DataTpl<Scalar,Options,JointCollectionTpl> & data2)
{
return
data1.joints == data2.joints
;
}
} // namespace pinocchio } // namespace pinocchio
/// @endcond /// @endcond
......
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