ShapeSpacePlanner3DR.h

Go to the documentation of this file.

#if defined(TGT_HAS_ARMS)

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

#include "Motion/Kinematics.h"
#include "Planners/RRT/ShapeSpacePlannerBase.h"

//================ RRTNode3DR ================

template <size_t N>
class ShapeSpacePlanner3DR;

template <size_t N>
class RRTNode3DR : public RRTNodeBase {
 public:
  
  class NodeValueWrapper {
  private:
    AngSignPi angles[(N==0)?1:N]; // funny array size to help older compilers that error on zero-sized arrays
  public:
    NodeValueWrapper() : angles() {}
      AngSignPi& operator[](int i)       { return angles[i]; }
      AngSignPi  operator[](int i) const { return angles[i]; }
  };
  
  typedef NodeValueWrapper NodeValue_t;
  typedef typename GenericRRTBase::PlannerResult<3> PlannerResult;
  NodeValue_t q;
  
  //! Constructor
  RRTNode3DR(const NodeValue_t &_q, unsigned int _parent) : RRTNodeBase(_parent), q(_q) {};
  
    class CollisionChecker : public ShapeSpaceCollisionCheckerBase<3> {
  protected:
    KinematicJoint* rootJ;
  public:
    fmat::Transform worldT;
  protected:
    
    class JointObstacle {
    public:
      BoxObstacle obstacle;
      KinematicJoint* joint;
      bool valid;
      
      JointObstacle() : obstacle(), joint(), valid() {}
      
        JointObstacle(KinematicJoint* _joint) : obstacle(), joint(_joint), valid(false) {
    obstacle.name = joint->outputOffset.get();
  }
      
    JointObstacle(const BoxObstacle& _obstacle,
      KinematicJoint* _joint,
      bool _valid) : obstacle(_obstacle), joint(_joint), valid(_valid) {}
      
            virtual ~JointObstacle() {}
      
      JointObstacle(const JointObstacle& other) : obstacle(other.obstacle), joint(other.joint), valid(other.valid) {}
        JointObstacle& operator=(const JointObstacle& other) {
                obstacle = other.obstacle;
                joint = other.joint;
                valid = other.valid;
                return *this;
        }
      
        bool collides(const JointObstacle& other) const { if (!other.valid) return false; return false; }
        bool collides(const PlannerObstacle3D& obs) const { return obs.collides(obstacle); }
      
        virtual bool setObstacle(const fmat::Transform& t, bool own=false);
    };
    
    class LinkObstacle : public JointObstacle {
    public:
      LinkComponent* link;
      
      LinkObstacle() : JointObstacle(), link() {}
      
        LinkObstacle(const JointObstacle& jObs) : JointObstacle(jObs.joint), link() {
    this->obstacle.name += "Component";
  }
      
    virtual ~LinkObstacle() {}
      
    void setLink(LinkComponent* _link) { link = _link; }
      
    LinkObstacle(const LinkObstacle& other) : JointObstacle(other.obstacle, other.joint, other.valid), link(other.link) {}
            LinkObstacle& operator=(const LinkObstacle& other) {
        JointObstacle(other.obstacle, other.joint, other.valid);
        link = other.link;
        return *this;
            }
      
            virtual bool setObstacle(const fmat::Transform& t, bool own=false); 
    };
    
  public:
    CollisionChecker(DualCoding::ShapeSpace & shs,
                     const DualCoding::Shape<DualCoding::PolygonData> &_worldBounds,
                     float _inflation,
                     unsigned int effectorOffset) :
      ShapeSpaceCollisionCheckerBase<3>(shs, _worldBounds, _inflation, false), rootJ(), worldT() {
      rootJ = kine->getKinematicJoint(effectorOffset)->cloneBranch();
      for (unsigned int i = 0; i < N; i++)
  rootJ = rootJ->getNextMobileAncestor();
      
      /* For the 3D arm planner, we have an arm that
       * has the potential to go beneath the ground.
       * Therefore, we add an obstacle that models
       * a large cube, 5m in length, width, and height
       * directly beneath the robot's position.
       * This ensures that no link in the arm will travel underground,
       * and potentially cause the robot to lift
       * itself off of the ground, which can be problematic. */
      const int size = 2500;
      fmat::Column<3> pos = worldT.translation();
      pos[2] -= float(2500);
      fmat::Matrix<3,3> rot = worldT.rotation();
      BoxObstacle* bo = new BoxObstacle(pos, fmat::pack(size,size,size), rot);
      bo->name = "GroundObstacle";
      obstacles.push_back(bo);
    }
    
