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 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); } }
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 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_); }