geometry-models.cpp
Go to the documentation of this file.
1 #include "pinocchio/multibody/fcl.hpp"
2 #include "pinocchio/parsers/urdf.hpp"
3 
4 #include "pinocchio/algorithm/joint-configuration.hpp"
5 #include "pinocchio/algorithm/kinematics.hpp"
6 #include "pinocchio/algorithm/geometry.hpp"
7 
8 #include <iostream>
9 
10 // PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
11 #ifndef PINOCCHIO_MODEL_DIR
12  #define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
13 #endif
14 
15 int main(int argc, char ** argv)
16 {
17  using namespace pinocchio;
18 
19  const std::string model_path = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots") : argv[1];
20  const std::string mesh_dir = (argc<=1) ? PINOCCHIO_MODEL_DIR : argv[1];
21  const std::string urdf_filename = model_path + "/ur_description/urdf/ur5_robot.urdf";
22 
23  // Load the urdf model
24  Model model;
25  pinocchio::urdf::buildModel(urdf_filename,model);
27  pinocchio::urdf::buildGeom(model, urdf_filename, COLLISION, collision_model, mesh_dir);
29  pinocchio::urdf::buildGeom(model, urdf_filename, VISUAL, visual_model, mesh_dir);
30  std::cout << "model name: " << model.name << std::endl;
31 
32  // Create data required by the algorithms
33  Data data(model);
34  GeometryData collision_data(collision_model);
35  GeometryData visual_data(visual_model);
36 
37  // Sample a random configuration
39  std::cout << "q: " << q.transpose() << std::endl;
40 
41  // Perform the forward kinematics over the kinematic tree
42  forwardKinematics(model,data,q);
43 
44  // Update Geometry models
45  updateGeometryPlacements(model, data, collision_model, collision_data);
46  updateGeometryPlacements(model, data, visual_model, visual_data);
47 
48  // Print out the placement of each joint of the kinematic tree
49  std::cout << "\nJoint placements:" << std::endl;
51  std::cout << std::setw(24) << std::left
52  << model.names[joint_id] << ": "
53  << std::fixed << std::setprecision(2)
54  << data.oMi[joint_id].translation().transpose()
55  << std::endl;
56 
57  // Print out the placement of each collision geometry object
58  std::cout << "\nCollision object placements:" << std::endl;
59  for(GeomIndex geom_id = 0; geom_id < (GeomIndex)collision_model.ngeoms; ++geom_id)
60  std::cout << geom_id << ": "
61  << std::fixed << std::setprecision(2)
62  << collision_data.oMg[geom_id].translation().transpose()
63  << std::endl;
64 
65  // Print out the placement of each visual geometry object
66  std::cout << "\nVisual object placements:" << std::endl;
67  for(GeomIndex geom_id = 0; geom_id < (GeomIndex)visual_model.ngeoms; ++geom_id)
68  std::cout << geom_id << ": "
69  << std::fixed << std::setprecision(2)
70  << visual_data.oMg[geom_id].translation().transpose()
71  << std::endl;
72 }
int njoints
Number of joints.
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.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
std::string name
Model name;.
GeometryModel & buildGeom(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const GeometryType type, GeometryModel &geom_model, const std::vector< std::string > &package_paths=std::vector< std::string >(), ::hpp::fcl::MeshLoaderPtr mesh_loader=::hpp::fcl::MeshLoaderPtr())
Build The GeometryModel from a URDF file. Search for meshes in the directories specified by the user ...
#define PINOCCHIO_MODEL_DIR
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...
Index ngeoms
The number of GeometryObjects.
Main pinocchio namespace.
Definition: timings.cpp:28
int main(int argc, char **argv)
void updateGeometryPlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data, const Eigen::MatrixBase< ConfigVectorType > &q)
Apply a forward kinematics and update the placement of the geometry objects.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
JointCollectionTpl & model


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