GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
#include "pinocchio/multibody/fcl.hpp" |
||
2 |
#include "pinocchio/parsers/urdf.hpp" |
||
3 |
|||
4 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
5 |
#include "pinocchio/algorithm/kinematics.hpp" |
||
6 |
#include "pinocchio/algorithm/geometry.hpp" |
||
7 |
|||
8 |
#include <iostream> |
||
9 |
|||
10 |
// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here. |
||
11 |
#ifndef PINOCCHIO_MODEL_DIR |
||
12 |
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir" |
||
13 |
#endif |
||
14 |
|||
15 |
1 |
int main(int argc, char ** argv) |
|
16 |
{ |
||
17 |
using namespace pinocchio; |
||
18 |
|||
19 |
✓✗✓✗ ✓✗✗✗ ✗✓✓✗ ✓✗✗✗ ✗✗✗✗ |
3 |
const std::string model_path = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots") : argv[1]; |
20 |
✗✓✓✗ |
2 |
const std::string mesh_dir = (argc<=1) ? PINOCCHIO_MODEL_DIR : argv[1]; |
21 |
✓✗ | 2 |
const std::string urdf_filename = model_path + "/ur_description/urdf/ur5_robot.urdf"; |
22 |
|||
23 |
// Load the urdf model |
||
24 |
✓✗ | 2 |
Model model; |
25 |
✓✗ | 1 |
pinocchio::urdf::buildModel(urdf_filename,model); |
26 |
✓✗ | 2 |
GeometryModel collision_model; |
27 |
✓✗ | 1 |
pinocchio::urdf::buildGeom(model, urdf_filename, COLLISION, collision_model, mesh_dir); |
28 |
✓✗ | 2 |
GeometryModel visual_model; |
29 |
✓✗ | 1 |
pinocchio::urdf::buildGeom(model, urdf_filename, VISUAL, visual_model, mesh_dir); |
30 |
✓✗✓✗ ✓✗ |
1 |
std::cout << "model name: " << model.name << std::endl; |
31 |
|||
32 |
// Create data required by the algorithms |
||
33 |
✓✗ | 2 |
Data data(model); |
34 |
✓✗ | 2 |
GeometryData collision_data(collision_model); |
35 |
✓✗ | 2 |
GeometryData visual_data(visual_model); |
36 |
|||
37 |
// Sample a random configuration |
||
38 |
✓✗ | 2 |
Eigen::VectorXd q = randomConfiguration(model); |
39 |
✓✗✓✗ ✓✗✓✗ |
1 |
std::cout << "q: " << q.transpose() << std::endl; |
40 |
|||
41 |
// Perform the forward kinematics over the kinematic tree |
||
42 |
✓✗ | 1 |
forwardKinematics(model,data,q); |
43 |
|||
44 |
// Update Geometry models |
||
45 |
✓✗ | 1 |
updateGeometryPlacements(model, data, collision_model, collision_data); |
46 |
✓✗ | 1 |
updateGeometryPlacements(model, data, visual_model, visual_data); |
47 |
|||
48 |
// Print out the placement of each joint of the kinematic tree |
||
49 |
✓✗✓✗ |
1 |
std::cout << "\nJoint placements:" << std::endl; |
50 |
✓✓ | 8 |
for(JointIndex joint_id = 0; joint_id < (JointIndex)model.njoints; ++joint_id) |
51 |
✓✗✓✗ |
7 |
std::cout << std::setw(24) << std::left |
52 |
✓✗✓✗ |
7 |
<< model.names[joint_id] << ": " |
53 |
✓✗✓✗ |
7 |
<< std::fixed << std::setprecision(2) |
54 |
✓✗✓✗ ✓✗ |
14 |
<< data.oMi[joint_id].translation().transpose() |
55 |
✓✗ | 7 |
<< std::endl; |
56 |
|||
57 |
// Print out the placement of each collision geometry object |
||
58 |
✓✗✓✗ |
1 |
std::cout << "\nCollision object placements:" << std::endl; |
59 |
✓✓ | 9 |
for(GeomIndex geom_id = 0; geom_id < (GeomIndex)collision_model.ngeoms; ++geom_id) |
60 |
✓✗✓✗ |
8 |
std::cout << geom_id << ": " |
61 |
✓✗✓✗ |
8 |
<< std::fixed << std::setprecision(2) |
62 |
✓✗✓✗ ✓✗ |
16 |
<< collision_data.oMg[geom_id].translation().transpose() |
63 |
✓✗ | 8 |
<< std::endl; |
64 |
|||
65 |
// Print out the placement of each visual geometry object |
||
66 |
✓✗✓✗ |
1 |
std::cout << "\nVisual object placements:" << std::endl; |
67 |
✓✓ | 8 |
for(GeomIndex geom_id = 0; geom_id < (GeomIndex)visual_model.ngeoms; ++geom_id) |
68 |
✓✗✓✗ |
7 |
std::cout << geom_id << ": " |
69 |
✓✗✓✗ |
7 |
<< std::fixed << std::setprecision(2) |
70 |
✓✗✓✗ ✓✗ |
14 |
<< visual_data.oMg[geom_id].translation().transpose() |
71 |
✓✗ | 7 |
<< std::endl; |
72 |
1 |
} |
Generated by: GCOVR (Version 4.2) |