ShapeSpacePlannerBase.h

Go to the documentation of this file.

//-*-c++-*-
#ifndef _SHAPE_SPACE_PLANNER_BASE_H_
#define _SHAPE_SPACE_PLANNER_BASE_H_

#include "Motion/KinematicJoint.h"
#include "Planners/PlannerObstacles.h"
#include "DualCoding/DualCoding.h"
#include "GenericRRT.h"

//================ ShapeSpaceCollisionCheckerBase ================

//! Base class for doing collision checking in world shape space
template <size_t N>
class ShapeSpaceCollisionCheckerBase {
protected:
  //! world bounds, must be closed to be used
  DualCoding::Shape<DualCoding::PolygonData> worldBounds;
  
  float const inflation;  //!< Amount in mm to add to obstacle bounding shape
  
  //! world map obstacles
  std::vector<PlannerObstacle<N>*> obstacles;
  
public:
  ShapeSpaceCollisionCheckerBase(ShapeSpace &shs,
                                 const Shape<PolygonData> &_worldBounds,
                                 float _inflation);
  
  virtual ~ShapeSpaceCollisionCheckerBase();
  
  const Shape<PolygonData> getWorldBounds() const { return worldBounds; }
  
  BoundingBox<N> getObstacleBoundingBox() const;
  
  //! create a HierarchicalObstacle to model the robot for collision detection
  void getRobotObstacle(const KinematicJoint &j, HierarchicalObstacle &hierObs) const;
  void addObstacleFromCollisionModel(const LinkComponent& j, const fmat::Transform& robotT, HierarchicalObstacle& hierObs) const;
  
  //! Debugging tool to make obstacles visible
  void addObstaclesToShapeSpace(DualCoding::ShapeSpace & shs) const;
};

template <size_t N>
BoundingBox<N> ShapeSpaceCollisionCheckerBase<N>::getObstacleBoundingBox() const {
  if (obstacles.empty()) return BoundingBox<N>();
  
  BoundingBox<N> bounds = obstacles.front()->getBoundingBox();
  
  for (unsigned int i = 0; i < obstacles.size(); i++) {
    bounds.expand(obstacles[i]->getBoundingBox());
  }
  return bounds;
}

template <size_t N>
ShapeSpaceCollisionCheckerBase<N>::ShapeSpaceCollisionCheckerBase(ShapeSpace &shs,
                                                                 const Shape<PolygonData> &_worldBounds,
                                                                 float _inflation) :
worldBounds(_worldBounds), inflation(_inflation), obstacles() {
  SHAPEROOTVEC_ITERATE(shs, s) {
    if ( s->getId() != DualCoding::VRmixin::theAgent->getId() && s->isObstacle() )
      PlannerObstacle<N>::convertShapeToPlannerObstacle(s, inflation, obstacles);
  } END_ITERATE;
  
  DualCoding::Point location = VRmixin::theAgent->getCentroid();
}

template <size_t N>
ShapeSpaceCollisionCheckerBase<N>::~ShapeSpaceCollisionCheckerBase() {
  for (unsigned int i = 0; i < obstacles.size(); i++)
    delete obstacles[i];
}

template <size_t N>
void ShapeSpaceCollisionCheckerBase<N>::addObstaclesToShapeSpace(ShapeSpace &shs) const {
  for (unsigned int i = 0; i < obstacles.size(); i++) {
    EllipticalObstacle *c = dynamic_cast<EllipticalObstacle*>(obstacles[i]);
    if (c) {
      fmat::Column<2> p = c->getCenter();
      NEW_SHAPE(obstacle, EllipseData, new EllipseData(shs, Point(p[0],p[1]), c->semimajor, c->semiminor));
      obstacle->setColor(rgb(0,255,0));
    }
    
    RectangularObstacle *r = dynamic_cast<RectangularObstacle*>(obstacles[i]);
    if (r) {
      std::vector<Point> pts;
      for (int j = 0; j < 4; j++) {
        fmat::Column<2> p = r->getCorner(static_cast<RectangularObstacle::CornerOrder>(j));
        pts.push_back(Point(p[0],p[1]));
      }
      
      NEW_SHAPE(obstacle, PolygonData, new PolygonData(shs, pts, true));
      obstacle->setColor(rgb(0,255,0));
    }
  }
}

