use from Ernesto Palacios
Dependents: RoboticArm DXL_SDK_Porting_Test
Fork of Matrix by
MatrixMath_Kinematics.cpp
- Committer:
- stanley1228
- Date:
- 2017-02-11
- Revision:
- 8:1774aa06ab99
- Child:
- 9:f19d5d6c3468
File content as of revision 8:1774aa06ab99:
/** * @brief Simple Kinematics Operations * @file MatrixMath_Kinematics.cpp * @author Ernesto Palacios * * Created on september 2011, 09:44 AM. * * Develop Under GPL v3.0 License * http://www.gnu.org/licenses/gpl-3.0.html */ //#include "mbed.h" #include "Matrix.h" #include "MatrixMath.h" Matrix MatrixMath::RotX( float radians ) { float cs = cos( radians ); float sn = sin( radians ); Matrix rotate( 4, 4 ); rotate << 1 << 0 << 0 << 0 << 0 << cs << -sn << 0 << 0 << sn << cs << 0 << 0 << 0 << 0 << 1; return rotate; } Matrix MatrixMath::RotY( float radians ) { float cs = cos( radians ); float sn = sin( radians ); Matrix rotate( 4, 4 ); rotate << cs << 0 << sn << 0 << 0 << 1 << 0 << 0 << -sn << 0 << cs << 0 << 0 << 0 << 0 << 1; return rotate; } Matrix MatrixMath::RotZ( float radians ) { float cs = cos( radians ); float sn = sin( radians ); Matrix rotate( 4, 4 ); rotate << cs << -sn << 0 << 0 << sn << cs << 0 << 0 << 0 << 0 << 1 << 0 << 0 << 0 << 0 << 1; return rotate; } Matrix MatrixMath::Tz( float d ) { Matrix z_d( 4, 4 ); z_d << 1 << 0 << 0 << 0 << 0 << 1 << 0 << 0 << 0 << 0 << 1 << d << 0 << 0 << 0 << 1; return z_d; } Matrix MatrixMath::Transl( float x, float y, float z ) { Matrix Translation = MatrixMath::Eye( 3 ); //Identity Matrix Matrix Position( 4, 1 ); // Position Matrix Position << x << y << z << 1; // position @ x,y,z Matrix::AddRow( Translation, 4 ); // Add Row Matrix::AddCol( Translation, Position, 4 ); // Add Position Matrix return Translation; }