Commit fae0ec16 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[examples] Add geometry-models

parent 3bf53d77
...@@ -10,6 +10,7 @@ SET(${PROJECT_NAME}_EXAMPLES ...@@ -10,6 +10,7 @@ SET(${PROJECT_NAME}_EXAMPLES
overview-urdf overview-urdf
interpolation-SE3 interpolation-SE3
build-reduced-model build-reduced-model
geometry-models
) )
ADD_DEFINITIONS(-DPINOCCHIO_MODEL_DIR="${PINOCCHIO_MODEL_DIR}") ADD_DEFINITIONS(-DPINOCCHIO_MODEL_DIR="${PINOCCHIO_MODEL_DIR}")
...@@ -41,6 +42,7 @@ IF(BUILD_PYTHON_INTERFACE) ...@@ -41,6 +42,7 @@ IF(BUILD_PYTHON_INTERFACE)
gepetto-viewer gepetto-viewer
meshcat-viewer meshcat-viewer
robot-wrapper-viewer robot-wrapper-viewer
geometry-models
) )
IF(HPP_FCL_FOUND) IF(HPP_FCL_FOUND)
......
#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>
// 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;
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";
// 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;
}
import pinocchio
pinocchio.switchToNumpyMatrix()
from sys import argv
from os.path import dirname, join, abspath
# This path refers to Pinocchio source code but you can define your own directory here.
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
model_path = join(pinocchio_model_dir,"others/robots") if len(argv)<2 else argv[1]
mesh_dir = model_path
urdf_model_path = join(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('q: %s' % 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 )))
\ No newline at end of file
...@@ -10,7 +10,7 @@ urdf_filename = pinocchio_model_dir + '/others/robots/ur_description/urdf/ur5_ro ...@@ -10,7 +10,7 @@ urdf_filename = pinocchio_model_dir + '/others/robots/ur_description/urdf/ur5_ro
# Load the urdf model # Load the urdf model
model = pinocchio.buildModelFromUrdf(urdf_filename) model = pinocchio.buildModelFromUrdf(urdf_filename)
print('model name: ',model.name) print('model name: ' + model.name)
# Create data required by the algorithms # Create data required by the algorithms
data = model.createData() data = model.createData()
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment