/// \brief The joint accelerations computed from ABA
Eigen::VectorXdddq;
VectorXsddq;
// ABA internal data
/// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
container::aligned_vector<Inertia::Matrix6>Yaba;// TODO: change with dense symmetric matrix6
container::aligned_vector<typenameInertia::Matrix6>Yaba;// TODO: change with dense symmetric matrix6
/// \brief Intermediate quantity corresponding to apparent torque [ABA]
Eigen::VectorXdu;// Joint Inertia
VectorXsu;// Joint Inertia
// CCRBA return quantities
/// \brief Centroidal Momentum Matrix
...
...
@@ -177,9 +204,16 @@ namespace se3
/// \brief Centroidal momentum quantity.
/// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame).
/// \note \f$ h_g = \left( m\dot{c}, L_{g} \right); \f$. \f$ h_g \f$ is the stack of the linear momentum and the angular momemtum vectors.
///
Forcehg;
/// \brief Centroidal momentum time derivative.
/// \note The centroidal momentum time derivative is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame).
/// \note \f$ \dot{h_g} = \left( m\ddot{c}, \dot{L}_{g} \right); \f$. \f$ \dot{h_g} \f$ is the stack of the linear momentum variation and the angular momemtum variation.
///
Forcedhg;
/// \brief Centroidal Composite Rigid Body Inertia.
/// \note \f$ hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroil momentum quantity.
InertiaIg;
...
...
@@ -193,16 +227,16 @@ namespace se3
std::vector<int>nvSubtree;
/// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition.
Eigen::MatrixXdU;
MatrixXsU;
/// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
Eigen::VectorXdD;
VectorXsD;
/// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
Eigen::VectorXdDinv;
VectorXsDinv;
/// \brief Temporary of size NV used in Cholesky Decomposition.
Eigen::VectorXdtmp;
VectorXstmp;
/// \brief First previous non-zero row in M (used in Cholesky Decomposition).
std::vector<int>parents_fromRow;
...
...
@@ -227,31 +261,31 @@ namespace se3
Matrix6xdAdv;
/// \brief Partial derivative of the joint torque vector with respect to the joint configuration.
Eigen::MatrixXddtau_dq;
MatrixXsdtau_dq;
/// \brief Partial derivative of the joint torque vector with respect to the joint velocity.
Eigen::MatrixXddtau_dv;
MatrixXsdtau_dv;
/// \brief Partial derivative of the joint acceleration vector with respect to the joint configuration.
Eigen::MatrixXdddq_dq;
MatrixXsddq_dq;
/// \brief Partial derivative of the joint acceleration vector with respect to the joint velocity.
Eigen::MatrixXdddq_dv;
MatrixXsddq_dv;
/// \brief Vector of joint placements wrt to algorithm end effector.
container::aligned_vector<SE3>iMf;
/// \brief Vector of subtree center of mass positions expressed in the root joint of the subtree. In other words, com[j] is the CoM position of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element com[0] corresponds to the center of mass position of the whole model and expressed in the global frame.
container::aligned_vector<Eigen::Vector3d>com;
container::aligned_vector<Vector3>com;
/// \brief Vector of subtree center of mass linear velocities expressed in the root joint of the subtree. In other words, vcom[j] is the CoM linear velocity of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element vcom[0] corresponds to the velocity of the CoM of the whole model expressed in the global frame.
container::aligned_vector<Eigen::Vector3d>vcom;
container::aligned_vector<Vector3>vcom;
/// \brief Vector of subtree center of mass linear accelerations expressed in the root joint of the subtree. In other words, acom[j] is the CoM linear acceleration of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element acom[0] corresponds to the acceleration of the CoM of the whole model expressed in the global frame.
container::aligned_vector<Eigen::Vector3d>acom;
container::aligned_vector<Vector3>acom;
/// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint \f$ j \f$. The element mass[0] corrresponds to the total mass of the model.
std::vector<double>mass;
std::vector<Scalar>mass;
/// \brief Jacobien of center of mass.
/// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass, expressed in the inertial frame. In other words, \f$ v_{\text{CoM}} = J_{\text{CoM}} \dot{q}\f$.
...
...
@@ -259,33 +293,33 @@ namespace se3
/// \brief Kinetic energy of the model.
doublekinetic_energy;
Scalarkinetic_energy;
/// \brief Potential energy of the model.
doublepotential_energy;
Scalarpotential_energy;
// Temporary variables used in forward dynamics
/// \brief Inverse of the operational-space inertia matrix
Eigen::MatrixXdJMinvJt;
MatrixXsJMinvJt;
/// \brief Cholesky decompostion of \f$\JMinvJt\f$.
Eigen::LLT<Eigen::MatrixXd>llt_JMinvJt;
Eigen::LLT<MatrixXs>llt_JMinvJt;
/// \brief Lagrange Multipliers corresponding to the contact forces in se3::forwardDynamics.
Eigen::VectorXdlambda_c;
VectorXslambda_c;
/// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
Eigen::MatrixXdsDUiJt;
MatrixXssDUiJt;
/// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$.
Eigen::VectorXdtorque_residual;
VectorXstorque_residual;
/// \brief Generalized velocity after impact.
Eigen::VectorXddq_after;
VectorXsdq_after;
/// \brief Lagrange Multipliers corresponding to the contact impulses in se3::impulseDynamics.
Eigen::VectorXdimpulse_c;
VectorXsimpulse_c;
// data related to regressor
Matrix3xstaticRegressor;
...
...
@@ -295,11 +329,11 @@ namespace se3
///
/// \param[in] model The model structure of the rigid body system.
