...
 
Commits (6)
......@@ -28,8 +28,7 @@ branches:
jobs:
include:
- if: branch = master
dist: xenial
- dist: xenial
env: BUILD_WITH_COLLISION_SUPPORT=OFF
- dist: trusty
env: BUILD_WITH_COLLISION_SUPPORT=ON
......
......@@ -194,6 +194,7 @@ namespace pinocchio
AppendJointOfModelAlgo::run (modelA.joints[jid], args);
}
#ifdef PINOCCHIO_WITH_HPP_FCL
// Add collision pairs of geomModelA and geomModelB
geomModel.collisionPairs.reserve(geomModelA.collisionPairs.size()
+ geomModelB.collisionPairs.size()
......@@ -221,6 +222,7 @@ namespace pinocchio
geomModel.addCollisionPair(CollisionPair(go1, go2));
}
}
#endif
}
} // namespace pinocchio
......
......@@ -111,8 +111,6 @@ struct GeometryObject
/// \brief Absolute path to the mesh texture file.
std::string meshTexturePath;
GeometryObject(const std::string & name, const FrameIndex parentF,
const JointIndex parentJ,
const boost::shared_ptr<fcl::CollisionGeometry> & collision,
......
......@@ -5,9 +5,7 @@
#ifndef __pinocchio_fcl_hxx__
#define __pinocchio_fcl_hxx__
#include <iostream>
#include <ostream>
namespace pinocchio
{
......
......@@ -32,55 +32,66 @@ namespace pinocchio
{
namespace details
{
struct UrdfTree
{
typedef boost::property_tree::ptree ptree;
typedef std::map<std::string, const ptree&> LinkMap_t;
void parse (const std::string & xmlStr)
{
urdf_ = ::urdf::parseURDF(xmlStr);
if (!urdf_) {
throw std::invalid_argument ("Enable to parse URDF");
}
std::istringstream iss(xmlStr);
using namespace boost::property_tree;
read_xml(iss, tree_, xml_parser::no_comments);
BOOST_FOREACH(const ptree::value_type & link, tree_.get_child("robot")) {
if (link.first == "link") {
std::string name = link.second.get<std::string>("<xmlattr>.name");
links_.insert(std::pair<std::string,const ptree&>(name, link.second));
}
} // BOOST_FOREACH
}
bool replaceCylinderByCapsule (const std::string & linkName,
const std::string & geomName) const
{
LinkMap_t::const_iterator _link = links_.find(linkName);
assert (_link != links_.end());
const ptree& link = _link->second;
if (link.count ("collision_checking") == 0)
return false;
BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
if (cc.first == "capsule") {
std::string name = cc.second.get<std::string>("<xmlattr>.name");
if (geomName == name) return true;
}
} // BOOST_FOREACH
return false;
}
// For standard URDF tags
::urdf::ModelInterfaceSharedPtr urdf_;
// For other tags
ptree tree_;
// A mapping from link name to corresponding child of tree_
LinkMap_t links_;
};
struct UrdfTree
{
typedef boost::property_tree::ptree ptree;
typedef std::map<std::string, const ptree&> LinkMap_t;
void parse (const std::string & xmlStr)
{
urdf_ = ::urdf::parseURDF(xmlStr);
if (!urdf_) {
throw std::invalid_argument ("Enable to parse URDF");
}
std::istringstream iss(xmlStr);
using namespace boost::property_tree;
read_xml(iss, tree_, xml_parser::no_comments);
BOOST_FOREACH(const ptree::value_type & link, tree_.get_child("robot")) {
if (link.first == "link") {
std::string name = link.second.get<std::string>("<xmlattr>.name");
links_.insert(std::pair<std::string,const ptree&>(name, link.second));
}
} // BOOST_FOREACH
}
bool replaceCylinderByCapsule (const std::string & linkName,
const std::string & geomName) const
{
LinkMap_t::const_iterator _link = links_.find(linkName);
assert (_link != links_.end());
const ptree& link = _link->second;
if (link.count ("collision_checking") == 0)
return false;
BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
if (cc.first == "capsule") {
std::string name = cc.second.get<std::string>("<xmlattr>.name");
if (geomName == name) return true;
}
} // BOOST_FOREACH
return false;
}
// For standard URDF tags
::urdf::ModelInterfaceSharedPtr urdf_;
// For other tags
ptree tree_;
// A mapping from link name to corresponding child of tree_
LinkMap_t links_;
};
template<typename Vector3>
static void retrieveMeshScale(const ::urdf::MeshSharedPtr & mesh,
const Eigen::MatrixBase<Vector3> & scale)
{
Vector3 & scale_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3,scale);
scale_ <<
mesh->scale.x,
mesh->scale.y,
mesh->scale.z;
}
#ifdef PINOCCHIO_WITH_HPP_FCL
/**
......@@ -109,8 +120,8 @@ namespace pinocchio
// Handle the case where collision geometry is a mesh
if (urdf_geometry->type == ::urdf::Geometry::MESH)
{
const ::urdf::MeshSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
std::string collisionFilename = collisionGeometry->filename;
const ::urdf::MeshSharedPtr urdf_mesh = ::urdf::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
std::string collisionFilename = urdf_mesh->filename;
meshPath = retrieveResourcePath(collisionFilename, package_dirs);
if (meshPath == "") {
......@@ -119,14 +130,12 @@ namespace pinocchio
throw std::invalid_argument (ss.str());
}
fcl::Vec3f scale = fcl::Vec3f(collisionGeometry->scale.x,
collisionGeometry->scale.y,
collisionGeometry->scale.z
fcl::Vec3f scale = fcl::Vec3f(urdf_mesh->scale.x,
urdf_mesh->scale.y,
urdf_mesh->scale.z
);
meshScale << collisionGeometry->scale.x,
collisionGeometry->scale.y,
collisionGeometry->scale.z;
retrieveMeshScale(urdf_mesh, meshScale);
// Create FCL mesh by parsing Collada file.
geometry = meshLoader->load (meshPath, scale, fcl::BV_OBBRSS);
......@@ -302,7 +311,8 @@ namespace pinocchio
#ifndef PINOCCHIO_WITH_HPP_FCL
PINOCCHIO_UNUSED_VARIABLE(tree);
PINOCCHIO_UNUSED_VARIABLE(meshLoader);
#endif
#endif // PINOCCHIO_WITH_HPP_FCL
typedef std::vector< PINOCCHIO_URDF_SHARED_PTR(GeometryType) > VectorSharedT;
typedef GeometryModel::SE3 SE3;
......@@ -329,11 +339,12 @@ namespace pinocchio
{
meshPath.clear();
#ifdef PINOCCHIO_WITH_HPP_FCL
#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
const std::string & geom_name = (*i)->group_name;
#else
const std::string & geom_name = (*i)->name;
#endif
#endif // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
const boost::shared_ptr<fcl::CollisionGeometry> geometry =
retrieveCollisionGeometry(tree, meshLoader, link_name, geom_name,
(*i)->geometry, package_dirs, meshPath, meshScale);
......@@ -342,6 +353,7 @@ namespace pinocchio
if (urdf_mesh)
{
meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs);
retrieveMeshScale(urdf_mesh, meshScale);
}
const boost::shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry());
......
//
// Copyright (c) 2016,2018 CNRS
// Copyright (c) 2016-2019 CNRS INRIA
//
#include "pinocchio/multibody/model.hpp"
......@@ -66,7 +66,7 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
std::transform (++manipulator.frames.begin(), manipulator.frames.end(),
++manipulator.frames.begin(), addManipulatorPrefix);
BOOST_MESSAGE(manipulator);
BOOST_TEST_MESSAGE(manipulator);
buildModels::humanoid(humanoid);
buildModels::humanoidGeometries(humanoid, geomHumanoid);
......@@ -77,7 +77,7 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
std::transform (++humanoid.frames.begin(), humanoid.frames.end(),
++humanoid.frames.begin(), addHumanoidPrefix);
BOOST_MESSAGE(humanoid);
BOOST_TEST_MESSAGE(humanoid);
//TODO fix inertia of the base
manipulator.inertias[0].setRandom();
......@@ -90,7 +90,7 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
appendModel (humanoid, manipulator, geomHumanoid, geomManipulator, fid,
SE3::Identity(), model, geomModel);
BOOST_MESSAGE(model);
BOOST_TEST_MESSAGE(model);
// Check the model
BOOST_CHECK_EQUAL(model.getJointId("humanoid/chest2_joint"),
......@@ -99,19 +99,19 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
// check the joint order and the inertias
JointIndex chest2 = model.getJointId("humanoid/chest2_joint");
for (JointIndex jid = 1; jid < chest2; ++jid) {
BOOST_MESSAGE("Checking joint " << jid << " " << model.names[jid]);
BOOST_TEST_MESSAGE("Checking joint " << jid << " " << model.names[jid]);
BOOST_CHECK_EQUAL(model.names[jid], humanoid.names[jid]);
BOOST_CHECK_EQUAL(model.inertias[jid], humanoid.inertias[jid]);
BOOST_CHECK_EQUAL(model.jointPlacements[jid], humanoid.jointPlacements[jid]);
}
BOOST_MESSAGE("Checking joint " << chest2 << " " << model.names[chest2]);
BOOST_TEST_MESSAGE("Checking joint " << chest2 << " " << model.names[chest2]);
BOOST_CHECK_EQUAL(model.names[chest2], humanoid.names[chest2]);
BOOST_CHECK_MESSAGE(model.inertias[chest2].isApprox(manipulator.inertias[0].se3Action(aMb) + humanoid.inertias[chest2]),
model.inertias[chest2] << " != " << manipulator.inertias[0].se3Action(aMb) + humanoid.inertias[chest2]);
BOOST_CHECK_EQUAL(model.jointPlacements[chest2], humanoid.jointPlacements[chest2]);
for (JointIndex jid = 1; jid < manipulator.joints.size(); ++jid) {
BOOST_MESSAGE("Checking joint " << chest2+jid << " " << model.names[chest2+jid]);
BOOST_TEST_MESSAGE("Checking joint " << chest2+jid << " " << model.names[chest2+jid]);
BOOST_CHECK_EQUAL(model.names[chest2+jid], manipulator.names[jid]);
BOOST_CHECK_EQUAL(model.inertias[chest2+jid], manipulator.inertias[jid]);
if (jid==1)
......@@ -120,7 +120,7 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_CHECK_EQUAL(model.jointPlacements[chest2+jid], manipulator.jointPlacements[jid]);
}
for (JointIndex jid = chest2+1; jid < humanoid.joints.size(); ++jid) {
BOOST_MESSAGE("Checking joint " << jid+manipulator.joints.size()-1 << " " << model.names[jid+manipulator.joints.size()-1]);
BOOST_TEST_MESSAGE("Checking joint " << jid+manipulator.joints.size()-1 << " " << model.names[jid+manipulator.joints.size()-1]);
BOOST_CHECK_EQUAL(model.names[jid+manipulator.joints.size()-1], humanoid.names[jid]);
BOOST_CHECK_EQUAL(model.inertias[jid+manipulator.joints.size()-1], humanoid.inertias[jid]);
BOOST_CHECK_EQUAL(model.jointPlacements[jid+manipulator.joints.size()-1], humanoid.jointPlacements[jid]);
......