手乗りサイズ頭の作成（コントローラ入力から眼球１つの動作まで）

最近は手乗りサイズのドールヘッドに動く瞳を搭載する試みを進めています。

ガラスアイなどをサーボモータで動かすのもまた面白いですね。ただし一方で機構が小型で複雑。こちらはもう少し技術力高めてから挑みます。

今回は小型ディスプレイを2枚埋め込んでの実現を試みます。

振り返り含めて、つらつらとコードなどなど書いていきますので、新規に触れる人がいてもこれ読めば比較的簡単に実装できるのではないかなぁと。

コントローラのアナログスティック入力について

今回はPS4コントローラで手首の回転を制御しようとした時に、どんな方法があるのかを考えてみました。
（おそらく車輪の再発明。ゲーム界隈ならもっと進んでいて便利な手法あるんでしょうね。ゲームのRTAとかでよく見かける√2走法なんかにも関わる内容ですから、きっと深い歴史があるはずです）
先に結論を簡潔にいうと、アナログ値2種からスティックの回転軸＆回転角を取得する方法2つを挙げています。
あと座標系とか回転とかは右手系でとってます。

まずアナログスティックとは、2つの回転角を取得するセンサを直交配置させているものを想定しています。
十字キーの派生版のような外見はスティックだけどスイッチ4つ付いただけのものではなく、2軸のアナログ値を一度に得られる入力装置です。
秋月でも取り扱っているRKJXV122400Rの写真とか見るとわかりやすいですね。勝手に写真貼っていいのかよく分からないので各自お調べください…

で、上下方向（ピッチ軸回転）に±100[%]、左右方向（ロール軸回転）に＋100[%]なアナログ値を取得できると考えましょう。
使用するライブラリによって諸々の具合は違うかもしれませんが、今回の記事ではそう考えておきます。

アナログスティックの定義

こんな感じでしょうか。
なにもしていないときが青色状態で、スティックを倒したときが赤色状態です。
この赤色状態のときに得られる2つのアナログ値をそれぞれSx[%]とSy[%]と置いてみます。
変数ってなんか暗黙の命名ルールあるらしいですよね。しらんけど。

長々とした前提はこれくらいにして、こうして得られたアナログ値2種からスティックの位置ベクトルCinを取得したいという目的があります。
なぜなら位置ベクトルが得られるということは、スティックが回転した際の回転軸ベクトルと回転量を取得できるということです。
つまりは回転動作の指示に使えるわけですね。具体例は後述。

ではベクトルを得るにはこのアナログ値2種をどうしたらよいか。
A)軸方向距離の変数として捉え、2次元ベクトルとして考える
B)軸中心回転角の変数として捉え、3次元ベクトルとして考える

2次元ベクトルで考えてみる

A)
xy平面があるとして、2値はそれぞれ±100[%]の範囲で動くわけですから正方形の範囲にプロットされることになります。
こうしてまずは

現在のスティック位置ベクトル：Cin=(Sx,Sy)

を取得できました。
次に、初期姿勢からCinへの移動はどのように記述できるのか、
言い換えると、回転軸と回転量はどう考えればよいのかを考えます。

まずは回転軸ですが、Cinに+90[deg]の回転を施せば得られます。
回転軸：λ=Rot(π/2)Cin=(-Sy,Sx)
これはCinが得られるときのスティックの倒れ込みを考えると分かりやすいです。
仮にatan(Sy/Sx)=30[deg]となるようなCinを考えたとき、λはatan(λy/λx)=120[deg]となるでしょう。
（もっと深掘りすると、そもそもCinは3次元ベクトルであると考え直してみて初期姿勢とCinの外積をとってみればよいです。結果として回転軸はxy平面状にしか存在できないので無駄に記述が長ったらしくなるだけと考えて割愛）

