Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #ifndef P2OS_DRIVER__KINECALC_HPP_
00025 #define P2OS_DRIVER__KINECALC_HPP_
00026 #include <cstdio>
00027
00028
00029 typedef struct
00030 {
00031 double x, y, z;
00032 } KineVector;
00033
00034
00035
00036 typedef struct
00037 {
00038 KineVector p;
00039 KineVector n;
00040 KineVector o;
00041 KineVector a;
00042 } EndEffector;
00043
00044 class KineCalc
00045 {
00046 public:
00047 KineCalc(void);
00048
00049
00050 void CalculateFK(const double fromJoints[]);
00051 bool CalculateIK(const EndEffector & fromPosition);
00052
00053
00054 const KineVector & GetP(void) const {return endEffector.p;}
00055 const KineVector & GetN(void) const {return endEffector.n;}
00056 const KineVector & GetO(void) const {return endEffector.o;}
00057 const KineVector & GetA(void) const {return endEffector.a;}
00058 void SetP(const KineVector & newP)
00059 {
00060 endEffector.p.x = newP.x; endEffector.p.y = newP.y; endEffector.p.z = newP.z;
00061 }
00062 void SetN(const KineVector & newN)
00063 {
00064 endEffector.n.x = newN.x; endEffector.n.y = newN.y; endEffector.n.z = newN.z;
00065 }
00066 void SetO(const KineVector & newO)
00067 {
00068 endEffector.o.x = newO.x; endEffector.o.y = newO.y; endEffector.o.z = newO.z;
00069 }
00070 void SetA(const KineVector & newA)
00071 {
00072 endEffector.a.x = newA.x; endEffector.a.y = newA.y; endEffector.a.z = newA.z;
00073 }
00074 void SetP(double newPX, double newPY, double newPZ);
00075 void SetN(double newNX, double newNY, double newNZ);
00076 void SetO(double newOX, double newOY, double newOZ);
00077 void SetA(double newAX, double newAY, double newAZ);
00078
00079 double GetTheta(unsigned int index);
00080 const double * GetThetas(void) const {return joints;}
00081 void SetTheta(unsigned int index, double newVal);
00082 void SetLinkLengths(
00083 double newLink1, double newLink2, double newLink3, double newLink4,
00084 double newLink5);
00085 void SetOffset(unsigned int joint, double newOffset);
00086 void SetJointRange(unsigned int joint, double min, double max);
00087
00088
00089 KineVector CalculateN(const EndEffector & pose);
00090 KineVector Normalise(const KineVector & vector);
00091
00092 protected:
00093 void CalcTheta4and5(double angles[], const EndEffector & fromPosition);
00094 int ChooseSolution(const EndEffector & fromPosition, const double solutions[][5]);
00095 double CalcSolutionError(const double solution[], const EndEffector & fromPosition);
00096 EndEffector CalcFKForJoints(const double angles[]);
00097 bool SolutionInRange(const double angles[]);
00098
00099 void PrintEndEffector(const EndEffector & endEffector);
00100
00101
00102
00103
00104
00105 EndEffector endEffector;
00106
00107
00108
00109
00110 double joints[5];
00111
00112 double jointOffsets[5];
00113
00114 double jointMin[5];
00115 double jointMax[5];
00116
00117
00118
00119
00120 double link1, link2, link3, link4, link5;
00121 };
00122 #endif // P2OS_DRIVER__KINECALC_HPP_