3 from os.path
import dirname, join, abspath
6 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
8 model_path = join(pinocchio_model_dir,
"others/robots")
if len(argv)<2
else argv[1]
10 urdf_model_path = join(model_path,
"ur_description/urdf/ur5_robot.urdf")
13 model, collision_model, visual_model = pinocchio.buildModelsFromUrdf(urdf_model_path, mesh_dir)
14 print(
'model name: ' + model.name)
17 data, collision_data, visual_data = pinocchio.createDatas(model, collision_model, visual_model)
31 print(
"\nJoint placements:")
32 for name, oMi
in zip(model.names, data.oMi):
33 print((
"{:<24} : {: .2f} {: .2f} {: .2f}" 34 .format( name, *oMi.translation.T.flat )))
37 print(
"\nCollision object placements:")
38 for k, oMg
in enumerate(collision_data.oMg):
39 print((
"{:d} : {: .2f} {: .2f} {: .2f}" 40 .format( k, *oMg.translation.T.flat )))
43 print(
"\nVisual object placements:")
44 for k, oMg
in enumerate(visual_data.oMg):
45 print((
"{:d} : {: .2f} {: .2f} {: .2f}" 46 .format( k, *oMg.translation.T.flat )))
#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>
#ifndef PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif
int main(int argc, char ** argv)
{
const std::string model_path = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/others/robots") : argv[1];
const std::string mesh_dir = model_path;
const std::string urdf_filename = model_path + "/ur_description/urdf/ur5_robot.urdf";
std::cout <<
"model name: " << model.
name << std::endl;
std::cout << "q: " << q.transpose() << std::endl;
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;
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;
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;
}