Loading the model

Python

import os
from pathlib import Path
from sys import argv

import pinocchio

# This path refers to Pinocchio source code but you can define your own directory here.
model_dir = Path(os.environ.get("EXAMPLE_ROBOT_DATA_MODEL_DIR"))

# You should change here to set up your own URDF file or just pass it as an argument of
# this example.
urdf_filename = (
model_dir / "ur_description/urdf/ur5_robot.urdf" if len(argv) < 2 else argv[1]
)

# Load the urdf model
model = pinocchio.buildModelFromUrdf(urdf_filename)
print("model name: " + model.name)

# Create data required by the algorithms
data = model.createData()

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

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

C++

#include <iostream>

#include <Eigen/Core>

#include <boost/core/ref.hpp>
#include <boost/fusion/algorithm.hpp>
#include <boost/fusion/functional.hpp>
#include <boost/variant.hpp>

#include <iostream>
#include <string>
#include <vector>

#include "pinocchio/algorithm/fwd.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/eigen-common.hpp"
#include "pinocchio/math.hpp"
#include "pinocchio/multibody.hpp"
#include "pinocchio/multibody/joint.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/spatial.hpp"
#include "pinocchio/utils/check.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;

// You should change here to set up your own URDF file or just pass it as an argument of this
// example.
const std::string urdf_filename =
(argc <= 1) ? EXAMPLE_ROBOT_DATA_MODEL_DIR + std::string("/ur_description/urdf/ur5_robot.urdf")
: argv[1];

// Load the urdf model
Model model;
pinocchio::urdf::buildModel(urdf_filename, model);
std::cout << "model name: " << model.name << std::endl;

// Create data required by the algorithms
Data data(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);

// Print out the placement of each joint of the kinematic tree
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;
}