Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

ShapeSpacePlannerBase.cc

Go to the documentation of this file.
00001 #include "ShapeSpacePlannerBase.h"
00002 
00003 template <>
00004 void ShapeSpaceCollisionCheckerBase<2>::addRobotObstacles(const KinematicJoint& j) {
00005   for (KinematicJoint::branch_iterator it = j.getBranches().begin(); it != j.getBranches().end(); ++it)
00006     addRobotObstacles(**it);
00007 
00008   // for 2D obstacles, to avoid projection geometry, we'll try modeling all obstacles as rectangles.
00009   RectangularObstacle* rect = NULL;
00010   if (j.collisionModel == "Cube" || j.collisionModel == "Cylinder" || j.collisionModel == "Sphere") {
00011     rect = new RectangularObstacle;
00012     j.getOwnBB2D(j.getFullT(), *rect);
00013     rect->setBodyObstacle();
00014     rect->name = j.outputOffset.get();
00015     if ( rect->name.size() == 0 )
00016       rect->name = j.model.get();
00017     obstacles.push_back(rect);
00018   }
00019   for (KinematicJoint::component_iterator it = j.components.begin(); it != j.components.end(); ++it) {
00020     // for 2D obstacles, to avoid projection geometry, we'll try modeling all obstacles as rectangles.
00021     if ((*it)->collisionModel == "Cube" || (*it)->collisionModel == "Cylinder" || (*it)->collisionModel == "Sphere") {
00022       rect = new RectangularObstacle;
00023       (*it)->getOwnBB2D(j.getFullT(), *rect);
00024       rect->setBodyObstacle();
00025       rect->name = (*it)->model.get();
00026       if ( rect->name.size() == 0 )
00027         rect->name = j.outputOffset.get() + "-component";
00028       obstacles.push_back(rect);
00029     }
00030   }
00031 }
00032 
00033 template <>
00034 void ShapeSpaceCollisionCheckerBase<3>::createBodyObstacle(const LinkComponent& j, const fmat::Transform& robotT) {
00035   // std::cout << "ShapeSpacePlannerBase::createBodyObstacle '" << j.collisionModel << "' for '" << j.model << "'" << std::endl;
00036   fmat::Transform t;
00037   j.getCollisionModelTransform(t);
00038   t = robotT * t;
00039   fmat::Column<3> scale;
00040   j.collisionModelScale.exportTo(scale);
00041   
00042   // create respective obstacle
00043   if (j.collisionModel == "Cube") {
00044     BoxObstacle* box = new BoxObstacle;
00045     j.getOwnBB3D(robotT, *box);
00046     box->setBodyObstacle();
00047     box->name = j.model.get();
00048     obstacles.push_back(box);
00049   }
00050   else if (j.collisionModel == "Cylinder") {
00051     fmat::Matrix<3,3> rot = t.rotation();
00052     fmat::Column<3> pos = t.translation();
00053     CylindricalObstacle* c = new CylindricalObstacle(pos, rot, std::max(scale[0], scale[1])/2, scale[2]/2);
00054     c->setBodyObstacle();
00055     c->name = j.model.get();
00056     obstacles.push_back(c);
00057   }
00058   else if (j.collisionModel == "Sphere") {
00059     fmat::Matrix<3,3> rot = t.rotation();
00060     EllipsoidObstacle* e = new EllipsoidObstacle(t.translation(), rot, scale);
00061     e->setBodyObstacle();
00062     e->name = j.model.get();
00063     obstacles.push_back(e);
00064   }
00065   else if ( j.collisionModel.size() > 0 )
00066     std::cout << "ShapeSpacePlannerBase::createBodyObstacle can't handle collision model '" 
00067         << j.collisionModel << "' for model '" << j.model << "'" << std::endl;
00068 }
00069 
00070 template <>
00071 void ShapeSpaceCollisionCheckerBase<3>::addRobotObstacles(const KinematicJoint& j) {
00072   // **************** THIS IS A STUB FUNCTION : MOST OF THE CODE IS MISSING ****************
00073   for (KinematicJoint::branch_iterator it = j.getBranches().begin(); it != j.getBranches().end(); ++it) {
00074     addRobotObstacles(**it);
00075   }
00076   createBodyObstacle(j, j.getFullT());
00077   for (KinematicJoint::component_iterator it = j.components.begin(); it != j.components.end(); ++it)
00078     createBodyObstacle(**it, j.getFullT());
00079 }
00080 
00081 template <>
00082 void ShapeSpaceCollisionCheckerBase<2>::addDisplayRobotObstacles(const KinematicJoint& j) {
00083   for (KinematicJoint::branch_iterator it = j.getBranches().begin(); it != j.getBranches().end(); ++it) {
00084     addDisplayRobotObstacles(**it);
00085   }
00086   
00087   fmat::Transform worldT;
00088   DualCoding::Point p = DualCoding::VRmixin::robotObstaclesPt;
00089   worldT.translation() = fmat::pack(p.coordX(),p.coordY(),p.coordZ());
00090   worldT.rotation() = fmat::rotationZ(DualCoding::VRmixin::robotObstaclesOri);
00091   //std::cout << "Display robot obstacles, use pos: " << p << ", ori: " << DualCoding::VRmixin::robotObstaclesOri << std::endl;
00092 
00093   // for 2D obstacles, to avoid projection geometry, we'll try modeling all obstacles as rectangles.
00094   RectangularObstacle* rect = NULL;
00095   if (j.collisionModel == "Cube" || j.collisionModel == "Cylinder" || j.collisionModel == "Sphere") {
00096     rect = new RectangularObstacle;
00097     j.getOwnBB2D(worldT * j.getFullT(), *rect);
00098     rect->setBodyObstacle();
00099     rect->name = j.outputOffset.get();
00100     if ( rect->name.size() == 0 )
00101       rect->name = j.model.get();
00102     displayRobotObstacles.push_back(rect);
00103   }
00104   for (KinematicJoint::component_iterator it = j.components.begin(); it != j.components.end(); ++it) {
00105     // for 2D obstacles, to avoid projection geometry, we'll try modeling all obstacles as rectangles.
00106     if ((*it)->collisionModel == "Cube" || (*it)->collisionModel == "Cylinder" || (*it)->collisionModel == "Sphere") {
00107       rect = new RectangularObstacle;
00108       (*it)->getOwnBB2D(worldT * j.getFullT(), *rect);
00109       rect->setBodyObstacle();
00110       rect->name = (*it)->model.get();
00111       if ( rect->name.size() == 0 )
00112   rect->name = j.outputOffset.get() + "-component";
00113       displayRobotObstacles.push_back(rect);
00114     }
00115   }
00116 }
00117 
00118 template <>
00119 void ShapeSpaceCollisionCheckerBase<3>::addDisplayRobotObstacles(const KinematicJoint& j) {
00120   for (KinematicJoint::branch_iterator it = j.getBranches().begin(); it != j.getBranches().end(); ++it)
00121     addDisplayRobotObstacles(**it);
00122 }
00123 
00124 template <>
00125 void ShapeSpaceCollisionCheckerBase<2>::addObstaclesToShapeSpace(ShapeSpace &shs, const fmat::Transform &t) {
00126   {
00127     GET_SHAPE(plannerObstacles, GraphicsData, shs);
00128     plannerObstacles.deleteShape();
00129   }
00130   NEW_SHAPE(plannerObstacles, GraphicsData, new GraphicsData(shs));
00131   typedef std::pair<float,float> xyPair;
00132   rgb obstacleColor(200, 200, 0);
00133   for (unsigned int i = 0; i < displayWorldObstacles.size(); i++) {
00134     { RectangularObstacle *r = dynamic_cast<RectangularObstacle*>(displayWorldObstacles[i]);
00135       if (r) {
00136   std::vector<xyPair> pts;
00137   for (int j = 0; j < RectangularObstacle::NUM_CORNERS; j++) {
00138     fmat::Column<2> p = r->getCorner(static_cast<RectangularObstacle::CornerOrder>(j));
00139     pts.push_back(xyPair(p[0],p[1]));
00140   }
00141   plannerObstacles->add(new GraphicsData::PolygonElement("plannerObstacle-"+r->name, pts, true, obstacleColor));
00142   continue;
00143       }
00144     }
00145     { CircularObstacle *c = dynamic_cast<CircularObstacle*>(displayWorldObstacles[i]);
00146       if (c) {
00147   fmat::Column<2> center = c->getCenter();
00148   float radius = c->getRadius();
00149   plannerObstacles->add(new GraphicsData::EllipseElement("plannerObstacle-"+c->name, xyPair(center[0],center[1]), 
00150                      radius, radius, 0, false, 
00151                      obstacleColor));
00152   continue;
00153       }
00154     }
00155     { EllipticalObstacle *e = dynamic_cast<EllipticalObstacle*>(displayWorldObstacles[i]);
00156       if (e) {
00157   fmat::Column<2> c = e->getCenter();
00158   fmat::Column<2> vec = e->focus1 - c;
00159   AngTwoPi orientation = AngTwoPi(std::atan2(vec[1],vec[0]));
00160   plannerObstacles->add(new GraphicsData::EllipseElement("plannerObstacle-"+e->name, xyPair(c[0],c[1]), 
00161                      e->semimajor, e->semiminor, orientation, false, 
00162                      obstacleColor));
00163   continue;
00164       }
00165     }
00166     { ConvexPolyObstacle *cp = dynamic_cast<ConvexPolyObstacle*>(displayWorldObstacles[i]);
00167       if ( cp ) {
00168   std::vector<xyPair> pts;
00169   pts.reserve(cp->getPoints().size());
00170   for (size_t p = 0; p < cp->getPoints().size(); p++)
00171     pts[p] = xyPair(cp->getPoints()[p][0], cp->getPoints()[p][1]);
00172   plannerObstacles->add(new GraphicsData::PolygonElement("plannerObstacle-"+cp->name, pts, true, obstacleColor));
00173   continue;
00174       }
00175     }
00176     std::cout << "Unhandled obstacle type " << *displayWorldObstacles[i] << std::endl;
00177   }
00178   
00179   // now add the robot's components
00180   addDisplayRobotObstacles(*(kine->getKinematicJoint(BaseFrameOffset)));
00181   for (unsigned int i = 0; i < displayRobotObstacles.size(); i++) {
00182     { EllipticalObstacle *e = dynamic_cast<EllipticalObstacle*>(displayRobotObstacles[i]);
00183       if (e) {
00184   fmat::Column<2> c = e->getCenter();
00185   fmat::Column<2> vec = e->focus1 - c;
00186   AngTwoPi orientation = AngTwoPi(std::atan2(vec[1],vec[0]));
00187   plannerObstacles->
00188     add(new GraphicsData::EllipseElement("plannerObstacle-"+e->name, 
00189                  xyPair(c[0],c[1]), e->semimajor, e->semiminor, 
00190                  orientation, false, rgb(150, 0, 255)));
00191   continue;
00192       }
00193     }
00194     
00195     { RectangularObstacle *r = dynamic_cast<RectangularObstacle*>(displayRobotObstacles[i]);
00196       if (r) {
00197   std::vector<xyPair> pts;
00198   for (int j = 0; j < RectangularObstacle::NUM_CORNERS; j++) {
00199     fmat::Column<2> p = r->getCorner(static_cast<RectangularObstacle::CornerOrder>(j));
00200     pts.push_back(xyPair(p[0],p[1]));
00201   }
00202   plannerObstacles->add(new GraphicsData::PolygonElement("plannerObstacle-"+r->name, pts, true, rgb(150, 0, 255)));
00203   continue;
00204       }
00205     }
00206   }
00207   plannerObstacles->setObstacle(false);
00208 }
00209 
00210 template <>
00211 void ShapeSpaceCollisionCheckerBase<3>::addObstaclesToShapeSpace(ShapeSpace &shs, const fmat::Transform &t) {
00212   {
00213     GET_SHAPE(plannerObstacles, GraphicsData, shs);
00214     plannerObstacles.deleteShape();
00215   }
00216   NEW_SHAPE(plannerObstacles, GraphicsData, new GraphicsData(shs));
00217   rgb obstacleColor(200, 200, 0);
00218   typedef std::pair<float,float> xyPair;
00219   for (unsigned int i = 0; i < displayWorldObstacles.size(); i++) {
00220     { CylindricalObstacle *c = dynamic_cast<CylindricalObstacle*>(displayWorldObstacles[i]);
00221       if (c) {
00222   fmat::Column<3> center = t * c->getCenter();
00223   float radius = c->getRadius();
00224   plannerObstacles->add(new GraphicsData::EllipseElement("plannerObstacle-"+c->name, xyPair(center[0],center[1]), 
00225                      radius, radius, 0, false, 
00226                      obstacleColor));
00227   continue;
00228       }
00229     }
00230     std::cout << "Unhandled obstacle type " << *displayWorldObstacles[i] << std::endl;
00231   }
00232 }

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:51 2016 by Doxygen 1.6.3