use from Ernesto Palacios
Dependents: RoboticArm DXL_SDK_Porting_Test
Fork of Matrix by
MatrixMath_Kinematics.cpp@8:1774aa06ab99, 2017-02-11 (annotated)
- Committer:
- stanley1228
- Date:
- Sat Feb 11 13:07:13 2017 +0000
- Revision:
- 8:1774aa06ab99
- Child:
- 9:f19d5d6c3468
1.fix some temporary return
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
stanley1228 | 8:1774aa06ab99 | 1 | /** |
stanley1228 | 8:1774aa06ab99 | 2 | * @brief Simple Kinematics Operations |
stanley1228 | 8:1774aa06ab99 | 3 | * @file MatrixMath_Kinematics.cpp |
stanley1228 | 8:1774aa06ab99 | 4 | * @author Ernesto Palacios |
stanley1228 | 8:1774aa06ab99 | 5 | * |
stanley1228 | 8:1774aa06ab99 | 6 | * Created on september 2011, 09:44 AM. |
stanley1228 | 8:1774aa06ab99 | 7 | * |
stanley1228 | 8:1774aa06ab99 | 8 | * Develop Under GPL v3.0 License |
stanley1228 | 8:1774aa06ab99 | 9 | * http://www.gnu.org/licenses/gpl-3.0.html |
stanley1228 | 8:1774aa06ab99 | 10 | */ |
stanley1228 | 8:1774aa06ab99 | 11 | |
stanley1228 | 8:1774aa06ab99 | 12 | //#include "mbed.h" |
stanley1228 | 8:1774aa06ab99 | 13 | #include "Matrix.h" |
stanley1228 | 8:1774aa06ab99 | 14 | #include "MatrixMath.h" |
stanley1228 | 8:1774aa06ab99 | 15 | |
stanley1228 | 8:1774aa06ab99 | 16 | |
stanley1228 | 8:1774aa06ab99 | 17 | Matrix MatrixMath::RotX( float radians ) |
stanley1228 | 8:1774aa06ab99 | 18 | { |
stanley1228 | 8:1774aa06ab99 | 19 | float cs = cos( radians ); |
stanley1228 | 8:1774aa06ab99 | 20 | float sn = sin( radians ); |
stanley1228 | 8:1774aa06ab99 | 21 | |
stanley1228 | 8:1774aa06ab99 | 22 | Matrix rotate( 4, 4 ); |
stanley1228 | 8:1774aa06ab99 | 23 | |
stanley1228 | 8:1774aa06ab99 | 24 | rotate << 1 << 0 << 0 << 0 |
stanley1228 | 8:1774aa06ab99 | 25 | << 0 << cs << -sn << 0 |
stanley1228 | 8:1774aa06ab99 | 26 | << 0 << sn << cs << 0 |
stanley1228 | 8:1774aa06ab99 | 27 | << 0 << 0 << 0 << 1; |
stanley1228 | 8:1774aa06ab99 | 28 | |
stanley1228 | 8:1774aa06ab99 | 29 | return rotate; |
stanley1228 | 8:1774aa06ab99 | 30 | |
stanley1228 | 8:1774aa06ab99 | 31 | } |
stanley1228 | 8:1774aa06ab99 | 32 | |
stanley1228 | 8:1774aa06ab99 | 33 | Matrix MatrixMath::RotY( float radians ) |
stanley1228 | 8:1774aa06ab99 | 34 | { |
stanley1228 | 8:1774aa06ab99 | 35 | float cs = cos( radians ); |
stanley1228 | 8:1774aa06ab99 | 36 | float sn = sin( radians ); |
stanley1228 | 8:1774aa06ab99 | 37 | |
stanley1228 | 8:1774aa06ab99 | 38 | Matrix rotate( 4, 4 ); |
stanley1228 | 8:1774aa06ab99 | 39 | |
stanley1228 | 8:1774aa06ab99 | 40 | rotate << cs << 0 << sn << 0 |
stanley1228 | 8:1774aa06ab99 | 41 | << 0 << 1 << 0 << 0 |
stanley1228 | 8:1774aa06ab99 | 42 | << -sn << 0 << cs << 0 |
stanley1228 | 8:1774aa06ab99 | 43 | << 0 << 0 << 0 << 1; |
stanley1228 | 8:1774aa06ab99 | 44 | |
stanley1228 | 8:1774aa06ab99 | 45 | return rotate; |
stanley1228 | 8:1774aa06ab99 | 46 | } |
stanley1228 | 8:1774aa06ab99 | 47 | |
stanley1228 | 8:1774aa06ab99 | 48 | Matrix MatrixMath::RotZ( float radians ) |
stanley1228 | 8:1774aa06ab99 | 49 | { |
stanley1228 | 8:1774aa06ab99 | 50 | float cs = cos( radians ); |
stanley1228 | 8:1774aa06ab99 | 51 | float sn = sin( radians ); |
stanley1228 | 8:1774aa06ab99 | 52 | |
stanley1228 | 8:1774aa06ab99 | 53 | Matrix rotate( 4, 4 ); |
stanley1228 | 8:1774aa06ab99 | 54 | |
stanley1228 | 8:1774aa06ab99 | 55 | rotate << cs << -sn << 0 << 0 |
stanley1228 | 8:1774aa06ab99 | 56 | << sn << cs << 0 << 0 |
stanley1228 | 8:1774aa06ab99 | 57 | << 0 << 0 << 1 << 0 |
stanley1228 | 8:1774aa06ab99 | 58 | << 0 << 0 << 0 << 1; |
stanley1228 | 8:1774aa06ab99 | 59 | |
stanley1228 | 8:1774aa06ab99 | 60 | return rotate; |
stanley1228 | 8:1774aa06ab99 | 61 | } |
stanley1228 | 8:1774aa06ab99 | 62 | |
stanley1228 | 8:1774aa06ab99 | 63 | Matrix MatrixMath::Tz( float d ) |
stanley1228 | 8:1774aa06ab99 | 64 | { |
stanley1228 | 8:1774aa06ab99 | 65 | |
stanley1228 | 8:1774aa06ab99 | 66 | Matrix z_d( 4, 4 ); |
stanley1228 | 8:1774aa06ab99 | 67 | |
stanley1228 | 8:1774aa06ab99 | 68 | z_d << 1 << 0 << 0 << 0 |
stanley1228 | 8:1774aa06ab99 | 69 | << 0 << 1 << 0 << 0 |
stanley1228 | 8:1774aa06ab99 | 70 | << 0 << 0 << 1 << d |
stanley1228 | 8:1774aa06ab99 | 71 | << 0 << 0 << 0 << 1; |
stanley1228 | 8:1774aa06ab99 | 72 | |
stanley1228 | 8:1774aa06ab99 | 73 | return z_d; |
stanley1228 | 8:1774aa06ab99 | 74 | } |
stanley1228 | 8:1774aa06ab99 | 75 | |
stanley1228 | 8:1774aa06ab99 | 76 | |
stanley1228 | 8:1774aa06ab99 | 77 | Matrix MatrixMath::Transl( float x, float y, float z ) |
stanley1228 | 8:1774aa06ab99 | 78 | { |
stanley1228 | 8:1774aa06ab99 | 79 | Matrix Translation = MatrixMath::Eye( 3 ); //Identity Matrix |
stanley1228 | 8:1774aa06ab99 | 80 | Matrix Position( 4, 1 ); // Position Matrix |
stanley1228 | 8:1774aa06ab99 | 81 | |
stanley1228 | 8:1774aa06ab99 | 82 | Position << x << y << z << 1; // position @ x,y,z |
stanley1228 | 8:1774aa06ab99 | 83 | |
stanley1228 | 8:1774aa06ab99 | 84 | Matrix::AddRow( Translation, 4 ); // Add Row |
stanley1228 | 8:1774aa06ab99 | 85 | Matrix::AddCol( Translation, Position, 4 ); // Add Position Matrix |
stanley1228 | 8:1774aa06ab99 | 86 | |
stanley1228 | 8:1774aa06ab99 | 87 | return Translation; |
stanley1228 | 8:1774aa06ab99 | 88 | } |