      CollisionChecker(const CollisionChecker& other) : rootJ(other.rootJ) {}
    
        CollisionChecker operator=(const CollisionChecker& other) {
    rootJ = other.rootJ;
    return *this;
        }
    
        virtual bool collides(const NodeValue_t &qnew, PlannerResult* result=NULL) const;
    
        template<class T, class U>
    static bool checkComponent(std::vector<T>& full, const U& newObs, T& collisionObs);
    
  };
  
  static const unsigned int maxInterpolations = 100; //!< Maximum number of interpolation steps in interpolate() when @a truncate is true
  
  virtual float distance(const NodeValue_t &target);
  static void generateSample(const NodeValue_t &lower, const NodeValue_t &upper, NodeValue_t &sample);
  static Interp_t interpolate(const NodeValue_t &start, const NodeValue_t &target, const NodeValue_t &interp, 
            bool truncate, CollisionChecker *cc, NodeValue_t &reached, bool fromOtherTree);
  
  virtual std::string toString() const;
};

template<size_t N>
float RRTNode3DR<N>::distance(const NodeValue_t &target) {
  float result = 0;
  for (size_t i = 0; i < N; i++) {
    result += target[i]*target[i];
  }
  return result;
}

template<size_t N>
void RRTNode3DR<N>::generateSample(const NodeValue_t &lower, 
                                   const NodeValue_t &upper,
                                   NodeValue_t &sample) {
  for (size_t i = 0; i < N; i++) {
    sample[i] = randRange(lower[i], upper[i]);
  }
}

template<size_t N>
RRTNodeBase::Interp_t RRTNode3DR<N>::interpolate(const NodeValue_t &start, const NodeValue_t &target, const NodeValue_t &interp, 
                                                 bool truncate, CollisionChecker *cc, NodeValue_t &reached, bool fromOtherTree) {
  NodeValue_t delta;
  int numSteps = 0;
  for (size_t i = 0; i < N; i++) {
    int steps = int(fabs(target[i]-start[i])/interp[i]);
    if (steps > numSteps)
      numSteps = steps;
  }
  for (size_t i = 0; i < N; i++) {
    delta[i] = (target[i]-start[i])/float(numSteps);
  }
  
  bool truncated = (unsigned int)numSteps > maxInterpolations && truncate;
  if ( truncated )
    numSteps = maxInterpolations;
  
  // Interpolate along the path and check for collisions
  reached = start;
  for (int t = 0; t < numSteps; t++) {
    for (unsigned int i = 0; i < N; i++) {
      reached[i] += delta[i];
    }
    if ( cc->collides(reached) )
      return COLLISION;
  }
  
  if ( !truncated )
    return REACHED;
  else
    return APPROACHED;
}

template<size_t N>
std::string RRTNode3DR<N>::toString() const {
  char buff[100];
  string result;
  for (size_t i = 0; i < N; i++) {
    sprintf(buff, "%7.2f ", float(q[i]));
    result.append(result);
  }
  return result;
}

