kni: kni: kinematics6M90T.h Source File

00001 /***************************************************************************
00002  *   Copyright (C) 2008 by Neuronics AG                                    *
00003  *   support@neuronics.ch                                                  *
00004  ***************************************************************************/
00005 
00010 #ifndef _KINEMATICS6M90T_H_
00011 #define _KINEMATICS6M90T_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 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 } // namespace
00149 
00150 #endif //_KINEMATICS6M90T_H_