| 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 |
|
|
|