Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

CBracketGrasperPredicate.h

Go to the documentation of this file.
00001 #ifndef _CBRACKET_GRASPER_PREDICATE_
00002 #define _CBRACKET_GRASPER_PREDICATE_
00003 
00004 #include "Planners/Manipulation/ShapeSpacePlanner2DR.h"
00005 #include "Motion/Kinematics.h"
00006 #include "Motion/IKSolver.h"
00007 #include "Shared/Measures.h"
00008 
00009 template <size_t N>
00010 class CBracketGrasperPredicate : public AdmissibilityPredicate<RRTNode2DR<N> > {
00011 public:
00012   typedef RRTNode2DR<N> NodeType_t;
00013   typedef typename NodeType_t::NodeValue_t NodeValue_t;
00014   
00015   KinematicJoint *gripperFrameKJ, *tmpKJ, *joints[N];
00016   typename NodeType_t::CollisionChecker *cc;
00017   fmat::Column<3> fromPT, toPT, goalPT;
00018   float fromOri, toOri, dir;
00019   
00020 public:
00021   //! constructor
00022   CBracketGrasperPredicate(): gripperFrameKJ(), tmpKJ(), joints(), cc(), fromPT(), toPT(), goalPT(), fromOri(), toOri(), dir() {
00023     gripperFrameKJ = kine->getKinematicJoint(GripperFrameOffset)->cloneBranch();
00024     if(gripperFrameKJ != NULL) {
00025       tmpKJ = gripperFrameKJ->getRoot();
00026       tmpKJ->buildMobileChildMap(joints, ArmOffset, N);
00027     }
00028     GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS);
00029     cc = new typename NodeType_t::CollisionChecker(VRmixin::worldShS, worldBounds, 10, GripperFrameOffset);
00030   }
00031   
00032   //! destructor
00033   ~CBracketGrasperPredicate() { delete gripperFrameKJ->getRoot(); delete cc; }
00034   
00035   //! admissibility check
00036   virtual bool admissible (NodeValue_t q, std::vector<NodeType_t>& tree, unsigned int parent) {
00037     const float range = 40 * float(M_PI/180);
00038     NodeType_t qFrom = tree[parent];
00039   
00040     fromPT = gripperPosition(qFrom.q);
00041     toPT = gripperPosition(q);
00042     NodeValue_t qOld = q;
00043     goalPT = toPT;
00044     
00045     fromOri = stateOrien(qFrom.q);
00046     toOri = stateOrien(q);
00047     
00048     float aDist, nOri;
00049     //dir = (forward) ? atan2(toPT[1]-fromPT[1], toPT[0]-fromPT[0]) : atan2(fromPT[1]-toPT[1], fromPT[0]-toPT[0]);
00050     //aDist = (forward) ? angdist(AngTwoPi(fromOri), AngTwoPi(dir)) : angdist(AngTwoPi(toOri), AngTwoPi(dir));
00051     dir = atan2(toPT[1]-fromPT[1], toPT[0]-fromPT[0]);
00052     aDist = angdist(AngTwoPi(fromOri), AngTwoPi(dir));
00053     
00054     //return aDist < range;
00055     // THE FOLLOWING CODE ADDS A NEW NODE WITH GOOD ORIENTATION. MAYBE USE IT.
00056     if( aDist > range ) {
00057       nOri = newOri(fromPT, goalPT, fromOri);
00058       bool success = gripperFrameKJ->getIK().solve(fmat::Column<3>(), IKSolver::Rotation(fmat::Quaternion()),
00059                                                    *gripperFrameKJ,
00060                                                    IKSolver::Point(fmat::pack(fromPT[0], fromPT[1], 0)), 0,
00061                                                    IKSolver::Rotation(fmat::Quaternion::aboutZ(nOri)), 1);
00062       if (!success) return false;
00063       for(unsigned j = 0; j < N; j++)
00064         q[j] = joints[j]->getQ();
00065       if (cc->collides(q)) return false;
00066     }
00067     NodeType_t qNode(q, parent);
00068     if (qNode.distance(qOld) < qFrom.distance(qOld))
00069       tree.push_back(qNode);
00070     return false;
00071   }
00072   
00073   //! generate new orientation
00074   float newOri(fmat::Column<3>& from, fmat::Column<3>& to, float fOri) {
00075     const float change = 5 * float(M_PI/180);
00076     float diff = fOri - atan2(to[1]-from[1], to[0]-from[0]);
00077     return AngSignPi( fOri + (fabsf(diff) < (float)M_PI ? 1 : -1) * ((diff < 0) ? 1 : -1) * change);
00078   }
00079   
00080   fmat::Column<3> gripperPosition(const NodeValue_t& q) {
00081     for(unsigned int j = 0; j < N; j++) { joints[j]->setQ(q[j]); }
00082     return gripperFrameKJ->getWorldPosition();
00083   }
00084   
00085   float stateOrien(const NodeValue_t& s) {
00086     float ori = 0;
00087     for(unsigned int j = 0; j < N; j++) { ori += s[j]; }
00088     return AngSignPi(ori);
00089   }
00090   
00091 private:
00092   CBracketGrasperPredicate& operator=(const CBracketGrasperPredicate &mp);
00093   CBracketGrasperPredicate(const CBracketGrasperPredicate& mp);
00094 };
00095 
00096 #endif

DualCoding 5.1CVS
Generated Mon May 9 04:56:25 2016 by Doxygen 1.6.3