Go to the documentation of this file.00001
00002
00003
00004
00005
00010 #ifndef _KINEMATICS6M90G_H_
00011 #define _KINEMATICS6M90G_H_
00012
00013 #include <string>
00014 #include <queue>
00015 #include <vector>
00016 #include <map>
00017
00018 #include "kinematics.h"
00019 #include "KatanaKinematicsDecisionAlgorithms.h"
00020 #include "MathHelperFunctions.h"
00021
00022
00023 namespace AnaGuess {
00024
00026
00030 class Kinematics6M90G : public Kinematics {
00031 private:
00033 int mNumberOfMotors;
00035 int mNumberOfSegments;
00037 std::vector<double> mSegmentLength;
00039 std::vector<double> mAngleOffset;
00041 std::vector<double> mAngleStop;
00043 std::vector<int> mEncodersPerCycle;
00045 std::vector<int> mEncoderOffset;
00047 std::vector<int> mRotationDirection;
00049 bool mIsInitialized;
00050
00052 struct position {
00053 double x;
00054 double y;
00055 double z;
00056 };
00057 struct angles_calc {
00058 double theta1;
00059 double theta2;
00060 double theta3;
00061 double theta4;
00062 double theta5;
00063 double theta234;
00064 double b1;
00065 double b2;
00066 double costh3;
00067 };
00068 typedef std::vector<angles_calc> angles_container;
00069 static const int cNrOfPossibleSolutions = 8;
00070
00072 bool initialize();
00073
00075 void IK_theta234theta5(angles_calc& angle, const position &p_gr) const;
00076 bool GripperTest(const position &p_gr, const angles_calc &angle) const;
00077 void IK_b1b2costh3_6MS(angles_calc &angle, const position &p) const;
00078 void thetacomp(angles_calc &angle, const position &p_m) const;
00079 bool PositionTest6MS(const angles_calc &a, const position &p) const;
00080 bool angledef(angles_calc &a) const;
00081 bool AnglePositionTest(const angles_calc &a) const;
00082
00083 protected:
00084
00085 public:
00087 Kinematics6M90G();
00088
00090 ~Kinematics6M90G();
00091
00093 std::vector<double> getLinkLength();
00095 std::vector<int> getEpc();
00097 std::vector<int> getEncOff();
00099 std::vector<int> getDir();
00101 std::vector<double> getAngOff();
00103 std::vector<double> getAngStop();
00105 std::vector<double> getAngRange();
00107 std::vector<double> getAngMin();
00109 std::vector<double> getAngMax();
00111 bool setLinkLength(const std::vector<double> aLengths);
00113 bool setAngOff(const std::vector<double> aAngOff);
00115 bool setAngStop(const std::vector<double> aAngStop);
00116
00121 bool enc2rad(std::vector<double>& aAngles, const std::vector<int> aEncoders);
00122
00127 bool rad2enc(std::vector<int>& aEncoders, const std::vector<double> aAngles);
00128
00133 bool directKinematics(std::vector<double>& aPosition, const std::vector<double> aAngles);
00134
00141 bool inverseKinematics(std::vector<double>& aAngles, const std::vector<double> aPosition, const std::vector<double> aStartingAngles);
00142 };
00144
00145 }
00146
00147 #endif //_KINEMATICS6M90G_H_