template<size_t N>
bool RRTNode3DR<N>::CollisionChecker::collides(const NodeValue_t &qnew, RRTNode3DR<N>::PlannerResult* result) const {
  // set joint angles to test the NodeValue
  KinematicJoint* joint = rootJ;
  for (unsigned int i = 0; i < N; i++) {
    if (!joint) { // just in case...
      std::cout << "*** ERROR: Collision checker cannot find arm joints. Assuming collision." << std::endl;
      return true;
    }
    
    joint->setQ(qnew[i]);
    joint = joint->nextJoint();
    while (joint != NULL && joint->isMobile() == false) joint = joint->nextJoint();
  }
  
  // for check component
  LinkObstacle collisionObs;
  
  // create and assign array of joints
  std::vector<JointObstacle> obs;
  for (joint = rootJ; joint != NULL; joint = joint->nextJoint()) {
    obs.push_back(JointObstacle(joint));
    for (KinematicJoint::branch_iterator it = joint->getBranches().begin(); it != joint->getBranches().end(); ++it) {
      obs.push_back(JointObstacle(*it));
    }
  }
  
  for (unsigned int i = 0; i < obs.size(); i++) {
    // if the joint has no boundingbox at all, skip it
    if (!obs[i].setObstacle(worldT))
      continue;
    
    // test if new joint obstacle collides with any other joint obstacle
    std::vector<int> collidingJObs; collidingJObs.reserve((i == 0) ? 0 : i-1);
    for (int j = 0; j < (int)i-1; j++) { // i-1 so that we don't check adjacent joint collision
      if (obs[j].valid && obs[i].collides(obs[j])) {
  collidingJObs.push_back(j);
      }
    }
    
    // test if new joint obstacle collides with any planner obstacle
    std::vector<PlannerObstacle3D*> collidingPObs; collidingPObs.reserve(obstacles.size());
    for (unsigned int k = 0; k < obstacles.size(); k++) {
      if ((i != 0 || !(obstacles[k]->isBodyObstacle())) && obs[i].collides(*(obstacles[k]))) {
  collidingPObs.push_back(obstacles[k]);
      }
    }
    
    // if there are no collisions at all, skip to the next joint
    if (collidingJObs.empty() && collidingPObs.empty())
      continue;
    
    /* Assuming there is any collision, we now test each LinkComponent,
     * so we need to assign a vector of components, including the
     * joint's own obstacle */
    JointObstacle jointOwnObs(obs[i].joint);
    jointOwnObs.setObstacle(worldT, true); // true flag specifies we're using the LinkComponent BB, (without children)
    
    std::vector<LinkObstacle> componentObs(obs[i].joint->components.size(), LinkObstacle(obs[i]));
    for (unsigned int c = 0; c < obs[i].joint->components.size(); c++) {
      componentObs[c].setLink(&(obs[i].joint->components[c]));
      componentObs[c].setObstacle(worldT, true);
    }
    
    // if there is a collision with a joint obstacle, check the sub-components to confirm
    for (unsigned int jObs = 0; jObs < collidingJObs.size(); jObs++) {
      unsigned int numComponents = obs[collidingJObs[jObs]].joint->components.size();
      
      // if there aren't any subcomponents in either, then there must be a collision
      if (componentObs.size() == 0 && numComponents == 0) {
  if (result) {
    result->movingObstacle = new BoxObstacle(obs[i].obstacle);
    result->collidingObstacle = new BoxObstacle(obs[collidingJObs[jObs]].obstacle);
  }
  return true;
      }
      
      // set the second joint's components
      // but whenever one is added, check for collision with previous
      JointObstacle collidingOwnObs(obs[collidingJObs[jObs]]);
      collidingOwnObs.setObstacle(worldT, true);
      
      // check if both joints' own obstacles collide
      if (collidingOwnObs.collides(jointOwnObs)) {
  if (result) {
    result->movingObstacle = new BoxObstacle(collidingOwnObs.obstacle);
    result->collidingObstacle = new BoxObstacle(jointOwnObs.obstacle);
  }
  return true;
      }
      // check if the colliding joint's own obstacle collides with joint's components
      if (checkComponent(componentObs, collidingOwnObs, collisionObs)) {
  if (result) {
    result->movingObstacle = new BoxObstacle(collidingOwnObs.obstacle);
    result->collidingObstacle = new BoxObstacle(collisionObs.obstacle);
  }
  return true;
      }
      
      // check if any of the colliding joint's components collides with the joint's own obstacle
      LinkObstacle componentOb(obs[collidingJObs[jObs]]);
      for (unsigned int c = 0; i < numComponents; c++) {
  componentOb.setLink(&(obs[collidingJObs[jObs]].joint->components[c]));
  componentOb.setObstacle(worldT, true);
  if (componentOb.collides(jointOwnObs)) {
    if (result) {
      result->movingObstacle = new BoxObstacle(componentOb.obstacle);
      result->collidingObstacle = new BoxObstacle(jointOwnObs.obstacle);
    }
    return true;
  }
        
  // check if any of both joints' components collide
  if (checkComponent(componentObs, componentOb, collisionObs)) {
    if (result) {
      result->movingObstacle = new BoxObstacle(componentOb.obstacle);
      result->collidingObstacle = new BoxObstacle(collisionObs.obstacle);
    }
    return true;
  }
      }
    }
    
    // if there is a collision with a planner obstacle, check the subcomponents to confirm
    for (unsigned int p = 0; p < collidingPObs.size(); p++) {
      if ((i != 0 || !(collidingPObs[p]->isBodyObstacle())) && jointOwnObs.collides(*(collidingPObs[p]))) {
  if (result) {
    result->movingObstacle = new BoxObstacle(jointOwnObs.obstacle);
    result->collidingObstacle = collidingPObs[p]->clone();
  }
  return true;
      }
      if ((i != 0 || !(collidingPObs[p]->isBodyObstacle())) && checkComponent(componentObs, *(collidingPObs[p]), collisionObs)) {
  if (result) {
    result->movingObstacle = new BoxObstacle(collisionObs.obstacle);
    result->collidingObstacle = collidingPObs[p]->clone();
  }
  return true;
      }
    }
  }
  
  // no collision detected
  return false;
}

