kni: kinematics6M90G.h Source File

Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2008 by Neuronics AG                                    *
00003  *   support@neuronics.ch                                                  *
00004  ***************************************************************************/
00005 
00010 #ifndef _KINEMATICS6M90G_H_
00011 #define _KINEMATICS6M90G_H_
00012 //std:
00013 #include <string>
00014 #include <queue>
00015 #include <vector>
00016 #include <map>
00017 //kinematics:
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 } // namespace
00146 
00147 #endif //_KINEMATICS6M90G_H_