hsu han-lin
RobotControl_7Axis
Dependents: RoboticArm DXL_SDK_Porting_Test
Diff: RobotControl_7Axis.cpp
- Revision:
- 2:71ed7da9ea36
- Parent:
- 1:584f36ed8717
- Child:
- 3:e380c7db9133
diff -r 584f36ed8717 -r 71ed7da9ea36 RobotControl_7Axis.cpp
--- a/RobotControl_7Axis.cpp Sat Feb 11 00:22:11 2017 +0800
+++ b/RobotControl_7Axis.cpp Sat Feb 11 13:06:49 2017 +0000
@@ -2,6 +2,12 @@
#include "dynamixel.h"
#include "RobotControl_7Axis.h"
+#include <vector>
+#include "Matrix.h"
+#include "MatrixMath.h"
+#include <math.h>
+
+
extern Serial pc;
@@ -286,3 +292,389 @@
return 0;
}
+
+
+
+//歐拉 Z1X2Y3 Intrinsic Rotaions相對於當前坐標系的的旋轉
+Matrix R_z1x2y3(float alpha,float beta,float gamma)
+{
+ Matrix ans(3,3);
+ ans << cos(alpha)*cos(gamma)-sin(alpha)*sin(beta)*sin(gamma) << -cos(beta)*sin(alpha) << cos(alpha)*sin(gamma)+cos(gamma)*sin(alpha)*sin(beta)
+ << cos(gamma)*sin(alpha)+cos(alpha)*sin(beta)*sin(gamma) << cos(alpha)*cos(beta) << sin(alpha)*sin(gamma)-cos(alpha)*cos(gamma)*sin(beta)
+ << -cos(beta)*sin(gamma) << sin(beta) << cos(beta)*cos(gamma);
+
+
+ return ans;
+}
+
+float norm(const Matrix& v)
+{
+ int i=0;
+ float ans=0;
+
+ for(i=1;i<=v.getRows();i++)
+ {
+ ans+=pow(v(i,1),2);
+ }
+
+ ans=sqrt(ans);
+
+ return ans;
+}
+
+
+Matrix Rogridues(float theta,const Matrix& V_A)
+{
+ Matrix R_a(4,4);
+
+ float cs = cos( theta );
+ float sn = sin( theta );
+
+ R_a << cs + pow(V_A.getNumber(1,1),2)*(1-cs) << V_A.getNumber(1,1)*V_A.getNumber(2,1)*(1-cs)-V_A.getNumber(3,1)*sn << V_A.getNumber(1,1)*V_A.getNumber(3,1)*(1-cs)+V_A.getNumber(2,1)*sn << 0
+ << V_A.getNumber(1,1)*V_A.getNumber(2,1)*(1-cs)+V_A.getNumber(3,1)*sn << cos(theta)+pow(V_A.getNumber(2,1),2)*(1-cs) << V_A.getNumber(2,1)*V_A.getNumber(3,1)*(1-cs)-V_A.getNumber(1,1)*sn << 0
+ << V_A.getNumber(1,1)*V_A.getNumber(3,1)*(1-cs)-V_A.getNumber(2,1)*sn << V_A.getNumber(2,1)*V_A.getNumber(3,1)*(1-cs)+V_A.getNumber(1,1)*sn << cs+pow(V_A.getNumber(3,1),2)*(1-cs) << 0
+ << 0 << 0 << 0 << 1;
+
+ return R_a;
+}
+//enum{
+// AXIS1=0,
+// AXIS2,
+// AXIS3,
+// AXIS4,
+// AXIS5,
+// AXIS6,
+// AXIS7
+//};
+
+int IK_7DOF(const float l1,const float l2,const float l3,const float x_base,const float y_base,const float z_base,const float x_end,const float y_end,const float z_end,const float alpha,const float beta,const float gamma,const float Rednt_alpha,float* theta)
+{
+ int i=0;
+
+ //Out put initial to zero
+ for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+ {
+ theta[i]=0;
+ }
+
+ Matrix R(3,3);
+ R=R_z1x2y3(alpha,beta,gamma);
+
+ Matrix V_H_hat_x(3,1);
+ V_H_hat_x=Matrix::ExportCol(R,1);//取出歐拉角轉換的旋轉矩陣，取出第1行為X軸旋轉後向量
+ V_H_hat_x*=1/norm(V_H_hat_x);
+
+ Matrix V_H_hat_y(3,1);
+ V_H_hat_y=Matrix::ExportCol(R,2);//取出歐拉角轉換的旋轉矩陣，取出第2行為Y軸旋轉後向量
+ V_H_hat_y*=1/norm(V_H_hat_y);
+
+
+ Matrix V_r_end(3,1);
+ V_r_end <<x_end-x_base
+ <<y_end-y_base
+ <<z_end-z_base;
+
+
+ Matrix V_r_h(3,1);
+ V_r_h=V_H_hat_x*L3;
+
+ Matrix V_r_wst(3,1);
+ V_r_wst=V_r_end-V_r_h;
+
+ //theat 4
+ theta[Index_AXIS4]=-(float)(DEF_PI-acos((pow(l1,2)+pow(l2,2)-pow(norm(V_r_wst),2))/(2*l1*l2)));
+
+
+ Matrix V_r_m(3,1);
+ V_r_m=(pow(l1,2)-pow(l2,2)+pow(norm(V_r_wst),2))/(2*pow(norm(V_r_wst),2))*V_r_wst;
+
+
+
+ //Redundant circle 半徑R
+ float Rednt_cir_R = pow(l1,2)- pow( (pow(l1,2)-pow(l2,2)+pow(norm(V_r_wst),2))/(2*norm(V_r_wst)) , 2);
+ Rednt_cir_R=sqrt(Rednt_cir_R);
+
+
+ //圓中心點到Elbow向量 V_r_u
+ Matrix V_shz(3,1);
+ V_shz <<0
+ <<0
+ <<1;
+
+ Matrix V_alpha_hat(3,1);//V_alpha_hat=cross(V_r_wst,V_shz)/norm(cross(V_r_wst,V_shz));
+ Matrix temp_cross(3,1);
+ temp_cross=MatrixMath::cross(V_r_wst,V_shz); //錯誤
+ V_alpha_hat=temp_cross*(1/norm(temp_cross));
+
+ Matrix V_beta_hat(3,1);//V_beta_hat=cross(V_r_wst,V_alpha_hat)/norm(cross(V_r_wst,V_alpha_hat));
+ temp_cross=MatrixMath::cross(V_r_wst,V_alpha_hat);
+ V_beta_hat=temp_cross*(1/norm(temp_cross));
+
+
+
+ Matrix temp(4,4);//temp=Rogridues(Rednt_alpha,V_r_wst/norm(V_r_wst)) *[Rednt_cir_R*V_beta_hat;1]; %Rednt_alpha的方向和論文上的方向性相反
+ Matrix V_r_wst_unit =V_r_wst*(1/norm(V_r_wst));
+ Matrix V_temp3x1(3,1);//需要寫一個可以補1的函試
+ Matrix V_temp4x1(4,1);
+ V_temp3x1=V_beta_hat*Rednt_cir_R;
+ V_temp4x1.Vec_ext_1_row(V_temp3x1,1); //3x1 extend to 4x1
+
+ temp=Rogridues(Rednt_alpha,V_r_wst_unit)*V_temp4x1;
+
+
+ Matrix V_R_u(3,1);
+ V_R_u.Vec_export_3_row(temp);
+
+
+ Matrix V_r_u(3,1);
+ V_r_u=V_r_m+V_R_u;
+
+ theta[Index_AXIS1]=atan2(-V_r_u(1,1),-V_r_u(3,1));//theta(1)=atan2(-V_r_u(1),-V_r_u(3));
+
+
+ if (theta[Index_AXIS1] !=0)
+ theta[Index_AXIS2]=atan2(V_r_u(2,1),-V_r_u(1,1)/sin(theta[Index_AXIS1]));
+ else
+ theta[Index_AXIS2]=atan2(-V_r_u(2,1),V_r_u(3,1));
+
+
+
+ //theat 3
+ //theta(3)=atan2( sin(theta(2))*sin(theta(1))*V_r_wst(1)+cos(theta(2))*V_r_wst(2)+sin(theta(2))*cos(theta(1))*V_r_wst(3),cos(theta(1))*V_r_wst(1)-sin(theta(1))*V_r_wst(3));
+ theta[Index_AXIS3]=atan2( sin(theta[Index_AXIS2])*sin(theta[Index_AXIS1])*V_r_wst(1,1)+cos(theta[Index_AXIS2])*V_r_wst(2,1)+sin(theta[Index_AXIS2])*cos(theta[Index_AXIS1])*V_r_wst(3,1),cos(theta[Index_AXIS1])*V_r_wst(1,1)-sin(theta[Index_AXIS1])*V_r_wst(3,1));
+
+
+
+ //theat 5
+ Matrix V_r_f(3,1);
+ V_r_f=V_r_wst-V_r_u;
+
+ Matrix V_Axis6(3,1);
+ V_Axis6=MatrixMath::cross(V_H_hat_y,-V_r_f)*(1/norm(MatrixMath::cross(V_H_hat_y,-V_r_f)));//V_Axis6=cross(V_H_hat_y,-V_r_f)/norm(cross(V_H_hat_y,-V_r_f));
+
+ Matrix V_r_wst_u(3,1);
+ V_r_wst_u=V_r_wst+V_Axis6;
+
+ Matrix A1_4(4,4);
+ A1_4=MatrixMath::RotY(theta[Index_AXIS1])*MatrixMath::RotX(theta[Index_AXIS2])*MatrixMath::RotZ(theta[Index_AXIS3])*MatrixMath::Tz(-l1)*MatrixMath::RotY(theta[Index_AXIS4]);//A1_4=Ry(theta(1))*Rx(theta(2))*Rz(theta(3))*Tz(-L1)*Ry(theta(4));
+
+
+ Matrix V_temp_f(4,1);
+ Matrix V_r_wst_u_4x1(4,1);
+ V_r_wst_u_4x1.Vec_ext_1_row(V_r_wst_u,1);
+
+
+ V_temp_f=MatrixMath::Inv(A1_4)*V_r_wst_u_4x1;//V_temp_f=inv(A1_4)*[V_r_wst_u;1]; %(3.31) 這個是補一列1上去的意思,need fix
+ theta[Index_AXIS5]=atan2(V_temp_f(2,1),V_temp_f(1,1));//theta(5)=atan2(V_temp_f(2),V_temp_f(1));
+
+
+ //theat 6
+ Matrix V_r_wst_r(3,1);
+ V_r_wst_r=V_r_wst+V_H_hat_y;
+
+ Matrix A1_5(4,4);
+ A1_5=A1_4*MatrixMath::RotZ(theta[Index_AXIS5])*MatrixMath::Tz(-l2);//A1_5=A1_4*Rz(theta(5))*Tz(-L2);
+
+ Matrix V_temp_g(4,4);
+ Matrix V_r_wst_r_4x1(4,1);
+ V_r_wst_r_4x1.Vec_ext_1_row(V_r_wst_r,1);
+
+ V_temp_g=MatrixMath::Inv(A1_5)*V_r_wst_r_4x1; //V_temp_g=inv(A1_5)*[V_r_wst_r;1]; %(3.38) 這個是補一列1上去的意思,need fix
+
+ theta[Index_AXIS6]=atan2(V_temp_g(3,1),V_temp_g(2,1));
+
+
+ //theat 7
+ Matrix V_r_wst_f(3,1);
+ V_r_wst_f=V_r_wst+V_H_hat_x;
+
+ Matrix A1_6(4,4);
+ A1_6=A1_5*MatrixMath::RotX(theta[Index_AXIS6]);
+
+ Matrix V_temp_h(3,1);
+ Matrix V_r_wst_f_4x1(4,1);
+ V_r_wst_f_4x1.Vec_ext_1_row(V_r_wst_f,1);
+
+ V_temp_h=MatrixMath::Inv(A1_6)*V_r_wst_f_4x1; //V_temp_h=inv(A1_6)*[V_r_wst_f;1];
+
+ theta[Index_AXIS7]=atan2(-V_temp_h(1,1),-V_temp_h(3,1));//theta(7)=atan2(-V_temp_h(1),-V_temp_h(3));
+
+
+ return 0;
+}
+
+
+
+//int keepitforawhile_deleteitafter()
+//{
+//
+// //DigitalOut myled(LED1);
+// //Timer t,t2;
+//
+// //t.start();
+// float theta[7]={0};
+// int rt = IK_7DOF_stanley(L1,L2,L3,0,0,0,60,0,0,0,0,0,(float)-M_PI*0.5,theta);
+////---
+// Matrix myMatrix(3,3);
+// Matrix anotherMatrix;
+//
+// // Fill Matrix with data.
+// myMatrix << 1 << 2 << 3
+// << 4 << 5 << 6
+// << 7 << 8 << 9;
+//
+// printf( "\nmyMatrix:\n\n");
+// myMatrix.print();
+// printf( "\n" );
+//
+// /*Matrix myMatrix2(3,3);
+// myMatrix2 << 1 << 0 << 0
+// << 0 << 1 << 0
+// << 0 << 0 << 1;*/
+// float mydet = MatrixMath::det( myMatrix );
+//
+// printf( "det mymatrix=%f:\n\n",mydet);
+//
+//
+//
+// // Matrix operations //
+//
+// // Add 5 to negative Matrix
+// anotherMatrix = - myMatrix + 5;
+//
+// printf( "Result Matrix: anotherMatrix = - myMatrix + 5\n\n" );
+// anotherMatrix.print();
+// printf( "\n" );
+//
+// // Matrix Multiplication *
+// anotherMatrix *= myMatrix;
+//
+// printf( "\nanotherMatrix = anotherMatrix * myMatrix\n\n" );
+// anotherMatrix.print();
+// printf( "\n" );
+//
+// // Scalar Matrix Multiplication anotherMatrix *= 0.5
+// anotherMatrix *= 0.5;
+//
+// printf( "\nResult Matrix *= 0.5:\n\n" );
+// anotherMatrix.print();
+// printf( " _______________________________ \n" );
+//
+//
+// printf("\n\n *** MEMBER OPERATIONS *** \n\n");
+//
+// //Copy myMatrix
+// Matrix temp( myMatrix );
+//
+// // Resize Matrix
+// temp.Resize(4,4);
+// printf("\nAdded one Column, one Row to the limits of myMatrix saved in temp Matrix\n");
+// temp.print();
+//
+// //Delete those new elements, we don't need them anyway.
+// Matrix::DeleteRow( temp, 4 );
+// Matrix::DeleteCol( temp, 4 );
+//
+// printf("\nBack to normal\n");
+// temp.print();
+//
+//
+// // Make room at the begining of Matrix
+// Matrix::AddRow( temp, 1 );
+// Matrix::AddCol( temp, 1 );
+//
+// printf("\nAdded Just one Row and column to the beginning\n");
+// temp.print();
+//
+// // Take the second Row as a new Matrix
+// anotherMatrix = Matrix::ExportRow( temp, 2 );
+// printf("\nExport Second Row \n");
+// anotherMatrix.print();
+//
+// // The second Column as a new Matrix, then transpose it to make it a Row
+// anotherMatrix = Matrix::ExportCol( temp, 2 );
+// anotherMatrix = MatrixMath::Transpose( anotherMatrix );
+// printf("\nAnd Export Second Column and Transpose it \n");
+// anotherMatrix.print();
+//
+// // This will Check to see if your are reduce to a single Row or Column
+// temp = Matrix::ToPackedVector( myMatrix );
+// printf("\nInitial Matrix turned into a single Row\n");
+// temp.print();
+//
+// // Matrix Math //
+// printf("\n\n *** Matrix Inverse and Determinant ***\n");
+//
+// //Matrix BigMat( 8, 8 );
+// //
+// //BigMat << 1 << 0.3 << 1.0 << 1 << 3 << 0.5 << 7.12 << 899
+// // << 2 << 3.2 << 4.1 << 0 << 4 << 0.8 << 9.26 << 321
+// // << 5 << 6.0 << 1 << 1 << 2 << 7.4 << 3.87 << 562
+// // << 1 << 0.0 << 2.7 << 1 << 1 << 4.6 << 1.21 << 478
+// // << 2 << 3.7 << 48 << 2 << 0 << 77 << 0.19 << 147
+// // << 1 << 1.0 << 3.8 << 7 << 1 << 9.9 << 7.25 << 365
+// // << 9 << 0.9 << 2.7 << 8 << 0 << 13 << 4.16 << 145
+// // << 7 << 23 << 28 << 9 << 9 << 1.7 << 9.16 << 156;
+//
+// //printf( "\nBigMat:\n");
+// //BigMat.print();
+// //printf( "\n" );
+//
+// //t2.start();
+// //float determ = MatrixMath::det( BigMat );//有問題
+//
+// //Matrix myInv = MatrixMath::Inv( BigMat );
+// ////t2.stop();
+//
+// //printf( "\nBigMat's Determinant is: %f \n", determ);
+// //printf( "\n" );
+// //
+// //printf( "\nBigMat's Inverse is:\n");
+// //myInv.print();
+// //printf( "\n" );
+//
+// //*** Homogenous Transformations **//
+//
+// printf( "\n\n *** TRANSFORMATIONS *** \n\n");
+//
+// Matrix rot;
+//
+// printf( " RotX 0.5 rad \n" );
+// rot = MatrixMath::RotX(0.5);
+// rot.print();
+// printf( " _______________________________ \n\n" );
+//
+// printf( " RotY 0.5 rad \n" );
+// rot = MatrixMath::RotY(0.5);
+// rot.print();
+// printf( " _______________________________ \n\n" );
+//
+// printf( " RotZ 0.5 rad \n" );
+// rot = MatrixMath::RotZ(0.5);
+// rot.print();
+// printf( " _______________________________ \n\n" );
+//
+// //printf( " Transl 5x 3y 4z\n" ); //有問題
+// //rot = MatrixMath::Transl( 5, 3, 4 );
+// //rot.print();
+// //printf( " _______________________________ \n\n" );
+//
+// //---
+//
+// //t.stop();
+//
+// //float bigtime = t2.read();
+// //float average = 12.149647 - bigtime;
+//
+// //printf( "\n\nThe time for all those operations in mbed was : %f seconds\n", t.read() );
+// //printf( "\nOnly operations witout any print takes: 12.149647 seconds\n" );
+// //printf( "\nDue to the 8x8 matrix alone takes: %f \n",bigtime );
+// //printf( "\nSo normal 4x4 matrix ops: %f\n", average );
+//
+// //while(1) {
+// // myled = 1;
+// // wait(0.2);
+// // myled = 0;
+// // wait(0.2);
+// //}
+//}