解决一般的手臂问题：场景，抓取goal放到固定位置（现在先考虑如何准确夹取问题）

• {B} 基座frame；
• {W} 手腕的frame；
• {T} 末端执行工具的frame；（施工）
• {S} 静止物，桌子的frame；
• {G} 目标物的frame；Tool如何与goal进行对应..

SITUATION： 机械手臂夹住放在桌上的杯子，移动手臂将杯子挂到墙上的杯架。

P.S. 一般杯子相对桌角的距离，可以通过顶部相机系统，得到相对关系。

现阶段任务：

为了使RRRRRR手臂能以下图姿态夹住杯子（任务起始点为C），手臂的6个joint angles需要是多少呢？

STEP1: 定义DH Table

目视即可得到具体的DH Table：

STEP2: 先找出cup相对于world的关系，子进一步找出6相对于0的关系  。

首先关于可以由【桌子相对于world】和【杯子相对于桌子】的相对关系得到：

其中可以由影像系统（视觉系统）俯拍得到。

进一步分析（已经求出）:

通过该式，我们可以求出：（Inverse）

可以分离出移动和转动矩阵：

STEP3:找出  (根据之前的Pieper's Solution）

• 根据位移矩阵，根据 求得角度：

• 将r和z列出来:

• 根据上式，求得  ：

• 根据r式，求解出  ：

• 根据x式，求解出  ：

• 在根据求出的求出  ：

求解出  我们可以方便知道，手臂各个joint angles为多少时，可以抓住杯子。

ZYZ Euler Angles求法：