static void extractMeshData(const aiScene *scene, const aiNode *node, const aiMatrix4x4 &parent_transform, const Eigen::Vector3d &scale,
EigenSTL::vector_Vector3d &vertices, std::vector<unsigned int> &triangles)
{
aiMatrix4x4 transform = parent_transform;
transform *= node->mTransformation;
for (unsigned int j = 0 ; j < node->mNumMeshes; ++j)
{
const aiMesh* a = scene->mMeshes[node->mMeshes[j]];
unsigned int offset = vertices.size();
for (unsigned int i = 0 ; i < a->mNumVertices ; ++i)
{
aiVector3D v = transform * a->mVertices[i];
vertices.push_back(Eigen::Vector3d(v.x * scale.x(), v.y * scale.y(), v.z * scale.z()));
}
for (unsigned int i = 0 ; i < a->mNumFaces ; ++i)
if (a->mFaces[i].mNumIndices == 3)
{
triangles.push_back(offset + a->mFaces[i].mIndices[0]);
triangles.push_back(offset + a->mFaces[i].mIndices[1]);
triangles.push_back(offset + a->mFaces[i].mIndices[2]);
}
}
for (unsigned int n = 0; n < node->mNumChildren; ++n)
extractMeshData(scene, node->mChildren[n], transform, scale, vertices, triangles);
}
void CollisionWorldDistanceField::updateDistanceObject(const std::string& id,
boost::shared_ptr<CollisionWorldDistanceField::DistanceFieldCacheEntry>& dfce,
EigenSTL::vector_Vector3d& add_points,
EigenSTL::vector_Vector3d& subtract_points)
{
std::map<std::string, std::vector<PosedBodyPointDecompositionPtr> >::iterator cur_it = dfce->posed_body_point_decompositions_.find(id);
if(cur_it != dfce->posed_body_point_decompositions_.end()) {
for(unsigned int i = 0; i < cur_it->second.size(); i++) {
subtract_points.insert(subtract_points.end(),
cur_it->second[i]->getCollisionPoints().begin(),
cur_it->second[i]->getCollisionPoints().end());
}
}
ObjectConstPtr object = getObject(id);
if(object) {
std::vector<PosedBodyPointDecompositionPtr> shape_points;
for(unsigned int i = 0; i < object->shapes_.size(); i++) {
BodyDecompositionConstPtr bd = getBodyDecompositionCacheEntry(object->shapes_[i],
resolution_);
shape_points.push_back(boost::make_shared<PosedBodyPointDecomposition>(bd, object->shape_poses_[i]));
add_points.insert(add_points.end(),
shape_points.back()->getCollisionPoints().begin(),
shape_points.back()->getCollisionPoints().end());
}
dfce->posed_body_point_decompositions_[id] = shape_points;
} else {
dfce->posed_body_point_decompositions_.erase(id);
}
}
void mesh_core::Plane::leastSquaresGeneral(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d* average)
{
if (points.empty())
{
normal_ = Eigen::Vector3d(0,0,1);
d_ = 0;
if (average)
*average = Eigen::Vector3d::Zero();
return;
}
// find c, the average of the points
Eigen::Vector3d c;
c.setZero();
EigenSTL::vector_Vector3d::const_iterator p = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; p != end ; ++p)
c += *p;
c *= 1.0/double(points.size());
// Find the matrix
Eigen::Matrix3d m;
m.setZero();
p = points.begin();
for ( ; p != end ; ++p)
{
Eigen::Vector3d cp = *p - c;
m(0,0) += cp.x() * cp.x();
m(1,0) += cp.x() * cp.y();
m(2,0) += cp.x() * cp.z();
m(1,1) += cp.y() * cp.y();
m(2,1) += cp.y() * cp.z();
m(2,2) += cp.z() * cp.z();
}
m(0,1) = m(1,0);
m(0,2) = m(2,0);
m(1,2) = m(2,1);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(m);
if (eigensolver.info() == Eigen::Success)
{
normal_ = eigensolver.eigenvectors().col(0);
normal_.normalize();
}
else
{
normal_ = Eigen::Vector3d(0,0,1);
}
d_ = -c.dot(normal_);
if (average)
*average = c;
}
bool robot_sphere_representation::RobotSphereRepresentation::saveToSrdfFile(const std::string& srdf_filename) const
{
genSpheresForAllLinks();
// Get an SRDFWriter with the data from the current RobotModel
moveit_setup_assistant::SRDFWriter writer;
writer.initModel( *robot_model_->getURDF(), *robot_model_->getSRDF() );
// Delete any existing spheres
writer.link_sphere_approximations_.clear();
// Insert generated spheres into SRDFWriter
EigenSTL::vector_Vector3d centers;
std::vector<double> radii;
std::map<std::string, LinkSphereRepresentation*>::const_iterator lsr = links_.begin();
std::map<std::string, LinkSphereRepresentation*>::const_iterator lsr_end = links_.end();
for ( ; lsr != lsr_end ; ++lsr )
{
centers.clear();
radii.clear();
lsr->second->getSpheres(centers, radii);
if (centers.empty())
{
// a link with no geometry is represented by a single radius=0 sphere
radii.clear();
radii.push_back(0);
centers.push_back(Eigen::Vector3d(0,0,0));
}
srdf::Model::LinkSpheres lsp;
lsp.link_ = lsr->first;
for ( std::size_t i = 0 ; i < centers.size() ; ++i )
{
srdf::Model::Sphere sphere;
sphere.center_x_ = centers[i].x();
sphere.center_y_ = centers[i].y();
sphere.center_z_ = centers[i].z();
sphere.radius_ = radii[i];
lsp.spheres_.push_back(sphere);
}
writer.link_sphere_approximations_.push_back(lsp);
}
// write the SRDF to file and return true on success.
return writer.writeSRDF(srdf_filename);
}
void mesh_core::appendPoints(
EigenSTL::vector_Vector3d& vector,
int npoints,
const double *data)
{
int base = vector.size();
vector.resize(base + npoints);
for (int i = 0; i < npoints ; ++i)
{
vector[base+i] = Eigen::Vector3d(data[i*3+0],
data[i*3+1],
data[i*3+2]);
}
}
TEST(SphereRayIntersection, SimpleRay2)
{
shapes::Sphere shape(1.0);
bodies::Body* sphere = new bodies::Sphere(&shape);
sphere->setScale(1.05);
Eigen::Vector3d ray_o(5, 0, 0);
Eigen::Vector3d ray_d(1, 0, 0);
EigenSTL::vector_Vector3d p;
bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);
delete sphere;
EXPECT_FALSE(intersect);
EXPECT_EQ(0, (int)p.size());
}
TEST(SphereRayIntersection, SimpleRay1)
{
shapes::Sphere shape(1.0);
bodies::Body* sphere = new bodies::Sphere(&shape);
sphere->setScale(1.05);
Eigen::Vector3d ray_o(5, 0, 0);
Eigen::Vector3d ray_d(-1, 0, 0);
EigenSTL::vector_Vector3d p;
bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);
delete sphere;
EXPECT_TRUE(intersect);
EXPECT_EQ(2, (int)p.size());
EXPECT_NEAR(p[0].x(), 1.05, 1e-6);
EXPECT_NEAR(p[1].x(), -1.05, 1e-6);
}
mesh_core::Plane::Plane(
const EigenSTL::vector_Vector3d& points)
{
if (points.size() <= 3)
from3Points(points);
else
leastSquaresGeneral(points);
}
void mesh_core::generateAABB(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d& min,
Eigen::Vector3d& max)
{
min = Eigen::Vector3d(std::numeric_limits<double>::max(),
std::numeric_limits<double>::max(),
std::numeric_limits<double>::max());
max = Eigen::Vector3d(-std::numeric_limits<double>::max(),
-std::numeric_limits<double>::max(),
-std::numeric_limits<double>::max());
EigenSTL::vector_Vector3d::const_iterator it = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; it != end ; ++it)
{
min = min.array().min(it->array());
max = max.array().max(it->array());
}
}
void mesh_core::Plane::leastSquaresFast(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d* average)
{
Eigen::Matrix3d m;
Eigen::Vector3d b;
Eigen::Vector3d c;
m.setZero();
b.setZero();
c.setZero();
EigenSTL::vector_Vector3d::const_iterator p = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; p != end ; ++p)
{
m(0,0) += p->x() * p->x();
m(1,0) += p->x() * p->y();
m(2,0) += p->x();
m(1,1) += p->y() * p->y();
m(2,1) += p->y();
b(0) += p->x() * p->z();
b(1) += p->y() * p->z();
b(2) += p->z();
c += *p;
}
m(0,1) = m(1,0);
m(0,2) = m(2,0);
m(1,2) = m(2,1);
m(2,2) = double(points.size());
c *= 1.0/double(points.size());
normal_ = m.colPivHouseholderQr().solve(b);
if (normal_.squaredNorm() > std::numeric_limits<double>::epsilon())
normal_.normalize();
d_ = -c.dot(normal_);
if (average)
*average = c;
}
SphereInfo::SphereInfo(
const EigenSTL::vector_Vector3d& points)
: points_(points)
, center_(0,0,0)
, radius_(0)
, radius_sq_(0)
{
int npoints = points.size();
list_.resize(npoints);
for (int i = 0 ; i < npoints ; i++)
list_[i] = &points[i];
}
void teleop_tracking::combineVertices(const std::vector<teleop_tracking::StlLoader::Facet> &facets,
EigenSTL::vector_Vector3d &vertices,
EigenSTL::vector_Vector3d &face_normals,
std::vector<unsigned> &face_indices)
{
// The assumption is that these source are empty
assert(vertices.empty());
assert(face_normals.empty());
assert(face_indices.empty());
EigenSTL::vector_Vector3f float_vector;
for (std::size_t i = 0; i < facets.size(); ++i)
{
const StlLoader::Facet& f = facets[i];
face_normals.push_back(toEigend(f.normal).normalized());
unsigned v0 = appendUnique(float_vector, toEigenf(f.vertices[0]));
unsigned v1 = appendUnique(float_vector, toEigenf(f.vertices[1]));
unsigned v2 = appendUnique(float_vector, toEigenf(f.vertices[2]));
// Small triangles should not have edges collapsed together
assert(v0 != v1);
assert(v0 != v2);
assert(v1 != v2);
face_indices.push_back(v0);
face_indices.push_back(v1);
face_indices.push_back(v2);
}
// copy the vector of single precision floats to double precision output
for (std::size_t i = 0; i < float_vector.size(); ++i)
{
Eigen::Vector3d v = float_vector[i].cast<double>();
vertices.push_back(v);
}
}
void mesh_core::generateBoundingSphere(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d& center,
double &radius)
{
SphereInfo info(points);
info.findStartingPoints();
info.findSphere(0, points.size());
center = info.center_;
radius = info.radius_;
}
Mesh* createMeshFromAsset(const aiScene* scene, const Eigen::Vector3d &scale, const std::string &resource_name)
{
if (!scene->HasMeshes())
{
logWarn("Assimp reports scene in %s has no meshes", resource_name.c_str());
return NULL;
}
EigenSTL::vector_Vector3d vertices;
std::vector<unsigned int> triangles;
extractMeshData(scene, scene->mRootNode, aiMatrix4x4(), scale, vertices, triangles);
if (vertices.empty())
{
logWarn("There are no vertices in the scene %s", resource_name.c_str());
return NULL;
}
if (triangles.empty())
{
logWarn("There are no triangles in the scene %s", resource_name.c_str());
return NULL;
}
return createMeshFromVertices(vertices, triangles);
}
void mesh_core::Plane::from3Points(
const EigenSTL::vector_Vector3d& points)
{
Eigen::Vector3d ab, ac, norm;
int npoints = points.size();
if (npoints > 2)
{
ab = points[1] - points[0];
ac = points[2] - points[0];
norm = ab.cross(ac);
}
else if (npoints == 2)
{
ab = points[1] - points[0];
ac(0) = ab(1);
ac(1) = ab(2);
ac(2) = ab(0);
norm = ab.cross(ac);
}
else if (npoints == 1)
{
*this = Plane(Eigen::Vector3d(0,0,1), points[0]);
return;
}
else
{
*this = Plane();
return;
}
if (norm.squaredNorm() <= std::numeric_limits<double>::epsilon())
{
ac(0) = ab(1);
ac(1) = ab(2);
ac(2) = ab(0);
norm = ab.cross(ac);
if (norm.squaredNorm() <= std::numeric_limits<double>::epsilon())
{
norm = Eigen::Vector3d(0,0,1);
}
}
*this = Plane(norm, points[0]);
}
void collision_detection::StaticDistanceField::determineCollisionPoints(
const bodies::Body& body,
double resolution,
EigenSTL::vector_Vector3d& points)
{
bodies::BoundingSphere sphere;
body.computeBoundingSphere(sphere);
double xval_s = std::floor((sphere.center.x() - sphere.radius - resolution) / resolution) * resolution;
double yval_s = std::floor((sphere.center.y() - sphere.radius - resolution) / resolution) * resolution;
double zval_s = std::floor((sphere.center.z() - sphere.radius - resolution) / resolution) * resolution;
double xval_e = sphere.center.x() + sphere.radius + resolution;
double yval_e = sphere.center.y() + sphere.radius + resolution;
double zval_e = sphere.center.z() + sphere.radius + resolution;
Eigen::Vector3d pt;
for(pt.x() = xval_s; pt.x() <= xval_e; pt.x() += resolution) {
for(pt.y() = yval_s; pt.y() <= yval_e; pt.y() += resolution) {
for(pt.z() = zval_s; pt.z() <= zval_e; pt.z() += resolution) {
if(body.containsPoint(pt)) {
points.push_back(pt);
}
}
}
}
}
void distance_field::findInternalPointsConvex(
const bodies::Body& body,
double resolution,
EigenSTL::vector_Vector3d& points)
{
bodies::BoundingSphere sphere;
body.computeBoundingSphere(sphere);
double xval_s = std::floor((sphere.center.x() - sphere.radius - resolution) / resolution) * resolution;
double yval_s = std::floor((sphere.center.y() - sphere.radius - resolution) / resolution) * resolution;
double zval_s = std::floor((sphere.center.z() - sphere.radius - resolution) / resolution) * resolution;
double xval_e = sphere.center.x() + sphere.radius + resolution;
double yval_e = sphere.center.y() + sphere.radius + resolution;
double zval_e = sphere.center.z() + sphere.radius + resolution;
Eigen::Vector3d pt;
for(pt.x() = xval_s; pt.x() <= xval_e; pt.x() += resolution) {
for(pt.y() = yval_s; pt.y() <= yval_e; pt.y() += resolution) {
for(pt.z() = zval_s; pt.z() <= zval_e; pt.z() += resolution) {
if(body.containsPoint(pt)) {
points.push_back(pt);
}
}
}
}
}
void collision_detection::StaticDistanceField::initialize(
const bodies::Body& body,
double resolution,
double space_around_body,
bool save_points)
{
points_.clear();
inv_twice_resolution_ = 1.0 / (2.0 * resolution);
logInform(" create points at res=%f",resolution);
EigenSTL::vector_Vector3d points;
determineCollisionPoints(body, resolution, points);
if (points.empty())
{
logWarn(" StaticDistanceField::initialize: No points in body. Using origin.");
points.push_back(body.getPose().translation());
if (body.getType() == shapes::MESH)
{
const bodies::ConvexMesh& mesh = dynamic_cast<const bodies::ConvexMesh&>(body);
const EigenSTL::vector_Vector3d& verts = mesh.getVertices();
logWarn(" StaticDistanceField::initialize: also using %d vertices.", int(verts.size()));
EigenSTL::vector_Vector3d::const_iterator it = verts.begin();
EigenSTL::vector_Vector3d::const_iterator it_end = verts.end();
for ( ; it != it_end ; ++it)
{
points.push_back(*it);
}
}
}
logInform(" StaticDistanceField::initialize: Using %d points.", points.size());
AABB aabb;
aabb.add(points);
logInform(" space_around_body = %f",space_around_body);
logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (pre-space)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
aabb.max_.x(),
aabb.max_.y(),
aabb.max_.z());
aabb.min_ -= Eigen::Vector3d(space_around_body, space_around_body, space_around_body);
aabb.max_ += Eigen::Vector3d(space_around_body, space_around_body, space_around_body);
logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (pre-adjust)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
aabb.max_.x(),
aabb.max_.y(),
aabb.max_.z());
aabb.min_.x() = std::floor(aabb.min_.x() / resolution) * resolution;
aabb.min_.y() = std::floor(aabb.min_.y() / resolution) * resolution;
aabb.min_.z() = std::floor(aabb.min_.z() / resolution) * resolution;
logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (post-adjust)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
aabb.max_.x(),
aabb.max_.y(),
aabb.max_.z());
Eigen::Vector3d size = aabb.max_ - aabb.min_;
double diagonal = size.norm();
logInform(" DF: sz=(%7.3f %7.3f %7.3f) cnt=(%d %d %d) diag=%f",
size.x(),
size.y(),
size.z(),
int(size.x()/resolution),
int(size.y()/resolution),
int(size.z()/resolution),
diagonal);
distance_field::PropagationDistanceField df(
size.x(),
size.y(),
size.z(),
resolution,
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
diagonal * 2.0,
true);
df.addPointsToField(points);
DistPosEntry default_entry;
default_entry.distance_ = diagonal * 2.0;
default_entry.cell_id_ = -1;
resize(size.x(),
size.y(),
size.z(),
resolution,
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
default_entry);
logInform(" copy %d points.",
getNumCells(distance_field::DIM_X) *
getNumCells(distance_field::DIM_Y) *
getNumCells(distance_field::DIM_Z));
int pdf_x,pdf_y,pdf_z;
int sdf_x,sdf_y,sdf_z;
Eigen::Vector3d pdf_p, sdf_p;
df.worldToGrid(aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), pdf_x,pdf_y,pdf_z);
worldToGrid(aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), sdf_x,sdf_y,sdf_z);
df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());
logInform(" DF: min=(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: min=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)",
pdf_p.x(),
pdf_p.y(),
pdf_p.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: min=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)",
sdf_p.x(),
sdf_p.y(),
sdf_p.z(),
sdf_x,
sdf_y,
sdf_z);
df.worldToGrid(0,0,0, pdf_x,pdf_y,pdf_z);
worldToGrid(0,0,0, sdf_x,sdf_y,sdf_z);
df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());
logInform(" DF: org=(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)",
0.0,
0.0,
0.0,
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: org=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)",
pdf_p.x(),
pdf_p.y(),
pdf_p.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: org=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)",
sdf_p.x(),
sdf_p.y(),
sdf_p.z(),
sdf_x,
sdf_y,
sdf_z);
df.worldToGrid(points[0].x(), points[0].y(), points[0].z(), pdf_x,pdf_y,pdf_z);
worldToGrid(points[0].x(), points[0].y(), points[0].z(), sdf_x,sdf_y,sdf_z);
df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());
logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)",
points[0].x(),
points[0].y(),
points[0].z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)",
pdf_p.x(),
pdf_p.y(),
pdf_p.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)",
sdf_p.x(),
sdf_p.y(),
sdf_p.z(),
sdf_x,
sdf_y,
sdf_z);
for (int z = 0 ; z < df.getZNumCells() ; ++z)
{
for (int y = 0 ; y < df.getYNumCells() ; ++y)
{
for (int x = 0 ; x < df.getXNumCells() ; ++x)
{
DistPosEntry entry;
double dist = df.getDistance(x, y, z);
const distance_field::PropDistanceFieldVoxel& voxel = df.getCell(x,y,z);
if (dist < 0)
{
// propogation distance field has a bias of -1*resolution on points inside the object
if (dist <= -resolution)
{
dist += resolution;
}
else
{
logError("PropagationDistanceField returned distance=%f between 0 and -resolution=%f."
" Did someone fix it?"
" Need to remove workaround from static_distance_field.cpp",
dist,-resolution);
dist = 0.0;
}
entry.distance_ = dist;
entry.cell_id_ = getCellId(
voxel.closest_negative_point_.x(),
voxel.closest_negative_point_.y(),
voxel.closest_negative_point_.z());
}
else
{
entry.distance_ = dist;
entry.cell_id_ = getCellId(
voxel.closest_point_.x(),
voxel.closest_point_.y(),
voxel.closest_point_.z());
}
setCell(x, y, z, entry);
}
}
}
if (save_points)
std::swap(points, points_);
}