1 #include "pinocchio/parsers/urdf.hpp" 3 #include "pinocchio/algorithm/joint-configuration.hpp" 4 #include "pinocchio/algorithm/rnea-derivatives.hpp" 9 #ifndef PINOCCHIO_MODEL_DIR 10 #define PINOCCHIO_MODEL_DIR "path_to_the_model_dir" 13 int main(
int argc,
char ** argv)
33 Eigen::MatrixXd djoint_torque_dq = Eigen::MatrixXd::Zero(model.
nv,model.
nv);
34 Eigen::MatrixXd djoint_torque_dv = Eigen::MatrixXd::Zero(model.
nv,model.
nv);
35 Eigen::MatrixXd djoint_torque_da = Eigen::MatrixXd::Zero(model.
nv,model.
nv);
41 std::cout <<
"Joint torque: " << data.
tau.transpose() << std::endl;
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
TangentVectorType tau
Vector of joint torques (dim model.nv).
int main(int argc, char **argv)
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
void computeRNEADerivatives(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 > &a, const Eigen::MatrixBase< MatrixType1 > &rnea_partial_dq, const Eigen::MatrixBase< MatrixType2 > &rnea_partial_dv, const Eigen::MatrixBase< MatrixType3 > &rnea_partial_da)
Computes the partial derivatives of the Recursive Newton Euler Algorithms with respect to the joint c...
#define PINOCCHIO_MODEL_DIR
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
Main pinocchio namespace.
int nv
Dimension of the velocity vector space.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
JointCollectionTpl & model