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
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
00033 ~CBracketGrasperPredicate() { delete gripperFrameKJ->getRoot(); delete cc; }
00034
00035
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
00050
00051 dir = atan2(toPT[1]-fromPT[1], toPT[0]-fromPT[0]);
00052 aDist = angdist(AngTwoPi(fromOri), AngTwoPi(dir));
00053
00054
00055
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
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