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 |
|
✗ |
bool is_in_vector(const std::vector<T> & vector, const T & elt) |
16 |
|
|
{ |
17 |
|
✗ |
return vector.end() != std::find(vector.begin(), vector.end(), elt); |
18 |
|
|
} |
19 |
|
|
|
20 |
|
✗ |
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 |
|
✗ |
+ std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf") |
29 |
|
✗ |
: argv[1]; |
30 |
|
|
|
31 |
|
|
// Load the urdf model |
32 |
|
✗ |
Model model; |
33 |
|
✗ |
pinocchio::urdf::buildModel(urdf_filename, model); |
34 |
|
|
|
35 |
|
|
// Create a list of joint to lock |
36 |
|
✗ |
std::vector<std::string> list_of_joints_to_lock_by_name; |
37 |
|
✗ |
list_of_joints_to_lock_by_name.push_back("elbow_joint"); |
38 |
|
✗ |
list_of_joints_to_lock_by_name.push_back("wrist_3_joint"); // It can be in the wrong order |
39 |
|
✗ |
list_of_joints_to_lock_by_name.push_back("wrist_2_joint"); |
40 |
|
✗ |
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 |
|
✗ |
std::vector<JointIndex> list_of_joints_to_lock_by_id; |
44 |
|
✗ |
for (std::vector<std::string>::const_iterator it = list_of_joints_to_lock_by_name.begin(); |
45 |
|
✗ |
it != list_of_joints_to_lock_by_name.end(); ++it) |
46 |
|
|
{ |
47 |
|
✗ |
const std::string & joint_name = *it; |
48 |
|
✗ |
if (model.existJointName(joint_name)) // do not consider joint that are not in the model |
49 |
|
✗ |
list_of_joints_to_lock_by_id.push_back(model.getJointId(joint_name)); |
50 |
|
|
else |
51 |
|
✗ |
std::cout << "joint: " << joint_name << " does not belong to the model" << std::endl; |
52 |
|
|
} |
53 |
|
|
|
54 |
|
|
// Sample any random configuration |
55 |
|
✗ |
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 |
|
✗ |
Eigen::VectorXd q_neutral = neutral(model); |
59 |
|
|
PINOCCHIO_UNUSED_VARIABLE(q_neutral); |
60 |
|
|
// std::cout << "q_neutral: " << q_neutral.transpose() << std::endl; |
61 |
|
|
|
62 |
|
✗ |
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 |
|
✗ |
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 |
|
✗ |
std::cout << "List of joints in the original model:" << std::endl; |
68 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < model.joints.size(); ++joint_id) |
69 |
|
✗ |
std::cout << "\t- " << model.names[joint_id] << std::endl; |
70 |
|
|
|
71 |
|
|
// Print the list of joints in the reduced model |
72 |
|
✗ |
std::cout << "List of joints in the reduced model:" << std::endl; |
73 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < reduced_model.joints.size(); ++joint_id) |
74 |
|
✗ |
std::cout << "\t- " << reduced_model.names[joint_id] << std::endl; |
75 |
|
|
|
76 |
|
✗ |
std::cout << "\n\nSECOND CASE: BUILD A REDUCED MODEL FROM A LIST OF JOINT TO KEEP UNLOCKED" |
77 |
|
✗ |
<< std::endl; |
78 |
|
|
// The same thing, but this time with an input list of joint to keep |
79 |
|
✗ |
std::vector<std::string> list_of_joints_to_keep_unlocked_by_name; |
80 |
|
✗ |
list_of_joints_to_keep_unlocked_by_name.push_back("shoulder_pan_joint"); |
81 |
|
✗ |
list_of_joints_to_keep_unlocked_by_name.push_back("shoulder_lift_joint"); |
82 |
|
✗ |
list_of_joints_to_keep_unlocked_by_name.push_back("wrist_1_joint"); |
83 |
|
|
|
84 |
|
✗ |
std::vector<JointIndex> list_of_joints_to_keep_unlocked_by_id; |
85 |
|
✗ |
for (std::vector<std::string>::const_iterator it = |
86 |
|
✗ |
list_of_joints_to_keep_unlocked_by_name.begin(); |
87 |
|
✗ |
it != list_of_joints_to_keep_unlocked_by_name.end(); ++it) |
88 |
|
|
{ |
89 |
|
✗ |
const std::string & joint_name = *it; |
90 |
|
✗ |
if (model.existJointName(joint_name)) |
91 |
|
✗ |
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 |
|
✗ |
list_of_joints_to_lock_by_id.clear(); |
98 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < model.joints.size(); ++joint_id) |
99 |
|
|
{ |
100 |
|
✗ |
const std::string joint_name = model.names[joint_id]; |
101 |
|
✗ |
if (is_in_vector(list_of_joints_to_keep_unlocked_by_name, joint_name)) |
102 |
|
✗ |
continue; |
103 |
|
|
else |
104 |
|
|
{ |
105 |
|
✗ |
list_of_joints_to_lock_by_id.push_back(joint_id); |
106 |
|
|
} |
107 |
|
|
} |
108 |
|
|
|
109 |
|
|
// Build the reduced model from the list of lock joints |
110 |
|
✗ |
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 |
|
✗ |
std::cout << "List of joints in the second reduced model:" << std::endl; |
114 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < reduced_model2.joints.size(); ++joint_id) |
115 |
|
✗ |
std::cout << "\t- " << reduced_model2.names[joint_id] << std::endl; |
116 |
|
|
} |
117 |
|
|
|