00001
00008 #ifndef WRIST_MECHANISM_H
00009 #define WRIST_MECHANISM_H
00010
00011 #include <eigen3/Eigen/Core>
00012 #include <eigen3/Eigen/LU>
00013 #include <eigen3/Eigen/Dense>
00014 #include <eigen3/Eigen/Eigenvalues>
00015 #include <vector>
00016
00017
00023 class WristMechanism{
00024 public:
00025
00026 WristMechanism();
00027 virtual ~WristMechanism();
00028
00029 void setSliderOffsetGain(Eigen::Vector2d sliderOffset, Eigen::Vector2d sliderGain);
00030 void setAngleOffsetGain(Eigen::Vector2d angleOffset, Eigen::Vector2d angleGain);
00031 void loadDesignParams(Eigen::Vector3d A, Eigen::Vector3d A0, Eigen::Vector3d B, Eigen::Vector3d C, Eigen::Vector3d D, Eigen::Vector3d D0, Eigen::Vector3d Pitch, Eigen::Vector3d Yaw, Eigen::Vector3d M, Eigen::Vector3d N, double linkLength);
00032 void applyLimits(double &pitch, double &yaw);
00033 void getLimits(double pitch, double yaw, double &upperPitch, double &lowerPitch, double &upperYaw, double &lowerYaw);
00034 void setLimits(const double &upperPitchMin,
00035 const double &upperPitchMax,
00036 const double &lowerPitchMin,
00037 const double &lowerPitchMax,
00038 const double &upperYawMin,
00039 const double &upperYawMax,
00040 const double &lowerYawMin,
00041 const double &lowerYawMax);
00042
00043
00044
00045 Eigen::Vector2d getAngleFromSlider(Eigen::Vector2d slider);
00046 Eigen::Vector2d getSliderFromAngle(Eigen::Vector2d ang);
00047
00048 Eigen::Vector2d getWristEncoderFromSlider(Eigen::Vector2d slider);
00049 Eigen::Vector2d getSliderFromWristEncoder(Eigen::Vector2d encoder);
00050
00051 void tareWristEncoders(Eigen::Vector2d encoder);
00052 void calWrist(Eigen::Vector2d encoder, Eigen::Vector2d calSliderPos, Eigen::Vector2d angle);
00053
00055 bool isCalibrated;
00056
00057 Eigen::Vector2d NewtonsMethod(Eigen::Vector2d ang, Eigen::Vector2d pos);
00059 int maxIt;
00061 double eps;
00062
00063
00064 protected:
00065
00066 double upperPitchMin;
00067 double upperPitchMax;
00068 double lowerPitchMin;
00069 double lowerPitchMax;
00070 double upperYawMin;
00071 double upperYawMax;
00072 double lowerYawMin;
00073 double lowerYawMax;
00074
00075 double mUpperPitchUpperYaw;
00076 double bUpperPitchUpperYaw;
00077 double mUpperPitchLowerYaw;
00078 double bUpperPitchLowerYaw;
00079 double mLowerPitchUpperYaw;
00080 double bLowerPitchUpperYaw;
00081 double mLowerPitchLowerYaw;
00082 double bLowerPitchLowerYaw;
00083
00084 Eigen::Vector2d sliderOffset;
00085 Eigen::Vector2d sliderGain;
00086 Eigen::Vector2d angleOffset;
00087 Eigen::Vector2d angleGain;
00088
00089 double linkLength;
00090 Eigen::Vector3d alpha,beta,gamma,delta,m,n,u,v,p;
00091
00092 double lengthSq;
00093
00094 Eigen::Matrix3d R1m, R1n, Rsm, Rsn, Rcm, Rcn;
00095
00096 Eigen::Vector2d encoderTarePos;
00097 Eigen::Vector2d calSliderPos;
00098 Eigen::Vector2d lastAng;
00099
00100 enum RotationType { ONE, SIN, COS };
00101
00102 Eigen::Matrix3d AA2Rot(Eigen::Vector3d axis, RotationType index);
00103 Eigen::Matrix3d AA2Rot(Eigen::Vector3d axis, double theta);
00104
00105 int doFwdKin(Eigen::Vector2d &ang, Eigen::Vector2d pos, Eigen::Vector2d lastang);
00106
00107 int doInvKin(Eigen::Vector2d &pos, Eigen::Vector2d ang);
00108
00109 struct Partials
00110 {
00111 double f0;
00112 double f;
00113 double fq;
00114 double ftheta;
00115 double fphi;
00116 };
00117
00118 Partials findPartialDerivatives(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d u, double q, Eigen::Vector2d ang);
00119
00120 Eigen::Matrix3d findFwdKinMatrix(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d u, double q);
00121
00122 void getNearest(double m, double b, double x_in, double y_in, double &x_out, double &y_out);
00123 void getLineFromPoints(double x1, double x2, double y1, double y2, double& m, double&b);
00124 std::string logCategory;
00125
00126 };
00127
00128 #endif // WRIST_MECHANISM_H