Line |
Branch |
Exec |
Source |
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 = |
20 |
|
✗ |
(argc <= 1) ? PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots") : argv[1]; |
21 |
|
✗ |
const std::string mesh_dir = (argc <= 1) ? PINOCCHIO_MODEL_DIR : argv[1]; |
22 |
|
✗ |
const std::string urdf_filename = model_path + "/ur_description/urdf/ur5_robot.urdf"; |
23 |
|
|
|
24 |
|
|
// Load the urdf model |
25 |
|
✗ |
Model model; |
26 |
|
✗ |
pinocchio::urdf::buildModel(urdf_filename, model); |
27 |
|
✗ |
GeometryModel collision_model; |
28 |
|
✗ |
pinocchio::urdf::buildGeom(model, urdf_filename, COLLISION, collision_model, mesh_dir); |
29 |
|
✗ |
GeometryModel visual_model; |
30 |
|
✗ |
pinocchio::urdf::buildGeom(model, urdf_filename, VISUAL, visual_model, mesh_dir); |
31 |
|
✗ |
std::cout << "model name: " << model.name << std::endl; |
32 |
|
|
|
33 |
|
|
// Create data required by the algorithms |
34 |
|
✗ |
Data data(model); |
35 |
|
✗ |
GeometryData collision_data(collision_model); |
36 |
|
✗ |
GeometryData visual_data(visual_model); |
37 |
|
|
|
38 |
|
|
// Sample a random configuration |
39 |
|
✗ |
Eigen::VectorXd q = randomConfiguration(model); |
40 |
|
✗ |
std::cout << "q: " << q.transpose() << std::endl; |
41 |
|
|
|
42 |
|
|
// Perform the forward kinematics over the kinematic tree |
43 |
|
✗ |
forwardKinematics(model, data, q); |
44 |
|
|
|
45 |
|
|
// Update Geometry models |
46 |
|
✗ |
updateGeometryPlacements(model, data, collision_model, collision_data); |
47 |
|
✗ |
updateGeometryPlacements(model, data, visual_model, visual_data); |
48 |
|
|
|
49 |
|
|
// Print out the placement of each joint of the kinematic tree |
50 |
|
✗ |
std::cout << "\nJoint placements:" << std::endl; |
51 |
|
✗ |
for (JointIndex joint_id = 0; joint_id < (JointIndex)model.njoints; ++joint_id) |
52 |
|
✗ |
std::cout << std::setw(24) << std::left << model.names[joint_id] << ": " << std::fixed |
53 |
|
✗ |
<< std::setprecision(2) << data.oMi[joint_id].translation().transpose() << std::endl; |
54 |
|
|
|
55 |
|
|
// Print out the placement of each collision geometry object |
56 |
|
✗ |
std::cout << "\nCollision object placements:" << std::endl; |
57 |
|
✗ |
for (GeomIndex geom_id = 0; geom_id < (GeomIndex)collision_model.ngeoms; ++geom_id) |
58 |
|
✗ |
std::cout << geom_id << ": " << std::fixed << std::setprecision(2) |
59 |
|
✗ |
<< collision_data.oMg[geom_id].translation().transpose() << std::endl; |
60 |
|
|
|
61 |
|
|
// Print out the placement of each visual geometry object |
62 |
|
✗ |
std::cout << "\nVisual object placements:" << std::endl; |
63 |
|
✗ |
for (GeomIndex geom_id = 0; geom_id < (GeomIndex)visual_model.ngeoms; ++geom_id) |
64 |
|
✗ |
std::cout << geom_id << ": " << std::fixed << std::setprecision(2) |
65 |
|
✗ |
<< visual_data.oMg[geom_id].translation().transpose() << std::endl; |
66 |
|
|
} |
67 |
|
|
|