template<size_t N>
template<class T, class U>
  bool RRTNode3DR<N>::CollisionChecker::checkComponent(std::vector<T>& full, const U& newObs, T& collisionObs) {
  for (unsigned int i = 0; i < full.size(); i++) {
    if (full[i].collides(newObs)) {
      collisionObs = full[i];
      return true;
    }
  }
  return false;
}

template<size_t N>
bool RRTNode3DR<N>::CollisionChecker::JointObstacle::setObstacle(const fmat::Transform& t, bool own) {
  if (joint == NULL) {
    std::cout << "***ERROR Joint value is null in CollisionChecker." << std::endl;
    valid = false;
  }
  else {
    if (own)
      valid = joint->getOwnBB3D(t * joint->getFullT(), obstacle);
    else
      valid = joint->getBB3D(t * joint->getFullT(), obstacle);
  }
  return valid;
}

template<size_t N>
bool RRTNode3DR<N>::CollisionChecker::LinkObstacle::setObstacle(const fmat::Transform& t, bool own) {
  if (this->joint == NULL || link == NULL) {
    std::cout << "***ERROR Joint/link value is null in CollisionChecker." << std::endl;
    this->valid = false;
  }
  else {
    if (own)
      this->valid = link->getOwnBB3D(t * this->joint->getFullT(), this->obstacle);
    else
      this->valid = link->getBB3D(t * this->joint->getFullT(), this->obstacle);
  }
  return this->valid;
}

//================ ShapeSpacePlanner3DR ================

//! Plans a path in a n-dimensional angular space, uses forward kinematics for collision testing
template <size_t N>
class ShapeSpacePlanner3DR : public GenericRRT<RRTNode3DR<N>, 3> {
 public:
  typedef RRTNode3DR<N> NodeType_t;
  typedef typename NodeType_t::NodeValue_t NodeValue_t;
  typedef typename GenericRRTBase::PlannerResult<3> PlannerResult;
  
  ShapeSpacePlanner3DR(DualCoding::ShapeSpace &shs,
           const DualCoding::Shape<DualCoding::PolygonData> &worldBounds,
           float inflation,
           unsigned int effectorOffset,
           AdmissibilityPredicate<NodeType_t> *predicate=NULL);
  
  ShapeSpacePlanner3DR(const ShapeSpacePlanner3DR& other) {
    for (size_t i = 0; i < N; i++)
      joints[i] = other.joints[i];
  }
  
  ShapeSpacePlanner3DR operator=(const ShapeSpacePlanner3DR& other) {
    for (size_t i = 0; i < N; i++)
      joints[i] = other.joints[i];
    return *this;
  }
  
  KinematicJoint* joints[N];
  fmat::Transform worldT;
  
  virtual ~ShapeSpacePlanner3DR() {}
  
  using GenericRRT<NodeType_t, 3>::planPath;
  PlannerResult
    planPath(NodeValue_t start,
       NodeValue_t end,
       NodeValue_t interpolationStep,
       const fmat::Transform& _worldT,
       unsigned int _maxIterations=40000,
       std::vector<NodeValue_t> *pathResult=NULL,
       std::vector<NodeType_t> *treeStartResult=NULL,
       std::vector<NodeType_t> *treeEndResult=NULL);
  
