Geometry models

Python

import os
from pathlib import Path
from sys import argv

import pinocchio

model_path = Path(
Path(os.environ.get("EXAMPLE_ROBOT_DATA_MODEL_DIR")) if len(argv) < 2 else argv[1]
)
mesh_dir = model_path.parent.parent
urdf_model_path = model_path / "ur_description/urdf/ur5_robot.urdf"

# Load the urdf model
model, collision_model, visual_model = pinocchio.buildModelsFromUrdf(
urdf_model_path, mesh_dir
)
print("model name:", model.name)

# Create data required by the algorithms
data, collision_data, visual_data = pinocchio.createDatas(
model, collision_model, visual_model
)

# Sample a random configuration
q = pinocchio.randomConfiguration(model)
print(f"q: {q.T}")

# Perform the forward kinematics over the kinematic tree
pinocchio.forwardKinematics(model, data, q)

# Update Geometry models
pinocchio.updateGeometryPlacements(model, data, collision_model, collision_data)
pinocchio.updateGeometryPlacements(model, data, visual_model, visual_data)

# Print out the placement of each joint of the kinematic tree
print("\nJoint placements:")
for name, oMi in zip(model.names, data.oMi):
print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat))

# Print out the placement of each collision geometry object
print("\nCollision object placements:")
for k, oMg in enumerate(collision_data.oMg):
print("{:d} : {: .2f} {: .2f} {: .2f}".format(k, *oMg.translation.T.flat))

# Print out the placement of each visual geometry object
print("\nVisual object placements:")
for k, oMg in enumerate(visual_data.oMg):
print("{:d} : {: .2f} {: .2f} {: .2f}".format(k, *oMg.translation.T.flat))

C++

#include "pinocchio/multibody/fcl.hpp"
#include "pinocchio/parsers/urdf.hpp"

#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/geometry.hpp"

#include <iostream>
#include <boost/filesystem.hpp>

// EXAMPLE_ROBOT_DATA_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef EXAMPLE_ROBOT_DATA_MODEL_DIR
#define EXAMPLE_ROBOT_DATA_MODEL_DIR "path_to_the_model_dir"
#endif

int main(int argc, char ** argv)
{
using namespace pinocchio;

const std::string model_path = (argc <= 1) ? EXAMPLE_ROBOT_DATA_MODEL_DIR : argv[1];
const std::string mesh_dir =
(argc <= 2)
? boost::filesystem::path(EXAMPLE_ROBOT_DATA_MODEL_DIR).parent_path().parent_path().string()
: argv[2];
const std::string urdf_filename = model_path + "/ur_description/urdf/ur5_robot.urdf";

// Load the urdf model
Model model;
pinocchio::urdf::buildModel(urdf_filename, model);
GeometryModel collision_model;
pinocchio::urdf::buildGeom(model, urdf_filename, COLLISION, collision_model, mesh_dir);
GeometryModel visual_model;
pinocchio::urdf::buildGeom(model, urdf_filename, VISUAL, visual_model, mesh_dir);
std::cout << "model name: " << model.name << std::endl;

// Create data required by the algorithms
Data data(model);
GeometryData collision_data(collision_model);
GeometryData visual_data(visual_model);

// Sample a random configuration
Eigen::VectorXd q = randomConfiguration(model);
std::cout << "q: " << q.transpose() << std::endl;

// Perform the forward kinematics over the kinematic tree
forwardKinematics(model, data, q);

// Update Geometry models
updateGeometryPlacements(model, data, collision_model, collision_data);
updateGeometryPlacements(model, data, visual_model, visual_data);

// Print out the placement of each joint of the kinematic tree
std::cout << "\nJoint placements:" << std::endl;
for (JointIndex joint_id = 0; joint_id < (JointIndex)model.njoints; ++joint_id)
std::cout << std::setw(24) << std::left << model.names[joint_id] << ": " << std::fixed
<< std::setprecision(2) << data.oMi[joint_id].translation().transpose() << std::endl;

// Print out the placement of each collision geometry object
std::cout << "\nCollision object placements:" << std::endl;
for (GeomIndex geom_id = 0; geom_id < (GeomIndex)collision_model.ngeoms; ++geom_id)
std::cout << geom_id << ": " << std::fixed << std::setprecision(2)
<< collision_data.oMg[geom_id].translation().transpose() << std::endl;

// Print out the placement of each visual geometry object
std::cout << "\nVisual object placements:" << std::endl;
for (GeomIndex geom_id = 0; geom_id < (GeomIndex)visual_model.ngeoms; ++geom_id)
std::cout << geom_id << ": " << std::fixed << std::setprecision(2)
<< visual_data.oMg[geom_id].translation().transpose() << std::endl;
}