机械臂正运动学末端位置与姿势C语言
- 1、下载文档前请自行甄别文档内容的完整性,平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益,请联系客服反馈,我们会尽快为您处理(人工客服工作时间:9:00-18:30)。
机械臂正运动学末端位置与姿势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,