また回転量Ω、つまりはスティックの倒し込み量について考えてみると、これはCinの大きさと捉えるとよいでしょう。
回転量：Ω=√(Sx^2+Sy^2)
しかしここで困ったことが起こります。
物理的な入力角と、計算値の回転角が一致しません。
具体的にいうと、Cin_a=(100,0）とCin_b=（100,100）における回転角は同じ値であって欲しいのに、Ωa=100[%]、Ωb=141[%]（←√2*100）となってしまいます。
こうしたズレはセンサで得られる値がそもそも機構的には回転であることに起因しています。

この手法はズレがある、つまりは倒し込む向きや量によって分解能が不均一で、直感的な操作からかけ離れる懸念がありますが、なによりも計算が簡単に済むという莫大なメリットがありますね。

3次元ベクトルで考えてみる

B)
ドーム状の表面をなぞる様に移動する、つまりは大きさが一定の3次元ベクトルとして考えて見ます。
ここでは計算の簡易化のため単位ベクトルとして考えていきます。
Sxはx軸中心の回転度合い、Syはy軸中心の回転度合いと捉えます。
ではSxとSyを用いてCinをどのように記述するのか。
まずスティック可動域を±π/2[rad]として
θv[rad]=π/2*Sx[%]
θh[rad]=π/2*Sy[%]
とします。次に
①初期姿勢からy軸回りにθ1だけ回転して、更にx軸回りにθvだけ回転した位置ベクトルCv
②初期姿勢からx軸回りにθ2だけ回転して、更にy軸回りにθhだけ回転した位置ベクトルCh
の2式からなる連立方程式を解けばCinは求められます。

回転軸λは外積から、回転量Ωは内積から導出できます。

長ったらしい計算式

(A)と(B)の手法の比較

(A)2次元ベクトルとして捉えたときの表

(B)3次元ベクトルとして捉えたときの表

