实验一 机械臂正逆运动学
实验一 机械臂正逆运动学
一、实验目的
1、巩固正逆运动学基础概念。
2、了解正逆运动学在机械臂控制中的实际用途。
二、实验内容
1、机械臂模型DH参数的计算。
2、机械臂正运动学的计算。
3、机械臂逆运动学的计算。
三、实验步骤
1、正运动学实验验证，编写程序计算以下三组关节角度对应的末端位姿，并通过位置控制使机器人运动到相应位置，将实际末端位置与你的计算值比较，求出绝对误差。
第一组：[0.927, -0.687, -0.396, 0, 1.083, 0.927]
第二组：[0.322, -0.855, -0.021, 0, 0.877, 0.322]
第三组：[-0.322, -0.636, -0.011, 0, 0.647, -0.322]
2、逆运动学实验验证，编写程序计算以下三组末端位姿对应的关节角度，并通过位置控制使机器人运动到你计算出的位置，将实际末端位置与所给的值比较，求绝对误差。
第一组：[0.2, 0.2, 0.2007,1.57, -1.57, 0]
第二组：[0.15 ,0.2, 0.2007, 0, 0, 0]
第三组：[0.3 ,0, 0.122, 1.57, 0, 0]
四、评定标准
1、实验结果的准确性
五、注意事项
1、本实验中姿态欧拉角与ROS中一致，全部采用弧度制，坐标全部以米为单位。
2、真实机器人实验中，如果机器人发生碰撞，一定要先扶住机械臂，再按下急停按钮，否则机械臂会倒下。
3、按下急停按钮后需要手动将机械臂复原到门位置，并重置控制内部机械臂位置。
4、真实机械臂实验过程中，按下急停按钮后复位的过程中最好按照原本的运动方向复位，不要让第一关节一直向着一个方向旋转以免造成线路缠绕。
5、机械臂启动时的位置被称为初始门位置（如下图），其末端位姿为[0.2289, 0, 0.454, 1.57, 0, 0]，机械臂各个关节的正方向如图所示。
代码，部分行从助教例程中直接复制，因此部分代码为无用代码，以及代码写了两次，懒得去其进行精简，所以很长。ubuntu带到win的注释乱码，也懒得修改了
正运动学
#include : ";
cin>>theta[i];
}
theta[6] = 0;
theta[7] = 0;
init_pos.data.at(0) = theta[0];
init_pos.data.at(1) = theta[1];
init_pos.data.at(2) = theta[2];
init_pos.data.at(3) = theta[3];
init_pos.data.at(4) = theta[4];
init_pos.data.at(5) = theta[5];
//瀹氫箟DH鍙傛暟
const double DH[8][3]={ 0,0,0.284,
M_PI/2,0,0,
0,0.225,0,
M_PI/2,0,0.2289,
-M_PI/2,0,0,
M_PI/2,0,0,
0,0,0.055,
-M_PI/2,0,0};
Eigen::Matrix<double,4,4> T[8];
//璁＄畻鍙樻崲鐭╅樀
for(int i=0;i<8;i++){
T[i] << cos(theta[i]),-sin(theta[i]),0,DH[i][1],
sin(theta[i])*cos(DH[i][0]),cos(theta[i])*cos(DH[i][0]),-sin(DH[i][0]),-sin(DH[i][0])*DH[i][2],
sin(theta[i])*sin(DH[i][0]),cos(theta[i])*sin(DH[i][0]),cos(DH[i][0]),cos(DH[i][0])*DH[i][2],
0,0,0,1;
}
T[1] << cos(theta[1]+M_PI/2),-sin(theta[1]+M_PI/2),0,DH[1][1],
sin(theta[1]+M_PI/2)*cos(DH[1][0]),cos(theta[1]+M_PI/2)*cos(DH[1][0]),-sin(DH[1][0]),-sin(DH[1][0])*DH[1][2],
sin(theta[1]+M_PI/2)*sin(DH[1][0]),cos(theta[1]+M_PI/2)*sin(DH[1][0]),cos(DH[1][0]),cos(DH[1][0])*DH[1][2],
0,0,0,1;
T[4]<< cos(theta[4]-M_PI/2),-sin(theta[4]-M_PI/2),0,DH[4][1],
sin(theta[4]-M_PI/2)*cos(DH[4][0]),cos(theta[4]-M_PI/2)*cos(DH[4][0]),-sin(DH[4][0]),-sin(DH[4][0])*DH[4][2],
sin(theta[4]-M_PI/2)*sin(DH[4][0]),cos(theta[4]-M_PI/2)*sin(DH[4][0]),cos(DH[4][0]),cos(DH[4][0])*DH[4][2],
0,0,0,1;
//璁＄畻鏈浣嶅Э鍙樻崲鐭╅樀
Eigen::Matrix<double,4,4> result;
result << 1,0,0,0,
0,1,0,0,
0,0,1,0,
0,0,0,1;
for(int i = 0;i<8;i++){
result = result * T[i];
//PrintMatrix(result);
}
//寰楀埌鏁版嵁
double pos_x = result(0,3);
double pos_y = result(1,3);
double pos_z = result(2,3);
double rot_x = atan2(result(2,1),result(2,2));
double rot_y = atan2(-result(2,0),sqrt(result(1,0)*result(1,0)+result(0,0)*result(0,0)));
double rot_z = atan2(result(1,0),result(0,0));
//杈撳嚭浣嶅Э
cout<<"pos_x:"<<pos_x<<endl;
cout<<"pos_y:"<<pos_y<<endl;
cout<<"pos_z:"<<pos_z<<endl;
cout<<"rot_x:"<<rot_x<<endl;
cout<<"rot_y:"<<rot_y<<endl;
cout<<"rot_z:"<<rot_z<<endl;
pos_pub.publish(init_pos);
ROS_INFO_STREAM("published");
}
逆运动学
#include