机械臂正运动学末端位置与姿势C语言

机械臂正运动学末端位置与姿势C语言

/*

JI PENG/2016-10-26

畚??

ROBOTICS Homework

*/

#include <math.h>

#include <stdio.h>

int main(void)

{

float A1, A2, A3, A4, A5, A6; //憷?

float a1, a2, a3, a4, a5, a6; //??

float L1, L2, L3;

float d1, d4;

float r11, r12, r13, r21, r22, r23, r31, r32, r33, r41, r42, r43, r44; //r 0/4R

float R11, R12, R13, R21, R22, R23, R31, R32, R33, R41, R42, R43, R44; //R 0/6R

float px, py, pz; //0/4p

float PX, PY, PZ; //0/6p

float b;

float Pi;

printf("JI PENG/2016-10-26\n??畚??\nROBOTICS Homework\n");

printf("\n\nInput the L1, L2, L3, and d1, d4: \n");

scanf("%f%f%f%f%f", &L1, &L2, &L3, &d1, &d4);

printf("\nInput the deg of A1, A2, A3, A4, A5 and A6: \n");

scanf("%f%f%f%f%f%f", &A1, &A2, &A3, &A4, &A5, &A6);

Pi=3.141592654;

a1=(A1*Pi)/180; //憷?---??

a2=(A2*Pi)/180;

a3=(A3*Pi)/180;

a4=(A4*Pi)/180;

a5=(A5*Pi)/180;

a6=(A6*Pi)/180;

r11 = cos(a1) * cos(a2) * cos(a3) * cos(a4) - cos(a1) * sin(a2) * sin(a3) * cos(a4) + sin(a1) *

sin(a4);

r21 = sin(a1) * cos(a2) * cos(a3) * cos(a4) - sin(a1) * sin(a2) * sin(a3) * cos(a4) - cos(a1) * sin(a4);

r31 =-sin(a2) * cos(a3) * cos(a4) - cos(a2) * sin(a3) * cos(a4);

r41 = 0;

r12 =-cos(a1) * cos(a2) * cos(a3) * sin(a4) + cos(a1) * sin(a2) * sin(a3) * sin(a4) + sin(a1) * cos(a4);

r22 =-sin(a1) * cos(a2) * cos(a3) * sin(a4) + sin(a1) * sin(a2) * sin(a3) * sin(a4) - cos(a1) * cos(a4);

r32 = sin(a2) * cos(a3) * sin(a4) + cos(a2) * cos(a3) * sin(a4);

r42 = 0;

r13 =-cos(a1) * cos(a2) * sin(a3) - cos(a1) * sin(a2) * cos(a3);

r23 =-sin(a1) * cos(a2) * sin(a3) - sin(a1) * sin(a2) * cos(a3);

r33 = sin(a2) * sin(a3) - cos(a2) * cos(a3);

r43 = 0;

px = cos(a1) * cos(a2) * (cos(a3) * L3 - sin(a3) * d4 + L2) - cos(a1) * sin(a2) * (sin(a3) * L3 + cos(a3) * d4) + cos(a1) * L1;

py = sin(a1) * cos(a2) * (cos(a3) * L3 - sin(a3) * d4 + L2) - sin(a1) * sin(a2) * (sin(a3) * L3 + cos(a3) * d4) + sin(a1) * L1;

pz =-sin(a2) * (cos(a3) * L3 - sin(a3) * d4 + L2) - cos(a2) * (sin(a3) * L3 + cos(a3) * d4) + d1;

r44 = 1;

R11 = r11 * cos(a5) * cos(a6) + r12 * sin(a6) + r13 * sin(a5) * cos(a6);

R21 = r21 * cos(a5) * cos(a6) + r22 * sin(a6) + r23 * sin(a5) * cos(a6);

R31 = r31 * cos(a5) * cos(a6) + r32 * sin(a6) + r33 * sin(a5) * cos(a6);

R41 = 0;

R12 =-r11 * cos(a5) * sin(a6) + r12 * cos(a6) - r13 * sin(a5) * sin(a6);

R22 =-r21 * cos(a5) * sin(a6) + r22 * cos(a6) - r23 * sin(a5) * sin(a6);

R32 =-r31 * cos(a5) * sin(a6) + r32 * cos(a6) - r33 * sin(a5) * sin(a6);

R42 = 0;

R13 =-r11 * sin(a5) + r13 * cos(a5);

R23 =-r21 * sin(a5) + r23 * cos(a5);

R33 =-r31 * sin(a5) + r33 * cos(a5);

R43 = 0;

PX = px;

PY = py;

PZ = pz;

R44 = 1;

printf("\n-------------------------------------------\n");

printf("----------------荩??棕俟-----------------\n");

printf("\t%.3f, %.3f, %.3f, %.3f\n", R11, R12, R13, PX);

printf("\t%.3f, %.3f, %.3f, %.3f\n", R21, R22, R23, PY);

printf("\t%.3f, %.3f, %.3f,