GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: examples/inverse-dynamics-derivatives.cpp Lines: 14 14 100.0 %
Date: 2024-01-23 21:41:47 Branches: 25 58 43.1 %

Line Branch Exec Source
1
#include "pinocchio/parsers/urdf.hpp"
2
3
#include "pinocchio/algorithm/joint-configuration.hpp"
4
#include "pinocchio/algorithm/rnea-derivatives.hpp"
5
6
#include <iostream>
7
8
// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
9
#ifndef PINOCCHIO_MODEL_DIR
10
  #define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
11
#endif
12
13
1
int main(int argc, char ** argv)
14
{
15
  using namespace pinocchio;
16
17
  // You should change here to set up your own URDF file or just pass it as an argument of this example.
18





3
  const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf") : argv[1];
19
20
  // Load the URDF model
21
2
  Model model;
22
1
  pinocchio::urdf::buildModel(urdf_filename,model);
23
24
  // Build a data related to model
25
2
  Data data(model);
26
27
  // Sample a random joint configuration as well as random joint velocity and acceleration
28
2
  Eigen::VectorXd q = randomConfiguration(model);
29

2
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
30

2
  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
31
32
  // Allocate result container
33

2
  Eigen::MatrixXd djoint_torque_dq = Eigen::MatrixXd::Zero(model.nv,model.nv);
34

2
  Eigen::MatrixXd djoint_torque_dv = Eigen::MatrixXd::Zero(model.nv,model.nv);
35

1
  Eigen::MatrixXd djoint_torque_da = Eigen::MatrixXd::Zero(model.nv,model.nv);
36
37
  // Computes the inverse dynamics (RNEA) derivatives for all the joints of the robot
38
1
  computeRNEADerivatives(model, data, q, v, a, djoint_torque_dq, djoint_torque_dv, djoint_torque_da);
39
40
  // Get access to the joint torque
41


1
  std::cout << "Joint torque: " << data.tau.transpose() << std::endl;
42
1
}