テストおわった。
逆キネで計算した姿勢がどうなっているか確かめるため、Inventorでダミー君を作った↓
S_2

角度拘束をエクセルでいじれるようになっている。