template <size_t N>
void ShapeSpaceCollisionCheckerBase<N>::getRobotObstacle(const KinematicJoint& j, HierarchicalObstacle& hierObs) const {
  // use every joint in the robot
  for (KinematicJoint::branch_iterator it = j.getBranches().begin(); it != j.getBranches().end(); ++it) {
    getRobotObstacle(**it, hierObs);
  }
  // for now, just using bounding box. need to work on projection geometry (addObstacleFromCollisionModel) a bit further...
  //addObstacleFromCollisionModel(j, j.getFullT(), hierObs);
  RectangularObstacle* rect = new RectangularObstacle;
  j.getOwnBB2D(j.getFullT(), *rect);
  hierObs.add(rect);
  for (KinematicJoint::component_iterator it = j.components.begin(); it != j.components.end(); ++it) {
    // for now, just using bounding box. need to work on projection geometry (addObstacleFromCollisionModel) a bit further...
    //addObstacleFromCollisionModel(**it, j.getFullT(), hierObs);
    RectangularObstacle* r = new RectangularObstacle;
    (*it)->getOwnBB2D(j.getFullT(), *r);
    hierObs.add(r);
  }
}

template <size_t N>
void ShapeSpaceCollisionCheckerBase<N>::addObstacleFromCollisionModel(const LinkComponent& j, const fmat::Transform& robotT, HierarchicalObstacle& hierObs) const {
  // get all relevant collision model data
  fmat::Transform t;
  j.getCollisionModelTransform(t);
  t = t * robotT;
  fmat::Column<3> scale;
  j.collisionModelScale.exportTo(scale);
  
  // create respective obstacle
  if (j.collisionModel == "Cube") {
    RectangularObstacle* rect = new RectangularObstacle;
    j.getOwnBB2D(fmat::Transform(), *rect);
    hierObs.add(rect);
  }
  else if (j.collisionModel == "Cylinder") {
    //EllipticalObstacle* ellipse = new EllipticalObstacle;
    //hierObs.add(ellipse);
  }
  else if (j.collisionModel == "Sphere") {
    // parallel projection of an ellipsoid onto the XY plane
    fmat::Matrix<3,3> scaleM;
    scaleM(0,0) = 1/(scale[0]*scale[0]);
    scaleM(1,1) = 1/(scale[1]*scale[1]);
    scaleM(2,2) = 1/(scale[2]*scale[2]);
    fmat::Matrix<2,2> m(t.rotation() * scaleM * t.rotation().transpose());
    
    fmat::fmatReal discriminant = std::sqrt(std::pow((m(0,0) - m(1,1)), 2) + 4*m(0,1)*m(0,1));
    fmat::fmatReal lambda1 = (m(0,0) + m(1,1) + discriminant) / 2;
    fmat::fmatReal lambda2 = (m(0,0) + m(1,1) - discriminant) / 2;
    
    fmat::fmatReal semimajor, semiminor, ori;
    semimajor = 1 / std::sqrt(lambda1);
    semiminor = 1 / std::sqrt(lambda2);
    
    if (m(0,1) == 0)
      ori = 0;
    else {
      fmat::Column<2> v;
      v[0] = m(0,1) / (lambda1 - m(0,0));
      v[1] = 1;
      ori = fmat::atan(v);
    }
    
    EllipticalObstacle* ellipse = new EllipticalObstacle(fmat::Column<2>(t.translation()), semimajor, semiminor, ori);
    std::cout << ellipse->toString() << std::endl;
    hierObs.add(ellipse);
  }
}


#endif