ShapeSpacePlannerXYTheta.cc

Go to the documentation of this file.

#include "ShapeSpacePlannerXYTheta.h"
#include "Shared/mathutils.h" // for isnan fix

//================ RRTNodeXYTheta ================

// RRTNodeXYTheta

const AngSignPi RRTNodeXYTheta::turnLimit = M_PI;  // was M_PI/18;

float RRTNodeXYTheta::distance(const NodeValue_t &target) {
  return (q.first.first-target.first.first)*(q.first.first-target.first.first) +
  (q.first.second-target.first.second)*(q.first.second-target.first.second);
}

void RRTNodeXYTheta::generateSample(const NodeValue_t &lower, 
                                    const NodeValue_t &upper,
                                    NodeValue_t &sample) {
  sample.first.first = randRange(lower.first.first, upper.first.first);
  sample.first.second = randRange(lower.first.second, upper.first.second);
  sample.second = randRange(lower.second, upper.second);
}

RRTNodeBase::Interp_t RRTNodeXYTheta::interpolate(const NodeValue_t &start, const NodeValue_t &target, const NodeValue_t &interp, 
                                                  bool truncate, CollisionChecker *cc, NodeValue_t &reached, bool fromOtherTree) {
  const float stepLength = interp.first.first;
  
  // length of vector
  float dist = sqrt( pow(target.first.first-start.first.first, 2) + pow(target.first.second-start.first.second, 2) );
  
  if (fromOtherTree) {
    // if distance to nearest neighbor is very small and the angle is within turn limit, consider it done; extending from one tree to another
    if (dist < stepLength && abs(AngSignPi((float)start.second - (float)target.second - M_PI)) < turnLimit) {
      reached = target;
      reached.second += M_PI;
      bool collision = cc->collides(reached);
      reached.second -= M_PI;
      if ( collision )
        return COLLISION;
      else
        return REACHED;
    }
  }
  
  // determine heading
  NodeValue_t targetNew;
  targetNew.second = AngTwoPi(atan2(target.first.second-start.first.second, target.first.first-start.first.first));
  targetNew.first.first = start.first.first + cos(targetNew.second) * stepLength;
  targetNew.first.second = start.first.second + sin(targetNew.second) * stepLength;
  
  if (!goodAngle(start, targetNew)) {
    // limit angle if it's too much.
    AngSignPi diff(targetNew.second - start.second);
    AngSignPi signedTurn(diff > 0 ? turnLimit : AngSignPi(-turnLimit));
    AngTwoPi newAngle(start.second + signedTurn);
    
    // new position
    targetNew.first.first = start.first.first + cos(newAngle) * stepLength;
    targetNew.first.second = start.first.second + sin(newAngle) * stepLength;
    targetNew.second = newAngle;
  }
  
  // recalculate heading
  targetNew.second = AngTwoPi(atan2(targetNew.first.second-start.first.second, targetNew.first.first-start.first.first));
  reached = targetNew;
  
  if (fromOtherTree)
    reached.second += M_PI;
  bool collision = cc->collides(reached);
  if (fromOtherTree)
    reached.second -= M_PI;
  if ( collision )
    return COLLISION;
  else
    return APPROACHED;
}

RRTNodeBase::Interp_t RRTNodeXYTheta::smoothInterpolate(const NodeValue_t &start, const NodeValue_t &target, const NodeValue_t &interp, CollisionChecker *cc) {
  if (abs(AngSignPi(start.second - target.second)) > turnLimit) {
    return COLLISION; // pretend we collided if angle is bad
  }
  
  int xsteps = int(fabs(target.first.first-start.first.first)/interp.first.first);
  int ysteps = int(fabs(target.first.second-start.first.second)/interp.first.second);
  int numSteps = std::max(xsteps,ysteps);
  float deltaX = (target.first.first-start.first.first)/float(numSteps);
  float deltaY = (target.first.second-start.first.second)/float(numSteps);
  
  // Interpolate along the path and check for collisions
  NodeValue_t reached = start;
  for (int t = 0; t < numSteps; t++) {
    reached.first.first += deltaX;
    reached.first.second += deltaY;
    if ( cc->collides(reached) )
      return COLLISION;
  }
  
  if ( cc->collides(target) )
    return COLLISION;
  else
    return REACHED;
}

