Manipulator Algorithms子模块库中包含求解机器人正动力学和逆动力学的模块，可以方便、快速地进行机器人动力学方程的求解。Forward Dynamics模块（正动力学计算模块）如图7.49所示。
图7.49 用于正动力学求解的Forward Dynamics模块
该模块用于在给定机器人各关节角度和关节速度下，以及各关节所受驱动力矩下计算各关节角加速度的大小。Config，JointVel和JointTorq接口接受N×1的向量，对应N个非固定关节的刚体树实例对象。其中Config接口接受N×1的关节坐标（单位为rad和m），JointVel接口接受N×1的角速度向量（单位为rad／s和m／s），JointTorq接口接受N×1的关节受力或力矩（单位为N和N·m）。除此之外，FExt接口用于接受一个6×M的末端执行器所受外力或外力矩矩阵，其中，M为刚体树实例对象的关节数目。模块输出接口JointAccel返回一个N×1的关节加速度向量，模块的模型参数为刚体树实例对象。
以Staubli TX200机器人为例来说明如何使用此模块计算机器人的正动力学，各关节的物理参数见第6章表6.6。
在sixJointRigidBodyTree.m脚本中分别添加各关节的相关物理参数定义：(www.xing528.com)
构建用于求解正动力学的Simulink模型，如图7.50所示。设置该求解模块的模型参数输入为前面已定义的treerbt对象。
输入的初始各关节转角坐标为：input_qn＝［0，-1.570 8，-2.356 2，0.785 4，0.785 4，0］。定义各关节角速度为：dq＝［0.1 0.1 0.1 0.1 0.1 0.1］，单位为rad／s；各关节所受力矩为：joint_torq＝［1 1 0 1 0 1］，单位为N·m，设末端执行器所受外力或外力矩均为0，即fext＝zeros（6，6）。
计算得到每个关节的加速度组成的向量为［19.51，22.92，12.12，-4.76，-50.63，23.96］，单位为rad／s2。
图7.50 用于正动力学求解的Simulink模型
免责声明：以上内容源自网络，版权归原作者所有，如有侵犯您的原创版权请告知，我们将尽快删除相关内容。
