C++ Joint::getMatrixPos方法代码示例

void CollisionSpace::drawCollisionPoints() {
  for (unsigned int i = 0; i < m_Robot->getNumberOfJoints(); i++) {
    Joint* jnt = m_Robot->getJoint(i);
    Eigen::Transform3d T = jnt->getMatrixPos();
    std::vector<CollisionPoint>& points = m_sampler->getCollisionPoints(jnt);

    for (unsigned int j = 0; j < points.size(); j++) {
      // if (points[j].getSegmentNumber() <ENV.getDouble(Env::extensionStep))
      //    continue;

      if (points[j].m_is_colliding) {
        double color[4];

        color[1] = 0.0;  // green
        color[2] = 0.0;  // blue
        color[0] = 1.0;  // red
        color[3] = 0.7;  // transparency

        g3d_set_color(Any, color);
      }

      bool yellow = (!points[j].m_is_colliding);

      points[j].draw(T, yellow);
    }
  }
}

double CollisionSpace::cost(const Configuration& q) const {
  // bool colliding = false;
  double distance = 0.0;
  double potential = 0.0;

  //    colliding =
  //    isRobotColliding( distance, potential );

  double cost = 0.0;

  Eigen::Vector3d position, gradient;

  for (unsigned int i = 0; i < m_Robot->getNumberOfJoints(); i++) {
    Joint* jnt = m_Robot->getJoint(i);

    std::vector<CollisionPoint>& points = m_sampler->getCollisionPoints(jnt);
    if (points.empty()) continue;

    Eigen::Transform3d T = jnt->getMatrixPos();

    for (unsigned int j = 0; j < points.size(); j++) {
      position = T * points[j].getPosition();
      getCollisionPointPotentialGradient(
          points[j], position, distance, potential, gradient);
      if (potential < EPS6) {
        potential = EPS6;
      }
      cost += potential;
    }
  }

  return 10 * cost;
}

void CollisionSpace::addEnvPoints() {
  // Add Static Obstacles
  for (int i = 0; i < XYZ_ENV->no; i++) {
    PointCloud& cloud = m_sampler->getPointCloud(XYZ_ENV->o[i]);

    for (unsigned int j = 0; j < cloud.size(); j++)
      m_points_to_add.push_back(cloud[j]);
  }

  // Add Moving Obstacles
  Scene* sc = global_Project->getActiveScene();

  for (unsigned int i = 0; i < sc->getNumberOfRobots(); i++) {
    Robot* mov_obst = sc->getRobot(i);

    // The robot is not a robot or a human
    if (!((mov_obst->getName().find("ROBOT") != std::string::npos) ||
          (mov_obst->getName().find("HUMAN") != std::string::npos) ||
          mov_obst->getName() == "rob1" || mov_obst->getName() == "rob2" ||
          mov_obst->getName() == "rob3" || mov_obst->getName() == "rob4")) {
      cout << "Adding : " << mov_obst->getName() << endl;

      Joint* jnt = mov_obst->getJoint(1);

      if (static_cast<p3d_jnt*>(jnt->getP3dJointStruct())->o == NULL) continue;

      PointCloud& cloud = m_sampler->getPointCloud(
          static_cast<p3d_jnt*>(jnt->getP3dJointStruct())->o);

      for (int j = 0; j < int(cloud.size()); j++) {
        m_points_to_add.push_back(jnt->getMatrixPos() * cloud[j]);
      }
    }
  }
}

void Boxes::drawCollisionPoints()
{
    for( size_t i=0; i<active_joints_.size(); i++ )
    {
        Joint* jnt = robot_->getJoint( active_joints_[i] );
        std::vector<CollisionPoint>& points = sampler_->getCollisionPoints(jnt);
        Eigen::Transform3d T = jnt->getMatrixPos();

        cout << "joint : " << active_joints_[i] << " , points size : " << points.size() << endl;

        for( size_t j=0; j<points.size(); j++ )
        {
            if( points[j].m_is_colliding )
            {
                double color[4];

                color[0] = 1.0;       // (r)ed
                color[1] = 0.0;       // (g)reen
                color[2] = 0.0;       // (b)lue
                color[3] = 0.7;       // transparency

                g3d_set_color(Any,color);
            }

            bool yellow = true;
            //bool yellow = (!points[j].m_is_colliding);

            points[j].draw( T, yellow );
        }
    }
}

//! Goes through all points and computes whether the robot is colliding
//! Computes the transform of all point from the joint transform
//! Uses the potential gradient function
bool CollisionSpace::isRobotColliding(double& dist, double& pot) const {
  // cout << "Test collision space is robot colliding" << endl;
  bool isRobotColliding = false;

  double potential = 0.0;
  double max_potential = -std::numeric_limits<double>::max();

  double distance = 0.0;
  double min_distance = std::numeric_limits<double>::max();

  Eigen::Vector3d position, gradient;

  for (unsigned int i = 0; i < m_Robot->getNumberOfJoints(); i++) {
    Joint* jnt = m_Robot->getJoint(i);

    std::vector<CollisionPoint>& points = m_sampler->getCollisionPoints(jnt);
    if (points.empty()) continue;

    Eigen::Transform3d T = jnt->getMatrixPos();

    for (unsigned int j = 0; j < points.size(); j++) {
      position = T * points[j].getPosition();

      // CollisionSpaceCell* cell = static_cast<CollisionSpaceCell*>( getCell(
      // position ) );
      // if( ( cell != NULL ? getDistance( cell ) : 0 ) <=
      // points[j].getRadius()
      // )

      if (getCollisionPointPotentialGradient(
              points[j], position, distance, potential, gradient)) {
        points[j].m_is_colliding = true;

        //cout << "point[" << j
         //    << "] in joint is in collision: " << points[j].joint()->getName()
         //    << endl;

        // Hack!!! to exclude certain points
        if (points[j].getSegmentNumber() > 1) {
          // cout << "points[" << j << "].getSegmentNumber()" << endl;
          isRobotColliding = true;
        }
      } else {
        points[j].m_is_colliding = false;
      }

      if (max_potential < potential) max_potential = potential;

      if (min_distance > distance) min_distance = distance;
    }
  }

  pot = max_potential;
  dist = min_distance;
  return isRobotColliding;
}