build-reduced-model.cpp
Go to the documentation of this file.
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 example.
25  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  Model model;
29  pinocchio::urdf::buildModel(urdf_filename,model);
30 
31  // Create a list of joint to lock
32  std::vector<std::string> list_of_joints_to_lock_by_name;
33  list_of_joints_to_lock_by_name.push_back("elbow_joint");
34  list_of_joints_to_lock_by_name.push_back("wrist_3_joint"); // It can be in the wrong order
35  list_of_joints_to_lock_by_name.push_back("wrist_2_joint");
36  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  std::vector<JointIndex> list_of_joints_to_lock_by_id;
40  for(std::vector<std::string>::const_iterator it = list_of_joints_to_lock_by_name.begin();
41  it != list_of_joints_to_lock_by_name.end(); ++it)
42  {
43  const std::string & joint_name = *it;
44  if(model.existJointName(joint_name)) // do not consider joint that are not in the model
45  list_of_joints_to_lock_by_id.push_back(model.getJointId(joint_name));
46  else
47  std::cout << "joint: " << joint_name << " does not belong to the model" << std::endl;
48  }
49 
50  // Sample any random configuration
51  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  Eigen::VectorXd q_neutral= neutral(model);
55  PINOCCHIO_UNUSED_VARIABLE(q_neutral);
56 // std::cout << "q_neutral: " << q_neutral.transpose() << std::endl;
57 
58  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  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  std::cout << "List of joints in the original model:" << std::endl;
64  for(JointIndex joint_id = 1; joint_id < model.joints.size(); ++joint_id)
65  std::cout << "\t- " << model.names[joint_id] << std::endl;
66 
67  // Print the list of joints in the reduced model
68  std::cout << "List of joints in the reduced model:" << std::endl;
69  for(JointIndex joint_id = 1; joint_id < reduced_model.joints.size(); ++joint_id)
70  std::cout << "\t- " << reduced_model.names[joint_id] << std::endl;
71 
72  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  std::vector<std::string> list_of_joints_to_keep_unlocked_by_name;
75  list_of_joints_to_keep_unlocked_by_name.push_back("shoulder_pan_joint");
76  list_of_joints_to_keep_unlocked_by_name.push_back("shoulder_lift_joint");
77  list_of_joints_to_keep_unlocked_by_name.push_back("wrist_1_joint");
78 
79  std::vector<JointIndex> list_of_joints_to_keep_unlocked_by_id;
80  for(std::vector<std::string>::const_iterator it = list_of_joints_to_keep_unlocked_by_name.begin();
81  it != list_of_joints_to_keep_unlocked_by_name.end(); ++it)
82  {
83  const std::string & joint_name = *it;
84  if(model.existJointName(joint_name))
85  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  list_of_joints_to_lock_by_id.clear();
92  for(JointIndex joint_id = 1; joint_id < model.joints.size(); ++joint_id)
93  {
94  const std::string joint_name = model.names[joint_id];
95  if(is_in_vector(list_of_joints_to_keep_unlocked_by_name,joint_name))
96  continue;
97  else
98  {
99  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  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  std::cout << "List of joints in the second reduced model:" << std::endl;
108  for(JointIndex joint_id = 1; joint_id < reduced_model2.joints.size(); ++joint_id)
109  std::cout << "\t- " << reduced_model2.names[joint_id] << std::endl;
110 
111 }
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
#define PINOCCHIO_UNUSED_VARIABLE(var)
Helper to declare that a parameter is unused.
Definition: src/macros.hpp:51
std::vector< std::string > names
Name of joint i
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
#define PINOCCHIO_MODEL_DIR
int main(int argc, char **argv)
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
Main pinocchio namespace.
Definition: timings.cpp:28
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
bool is_in_vector(const std::vector< T > &vector, const T &elt)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
JointCollectionTpl & model
void buildReducedModel(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, std::vector< JointIndex > list_of_joints_to_lock, const Eigen::MatrixBase< ConfigVectorType > &reference_configuration, ModelTpl< Scalar, Options, JointCollectionTpl > &reduced_model)
Build a reduced model from a given input model and a list of joint to lock.


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:28