1 from __future__
import print_function
2 import pinocchio
as pin, hppfcl
3 pin.switchToNumpyMatrix()
6 from os.path
import dirname, join, abspath
8 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
10 model_path = join(pinocchio_model_dir,
"others/robots")
12 urdf_filename =
"romeo_small.urdf" 13 urdf_model_path = join(join(model_path,
"romeo_description/urdf"),urdf_filename)
16 model = pin.buildModelFromUrdf(urdf_model_path,pin.JointModelFreeFlyer())
19 geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,model_path,pin.GeometryType.COLLISION)
22 geom_model.addAllCollisionPairs()
23 print(
"num collision pairs - initial:",len(geom_model.collisionPairs))
26 srdf_filename =
"romeo.srdf" 27 srdf_model_path = model_path +
"/romeo_description/srdf/" + srdf_filename
29 pin.removeCollisionPairs(model,geom_model,srdf_model_path)
30 print(
"num collision pairs - after removing useless collision pairs:",len(geom_model.collisionPairs))
33 pin.loadReferenceConfigurations(model,srdf_model_path)
36 q = model.referenceConfigurations[
"half_sitting"]
39 data = model.createData()
40 geom_data = pin.GeometryData(geom_model)
43 pin.computeCollisions(model,data,geom_model,geom_data,q,
False)
46 for k
in range(len(geom_model.collisionPairs)):
47 cr = geom_data.collisionResults[k]
48 cp = geom_model.collisionPairs[k]
49 print(
"collision pair:",cp.first,
",",cp.second,
"- collision:",
"Yes" if cr.isCollision()
else "No")
52 pin.updateGeometryPlacements(model,data,geom_model,geom_data,q)
53 pin.computeCollision(geom_model,geom_data,0)
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"
#include "pinocchio/algorithm/joint-configuration.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 , char ** )
{
const std::string robots_model_path = PINOCCHIO_MODEL_DIR + std::string("/others/robots/");
const std::string urdf_filename = robots_model_path + std::string("talos_data/urdf/talos_reduced.urdf");
const std::string srdf_filename = robots_model_path + std::string("talos_data/srdf/talos.srdf");
{
const hpp::fcl::CollisionResult & cr = geom_data.collisionResults[k];
std::cout << "collision pair: " << cp.first << " , " << cp.second << " - collision: ";
std::cout << (cr.isCollision() ? "yes" : "no") << std::endl;
}
const PairIndex pair_id = 2;
return 0;
}