Python を使って、ロボットの足、ロボットアームなどのリンク機構のシミュレーションをしてみます。
固定点や可動点、各アームの長さ、接続する点番号などを設定しておくと、各点の座標を計算し、画像や座標を出力します。
実質的に、Python で記載した、オープンソースのリンク機構シミュレータです。
以下の環境で動作確認をしています。
環境： Windows パソコン、Python 3 （matplotlib, pandas 設定済み）
★ Windows で動作確認をしていますが、Python 3 が動けば、Mac でも Linux でも動くと思います。
背景
Python を使って、Raspberry Pi でプログラミングをすると、サーボモーターなどを簡単に制御することができます。（配線３本をつなぐだけです。）
すると、何か動くモノを試作してみたくなります。
ところが、モーターでできる動作は通常、回転運動程度です。複雑な動きは難しいです。
現実に役に立つ動作、ちょっとした人間の動きに似た動作をさせようとすると、回転運動を、直線運動や自由曲線の運動に変換する必要が出てきます。
ネット検索をしてみると、使えそうな技術としては、テオ・ヤンセン（Theo Jansen）機構などのリンク機構の事例が出てきます。単純な回転運動を二足歩行のような複雑な動きに変換することができます。
リンク機構の計算さえできれば、単純な動きからであっても、自由曲線のような複雑な動きを積極的に設計し、現実に実現できる可能性が出てくる、ということになります。
そこで、リンク機構を計算する、汎用な Python のスクリプトをまとめ、公開しておくことにします。
回転運動、直線運動、自由曲線運動などを、手軽に、自由に変換できるようになると、歩行動作以外であっても、モノをつかむ、精密な手作業をする、ほうきで掃除をする、ハサミなどで加工する、などといった複雑な動作を機構設計で実現できる可能性が出てきます。精密な／繊細な動きをするロボットアームなどが実現できる可能性があります。
なお今回は、指定した座標等から演繹的に、最終的な点の座標・軌跡を求めるところまでをまとめておくことにします。
逆に、欲しい軌跡を与えたとき、パラメータを振ってアームの長さなどを最適化するという機能を入れ込みたいところですが、説明やプログラムが冗長になってしまうため、別途、検討することにします。（簡単にプログラムを作りましたが、人に見せられる状態ではないため省略します。）
設定手順
① 以下を例に、パソコンに機構設計用のフォルダを作成します。
例： c:\user\linkage_simulator\
② さらに、①のフォルダの中に “model01” というフォルダを作成してください。
例： c:\user\linkage_simulator\model01\
③ ①のフォルダ内に linkage_simulator1.py 等の名前でテキストファイルを作成し、下記のサンプルスクリプトを貼りつけて保存してください。
実行方法
④ ②のフォルダの中に、テキストファイルを１つ作成し、リンク情報を記載した設計データを記載してください。
最初に動かす際は、下記の設計例 linkage01.txt、または linkage02.txt の内容を貼りつけてみてください。
⑤ コマンドプロンプト（または、Anaconda Prompt）を起動し、③のスクリプトを実行してください。
例： > python c:\user\linkage_simulator\linkage_simulator1.py
→ ②のフォルダ内 “model01” 内に、シミュレーション結果が出力されたら成功です！
★ 設計ファイルで設定した、各ステップ（時刻）での各点の位置が画像データで出力されます。
（step001.png, step002.png, …）
★ 各点の数値データ（座標）が HTML ファイルで出力されます。（linkage1.html）
★ ”model01″ フォルダ内で、最初に見つかったテキストファイル（１つ）に対し、計算を実行します。
うまく動いたら
・ うまく動いたら、設計データの数値（アームの寸法など、設計例の数値）を少し変えて、動かしてみてください。
設計例１のアームの接続データ、寸法データは、よく知られているテオ・ヤンセン機構に基づいて、設定してあります。
・ 設計例２はロボットハンドの把持部の例としています。
・ リンク機構の設計にあたり、独特な制約条件（癖）があります。そこで、いくらかパラメータを変えて、まずは動かしてみてください。
アームの長さを極端に大きく変えてしまうと、なかなか成立する条件を見つけにくくなることがあります。
具体的には、各アームのそれぞれが、互いに接続可能な寸法の関係になっていることが必要です。
たとえば、アームで接続されている三角形や四角形の一辺を、つなげられないほどの長い距離で設定してしまうと、正常な計算ができません。
また、可動部・回転部が動いた場合であっても、つねにアームが接続可能な長さの関係になっている必要があります。
テオ・ヤンセン機構についての説明
上の図で、P0 は固定点です。P1 が、モーターなどによる回転運動です。
回転運動 P1 を与えると最終的に、歩行時の足のような動作 P6 が得られる（タイミング／位相を変えた足を複数準備すれば、歩行が可能になる）、ということです。
各点の座標を決めるにあたり、まず最初に２点の座標を決めないと、３点目の座標が決まらないことになります。そこで、スクリプトでは、固定点 P0 と回転運動に対応した P1 の座標を生成しています。
２点の座標が決まると、クランクやてこなどのリンク機構を用いることで、第３点目を決めることができます。具体的には、２つの点の座標と、その２点につなぐアームの長さから、第３点めの座標と動きを設定することができます。
図では、P0 と P1 の座標と、それぞれに接続したアームの長さから、P2 を設定しています。また、P3 も P0 と P1 から設定しています。
すでに設定した点（P0 ～ Pn）のうち、任意の２つの点と、それらにつなぐ２つのアームの長さから、新たな点（P3, P4, P5, P6, …）を順次、設定していくことができます。
出来上がったリンク構造で、P6 が、歩行動作をさせるときの足の動きになっています。最終的に欲しい動きをさせる点です。
テオ・ヤンセン機構では、最終的な軌跡（P6）の下の部分がほぼ直線運動となり、かつ、P1 の回転の大半がその直線運動に対応する（地面に足がついている）よう調整してあります。
設計ファイル（リンク機構）の記載方法
設計ファイル（例：linkage01.txt, linkage02.txt）について説明します。
・ パラメータ設定はすべてカンマ区切りとしています。各行ごとに、パラメータ設定、リンク要素の追加をしていきます。
・ １行目の param1, … とした部分は、シミュレーションでのパラメータ設定の行です。必須の行です。
左から順に、表示する画像の総枚数 N1 、画像に表示する数値の増分（005/360などの表示で、分子を5ずつ増加させる）、画像に表示する数字（005/360など）の座標位置 X, Y、となっています。詳細は、動かして数値を変えてみるとわかるかと思います。
・ 以後、add_xxx としている行で、リンク機構の構成要素を追加（定義）しています。
テキストファイル内で最初に add_xxx となっている行で、最初の点 P0 （ジョイント）を定義しています。
以後、add_xxx の記載に応じて、P1, P2, … を定義し、リンクの結節点 Pn の番号 n が決まります。
・ add_fixed_point1 と記載すると、固定点が定義されます。
例えば、”add_fixed_point1, 0, 0″ と記載すると、固定点（P0 など）を (x, y) = (0, 0) となる座標位置に定義する、という意味です。
・ add_rotating_point1 とすると、円を描く点（ジョイントの座標）が定義されます。画像の総枚数 N1 で定義した各画像ごとに、円上の座標を生成します。
例えば、add_rotating_point1, 39, 8, 15, 0, 5 とすると、(x, y) = (39, 8) を中心とし、半径 R1 = 15 となる円を描くという意味です。また、続く２つの数値 0, 5 は回転角について設定しています。それぞれ、初期時の回転角（画面右向きを０度にとったとき、最初の回転角を0度とする）、画像ごとの回転角（画像ごとに5 度ずつ反時計回りに角度を進める）といった意味です。
・ リンク機構の計算をするにあたり、最初に２点の座標と、それら２点からの距離がわかっていないと、３点目以降の座標を決めることができません。
そこで、設計データの冒頭の１行は、固定点を定義するようにします。加えて、冒頭の行を複数使って、固定点（add_fixed_point1）、あるいは回転要素（add_rotationg_point1）を定義するようにします。冒頭の２行以上を使って、固定点や回転要素を設定するようにします。
つまり、P0 はかならず固定点であって、P1 は、固定点か回転要素のいずれか、ということになります。P2 以降は、自由に定義できます（固定点であっても、回転要素であっても、一般のリンク要素であっても構いません）。
・ つぎに、add_linkage1 とした行で、リンク要素を追加していきます。
例えば、add_linkage1, 0, 1, 42, 50 とすると、すでに定義されているリンク要素 P0, P1 から、それぞれ、42, 50 となる距離に、つぎの点（P2 など）を設定する、という意味です。
リンク要素の定義を改めて書くとつぎのようになります。
[a] add_linkage1 以降に並ぶ 4つの数値のうち、最初の２つの要素は、リンクさせる２点の点番号を指定しています。設計例１の “add_linkage1, 0, 1, 42, 50” の事例では、「P2 は、P0 と P1 に接続されている」という意味です。
[b] 後半の２つの要素は、[a] で指定した２つの点（P0, P1）から新たに設定する点（P2）までの距離です。具体的には、「P2-P0 間の距離は 42、P2-P1 間の距離は 50」という意味です。
[c] また、最初の２つの要素の並び順について、すでに定義されている２点と、新たに定義する点(P2)は、反時計回りとなる順番となるよう記載します。具体的には、冒頭の図で、P0 → P1 → P2 の並びで三角形をたどると、反時計回りとなります。そこで、[0, 1, … ] という並びで P2 の点を定義します。新たな点の位置が逆側になってしまった場合は、[0, 1, …] などとなっている数値を [1, 0, …] などとなるように変更してください。
最初の２点があったとき、リンクの長さの情報だけからは、新たに追加する３点目を、２点のなす線分のどちら側に設定してよいのか特定することができません。解が２つ存在することになってしまいます。そこで、最初に指定する２点と新たな１点は、左回りの並び（数学の回転座標Θの取り方と同じ）となるよう、最初の２点の並びを設定する、と決めておきます。これにより、３点目をどちら側に設定するのか、数学的に確定するようにしています。
第４点目の点 P3 についても同様です。具体的には、「P3 は P1 と P0 に接続」されており、「P3-P1 の距離は 62」、「P3-P0 の距離は 40」ということです。
点の並びについては、P2 と P3 はともに、P0、P1 に接続されています。しかし、線分 P0-P1 に対し、P2 は P3 とは逆側に接続したいです。すると、P1 → P0 → P3 とたどったとき反時計回りとなるように設定すればよいので、[1, 0, … ] という並びで P3 を定義すればよい、ということになります。
以下、同様に、P4, P5, P6 を追加していけば、リンク機構を設計することが可能です。
また、P4, P5, P6 に対応する要素を削除すると、３点だけの簡単な機構になります。また、P6 以降に続けて、要素を追加していくと、より複雑な機構であっても設計できます。
ただし、リンクの長さや位置関係は、P1 が回転運動をしたとき、どのような状態であっても、常に成り立つよう／矛盾がないよう、設定する必要があります。
・ 上記の記載ルールに沿ってモデルを作成・構築していくことで、任意のリンク機構についてシミュレーションが可能です。
・ また、add_rotating_point1 を複数行定義することで、回転モーターが複数あるような場合でも、回転方向が逆などの場合でも、定義することができます。
スクリプトの説明
・ 冒頭の import math, matplotlib.pyplot, os, pandas …としたところで、必要なパッケージを読み込んでいます。
・ def read1( … )、… def write_html1( … ) とした行は、テキストファイルを読み書きする汎用関数です。Python の基本的なスクリプトについては、このサイトで説明していますので、参照してみてください。
・ def calc_xxx とした以降で、各計算を行う関数を定義しています。
具体的には、各点の座標 [x, y] を引数で与えたとき、ベクトルを求める、ベクトルの長さを求める、単位ベクトルを求める、係数倍する、直交ベクトルを求める、などです。
・ def calc_triangle1( … ) としたところで、与えられた２点の座標とリンクの長さから、第３点めの座標を求めています。
・ def calc_circle1( … ) は、最初の２点を決めるときに回転運動を作りたいので、中心点と半径から、円の座標を計算しています。
・ def draw_xxx としたところは、matplotlib を用いた描画関連の関数です。
軌跡の描画と、リンクのアームの描画、各点 P0, P1, … の描画、文字の表示などを行っています。
・ def add_fixed_point1( … )、add_rotationg_point1( … ) としたところで、最初の固定点、回転部分の座標を生成しています。
・ def add_linkage1( … ) としたところで、以降のリンク要素を追加しています。具体的には、すでに定義されているリンクデータ P0_list から２点を参照し、設定されている２本のアームの長さから、第３点目にあたる座標を求めています。
・ もしこの後、最適化計算のアルゴを追加したい場合は、計算結果は P0_list に入っていますので、この出力値と、目標とする経路の座標との差分を評価し、相違が最小化するよう初期パラメータを振ってやればよい、ということになります。（しかしながら、記載が冗長化するので、ここでは詳細は省略します。）
まとめ
リンク機構の動作をシミュレーションする Python のスクリプトについてまとめました。
簡単に、テオ・ヤンセン機構のようなリンク機構をシミュレーションできます。
また、モーターなどの駆動系があったとき、どのような機構・アームを連結するとどう動くのか、設計することが可能です。
回転運動を変換するスクリプトとしていますが、回転運動以外にも、任意の関数を追加していけば、直線運動を自由曲線の運動に変えるなど、自由なカスタマイズも可能です。
※ 気軽な気持ちで上記のテオ・ヤンセン機構の gif 画像を作ったのですが、なぜかずっと見てしまうんですよね。どう表現したらよいのでしょうか、この動きは。。
関連リンク
・ サーボモーターを動かす 【Raspberry Pi】
・ Python で自由曲線を描く 【HTML & SVG】
外部リンク
・ 「めっちゃ、メカメカ！リンク機構99→∞ー機構アイデア発想のネタ帳」（参考書）
・ https://theojansen.net/
設計例
設計例１ linkage01.txt： テオ・ヤンセンのリンク機構
param1 , 73, 5, -65, 30
add_fixed_point1 , 0, 0
add_rotating_point1 , 39, 8, 15, 0, 5
add_linkage1 , 0, 1, 42, 50
add_linkage1 , 1, 0, 62, 40
add_linkage1 , 0, 2, 40, 56
add_linkage1 , 3, 4, 37, 40
add_linkage1 , 3, 5, 49, 66設計例２ linkage02.txt： ロボットの把持部
param1 , 25, 15, 0, -5
add_fixed_point1 , 0, 0
add_fixed_point1 , 40, 0
add_rotating_point1 , 20,-20, 5, 90, 15
add_linkage1 , 0, 2, 20, 20
add_linkage1 , 0, 3, 40.3, 40.0
add_linkage1 , 3, 1, 40.0, 40.3
サンプルスクリプト linkage_simulator1.py
import math as m1
import matplotlib.pyplot as plt1
import os
import pandas as pd1
import glob
def read1( file1 ):
with open( file1, 'r', encoding='utf-8' ) as f1:
str1 = f1.read()
return str1
def get_a1( file1 ):
a1 = read1( file1 ).strip().replace( "\n\n", "\n" ).replace( " ", "" ).split( "\n" )
for i1 in range( len( a1 ) ):
a1[i1] = a1[i1].split( "," )
return a1
def write_html1( file1, a0, a1 ):
df1 = pd1.DataFrame( a1 )
if len( a0 ) == len( a1[0] ):
df1.columns= a0
df1.to_html( file1, escape=False )
def calc_vector1( A1, B1 ):
return [ B1[0]-A1[0], B1[1]-A1[1] ]
def calc_len1( A1 ):
return m1.sqrt( A1[0]**2 + A1[1]**2 )
def calc_unit1( A1 ):
len1 = calc_len1( A1 )
return [ A1[0]/len1, A1[1]/len1 ]
def calc_multi1( C1, L1 ):
return [ L1*C1[0], L1*C1[1] ]
def calc_orthogonal1( A1 ):
return [ -A1[1], A1[0] ]
def calc_triangle1( A1, B1, AC1, BC1 ):
AB1 = calc_len1( calc_vector1( A1, B1 ) )
AD1 = (AB1**2 + AC1**2 - BC1**2)/(2.0*AB1)
v1 = AC1**2 - AD1**2
if v1 >= 0:
DC1 = m1.sqrt( AC1**2 - AD1**2 )
AD1_vec = calc_multi1( calc_unit1( calc_vector1( A1, B1 ) ), AD1 )
DC1_vec = calc_multi1( calc_unit1( calc_orthogonal1( calc_vector1( A1, B1 ) ) ), DC1 )
C1 = [ A1[0] + AD1_vec[0] + DC1_vec[0], A1[1] + AD1_vec[1] + DC1_vec[1] ]
else:
print( "error: " + str(AC1) + " " + str(BC1) )
C1 = [ 0, 0 ]
return C1
def calc_circle1( A1, R1, Theta1 ):
return [ A1[0] + R1*m1.cos( Theta1 ), A1[1] + R1*m1.sin( Theta1 ) ]
def color1( n0 ):
color0 = ['#ffaaaa', '#ffcc88', '#eeee66', '#99ee99', '#aaaaff', '#6666cc', '#ee77ee' ]
n1 = n0 % len( color0 )
return color0[ n1 ]
def draw_track1( plot1, P0 ):
for i1 in range( len( P0[0] ) ):
P1 = []
for i2 in range( len( P0 ) ):
P1.append( P0[i2][i1] )
draw_track0( plot1, P1, color1(i1-1) )
def draw_track0( plot1, P0, color0 ):
P1 = [[], []]
for i1 in range( len( P0 ) ):
P1[0].append( P0[i1][0] )
P1[1].append( P0[i1][1] )
plot1.plot( P1[0], P1[1], color=color0, linewidth=1 )
def draw_link1( plot1, P1, link0 ):
draw_link0( plot1, P1, link0 )
draw_dot0( plot1, P1 )
def draw_link0( plot1, P1, link0 ):
n1 = len( P1 ) - len( link0 )
for i1 in range( len( link0 ) ):
P2 = P1[ n1+i1 ]
P3 = P1[ link0[0+i1][0] ]
P4 = P1[ link0[0+i1][1] ]
plot1.plot( [ P2[0], P3[0] ], [ P2[1], P3[1] ], color=color1(i1+n1-1), linewidth=4 )
plot1.plot( [ P2[0], P4[0] ], [ P2[1], P4[1] ], color=color1(i1+n1-1), linewidth=4 )
plot1.plot( P2[0], P2[1], marker='o' )
plot1.plot( P3[0], P3[1], marker='o' )
plot1.plot( P4[0], P4[1], marker='o' )
def draw_dot0( plot1, P1 ):
for i1 in range( len( P1 ) ):
P2 = P1[i1]
plot1.text( P2[0]+3, P2[1]+2, "P" + str(i1) )
def add_fixed_point1( P0_list, P0 ):
P1_list = P0_list
for i1 in range( len( P1_list ) ):
if P1_list[i1][0] == None:
P1_list[i1] = [ P0 ]
else:
P1_list[i1].append( P0 )
return P1_list
def add_rotating_point1( P0_list, C1, R1, Theta0, Theta0_step ):
P1_list = P0_list
for i1 in range( len( P1_list ) ):
Theta1 = m1.pi/180.0 * ( Theta0 + Theta0_step *i1 )
P1_list[i1].append( calc_circle1(C1, R1, Theta1) )
return P1_list
def add_linkage1( P0_list, linkage1 ):
P1_list = P0_list
for i1 in range( len( P1_list ) ):
C1 = calc_triangle1( P1_list[i1][ linkage1[0] ], P1_list[i1][ linkage1[1] ], linkage1[2], linkage1[3] )
P1_list[i1].append( C1 )
return P1_list
def generate_images1( path1, plot1, P0_list, linkage1, param1 ):
plot1.set_aspect('equal')
n1 = len( P0_list )
theta1_step = param1[1]
x1 = param1[2]
y1 = param1[3]
for i1 in range( n1 ):
plt1.cla()
draw_track1( plot1, P0_list )
draw_link1( plot1, P0_list[i1], linkage1 )
plot1.text( x1, y1, str(i1*theta1_step).zfill(3) + "/" + str((n1-1)*theta1_step).zfill(3) )
file1 = path1 + "step" + str(i1).zfill(3) + ".png"
plt1.savefig( file1 )
def generate_linkage1( file1 ):
a1 = get_a1( file1 )
param1 = [-1, 1, 0, 100]
P0_list = []
linkage1 = []
for i1 in range( len( a1 ) ):
str1 = a1[i1][0]
if str1 == "param1":
param1[0] = int( a1[i1][1] )
param1[1] = int( a1[i1][2] )
param1[2] = int( a1[i1][3] )
param1[3] = int( a1[i1][4] )
break
if param1[0] > 0:
P0_list = [[None]]*param1[0]
for i1 in range( len( a1 ) ):
str1 = a1[i1][0]
if str1 == "add_fixed_point1":
P0 = [ float( a1[i1][1] ), float( a1[i1][2] ) ]
P0_list = add_fixed_point1( P0_list, P0 )
elif str1 == "add_rotating_point1":
C1 = [ float( a1[i1][1] ), float( a1[i1][2] ) ]
R1 = float( a1[i1][3] )
theta1 = float( a1[i1][4] )
theta1_step = float( a1[i1][5] )
P0_list = add_rotating_point1( P0_list, C1, R1, theta1, theta1_step ) # P1
for i1 in range( len( a1 ) ):
str1 = a1[i1][0]
if str1 == "add_linkage1":
linkage2 = [ int( a1[i1][1] ), int( a1[i1][2] ), float( a1[i1][3] ), float( a1[i1][4] ) ]
add_linkage1( P0_list, linkage2 )
linkage1.append( linkage2 )
return param1, P0_list, linkage1
path1 = os.path.dirname(__file__) + "/model01/"
list1 = glob.glob( path1 + "*.txt" )
file1 = list1[0]
param1, P0_list, linkage1 = generate_linkage1( file1 )
file1 = path1 + "linkage1.html"
write_html1( file1, ["P0", "P1", "P2", "P3", "P4", "P5", "P6" ], P0_list )
plot1 = plt1.subplot()
generate_images1( path1, plot1, P0_list, linkage1, param1 )
