本系列文章是以林沛群老师在coursera上的课程机器人学一为参考，进行归纳整理。
任务概述
机械手臂需要夹住放在桌子上的杯子，然后将杯子挂到墙上的杯架，位置参数以及手臂的DH参数如下图：
条件设定
- 我们需要规划手臂将杯子从桌子上拿起然后放到杯架上的整个轨迹。为了让轨迹变得顺滑，我们可以先让手臂拿起杯子一小段距离到$f_{c_1}$，然后到达杯架前，调整到适当姿态，然后将杯子平移到杯架上，如下图所示：
- 我们要定义杯子在各点（$P_{0-f}$）的时间、坐标位置以及姿态。
- 我们就可以求出杯子在各点处相对于世界坐标的Transformation Matrix （$^0_CT$）
- 再推导出手臂末端点在各点处相对于世界坐标的Transformation Matrix （$^0_6T$）
- 从$^0_6T$得知$^0P_{6org}$在各点的位置和姿态。
以linear function with parabolic blends在Cartesian-space下规划轨迹
- 求出$^0P_{6org}$在各DOF（$X,Y,Z,\Phi_X,\Phi_Y,\Phi_Z$）每段的速度和加速度
- 建立并绘出各DOF在每个时间区段的轨迹
- 此时轨迹规划已经完成，接下来就要用IK计算出轨迹上所有设定点以及轨迹内插点的六轴转角（计算量很大）。以第二个via point为例：
note: 当有多个解时，可以将它带入Transformation Matrix里来验证哪一组解是正确的。
- 将解出的轨迹设定点以及轨迹内插点的六轴转角对各自的时间进行画图：
- 将转角带入FK，画出杯子的坐标及姿态，已确认轨迹规划的正确性，同样以第二个via point为例：
以linear function with parabolic blends在joint-space下规划轨迹
- 以IK先计算出各点的六轴转角
- 求出各轴（$\theta_1-\theta_6$）在每个时段的速度及加速度
- 建立各轴在每个时段的轨迹
- 以FK画出杯子对事件的轨迹，确认轨迹有通过设定点。
- 在Cartensian-space下和在joint-space下轨迹比较。
