pinocchio  2.4.0-dirty
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading the model

Python

1 import pinocchio
2 from sys import argv
3 from os.path import dirname, join, abspath
4 
5 # This path refers to Pinocchio source code but you can define your own directory here.
6 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
7 
8 # You should change here to set up your own URDF file or just pass it as an argument of this example.
9 urdf_filename = pinocchio_model_dir + '/others/robots/ur_description/urdf/ur5_robot.urdf' if len(argv)<2 else argv[1]
10 
11 # Load the urdf model
12 model = pinocchio.buildModelFromUrdf(urdf_filename)
13 print('model name: ' + model.name)
14 
15 # Create data required by the algorithms
16 data = model.createData()
17 
18 # Sample a random configuration
20 print('q: %s' % q.T)
21 
22 # Perform the forward kinematics over the kinematic tree
23 pinocchio.forwardKinematics(model,data,q)
24 
25 # Print out the placement of each joint of the kinematic tree
26 for name, oMi in zip(model.names, data.oMi):
27  print(("{:<24} : {: .2f} {: .2f} {: .2f}"
28  .format( name, *oMi.translation.T.flat )))

C++

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include <iostream>
// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef PINOCCHIO_MODEL_DIR
#define PINOCCHIO_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) ? PINOCCHIO_MODEL_DIR + std::string("/others/robots/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;
}