bool RRTNodeXYTheta::goodAngle(const NodeValue_t &q1, const NodeValue_t &qnew) {
  AngSignPi currentAngle(q1.second);
  AngSignPi newAngle(atan2(qnew.first.second - q1.first.second, qnew.first.first - q1.first.first));
  return abs(AngSignPi(newAngle-currentAngle)) < turnLimit;
}

std::string RRTNodeXYTheta::toString() const {
  char buff[100];
  sprintf(buff, "%7.2f %7.2f %7.2f", q.first.first, q.first.second, (float)(q.second));
  return string(buff);
}

bool RRTNodeXYTheta::CollisionChecker::collides(const NodeValue_t &qnew, ShapeSpacePlannerXYTheta::PlannerResult* result) const {
  HierarchicalObstacle robot;
  getRobotObstacle(*kine->getKinematicJoint(BaseFrameOffset), robot);
  robot.updatePosition(fmat::pack(qnew.first.first, qnew.first.second));
  robot.updateRotation(fmat::rotation2D(qnew.second));
  
  // treat as collision if outside world bounds
  if ( worldBounds.isValid() && !worldBounds->isInside(Point(qnew.first.first, qnew.first.second)) ) {
    if (result) {
      ostringstream os;
      os << VRmixin::theAgent->getName() << "-" << VRmixin::theAgent->getId();
      result->movingObstacle = new HierarchicalObstacle(robot); // should be improved so that of the hierarchical obstacles, you return which one is colliding.
      result->movingObstacle->name = os.str();
      ostringstream os2;
      os2 << worldBounds->getName() << "-" << worldBounds->getId();
      result->collidingObstacle = new HierarchicalObstacle;
      result->collidingObstacle->name = os2.str();
    }
    return true;
  }
  
  for (size_t i = 0; i < obstacles.size(); i++)
    if (obstacles[i]->collides(robot)) {
      if (result) {
        ostringstream os;
        os << VRmixin::theAgent->getName() << "-" << VRmixin::theAgent->getId();
        result->movingObstacle = new HierarchicalObstacle(robot); // should be improved so that of the hierarchical obstacles, you return which one is colliding.
        result->movingObstacle->name = os.str();
        result->collidingObstacle = dynamic_cast<PlannerObstacle2D*>(obstacles[i]->clone());
      }
      // std::cout << "Collision: " << robot.toString() << " with " << obstacles[i]->toString() << std::endl;
      return true;
    }
  return false;
}

//================ ShapeSpacePlannerXYTheta ================

// ShapeSpacePlannerXYTheta

ShapeSpacePlannerXYTheta::ShapeSpacePlannerXYTheta(ShapeSpace &shs, const Shape<PolygonData> &worldBounds,
                                                   float inflation) :
GenericRRT<NodeType_t, 2>(new NodeType_t::CollisionChecker(shs, worldBounds, inflation)), targetHeading() {
  NodeValue_t interp;
  interp.first.first = interp.first.second = 25; // step size in mm
  setInterpolation(interp);
}

void ShapeSpacePlannerXYTheta::initialize(const NodeValue_t &start, std::vector<NodeType_t> &treeStartResult,
                                          const NodeValue_t &end, std::vector<NodeType_t> &treeEndResult) {
  GenericRRT<NodeType_t, 2>::initialize(start, treeStartResult, end, treeEndResult);
  if (std::isnan((float)targetHeading)) {
    for (float theta = 0; theta < 2*M_PI; theta += M_PI/9) {
      NodeValue_t qnew;
      qnew.first.first = end.first.first + cos(theta) * extendingInterpolationStep.first.first;
      qnew.first.second = end.first.second + sin(theta) * extendingInterpolationStep.first.first;
      qnew.second = theta;
      addNode(&treeEndResult, qnew, 0);
    }
  }
}

