00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef KNIKATANAKINEMATICS5M180_H
00021 #define KNIKATANAKINEMATICS5M180_H
00022
00023 #include "common/dllexport.h"
00024
00025 #include "KNI_InvKin/KatanaKinematics.h"
00026 #include "KNI_InvKin/KatanaKinematicsDecisionAlgorithms.h"
00027
00028 #include <vector>
00029
00030
00031 namespace KNI {
00032
00039 class DLLDIR_IK KatanaKinematics5M180 : public KatanaKinematics {
00040
00041 public:
00042
00043 void init(metrics const& length, parameter_container const& parameters);
00044
00045
00046 void DK(coordinates& solution, encoders const& current_encoders) const;
00047 void IK(encoders::iterator solution, coordinates const& pose, encoders const& cur_angles) const;
00048
00049
00050 private:
00051
00052 struct position {
00053 double x;
00054 double y;
00055 double z;
00056 };
00057
00058 struct angles_calc {
00059 double theta1;
00060 double theta2;
00061 double theta3;
00062 double theta4;
00063 double theta5;
00064 double theta234;
00065 double b1;
00066 double b2;
00067 double costh3;
00068 };
00069
00070 typedef std::vector<angles_calc> angles_container;
00071
00072 metrics _length;
00073 parameter_container _parameters;
00074
00075 static const double _tolerance;
00076 static const int _nrOfPossibleSolutions;
00077
00078 void _setLength(metrics const& length) { _length = length; }
00079 void _setParameters(parameter_container const& parameters) { _parameters = parameters; }
00080
00081
00082 };
00083
00084
00085
00086
00087
00088 }
00089
00090 #endif