###### tags: `Notes`

# inverse_kinematics / inverse_kinematics_frame / Orientation
## 1. 兩者有沒有差別？
- 有，的確能依據目前角度計算更佳的運動方式
- 實驗設計: 球先擺在(0.3, 0.7, 0)，手臂到定位後，暫停模擬，把球移到(0.3, 0.3, 0)，繼續模擬，截圖。
- 起始 (是以inverse_kinematics_frame先到定位，但我想應該是一樣的)

- inverse_kinematics

ikResults = armChain.inverse_kinematics(targetPosition)


- inverse_kinematics_frame

ikResults = armChain.inverse_kinematics_frame(frame_target, psValue)


## 2. 新發現
使用以下方式呼叫`inverse_kinematics`，(目視)結果與`inverse_kinematics_frame`相同

ikResults = armChain.inverse_kinematics(
    targetPosition, 
    initial_position=psValue)

> 原本是
> ``` python
> ikResults = armChain.inverse_kinematics_frame(...)
> ```


原因應該在於，[`inverse_kinematics`](https://ikpy.readthedocs.io/en/latest/_modules/ikpy/chain.html#Chain.inverse_kinematics)的定義:

def inverse_kinematics(self, target_position=None, 
    target_orientation=None, orientation_mode=None, 
    **kwargs):

後面的`**kwargs`，會傳給下面[```inverse_kinematics_frame```](https://ikpy.readthedocs.io/en/latest/_modules/ikpy/chain.html#Chain.inverse_kinematics_frame)

def inverse_kinematics_frame(self, target,
    initial_position=None, **kwargs):

而`inverse_kinematics_frame`也會把`**kwargs`傳給最後的[`inverse_kinematic_optimization`](https://ikpy.readthedocs.io/en/latest/_modules/ikpy/inverse_kinematics.html#inverse_kinematic_optimization)，所以(如果需要)應該可以這樣傳入需要的參數

def inverse_kinematic_optimization(chain, target_frame, 
    starting_nodes_angles, regularization_parameter=None, 
    max_iter=None, orientation_mode=None, 
    no_position=False):

## 3. Orientation
以下的code就可以達到指定手指方向的效果。
* `orientation_mode`必須是`Z`(因為URDF我是這樣寫)
* `target_position`就可以指定手指要指向哪裡

ikResults = armChain.inverse_kinematics(
    target_position=targetPosition, 
    target_orientation=[0,0,-1],
    orientation_mode="Z",
    initial_position=psValue)

## 4. 只可惜...
> 實驗設計： 使用 `3. Orientation` 的code，目標放在<0.6, 0.1, 0>，變換不同的模擬速度，i.e. `timestep = int(XXX*supervisor.getBasicTimeStep())` 變換XXX為1, 2, 4, 8, 16...
1. 使用上面的code，在不同的模擬速度下，產生不同的結果；
以及，可以看出IK"收斂"的過程&結果相當不穩
[Google Drive影片&圖片資料夾](https://drive.google.com/drive/folders/1dzCOYk1ur7Op-Tiyfq957qiuhnTr7mvj?usp=sharing)

2. 這些影片是，從模擬開始就使用上面的code，但如果先使用沒有指定orientation的IK，或者，**使用7倍BasicTimeStep**，會直接卡在這個位置:

3. 原本以為是IKpy的orientation功能在穩定版沒有開發完全，因為在jupyter印出來的說明有提到尚未支援的部分
後來發現是搞錯了，前面的code並沒有使用到尚未支援的部分。
這可能就代表沒辦法直接套用IKpy提供的orientation方法。

## 5. 可能的解法
下面是用IKpy提供的函式畫出來的圖。
藍色是手臂的初始姿勢，
紅點是前面提到<0.6, 0.1, 0>的位置(*webots裡的手臂有旋轉過，手臂坐標系的<0.6, 0.1, 0>是世界坐標系的<0.6, 0, 0.1>*)，
橘色是手臂試圖維持手指朝向下，同時移動到目標點，最後算出的姿勢。