Delta Robot example
Dependencies: BufferedSerial Eigen
Fork of TCPSocket_Example by
DeltaKinematics.h@5:01e1e68309ae, 2018-10-15 (annotated)
- Committer:
- je310
- Date:
- Mon Oct 15 18:30:20 2018 +0000
- Revision:
- 5:01e1e68309ae
- Parent:
- 4:778bc352c47f
testing eigen;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
je310 | 4:778bc352c47f | 1 | // DeltaKinematics - kinematics computation for delta parallel robot arm |
je310 | 4:778bc352c47f | 2 | // Copyright(C) 2018 Szymon Szantula |
je310 | 4:778bc352c47f | 3 | // |
je310 | 4:778bc352c47f | 4 | // This program is free software : you can redistribute it and / or modify |
je310 | 4:778bc352c47f | 5 | // it under the terms of the GNU General Public License as published by |
je310 | 4:778bc352c47f | 6 | // the Free Software Foundation, either version 3 of the License, or |
je310 | 4:778bc352c47f | 7 | // (at your option) any later version. |
je310 | 4:778bc352c47f | 8 | // |
je310 | 4:778bc352c47f | 9 | // This program is distributed in the hope that it will be useful, |
je310 | 4:778bc352c47f | 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of |
je310 | 4:778bc352c47f | 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the |
je310 | 4:778bc352c47f | 12 | // GNU General Public License for more details. |
je310 | 4:778bc352c47f | 13 | // |
je310 | 4:778bc352c47f | 14 | // You should have received a copy of the GNU General Public License |
je310 | 4:778bc352c47f | 15 | // along with this program.If not, see <http://www.gnu.org/licenses/.> |
je310 | 4:778bc352c47f | 16 | |
je310 | 4:778bc352c47f | 17 | #ifndef __DELTAKINEMATICS_H__ |
je310 | 4:778bc352c47f | 18 | #define __DELTAKINEMATICS_H__ |
je310 | 4:778bc352c47f | 19 | |
je310 | 4:778bc352c47f | 20 | /** |
je310 | 4:778bc352c47f | 21 | * @brief For Visual Sudio only |
je310 | 4:778bc352c47f | 22 | */ |
je310 | 4:778bc352c47f | 23 | #define _USE_MATH_DEFINES |
je310 | 4:778bc352c47f | 24 | |
je310 | 4:778bc352c47f | 25 | #include <cmath> |
je310 | 4:778bc352c47f | 26 | |
je310 | 4:778bc352c47f | 27 | #ifdef LIB_TEST_MODE |
je310 | 4:778bc352c47f | 28 | #include <iostream> |
je310 | 4:778bc352c47f | 29 | #endif |
je310 | 4:778bc352c47f | 30 | |
je310 | 4:778bc352c47f | 31 | /** |
je310 | 4:778bc352c47f | 32 | * @brief PI value |
je310 | 4:778bc352c47f | 33 | */ |
je310 | 4:778bc352c47f | 34 | #define M_PI 3.14159265358979323846 |
je310 | 4:778bc352c47f | 35 | |
je310 | 4:778bc352c47f | 36 | /** |
je310 | 4:778bc352c47f | 37 | * @brief Delta Robot computation class. |
je310 | 4:778bc352c47f | 38 | * |
je310 | 4:778bc352c47f | 39 | * Performs kinematics computation for delta parallel robot with revolute inputs. |
je310 | 4:778bc352c47f | 40 | */ |
je310 | 4:778bc352c47f | 41 | |
je310 | 4:778bc352c47f | 42 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 43 | class DeltaKinematics |
je310 | 4:778bc352c47f | 44 | { |
je310 | 4:778bc352c47f | 45 | public: |
je310 | 4:778bc352c47f | 46 | /** |
je310 | 4:778bc352c47f | 47 | * @brief Structure that represents basic vector. |
je310 | 4:778bc352c47f | 48 | * |
je310 | 4:778bc352c47f | 49 | * This structure might be used to describe tcp position, velocity and acceleration. |
je310 | 4:778bc352c47f | 50 | * The below description of structure members refer to tcp position. |
je310 | 4:778bc352c47f | 51 | * @sa DeltaTrajectory |
je310 | 4:778bc352c47f | 52 | */ |
je310 | 4:778bc352c47f | 53 | struct DeltaVector |
je310 | 4:778bc352c47f | 54 | { |
je310 | 4:778bc352c47f | 55 | RealDataType x; ///< cartesian position in base reference frame |
je310 | 4:778bc352c47f | 56 | RealDataType y; ///< cartesian position in base reference frame |
je310 | 4:778bc352c47f | 57 | RealDataType z; ///< cartesian position in base reference frame |
je310 | 4:778bc352c47f | 58 | RealDataType phi1; ///< joint 1 angle [deg] ( negative above the base platform ! ) |
je310 | 4:778bc352c47f | 59 | RealDataType phi2; ///< joint 2 angle [deg] ( negative above the base platform ! ) |
je310 | 4:778bc352c47f | 60 | RealDataType phi3; ///< joint 3 angle [deg] ( negative above the base platform ! ) |
je310 | 4:778bc352c47f | 61 | |
je310 | 4:778bc352c47f | 62 | /** |
je310 | 4:778bc352c47f | 63 | * @brief Prints values of all the position variables. |
je310 | 4:778bc352c47f | 64 | */ |
je310 | 4:778bc352c47f | 65 | void Print(); |
je310 | 4:778bc352c47f | 66 | |
je310 | 4:778bc352c47f | 67 | /** |
je310 | 4:778bc352c47f | 68 | * @brief Sets all position parameters to zero; |
je310 | 4:778bc352c47f | 69 | */ |
je310 | 4:778bc352c47f | 70 | void Clear(); |
je310 | 4:778bc352c47f | 71 | }; |
je310 | 4:778bc352c47f | 72 | |
je310 | 4:778bc352c47f | 73 | /** |
je310 | 4:778bc352c47f | 74 | * @brief Struct that represents element of trajectory. |
je310 | 4:778bc352c47f | 75 | * |
je310 | 4:778bc352c47f | 76 | * Trajectory is a time history of position, velocity and acceleration for each DOF. |
je310 | 4:778bc352c47f | 77 | */ |
je310 | 4:778bc352c47f | 78 | struct DeltaTrajectory |
je310 | 4:778bc352c47f | 79 | { |
je310 | 4:778bc352c47f | 80 | DeltaVector pos; ///< tcp position |
je310 | 4:778bc352c47f | 81 | DeltaVector vel; ///< tcp velocity |
je310 | 4:778bc352c47f | 82 | DeltaVector accel; ///< tcp acceleration |
je310 | 4:778bc352c47f | 83 | }; |
je310 | 4:778bc352c47f | 84 | |
je310 | 4:778bc352c47f | 85 | /** |
je310 | 4:778bc352c47f | 86 | * @brief Delta Robot geometric dimmensions and constraints struct. |
je310 | 4:778bc352c47f | 87 | * |
je310 | 4:778bc352c47f | 88 | * The required input for the class. See the description of individual members. |
je310 | 4:778bc352c47f | 89 | */ |
je310 | 4:778bc352c47f | 90 | struct DeltaGeometricDim |
je310 | 4:778bc352c47f | 91 | { |
je310 | 4:778bc352c47f | 92 | RealDataType sb; ///< base equilateral triangle side [ mm ] |
je310 | 4:778bc352c47f | 93 | RealDataType sp; ///< platform equilateral triangle side [ mm ] |
je310 | 4:778bc352c47f | 94 | RealDataType L; ///< upper legs length [ mm ] |
je310 | 4:778bc352c47f | 95 | RealDataType l; ///< lower legs parallelogram length [ mm ] |
je310 | 4:778bc352c47f | 96 | RealDataType h; ///< lower legs prallelogram width [ mm ] |
je310 | 4:778bc352c47f | 97 | RealDataType max_neg_angle; ///< max negative angle that each arm can achive ( knee above the fixed-base plane ) [ deg ] |
je310 | 4:778bc352c47f | 98 | RealDataType min_parallelogram_angle; ///< the limitation introduced by universal joints [ deg ] |
je310 | 4:778bc352c47f | 99 | }; |
je310 | 4:778bc352c47f | 100 | |
je310 | 4:778bc352c47f | 101 | /** |
je310 | 4:778bc352c47f | 102 | * @brief DeltaKinematics constructor. |
je310 | 4:778bc352c47f | 103 | * |
je310 | 4:778bc352c47f | 104 | * @param dim a 7 element struct of delta geometric features and constraints. |
je310 | 4:778bc352c47f | 105 | */ |
je310 | 4:778bc352c47f | 106 | DeltaKinematics(DeltaGeometricDim dim); |
je310 | 4:778bc352c47f | 107 | |
je310 | 4:778bc352c47f | 108 | /** |
je310 | 4:778bc352c47f | 109 | * @brief Calculates inverse position kinematics for delta parallel manipulator. |
je310 | 4:778bc352c47f | 110 | * |
je310 | 4:778bc352c47f | 111 | * @param v a pointer to the matrix of position vectors, only joint coordinates are changed |
je310 | 4:778bc352c47f | 112 | * @param num a number of vectors in the matrix |
je310 | 4:778bc352c47f | 113 | * @return 1 if 1 if there was unreachable position, 0 if success |
je310 | 4:778bc352c47f | 114 | * @sa CalculateFpk |
je310 | 4:778bc352c47f | 115 | */ |
je310 | 4:778bc352c47f | 116 | int CalculateIpk(DeltaVector *v, int num); |
je310 | 4:778bc352c47f | 117 | |
je310 | 4:778bc352c47f | 118 | /** |
je310 | 4:778bc352c47f | 119 | * @brief Calculates forward position kinematics for delta parallel manipulator. |
je310 | 4:778bc352c47f | 120 | * |
je310 | 4:778bc352c47f | 121 | * @param v a pointer to the matrix of position vectors, only cartesian coordinates are changed |
je310 | 4:778bc352c47f | 122 | * @param num a number of vectors in the matrix |
je310 | 4:778bc352c47f | 123 | * @return 1 if there was unreachable position or if singularity happend, 0 if success |
je310 | 4:778bc352c47f | 124 | * @sa CalculateIpk |
je310 | 4:778bc352c47f | 125 | */ |
je310 | 4:778bc352c47f | 126 | int CalculateFpk(DeltaVector *v, int num); |
je310 | 4:778bc352c47f | 127 | |
je310 | 4:778bc352c47f | 128 | // /** |
je310 | 4:778bc352c47f | 129 | // * @brief Calculates |
je310 | 4:778bc352c47f | 130 | // * |
je310 | 4:778bc352c47f | 131 | // * |
je310 | 4:778bc352c47f | 132 | // */ |
je310 | 4:778bc352c47f | 133 | // int CalculateCartesianVelocity( DeltaVector *v, int num); |
je310 | 4:778bc352c47f | 134 | // int CalculateJointVelocity( DeltaVector *v, int num); |
je310 | 4:778bc352c47f | 135 | |
je310 | 4:778bc352c47f | 136 | private: |
je310 | 4:778bc352c47f | 137 | RealDataType _sb; ///< base equilateral triangle side [ mm ] |
je310 | 4:778bc352c47f | 138 | RealDataType _sp; ///< platform equilateral triangle side [ mm ] |
je310 | 4:778bc352c47f | 139 | RealDataType _Ll; ///< upper legs length [ mm ] |
je310 | 4:778bc352c47f | 140 | RealDataType _l; ///< lower legs parallelogram length [ mm ] |
je310 | 4:778bc352c47f | 141 | RealDataType _h; ///< lower legs prallelogram width [ mm ] |
je310 | 4:778bc352c47f | 142 | RealDataType _wb; ///< planar distance from base reference frame to near base side [ mm ] |
je310 | 4:778bc352c47f | 143 | RealDataType _ub; ///< planar distance from base reference frame to a base vertex [ mm ] |
je310 | 4:778bc352c47f | 144 | RealDataType _wp; ///< planar distance from platform reference frame to near platform side [ mm ] |
je310 | 4:778bc352c47f | 145 | RealDataType _up; ///< planar distance from platform reference frame to a platform vertex [ mm ] |
je310 | 4:778bc352c47f | 146 | RealDataType _Pp1[3]; ///< platorm-fixed U-joint virtual connection in the local platform frame |
je310 | 4:778bc352c47f | 147 | RealDataType _Pp2[3]; ///< platorm-fixed U-joint virtual connection in the local platform frame |
je310 | 4:778bc352c47f | 148 | RealDataType _Pp3[3]; ///< platorm-fixed U-joint virtual connection in the local platform frame |
je310 | 4:778bc352c47f | 149 | RealDataType _B1[3]; ///< fixed-base revolute joint point |
je310 | 4:778bc352c47f | 150 | RealDataType _B2[3]; ///< fixed-base revolute joint point |
je310 | 4:778bc352c47f | 151 | RealDataType _B3[3]; ///< fixed-base revolute joint point |
je310 | 4:778bc352c47f | 152 | RealDataType _b1[3]; ///< fixed-base vertex |
je310 | 4:778bc352c47f | 153 | RealDataType _b2[3]; ///< fixed-base vertex |
je310 | 4:778bc352c47f | 154 | RealDataType _b3[3]; ///< fixed-base vertex |
je310 | 4:778bc352c47f | 155 | RealDataType _A1[3]; ///< first knee point |
je310 | 4:778bc352c47f | 156 | RealDataType _A2[3]; ///< second knee point |
je310 | 4:778bc352c47f | 157 | RealDataType _A3[3]; ///< third knee point |
je310 | 4:778bc352c47f | 158 | RealDataType _max_neg_angle; ///< max negative angle that each arm can achive ( knee above the fixed-base plane ) [ deg ] |
je310 | 4:778bc352c47f | 159 | RealDataType _min_parallelogram_angle; ///< the limitation introduced by universal joints [ deg ] |
je310 | 4:778bc352c47f | 160 | static const RealDataType _SQRT3; ///< sqrt(3) |
je310 | 4:778bc352c47f | 161 | static const RealDataType _HSQRT3; ///< sqrt(3)/2 |
je310 | 4:778bc352c47f | 162 | static const RealDataType _ROTZ120[3][3]; ///< basic rotation matrix for z axis and 120 degree |
je310 | 4:778bc352c47f | 163 | static const RealDataType _MROTZ120[3][3]; ///< basic roation matrix for z axis and -120 degree |
je310 | 4:778bc352c47f | 164 | static const RealDataType _DEG2RAD_FACTOR; ///< basicly equals M_PI/180 |
je310 | 4:778bc352c47f | 165 | static const RealDataType _RAD2DEG_FACTOR; ///< basilcy equals 180/M_PI |
je310 | 4:778bc352c47f | 166 | |
je310 | 4:778bc352c47f | 167 | /** |
je310 | 4:778bc352c47f | 168 | * @brief Initialise the class members e.g. _Pp1. |
je310 | 4:778bc352c47f | 169 | */ |
je310 | 4:778bc352c47f | 170 | void Initialise(); |
je310 | 4:778bc352c47f | 171 | |
je310 | 4:778bc352c47f | 172 | /** |
je310 | 4:778bc352c47f | 173 | * @brief Performs vector rotation by given rotation matrix. |
je310 | 4:778bc352c47f | 174 | * |
je310 | 4:778bc352c47f | 175 | * @param point a position vector |
je310 | 4:778bc352c47f | 176 | * @param matrix a rotation matrix |
je310 | 4:778bc352c47f | 177 | */ |
je310 | 4:778bc352c47f | 178 | void RotateByMatrix(RealDataType *point, const RealDataType (*matrix)[3]); |
je310 | 4:778bc352c47f | 179 | |
je310 | 4:778bc352c47f | 180 | /** |
je310 | 4:778bc352c47f | 181 | * @brief Calculates a joint angle . |
je310 | 4:778bc352c47f | 182 | * |
je310 | 4:778bc352c47f | 183 | * @param B a rotated base joint point |
je310 | 4:778bc352c47f | 184 | * @param P a rotated platform joint point |
je310 | 4:778bc352c47f | 185 | * @param phi an angle that will be calculated |
je310 | 4:778bc352c47f | 186 | * @return 0 if success 1 if error |
je310 | 4:778bc352c47f | 187 | */ |
je310 | 4:778bc352c47f | 188 | int CalculateAngle(const RealDataType *B, const RealDataType *P, RealDataType *phi); |
je310 | 4:778bc352c47f | 189 | |
je310 | 4:778bc352c47f | 190 | /** |
je310 | 4:778bc352c47f | 191 | * @brief Three spheres intersection algorithm ( different z hights ). |
je310 | 4:778bc352c47f | 192 | * |
je310 | 4:778bc352c47f | 193 | * @param v a position vector |
je310 | 4:778bc352c47f | 194 | * @return 0 if success 1 if error |
je310 | 4:778bc352c47f | 195 | */ |
je310 | 4:778bc352c47f | 196 | int ThreeSpheresIntersectionA(DeltaVector *v); |
je310 | 4:778bc352c47f | 197 | |
je310 | 4:778bc352c47f | 198 | /** |
je310 | 4:778bc352c47f | 199 | * @brief Three spheres intersection algorithm ( the same z hights ). |
je310 | 4:778bc352c47f | 200 | * |
je310 | 4:778bc352c47f | 201 | * @param v a position vector |
je310 | 4:778bc352c47f | 202 | * @return 0 if success 1 if error |
je310 | 4:778bc352c47f | 203 | */ |
je310 | 4:778bc352c47f | 204 | int ThreeSpheresIntersectionB(DeltaVector *v); |
je310 | 4:778bc352c47f | 205 | }; |
je310 | 4:778bc352c47f | 206 | #endif /* __DELTAKINEMATICS_H__ */ |