  //! Populates a Shape<GraphicsData> with BoundingBoxes
  void plotTree(const std::vector<NodeType_t> &tree,
    Shape<GraphicsData> &graphics,
    rgb color=rgb(255,0,0));
  
  //! Populates a Shape<GraphicsData> with BoundingBoxes
  void plotPath(const std::vector<NodeValue_t> &tree,
    Shape<GraphicsData> &graphics,
    rgb color=rgb(255,0,0));
  
  //! Returns BoundingBox of each link, for plotTree()
  void getBoxes(std::vector<std::vector<std::pair<float,float> > >& boxes, const KinematicJoint& joint);
  
};

template<size_t N>
ShapeSpacePlanner3DR<N>::ShapeSpacePlanner3DR(DualCoding::ShapeSpace &shs,
                                              const DualCoding::Shape<DualCoding::PolygonData> &worldBounds,
                                              float inflation,
                                              unsigned int effectorOffset,
                                              AdmissibilityPredicate<NodeType_t> *_predicate) :
  GenericRRT<NodeType_t, 3>::GenericRRT(new typename NodeType_t::CollisionChecker(shs, worldBounds, inflation, effectorOffset), _predicate),
  worldT() {
  KinematicJoint* joint = kine->getKinematicJoint(effectorOffset)->cloneBranch();
  if (joint->isMobile() == false) joint = joint->getNextMobileAncestor();
  NodeValue_t lower, upper;
  
  for (int i = (int)N-1; i >= 0; i--) {
    lower[i] = joint->qmin;
    upper[i] = joint->qmax;
    joints[i] = joint;
    joint = joint->getNextMobileAncestor();
  }
  
  this->setLimits(lower, upper);
}

template<size_t N>
typename ShapeSpacePlanner3DR<N>::PlannerResult
ShapeSpacePlanner3DR<N>::planPath(NodeValue_t start,
                                  NodeValue_t end,
                                  NodeValue_t interpolationStep,
                                  const fmat::Transform& _worldT,
                                  unsigned int maxIterations,
                                  std::vector<NodeValue_t> *pathResult,
                                  std::vector<NodeType_t> *treeStartResult,
                                  std::vector<NodeType_t> *treeEndResult) {
  this->setInterpolation(interpolationStep);
  this->worldT = _worldT;
  this->cc->worldT = _worldT;
  return GenericRRT<NodeType_t, 3>::planPath(start, end, maxIterations,
               pathResult, treeStartResult, treeEndResult);
}

template<size_t N>
void ShapeSpacePlanner3DR<N>::plotTree(const std::vector<NodeType_t> &tree,
                                       Shape<GraphicsData> &graphics,
                                       rgb color) {
  for ( unsigned int i = 0; i < tree.size(); i++ ) {
    // set joint values
    for (size_t j = 0; j < N; j++)
      joints[j]->setQ(tree[i].q[j]);
    
    // for now, just plot the location of the Gripper
    KinematicJoint* j = joints[N-1];
    
    while (j->nextJoint() != NULL) {
      j = j->nextJoint();
    }
    
    fmat::Column<3> pos = j->getWorldPosition();
    
    graphics->add(new GraphicsData::CircleElement("start", DualCoding::GraphicsData::xyPair(pos[0],pos[1]), 5, true, color));
    
    /*// get bounding box of every link
      std::vector<std::vector<std::pair<float,float> > > boxes;
      getBoxes(boxes, *joints[0]);*/
    
    /*// add to element
      for (size_t b = 0; b < boxes.size(); b++) {
      graphics->add(new GraphicsData::PolygonElement(boxes[b], true, color));
      }*/
  }
}

