2軸ロボットアームの衝突検知を１からしてみた話

初めに

これは以前にキータでまとめたものの総集編です．
コメントは入れていますが以下の二つの記事に詳細は述べています．
順運動学
2Dの2直線交差判定
以上の二つをまとめたプログラムが下のものとなります．
実際に動かすと以下のようになります．

arm.py

import math
import numpy as np
import matplotlib.pyplot as plt
import copy#値渡しのために使う(deepcopy)
from matplotlib.widgets import Slider

def update(val):
    deg1 = theta1.val
    deg2 = theta2.val
    x1 = L1 * np.cos(math.radians(deg1))
    y1 = L1 * np.sin(math.radians(deg1))
    x2 = x1 + L2 * np.cos(math.radians(deg1+deg2))
    y2 = y1 + L2 * np.sin(math.radians(deg1+deg2))
    x = [0.0, x1, x2]
    y = [0.0, y1, y2]
    col = False
    #以下衝突検知のアルゴリズム
    #アームの数だけforループを回す
    for i in range(len(x)-1):
        a = coordinate2d()
        #[i]始点 [i+1]終点
        a.xs = x[i]
        a.ys = y[i]
        a.xd = x[i+1]
        a.yd = y[i+1]            
        #障害物の数だけforループを回す
        for j in objects:
            #衝突検知するとcol=trueとなる
            col = collision(a,j)
            if col == True:
                break
        if col == True:
            break
    if col:
        plots.set_color("green")
        cfg_ax.plot(math.radians(deg1),math.radians(deg2),marker='.',markersize=m_size,color="r")
    else:
        plots.set_color("red")
        cfg_ax.plot(math.radians(deg1),math.radians(deg2),marker='.',markersize=m_size,color="b")

    ax.axis('equal')
    ax.set_xlim(-11,11)
    ax.set_ylim(-11,11)

    cfg_ax.axis('equal')
    cfg_ax.set_xlim(-math.pi-1,math.pi+1)
    cfg_ax.set_ylim(-math.pi-1,math.pi+1)

    #データを更新する
    plots.set_data(x,y)
    #cfg_plots.set_data(math.radians(deg1),math.radians(deg2))

    fig.canvas.draw_idle()

class coordinate2d:
    #座標を扱うためのクラス
    def __init__(self):
        self.xs = 0.0
        self.ys = 0.0
        self.xd = 0.0
        self.yd = 0.0
    def get(self):
        return[[self.xs,self.ys],[self.xd,self.yd]]
    def print(self):
        print('{:.5g}'.format(self.xs)," ",'{:.5g}'.format(self.ys)," ",'{:.5g}'.format(self.xd)," ",'{:.5g}'.format(self.yd),end="")

def collision(_a,_b):
    a = copy.deepcopy(_a)
    b = copy.deepcopy(_b)
    aa = copy.deepcopy(_a)
    bb = copy.deepcopy(_b)
    #点aの始点を原点に移動
    a.xd -= _a.xs
    a.yd -= _a.ys
    b.xs -= _a.xs
    b.ys -= _a.ys
    b.xd -= _a.xs
    b.yd -= _a.ys
    #a-bとa-c,a-dの外積 2次元ベクトルの場合外積は1次元である
    temp1 = a.xd*b.ys - a.yd*b.xs
    temp2 = a.xd*b.yd - a.yd*b.xd

    #点cの始点を原点に移動
    aa.xs -= _b.xs
    aa.ys -= _b.ys
    aa.xd -= _b.xs
    aa.yd -= _b.ys
    bb.xd -= _b.xs
    bb.yd -= _b.ys
    #c-dとc-b,c-dの外積を求める
    temp3 = bb.xd*aa.ys - bb.yd*aa.xs
    temp4 = bb.xd*aa.yd - bb.yd*aa.xd

    #外積二つの外積が同符号なら無干渉,異符号なら干渉
    #temp1と2，temp3と4それぞれが異符号の時衝突
    if temp1*temp2 <= 0.0 and temp3*temp4 <= 0.0:
        return True
    else:
        return False
#figureとaxesの初期化
fig, (ax,cfg_ax) = plt.subplots(1,2,figsize=(8,4))
#スライダーとグラフがかぶるので調整
plt.subplots_adjust(left=0.25, bottom=0.25)

#アスペクト比とスケールの指定
ax.axis('equal')
ax.set_xlim(-10,10)
ax.set_ylim(-10,10)

cfg_ax.axis('equal')
cfg_ax.set_xlim(-math.pi,math.pi)
cfg_ax.set_ylim(-math.pi,math.pi)

#Linkの長さ 
L1 = 5.0
L2 = 5.0

#各リンクの角度指定
deg1 = 30.0
deg2 = 50.0

#コンフィグレーションスペースのプロットのサイズ
m_size = 5

#障害物の設定
object1 = coordinate2d()
object1.xs = 2.0
object1.ys = 2.0
object1.xd = 4.0
object1.yd = 2.0 
objects = np.array([])
objects = np.append(objects,object1)

#描画の為障害物の情報をリストに入れておく
ox = []
oy = []
for i in objects:
    ox.append(i.xs)
    ox.append(i.xd)
    oy.append(i.ys)
    oy.append(i.yd)

#matplotlibのスライダー
theta1 = Slider(plt.axes([0.25,0.0,0.65,0.03]), 'theta1', -180, 180, valinit=deg1)
theta2 = Slider(plt.axes([0.25,0.05,0.65,0.03]), 'theta2', -180, 180, valinit=deg2)

#リンク１の座標算出
x1 = L1 * np.cos(math.radians(deg1))
y1 = L1 * np.sin(math.radians(deg1))

#リンク2の座標算出
x2 = x1 + L2 * np.cos(math.radians(deg1+deg2))
y2 = y1 + L2 * np.sin(math.radians(deg1+deg2))

#算出結果を格納
#リンクは[x,y]=[0,0]から表示するため最初の要素に0を代入
x = [0.0, x1, x2]
y = [0.0, y1, y2]

#表示
#線分表示↓
plots, = ax.plot(x,y,"r-")
#干渉部分とそれ以外で色変えたいので初めの干渉判定までは塗らない
#cfg_plots, = cfg_ax.plot(math.radians(deg1),math.radians(deg2),marker='.',markersize=m_size,color="b")
ax.plot(ox,oy,"g-")
#スライダーの値が変わったらupdate関数を起動する
theta1.on_changed(update)
theta2.on_changed(update)

#表示実行
plt.show()