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