GCC Code Coverage Report

Directory: ./
File: examples/build-reduced-model.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 54 55 98.2%
Branches: 77 150 51.3%
Line Branch Exec Source
1 #include "pinocchio/parsers/urdf.hpp"
2
3 #include "pinocchio/algorithm/joint-configuration.hpp"
4 #include "pinocchio/algorithm/model.hpp"
5
6 #include <iostream>
7 #include <algorithm>
8
9 // PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
10 #ifndef PINOCCHIO_MODEL_DIR
11 #define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
12 #endif
13
14 template<typename T>
15 6 bool is_in_vector(const std::vector<T> & vector, const T & elt)
16 {
17 6 return vector.end() != std::find(vector.begin(), vector.end(), elt);
18 }
19
20 1 int main(int argc, char ** argv)
21 {
22 using namespace pinocchio;
23
24 // You should change here to set up your own URDF file or just pass it as an argument of this
25 // example.
26 const std::string urdf_filename =
27 (argc <= 1) ? PINOCCHIO_MODEL_DIR
28
2/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 2 + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf")
29
4/12
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 2 : argv[1];
30
31 // Load the urdf model
32
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Model model;
33
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pinocchio::urdf::buildModel(urdf_filename, model);
34
35 // Create a list of joint to lock
36 1 std::vector<std::string> list_of_joints_to_lock_by_name;
37
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 list_of_joints_to_lock_by_name.push_back("elbow_joint");
38
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 list_of_joints_to_lock_by_name.push_back("wrist_3_joint"); // It can be in the wrong order
39
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 list_of_joints_to_lock_by_name.push_back("wrist_2_joint");
40
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 list_of_joints_to_lock_by_name.push_back("blabla"); // Joint not in the model
41
42 // Print the list of joints to remove + retrieve the joint id
43 1 std::vector<JointIndex> list_of_joints_to_lock_by_id;
44 1 for (std::vector<std::string>::const_iterator it = list_of_joints_to_lock_by_name.begin();
45
2/2
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 1 times.
 5 it != list_of_joints_to_lock_by_name.end(); ++it)
46 {
47 4 const std::string & joint_name = *it;
48
3/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 1 times.
 4 if (model.existJointName(joint_name)) // do not consider joint that are not in the model
49
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 list_of_joints_to_lock_by_id.push_back(model.getJointId(joint_name));
50 else
51
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 std::cout << "joint: " << joint_name << " does not belong to the model" << std::endl;
52 }
53
54 // Sample any random configuration
55
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Eigen::VectorXd q_rand = randomConfiguration(model);
56 // std::cout << "q_rand: " << q_rand.transpose() << std::endl;
57 // But should be also a neutral configuration
58
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Eigen::VectorXd q_neutral = neutral(model);
59 PINOCCHIO_UNUSED_VARIABLE(q_neutral);
60 // std::cout << "q_neutral: " << q_neutral.transpose() << std::endl;
61
62
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "\n\nFIRST CASE: BUILD A REDUCED MODEL FROM A LIST OF JOINT TO LOCK" << std::endl;
63 // Build the reduced model from the list of lock joints
64
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Model reduced_model = buildReducedModel(model, list_of_joints_to_lock_by_id, q_rand);
65
66 // Print the list of joints in the original model
67
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "List of joints in the original model:" << std::endl;
68
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 1 times.
 7 for (JointIndex joint_id = 1; joint_id < model.joints.size(); ++joint_id)
69
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
 6 std::cout << "\t- " << model.names[joint_id] << std::endl;
70
71 // Print the list of joints in the reduced model
72
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "List of joints in the reduced model:" << std::endl;
73
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 4 for (JointIndex joint_id = 1; joint_id < reduced_model.joints.size(); ++joint_id)
74
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 3 std::cout << "\t- " << reduced_model.names[joint_id] << std::endl;
75
76
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::cout << "\n\nSECOND CASE: BUILD A REDUCED MODEL FROM A LIST OF JOINT TO KEEP UNLOCKED"
77
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
78 // The same thing, but this time with an input list of joint to keep
79 1 std::vector<std::string> list_of_joints_to_keep_unlocked_by_name;
80
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 list_of_joints_to_keep_unlocked_by_name.push_back("shoulder_pan_joint");
81
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 list_of_joints_to_keep_unlocked_by_name.push_back("shoulder_lift_joint");
82
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 list_of_joints_to_keep_unlocked_by_name.push_back("wrist_1_joint");
83
84 1 std::vector<JointIndex> list_of_joints_to_keep_unlocked_by_id;
85 1 for (std::vector<std::string>::const_iterator it =
86 1 list_of_joints_to_keep_unlocked_by_name.begin();
87
2/2
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 1 times.
 4 it != list_of_joints_to_keep_unlocked_by_name.end(); ++it)
88 {
89 3 const std::string & joint_name = *it;
90
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 if (model.existJointName(joint_name))
91
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 list_of_joints_to_keep_unlocked_by_id.push_back(model.getJointId(joint_name));
92 else
93 ✗ std::cout << "joint: " << joint_name << " does not belong to the model";
94 }
95
96 // Transform the list into a list of joints to lock
97 1 list_of_joints_to_lock_by_id.clear();
98
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 1 times.
 7 for (JointIndex joint_id = 1; joint_id < model.joints.size(); ++joint_id)
99 {
100
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 const std::string joint_name = model.names[joint_id];
101
3/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 3 times.
 6 if (is_in_vector(list_of_joints_to_keep_unlocked_by_name, joint_name))
102 3 continue;
103 else
104 {
105
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 list_of_joints_to_lock_by_id.push_back(joint_id);
106 }
107
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 3 times.
 6 }
108
109 // Build the reduced model from the list of lock joints
110
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Model reduced_model2 = buildReducedModel(model, list_of_joints_to_lock_by_id, q_rand);
111
112 // Print the list of joints in the second reduced model
113
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "List of joints in the second reduced model:" << std::endl;
114
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 4 for (JointIndex joint_id = 1; joint_id < reduced_model2.joints.size(); ++joint_id)
115
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 3 std::cout << "\t- " << reduced_model2.names[joint_id] << std::endl;
116 1 }
117