MATLAB Robotics System Toolbox学习(2)----导入urdf文件与机器人逆动力学
这个算是个开发备忘吧，搞不好以后又忘了呢（笑）。
创建并导入urdf文件
利用solidworks导出urdf文件
其实可以不使用solidworks导出urdf文件，没记错的话，ROS也有Inventor也有导出urdf的插件。如果使用Fusion360的朋友也可以使用GitHub上的一个项目，虽然这个插件有一定的限制，但是也可以导出urdf文件。但是solidworks中装配体的操作是明显好于Fusion360，除了大和难以白嫖正版，solidworks还是挺推荐的。
具体的内容可以参照ROS 用Solidworks插件生成URDF和导入Simscape URDF格式
这里主要强调一下几个操作：
- 关节的运动限制一定要添加，但是homeconfiguration可能会超出你的关节运动限制，所以还是推荐自己配置configuration。
- 最后导出时，要对文件进行重命名，不能用“xxxx.SLDASM”，不能和已有文件夹重名，不要有中文、标点，和变量命名一样就好。
导入urdf文件
MATLAB 中导入urdf文件有两种方式。一是利用SimScape多体工具箱进行导入，导入后可以进入Simulink进行仿真，其格式为：
smimport("xxxx.urdf");
但是，不知道为啥，我死活下不下来，每次在安装35%就报错（摊手，因此就只能用第二种方式。
第二种方式是利用Robotics System工具箱，但是在利用Simulink仿真时没有多体工具箱方便。其格式为：
robot = importrobot("xxx.urdf");
importrobot会返回一个刚体树模型，然后就可以利用Robotics System工具箱中的函数进行进一步操作。
其实还有第三种方式，就是手动导入（笑），这个方式适合自己写程序求封闭解的情况，因为谁都不想算惯性张量矩阵嘛。所以这里以基坐标系为例简单说明一下urdf中有用的参数：
我们感兴趣的参数是
在导入后show(robot);的效果为：
如果没有stl文件的话，显示出来的只有几个坐标系。
机器人动力学
封闭解
MATLAB Robotics System 工具箱中的Dynamics和InvDynamics函数并不支持接收带有syms的变量的矩阵为参数，因此无法求得机器人动力学的封闭形式解，如果要求封闭解，自己写程序吧(lll￢ω￢)。
因为刚开始学习机器人学，理论力学基础又不咋的，所以这里拿牛顿-欧拉动力学递推方程来写算法，因为拉格朗日法不会啊（笑抽。由于程序现在写的有BUG，因此就留个坑，以后再填，具体可以参考牛顿-欧拉法，不过这篇博文只给出了转动关节的动力学递推方程，对于移动关节并没有给出（虽然只要改几个地方）。
数值解
因为只用过Robotics System Toolbox，所以只会用这个工具箱的逆动力学（笑）。
首先要设定重力加速度，因为默认状况下的重力加速度为0。
robot.Gravity = [0 0 -9.81];
然后再进行逆动力学求解，其函数原型为jointTorq = inverseDynamics(robot,configuration,jointVel,jointAccel,fext)
第一个参数robot为一个刚体树模型构成的机器人；configuration为机器人的姿态，如果读取数据格式为行的话，是一个1xn的行向量，n为关节数，单位为rad或m；jointVel为关节的转动（移动）速度，也是个1xn矩阵，单位为rad/s或m/s；jointAccel为关节的加速度，单位为 r a d / s 2 rad/s^{2} rad/s2或者 m / s 2 m/s^{2} m/s2；最后的fext为施加在连杆上的力和力矩，为6xn或者nx6矩阵(与你选择的dataformat有关)，可以通过externalForce(robot,bodyname,wrench,configuration)来获得矩阵，第一项robot和逆动力学的第一个参数一样；第二个为在哪个连杆上施加的力与力矩，为一个字符串如“tool”，这个根据你自己的连杆命名来；wrench为施加的力与力矩，其格式为 [ T x , T y , T z , F x , F y , F z ] [T_{x},T_{y},T_{z},F_{x},F_{y},F_{z}] [Tx,Ty,Tz,Fx,Fy,Fz];最后是机器人的配置。
因为不太会使simulink，所以就自己手写代码去生成关节位置、关节速度、关节加速度，然后写循环导出个矩阵再画图，这里把代码给大家贴一下：
robot = importrobot('scararobotics.urdf');
show(robot);
showdetails(robot);
robot.DataFormat ='row';
robot.Gravity = [0 0 -9.81];
cd C:\Users\Lin\Desktop\dynamics
[conf,JointVel,JointAccel,t]=stepper([0.04 -1.57 -2;0.2 1.57 2],1,100);
%conf,Vel,Accel的行为关节的相关参数
[row,column] = size(t);
%column为细分数
tau = zeros(column,3);
%tau 的每列是单个关节的输出
for i=1:1:column
fext = externalForce(robot,'Tool',[0 0 0 0 0 -10],conf(i,:));
tau(i,:) = inverseDynamics(robot,conf(i,:),JointVel(i,:),JointAccel(i,:),fext);
end
subplot(2,2,1);plot(t,tau(:,1)');grid on;xlabel('t/s');ylabel('f/N');title('joint1');
subplot(2,2,2);plot(t,tau(:,2)');grid on;xlabel('t/s');ylabel('M/Nm');title('joint3');
subplot(2,2,3);plot(t,tau(:,3)');grid on;xlabel('t/s');ylabel('M/Nm');title('joint3');
stepper参数为[初始关节状态、末关节状态]、运动时间、细分点，返回依据轨迹规划算法得到的关节配置、关节速度、关节加速度、时间矩阵。
一开始使用梯形的加减速算法（因为项目实际采用步进电机进行驱动），得到的力/力矩变化图惨不忍睹，有明显的突变。
使用三次项轨迹规划之后力/力矩的过渡就平滑多了
这里第一个关节为移动关节，故输出为力；其余均为转动关节，输出为力矩。