Exemplo n.º 1
0
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);
        }
      }
    }
  }
}
Exemplo n.º 5
0
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_);
}