Go to the documentation of this file.00001
00002
00003
00004
00005
00010 #ifndef _KINEMATICS6M90T_H_
00011 #define _KINEMATICS6M90T_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 Kinematics6M90T : 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 theta6;
00064 double theta234;
00065 double b1;
00066 double b2;
00067 double costh3;
00068 };
00069 typedef std::vector<angles_calc> angles_container;
00070 static const int cNrOfPossibleSolutions = 8;
00071
00073 bool initialize();
00074
00076 void IK_theta234theta5(angles_calc& angle, const position &p_gr) const;
00077 bool GripperTest(const position &p_gr, const angles_calc &angle) const;
00078 void IK_b1b2costh3_6MS(angles_calc &angle, const position &p) const;
00079 double findFirstEqualAngle(const std::vector<double>& v1, const std::vector<double>& v2) const;
00080 void thetacomp(angles_calc &angle, const position &p_m, const std::vector<double>& pose) const;
00081 bool PositionTest6MS(const double& theta1, const double& theta2, const double& theta3,
00082 const double& theta234, const position &p) const;
00083 bool angledef(angles_calc &a) const;
00084 bool AnglePositionTest(const angles_calc &a) const;
00085
00086 protected:
00087
00088 public:
00090 Kinematics6M90T();
00091
00093 ~Kinematics6M90T();
00094
00096 std::vector<double> getLinkLength();
00098 std::vector<int> getEpc();
00100 std::vector<int> getEncOff();
00102 std::vector<int> getDir();
00104 std::vector<double> getAngOff();
00106 std::vector<double> getAngStop();
00108 std::vector<double> getAngRange();
00110 std::vector<double> getAngMin();
00112 std::vector<double> getAngMax();
00114 bool setLinkLength(const std::vector<double> aLengths);
00116 bool setAngOff(const std::vector<double> aAngOff);
00118 bool setAngStop(const std::vector<double> aAngStop);
00119
00124 bool enc2rad(std::vector<double>& aAngles, const std::vector<int> aEncoders);
00125
00130 bool rad2enc(std::vector<int>& aEncoders, const std::vector<double> aAngles);
00131
00136 bool directKinematics(std::vector<double>& aPosition, const std::vector<double> aAngles);
00137
00144 bool inverseKinematics(std::vector<double>& aAngles, const std::vector<double> aPosition, const std::vector<double> aStartingAngles);
00145 };
00147
00148 }
00149
00150 #endif //_KINEMATICS6M90T_H_