Python
3 from os.path
import dirname, join, abspath
6 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
9 urdf_filename = pinocchio_model_dir +
'/others/robots/ur_description/urdf/ur5_robot.urdf' if len(argv)<2
else argv[1]
12 model = pinocchio.buildModelFromUrdf(urdf_filename)
13 print(
'model name: ' + model.name)
16 data = model.createData()
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>
#ifndef PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif
int main(int argc, char ** argv)
{
const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/others/robots/ur_description/urdf/ur5_robot.urdf") : argv[1];
std::cout <<
"model name: " << model.
name << std::endl;
std::cout << "q: " << q.transpose() << 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;
}