※追記（22/5/3）
(A)および(A')の表がぜんっぜん正しくないことに気づきました。正負の方向とり方とか。しかし不誠実ながら(B)採用したいのが前提の記事なのでそのまんまにしておきます。

申し訳程度に追加したすんごい雑な修正概要図

この2つの図は自分が分かれば良いやくらいに雑な表なんですが、
Sx=-33[%]、Sy=50[%]という共通の入力条件としたときに
(A)Ω=54.08[deg](5/3修正)
(B)Ω=49.11[deg]
といった具合に回転量が変わります。
特に注目すべきは(B)の表はどの値も90[deg]を越えない点ですね。

また、(A)であっても少し手を加えて「絶対に90[deg]を超えない」という目的のために一括で√2で割る（イメージ的には正方形の可動域に内接する円）ことで

(A’)2次元ベクトルとして捉えてかつ90°を超えない処理をしたときの表

このようなことも出来はします。
ちなみに(A')Ω=38.24[deg](5/3修正)

【結論】
まだ手首を動かすプログラムすら組んでない状況ですがBで進めたいなーと思いました。
以上。

手首を作る 途中経過（機構、外装、制御）

最近はやること多くて少し体力保たないんですがここで書いとかないと今後やらなくなりそうなので覚書程度につらつらと。

まずは機構を考えてみて、それの収まる最小サイズを検討してみました。今回使った歯車はm0.5で、減速比は3ですね。もっとコンパクトな機構となるとワイヤなどになるんでしょうかね。ワイヤ端子の圧着とか大変そう 前回の記事の減速比だとか回転行列だとか色々間違ってますね。気が向いたら直します。 手首の球がφ40なら成立しそうだったので手元にあるドールさんを参考に比率を考えてみて粘土で形を整えます。 Zbrushも買ったんですが専らデータ形式の変換とか後述のスキャン時のゴミ処理とかにしか使えてないです。PC上で形状まで作れたら作業短縮できそうだなぁとは思ってますが、今のところ粘土を手でこねてスキャナでCADに取り込むやり方で安定してます。粘土触るのも楽しいですし。今使ってる粘土は大学生の頃から使ってるインダストリアルクレイってやつですね。45度まで温めると柔らかくなるやつ。 サーボと制御基板rcb4-miniに火入をしてさぁ動かすぞってことでHTH4でソフトを完結させたかったんですが、なかなか難しそうと判明。 そもそも近藤サーボは出力される姿勢角がHTH4経由だと直接触ることができず（詳しく書くとmix1とmix2の値は触れたり、モーション再生などはできる）なかなか面倒だったのでraspberry piとrcb4-miniをUSBで繋げてpythonで動かすことに。 公式を覗けばすごーく丁寧にraspberry piとrcb4の繋ぎ方からpythonライブラリまで誘導あるので読めばわかります。 詳しく読まずにメールで聞くという悪行をしてしまったにもかかわらず優しく丁寧に対応してくれた近藤科学株式会社のサービス部は神。 バックラッシュがひどくて稼働範囲はなかなか狭いんですが、満足のいく手となってくれました。

https://twitter.com/ykch__/status/1489968427972558848?s=21

次はPS4あたりのコントローラ入力と任意角周りの回転を実装して、肘の設計に移りたいなぁ

3DoFの機構の設計と制御

入力軸と出力軸の関係（簡易モデル）

関節というものは実にさまざまな種類がありますが、ボールジョイントはかなり自由な姿勢を取れる代表的な関節といえます。フィギュアやプラモでもよく見かけますね。

そもそも3次元空間において剛体のとり得る運動というのは並進3軸＋回転3軸の6DoF（Degree of Freedom）と考えられます。最近盛り上がっているVR界隈でもこの言葉をよく見かけますね。
一般的なボールジョイントは3DoFであり、並進移動はできないけれども直行3軸が1点で交わり、それぞれの軸中心に回転が出来る関節を指しますから、その自由度の高さが伺えます。

さて、ボールジョイントのような受動的で1点交差な3DoFを実現するのは比較的容易なのですが、能動的な3DoFとなるととても難しくなります。

その中でも、産業用ロボットの手首機構などで比較的よく見かけるYPY配置（ヨー・ピッチ・ヨー）の機構はまだ複雑さは穏やかでかつ設計も容易に見えたので、動かしていくために考えたことをまとめておこうとした次第です。

5節リンク機構の1節を伸ばして作用点にするやつ

パラレルリンクはリンク根元にモータ類を置けるのでとても便利です。
ディズニーのアニマトロニクスのアバターの顔面の中の機構と同じですね。口角の一番端に使われてたり。*1
その上5節リンクはこちらで*2丁寧に解説なされているので活用しない手は無いでしょう。

基本的に座標の配置や文字は上記サイトに準じて考えます。

で、本題なのですが、M1X（あるいはXM2）のリンクをX方向に伸ばして作用点とする場合にどうなるのか考えて見ます。下図参照。
要はチェビシェフリンクのようなものです。

こういうやつ

XとMに回転軸を持っていてPで外へ点接触するような1つの剛体を足先リンクとします。M-X軸上にPを置く感じ。実際にはPが球状になっているイメージです。

*1:DISNEY’S NEW ANIMATRONICShttps://www.youtube.com/watch?v=bFU9Qg_6EsY

*2:5 節リンク機構の運動学
Kinematics of 5-Bar-Link Mechanism
小関義彦：産業技術総合研究所人間福祉医工学研究部門
https://staff.aist.go.jp/koseki-y/tutorial/parallel/kine5bar1/kine5bar1.htm

誤差の足し算の考え方

設計には誤差が付きまとうので、いつか誤差に向き合わなければならない

ということで誤差の足し算に統計を導入したやつまとめていきます。

誤差というのは安全率の親戚のようで親戚でない曖昧でフワフワした存在であり、どう扱うかが設計者のセンスな気がします。
とくに小さな会社とかでは口伝だとか経験で覚えろとかそういうことが多い気がします。
他意はないですけど。
ですがそんな誤差も当然ながらきちんとした管理方法も確立されていますのでそれをまとめておこうという方針です。
とりあえず大きめな安全率をとっておこうとか、公差をとりあえず細かくしておこうという逃げはなるべく廃しておきたいですよね。高くなるし。

分散の加法性

KRS3402のやつ② 5節リンクの足のリンク長とか設計途中の色々

前の記事の続きです。

1.0[kg]で1つの金属サーボホーンで受けるとき36.1[mm]ということで落ち着きました。
で、足を作っていくとき五節リンクやるとしたら2つのサーボとなり雑に考えて2つに分散、
つまり荷重は0.5[kg]相当になり72.2[mm]となるでしょう。

次に、曖昧にレイアウト考えていると足のリンクは片持ちにしたいなーとなったので
サーボホーンの周りにスラストベアリングを挟みます。