00001 /*************************************************************************** 00002 * Copyright (C) 2008 by Neuronics AG * 00003 * support@neuronics.ch * 00004 ***************************************************************************/ 00005 00010 #ifndef _KINEMATICS6M180_H_ 00011 #define _KINEMATICS6M180_H_ 00012 00013 //std: 00014 #include <string> 00015 #include <queue> 00016 #include <vector> 00017 #include <map> 00018 //kinematics: 00019 #include "kinematics.h" 00020 #include "KatanaKinematicsDecisionAlgorithms.h" 00021 #include "MathHelperFunctions.h" 00022 00023 namespace AnaGuess { 00024 00026 00030 class Kinematics6M180 : 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_b1b2costh3_6M180(angles_calc &a, const position &p) const; 00076 void thetacomp(angles_calc &a, const position &p_m) const; 00077 bool angledef(angles_calc &a) const; 00078 bool AnglePositionTest(const angles_calc &a) const; 00079 bool PositionTest6M180(const angles_calc &a, const position &p) const; 00080 00081 00082 protected: 00083 00084 00085 public: 00087 Kinematics6M180(); 00088 00090 ~Kinematics6M180(); 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 //_KINEMATICS6M180_H_