GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: examples/build-reduced-model.cpp Lines: 50 51 98.0 %
Date: 2023-08-09 08:43:58 Branches: 75 148 50.7 %

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
}