joshua Elsdon / Mbed OS DeltaRobotFirmware

Delta Robot example

Dependencies:   BufferedSerial Eigen

Fork of TCPSocket_Example by mbed_example

DeltaKinematics.h@4:778bc352c47f, 2018-10-07 (annotated)

Committer:
je310
Date:
Sun Oct 07 19:40:12 2018 +0000
Revision:
4:778bc352c47f
can work smoothly, though voltage reading not working;

Who changed what in which revision?

UserRevisionLine numberNew 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__ */