template<size_t N>
void ShapeSpacePlanner3DR<N>::plotPath(const std::vector<NodeValue_t> &path,
                                       Shape<GraphicsData> &graphics,
                                       rgb color) {
  for ( unsigned int i = 0; i < path.size(); i++ ) {
    // set joint values
    for (size_t j = 0; j < N; j++)
      joints[j]->setQ(path[i][j]);
    
    // set robot transformation in WorldSpace (converts from BaseFrame -> "WorldFrame")
    DualCoding::Point p = VRmixin::theAgent->getCentroid();
    worldT.translation() = -fmat::pack(p.coordX(),p.coordY(),p.coordZ());
    worldT.rotation() = fmat::rotationZ(-VRmixin::theAgent->getOrientation());
    
    // set first and last to different colors
    rgb theColor;
    if (i == 0) theColor = rgb(255,0,0);
    else if (i == path.size()-1) theColor = rgb(0,255,0);
    else theColor = color;
        
    KinematicJoint * j = joints[0];

    // testing method: boundingbox using the width, height, length values of the BoxObstacle object of each joint and their components
        
    while (j->nextJoint() != NULL)
      {
  BoxObstacle bo;
  float w, h, l;
  fmat::Quaternion q;
  fmat::Column<3> centroid;
  j->getOwnBB3D(worldT * j->getFullT(), bo);
  if (bo.getExtents() != fmat::Column<3>()) {
    // set points
    centroid = bo.getCenter();
    w = bo.getWidth();
    h = bo.getHeight();
    l = bo.getLength();
    q = fmat::Quaternion::fromMatrix(bo.getOrientation());
    graphics->add(new GraphicsData::BoundingBoxElement(j->outputOffset.get(), q, centroid, l, w, h));
                
    BoxObstacle boc;
    float cw, ch, cl;
    fmat::Quaternion cq;
    fmat::Column<3> ccentroid;
    for (KinematicJoint::component_iterator it = j->components.begin(); it != j->components.end(); ++it) {
      (*it)->getOwnBB3D(worldT * j->getFullT(), boc);
      ccentroid = boc.getCenter();
      cw = boc.getWidth();
      ch = boc.getHeight();
      cl = boc.getLength()/2;
      cq = fmat::Quaternion::fromMatrix(boc.getOrientation());
      graphics->add(new GraphicsData::BoundingBoxElement("component", cq, ccentroid, cl, cw, ch));
    }
  }
  j = j->nextJoint();
      }
    // Previous method: for now, just plot the location of the Gripper
    /* KinematicJoint* j = joints[N-1]; */
    
    /* while (j->nextJoint() != NULL) { */
    /*     j = j->nextJoint(); */
    /* } */
    
    /* fmat::Column<3> pos = j->getWorldPosition(); */
    
    /* graphics->add(new GraphicsData::CircleElement(DualCoding::GraphicsData::xyPair(pos[0],pos[1]), 5, true, theColor)); */
    
    /*// get bounding box of every link
      std::vector<std::vector<std::pair<float,float> > > boxes;
      getBoxes(boxes, *joints[0]);*/
    
    /*// add to element
      for (size_t b = 0; b < boxes.size(); b++) {
      graphics->add(new GraphicsData::PolygonElement(boxes[b], true, theColor));
      }*/
  }
}
template<size_t N>
void ShapeSpacePlanner3DR<N>::getBoxes(std::vector<std::vector<std::pair<float,float> > >& boxes, const KinematicJoint& joint) {
  // we don't have to go through all branches, because we already cloned the branch
  if (joint.nextJoint() != NULL)
    getBoxes(boxes, *joint.nextJoint());
  
  // get the joint's BB (in obstacle format)
  std::vector<std::pair<float,float> > points(BoxObstacle::NUM_CORNERS);
  BoxObstacle bo;
  joint.getOwnBB3D(worldT * joint.getFullT(), bo);
  
  if (bo.getExtents() != fmat::Column<3>()) {
    // set points
    for (int p = 0; p < BoxObstacle::NUM_CORNERS; p++) {
      fmat::Column<3> corner = bo.getCorner(static_cast<BoxObstacle::CornerOrder>(p));
      points[p] = std::pair<float,float>(corner[0], corner[1]);
    }
    boxes.push_back(points);
  }
  
  // do the same for each link
  for (KinematicJoint::component_iterator it = joint.components.begin(); it != joint.components.end(); ++it) {
    // set points
    (*it)->getOwnBB3D(worldT * joint.getFullT(), bo);
    for (int p = 0; p < BoxObstacle::NUM_CORNERS; p++) {
      fmat::Column<3> corner = bo.getCorner(static_cast<BoxObstacle::CornerOrder>(p));
      points[p] = std::pair<float,float>(corner[0], corner[1]);
    }
    boxes.push_back(points);
  }
}

#endif

#endif // TGT_HAS_ARMS