[三]正运动学与DH坐标变换

附上一段矩阵运算的C语言代码

#include<stdio.h>
#include<math.h>

#define unExist 9999
#define Free_N 5						//自由度个数为N则DH表N+1
#define pi 3.1415926
#define Arm_a 9
#define Arm_b 10
#define Arm_c 11
#define Arm_d 12

float theit_1;
float theit_2;
float theit_3;
float theit_4;
float theit_5;
//---------------------------------L(i-1)--α(i-1)----D(i)
float DH_table[Free_N+1][3] = {	{       0,      0,  Arm_a} ,
								{       0, pi / 2,		0} ,
								{   Arm_b,      0,      0} ,
								{   Arm_c,      0,	    0} ,
								//{   Arm_d,      0,      0} ,
								{   Arm_d,      0,      0}};

double DH_C(/*float Alf,float D,float L*/)
{
	int i;
	for (i = 0; i <= Free_N; i++)
	{
		printf("-------------------------第%d组-------------------------\r\n",i+1);
		printf("第一行：  cos        -sin       0        %2.2f  \r\n", DH_table[i][0]);
		printf("第二行：  %2.2fsin   %2.2fcos   %2.2f    %2.2f  \r\n", cos(DH_table[i][1]), cos(DH_table[i][1]), -sin(DH_table[i][1]), -sin(DH_table[i][1]) * DH_table[i][2]);
		printf("第一行：  %2.2fsin   %2.2fcos   %2.2f    %2.2f  \r\n", sin(DH_table[i][1]), sin(DH_table[i][1]),  cos(DH_table[i][1]),  cos(DH_table[i][1]) * DH_table[i][2]);
	}
}
int main()
{
	DH_C();
	//printf("%f", cos(pi/2));
}