| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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 example. |
||
25 |
✓✗✓✗ ✓✗✗✗ ✗✓✓✗ ✓✗✗✗ ✗✗✗✗ |
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]; |
26 |
|||
27 |
// Load the urdf model |
||
28 |
✓✗ | 2 |
Model model; |
29 |
✓✗ | 1 |
pinocchio::urdf::buildModel(urdf_filename,model); |
30 |
|||
31 |
// Create a list of joint to lock |
||
32 |
2 |
std::vector<std::string> list_of_joints_to_lock_by_name; |
|
33 |
✓✗✓✗ |
1 |
list_of_joints_to_lock_by_name.push_back("elbow_joint"); |
34 |
✓✗✓✗ |
1 |
list_of_joints_to_lock_by_name.push_back("wrist_3_joint"); // It can be in the wrong order |
35 |
✓✗✓✗ |
1 |
list_of_joints_to_lock_by_name.push_back("wrist_2_joint"); |
36 |
✓✗✓✗ |
1 |
list_of_joints_to_lock_by_name.push_back("blabla"); // Joint not in the model |
37 |
|||
38 |
// Print the list of joints to remove + retrieve the joint id |
||
39 |
2 |
std::vector<JointIndex> list_of_joints_to_lock_by_id; |
|
40 |
5 |
for(std::vector<std::string>::const_iterator it = list_of_joints_to_lock_by_name.begin(); |
|
41 |
✓✓ | 9 |
it != list_of_joints_to_lock_by_name.end(); ++it) |
42 |
{ |
||
43 |
4 |
const std::string & joint_name = *it; |
|
44 |
✓✗✓✓ |
4 |
if(model.existJointName(joint_name)) // do not consider joint that are not in the model |
45 |
✓✗✓✗ |
3 |
list_of_joints_to_lock_by_id.push_back(model.getJointId(joint_name)); |
46 |
else |
||
47 |
✓✗✓✗ ✓✗✓✗ |
1 |
std::cout << "joint: " << joint_name << " does not belong to the model" << std::endl; |
48 |
} |
||
49 |
|||
50 |
// Sample any random configuration |
||
51 |
✓✗ | 2 |
Eigen::VectorXd q_rand = randomConfiguration(model); |
52 |
// std::cout << "q_rand: " << q_rand.transpose() << std::endl; |
||
53 |
// But should be also a neutral configuration |
||
54 |
✓✗ | 2 |
Eigen::VectorXd q_neutral= neutral(model); |
55 |
PINOCCHIO_UNUSED_VARIABLE(q_neutral); |
||
56 |
// std::cout << "q_neutral: " << q_neutral.transpose() << std::endl; |
||
57 |
|||
58 |
✓✗✓✗ |
1 |
std::cout << "\n\nFIRST CASE: BUILD A REDUCED MODEL FROM A LIST OF JOINT TO LOCK" << std::endl; |
59 |
// Build the reduced model from the list of lock joints |
||
60 |
✓✗ | 2 |
Model reduced_model = buildReducedModel(model,list_of_joints_to_lock_by_id,q_rand); |
61 |
|||
62 |
// Print the list of joints in the original model |
||
63 |
✓✗✓✗ |
1 |
std::cout << "List of joints in the original model:" << std::endl; |
64 |
✓✓ | 7 |
for(JointIndex joint_id = 1; joint_id < model.joints.size(); ++joint_id) |
65 |
✓✗✓✗ ✓✗ |
6 |
std::cout << "\t- " << model.names[joint_id] << std::endl; |
66 |
|||
67 |
// Print the list of joints in the reduced model |
||
68 |
✓✗✓✗ |
1 |
std::cout << "List of joints in the reduced model:" << std::endl; |
69 |
✓✓ | 4 |
for(JointIndex joint_id = 1; joint_id < reduced_model.joints.size(); ++joint_id) |
70 |
✓✗✓✗ ✓✗ |
3 |
std::cout << "\t- " << reduced_model.names[joint_id] << std::endl; |
71 |
|||
72 |
✓✗✓✗ |
1 |
std::cout << "\n\nSECOND CASE: BUILD A REDUCED MODEL FROM A LIST OF JOINT TO KEEP UNLOCKED" << std::endl; |
73 |
// The same thing, but this time with an input list of joint to keep |
||
74 |
2 |
std::vector<std::string> list_of_joints_to_keep_unlocked_by_name; |
|
75 |
✓✗✓✗ |
1 |
list_of_joints_to_keep_unlocked_by_name.push_back("shoulder_pan_joint"); |
76 |
✓✗✓✗ |
1 |
list_of_joints_to_keep_unlocked_by_name.push_back("shoulder_lift_joint"); |
77 |
✓✗✓✗ |
1 |
list_of_joints_to_keep_unlocked_by_name.push_back("wrist_1_joint"); |
78 |
|||
79 |
2 |
std::vector<JointIndex> list_of_joints_to_keep_unlocked_by_id; |
|
80 |
4 |
for(std::vector<std::string>::const_iterator it = list_of_joints_to_keep_unlocked_by_name.begin(); |
|
81 |
✓✓ | 7 |
it != list_of_joints_to_keep_unlocked_by_name.end(); ++it) |
82 |
{ |
||
83 |
3 |
const std::string & joint_name = *it; |
|
84 |
✓✗✓✗ |
3 |
if(model.existJointName(joint_name)) |
85 |
✓✗✓✗ |
3 |
list_of_joints_to_keep_unlocked_by_id.push_back(model.getJointId(joint_name)); |
86 |
else |
||
87 |
std::cout << "joint: " << joint_name << " does not belong to the model"; |
||
88 |
} |
||
89 |
|||
90 |
// Transform the list into a list of joints to lock |
||
91 |
1 |
list_of_joints_to_lock_by_id.clear(); |
|
92 |
✓✓ | 7 |
for(JointIndex joint_id = 1; joint_id < model.joints.size(); ++joint_id) |
93 |
{ |
||
94 |
✓✗ | 6 |
const std::string joint_name = model.names[joint_id]; |
95 |
✓✗✓✓ |
6 |
if(is_in_vector(list_of_joints_to_keep_unlocked_by_name,joint_name)) |
96 |
3 |
continue; |
|
97 |
else |
||
98 |
{ |
||
99 |
✓✗ | 3 |
list_of_joints_to_lock_by_id.push_back(joint_id); |
100 |
} |
||
101 |
} |
||
102 |
|||
103 |
// Build the reduced model from the list of lock joints |
||
104 |
✓✗ | 2 |
Model reduced_model2 = buildReducedModel(model,list_of_joints_to_lock_by_id,q_rand); |
105 |
|||
106 |
// Print the list of joints in the second reduced model |
||
107 |
✓✗✓✗ |
1 |
std::cout << "List of joints in the second reduced model:" << std::endl; |
108 |
✓✓ | 4 |
for(JointIndex joint_id = 1; joint_id < reduced_model2.joints.size(); ++joint_id) |
109 |
✓✗✓✗ ✓✗ |
3 |
std::cout << "\t- " << reduced_model2.names[joint_id] << std::endl; |
110 |
|||
111 |
1 |
} |
| Generated by: GCOVR (Version 4.2) |