void ShapeSpacePlannerXYTheta::buildPath(const std::vector<NodeType_t> *treeStart, const std::vector<NodeType_t> *treeEnd,
                                         std::vector<NodeValue_t> &path) {
  
  // connection point is last value in each tree
  unsigned int n = treeStart->size() - 1;
  while (n != 0) {
    n = (*treeStart)[n].parent;
    path.push_back((*treeStart)[n].q);
  }
  
  std::reverse(path.begin(), path.end());
  path.push_back(treeEnd->back().q);
  
  n = treeEnd->size() - 1;
  while (n != 0) {
    n = (*treeEnd)[n].parent;
    NodeValue_t value = (*treeEnd)[n].q;
    value.second = AngSignPi((float)value.second + M_PI);
    path.push_back(value);
  }
  
  // smooth path
  size_t maxIter = path.size();
  
  for (size_t i = 0; i < maxIter; i++) {
    int a = rand() % path.size();
    int b = rand() % path.size();
    if (a > b)
      std::swap(a,b);
    else if (a == b)
      continue;
    
    if ( NodeType_t::smoothInterpolate(path[a], path[b], smoothingInterpolationStep, cc) == RRTNodeBase::REACHED ) {
      // remove a -> b
      //      cout << "Erasing " << a << " to " << b << endl;
      //      cout << path.size() << endl;
      path.erase(path.begin()+a+1,path.begin()+b);
      //      cout << path.size() << endl;
    }
  }
  
  for (size_t i = 0; i+2 < path.size(); i++) {
    size_t j;
    for (j = i + 2; j < path.size(); j++) {
      if ( NodeType_t::smoothInterpolate(path[i], path[j], smoothingInterpolationStep, cc) != RRTNodeBase::REACHED )
        break;
    }
    if (j > i + 3 && j < path.size())
      path.erase(path.begin()+i+1, path.begin()+j-1);
  }
}

ShapeSpacePlannerXYTheta::PlannerResult
ShapeSpacePlannerXYTheta::planPath(const Point& start, const Point &end,
                                   const AngTwoPi &initialHeading,
                                   const AngTwoPi &_targetHeading,
                                   unsigned int maxIterations,
                                   std::vector<NodeValue_t> *pathResult,
                                   std::vector<NodeType_t> *treeStartResult,
                                   std::vector<NodeType_t> *treeEndResult) {
  NodeValue_t lower, upper;
  if ( cc->getWorldBounds().isValid() ) {
    BoundingBox2D b = cc->getWorldBounds()->getBoundingBox();
    lower.first.first = b.min[0]; lower.first.second = b.min[1];
    upper.first.first = b.max[0]; upper.first.second = b.max[1];
  } else {
    // combine with obstacles + start and end points
    BoundingBox2D b = cc->getObstacleBoundingBox();
    b.expand(fmat::SubVector<2,const fmat::fmatReal>(start.coords));
    b.expand(fmat::SubVector<2,const fmat::fmatReal>(end.coords));
    HierarchicalObstacle robot;
    cc->getRobotObstacle(*kine->getKinematicJoint(BaseFrameOffset), robot);
    fmat::Column<2> robotBounds = robot.getBoundingBox().getDimensions();
    float extra = std::max(robotBounds[0], robotBounds[1]);
    lower.first.first =  b.min[0] - 2*extra;
    lower.first.second = b.min[1] - 2*extra;
    upper.first.first =  b.max[0] + 2*extra;
    upper.first.second = b.max[1] + 2*extra;
  }
  lower.second = 0; upper.second = 2*M_PI;
  std::cout << "World bounds: [" << lower.first.first << "," << lower.first.second << "," << lower.second
  << "]  to  [" << upper.first.first << "," << upper.first.second << "," << upper.second << "]" << std::endl;
  setLimits(lower,upper);
  
  targetHeading = _targetHeading;
  float endHeading = std::isnan((float)_targetHeading) ? 0 : targetHeading - M_PI;
  
  std::pair<float, float> startvalpos(start.coordX(), start.coordY());
  NodeValue_t startval(startvalpos, initialHeading);
  std::pair<float, float> endvalpos(end.coordX(), end.coordY());
  NodeValue_t endval(endvalpos, AngTwoPi(endHeading));
  return GenericRRT<NodeType_t, 2>::planPath(startval, endval, maxIterations, 
                                          pathResult, treeStartResult, treeEndResult);
}

void ShapeSpacePlannerXYTheta::plotPath(const std::vector<NodeValue_t> &path,
          Shape<GraphicsData> &graphics,
          rgb color) {
  for ( unsigned int i = 1; i < path.size(); i++ )
    graphics->add(new GraphicsData::LineElement(path[i-1].first, path[i].first, color));
}

void ShapeSpacePlannerXYTheta::plotTree(const std::vector<NodeType_t> &tree,
                                        Shape<GraphicsData> &graphics,
                                        rgb color) {
  for ( unsigned int i = 1; i < tree.size(); i++ ) // start from 1 because root has no parent
    graphics->add(new GraphicsData::LineElement(tree[tree[i].parent].q.first, tree[i].q.first, color));
}