5 #include "pinocchio/algorithm/joint-configuration.hpp" 6 #include "pinocchio/algorithm/kinematics.hpp" 7 #include "pinocchio/algorithm/geometry.hpp" 8 #include "pinocchio/parsers/urdf.hpp" 9 #include "pinocchio/parsers/sample-models.hpp" 10 #include "pinocchio/multibody/geometry.hpp" 11 #include "pinocchio/utils/timer.hpp" 18 using namespace Eigen;
23 const unsigned int NBT = 1000*100;
24 const unsigned int NBD = 1000;
26 const unsigned int NBT = 1;
27 const unsigned int NBD = 1;
28 std::cout <<
"(the time score in debug mode is not relevant) " << std::endl;
31 std::string romeo_filename =
PINOCCHIO_MODEL_DIR + std::string(
"/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
33 std::string romeo_meshDir =
PINOCCHIO_MODEL_DIR + std::string(
"/example-robot-data/robots");
34 package_dirs.push_back(romeo_meshDir);
39 #ifdef PINOCCHIO_WITH_HPP_FCL 41 #endif // PINOCCHIO_WITH_HPP_FCL 45 VectorXd qmax = Eigen::VectorXd::Ones(model.
nq);
51 for(
size_t i=0;
i<NBT;++
i)
54 qdots_romeo[
i] = Eigen::VectorXd::Random(model.
nv);
55 qddots_romeo[
i] = Eigen::VectorXd::Random(model.
nv);
73 #ifdef PINOCCHIO_WITH_HPP_FCL 78 for (std::vector<pinocchio::CollisionPair>::iterator it = geom_model.
collisionPairs.begin(); it != geom_model.
collisionPairs.end(); ++it)
80 computeCollision(geom_model,geom_data,std::size_t(it-geom_model.
collisionPairs.begin()));
84 std::cout <<
"Collision test between two geometry objects (mean time) = \t" << collideTime / double(geom_model.
collisionPairs.size())
94 std::cout <<
"Collision Test : robot in collision? = \t" << is_colliding_time
101 computeDistances(model,data,geom_model,geom_data,qs_romeo[_smooth]);
104 std::cout <<
"Compute distance between two geometry objects (mean time) = \t" << computeDistancesTime / double(geom_model.
collisionPairs.size())
107 #endif // PINOCCHIO_WITH_HPP_FCL
static std::string unitName(Unit u)
CollisionPairVector collisionPairs
Vector of collision pairs.
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
#define PINOCCHIO_MODEL_DIR
bool computeCollisions(const int num_threads, const GeometryModel &geom_model, GeometryData &geom_data, const bool stopAtFirstCollision=false)
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
GeometryModel & buildGeom(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const GeometryType type, GeometryModel &geom_model, const std::vector< std::string > &package_paths=std::vector< std::string >(), ::hpp::fcl::MeshLoaderPtr mesh_loader=::hpp::fcl::MeshLoaderPtr())
Build The GeometryModel from a URDF file. Search for meshes in the directories specified by the user ...
#define PINOCCHIO_ALIGNED_STD_VECTOR(Type)
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
void addAllCollisionPairs()
Add all possible collision pairs.
Main pinocchio namespace.
int nv
Dimension of the velocity vector space.
void updateGeometryPlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data, const Eigen::MatrixBase< ConfigVectorType > &q)
Apply a forward kinematics and update the placement of the geometry objects.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
int nq
Dimension of the configuration vector representation.