マイコン x Linux のお勉強
やりたいこと
UbuntuやTX2からマイコンを操作し、いろいろなデバイスを動かしたい
TX2での準備
以下を実行し、ACMを使えるようにする必要がある
Build Kernel and ttyACM Module – NVIDIA Jetson TX2 | JetsonHacks
Arduinoの設定
UbuntuにArduino IDE（1.8.5）インストール | ねこめも を参考に64bitをダウンロード
sudo chmod a+rw /dev/ttyACM0
ビルド -> arduinoに転送
ToDo: /dev/ttyACM0に権限をつけておく方法調査
mbedの設定
OSにあまり関係しない
ブラウザで開発
ビルドでできたバイナリをmbedフォルダに移動
マイコンとの通信
準備
- pip install pyserial
PC側実装
標準入力のコマンドをマイコンに送る
注意！
serialのopen()してからtime.sleep(2.0)のように2秒以上いれないと通信できないことがあった！ Debugの時は念の為長くとっておこう。
python3では文字のエンコードがpython2と異なるため、serial writeする時に<送りたい文字列>.encode()とする必要がある。
#!/usr/bin/env python # coding: utf-8 import serial import time if __name__ == '__main__': print "hello serial!" ser = serial.Serial() ser.port = '/dev/ttyACM0' ser.baudrate = 9600 ser.open() time.sleep(2.0) while(1): position = bytes(raw_input()) ser.write(position) # for Debug read_ser = ser.readline().rstrip() print read_ser
Arduino側実装
#include "enum.h" const char RX_FOOT = ';'; void setup() { // settings Serial.begin(9600); Serial.setTimeout(1500); } void loop() { // シリアル受信 if(Serial.available() > 0) { // 受信しきるまでのバッファ時間 delay(5); // データ受信 String buf = Serial.readStringUntil(RX_FOOT); Serial.println(buf); } delay(1); }
mbed側実装
#include "mbed.h" #include "MODSERIAL.h" #include <list> #include <string> #include <cstdlib> int main() { MODSERIAL pc(USBTX, USBRX, 8192); pc.baud(9600); char data[1100]; int data_index = 0; while(1) { if (!pc.readable()) continue; char c = pc.getc(); data[data_index++] = c; if (c == '\n') { data[data_index] = '\0'; string command = data; pc.puts(command.c_str()); data_index = 0; } }
mbedの遠隔リセット
ROSの基礎（通信編）
やりたいこと
ROSの復習がてら、基礎の部分で理解が浅い部分をまとめる
ピアツーピア設計方針
参考：ROS/Technical Overview - ROS Wiki
「ピアツーピア」ではコンピュータさんの役割が決まっていません。 すべてのコンピュータさんが必要に応じてサーバにもなればクライアントにもなるのです。
参考：http://wa3.i-3-i.info/word1306.html
ピアアドレスの確認
$ rosnode list -a
-> http://localhost:58031/ /rosout
これによってlocalhost:58031というピアアドレスが使用されていることがわかる
XML-RPC（eXtensible Markup Language Remote Procedure Cal）
まぁ「XML-RPC」って単語が出てきたら「XML形式のデータをHTTPでやり取りするんだな～」と、お考えください。
point通信プロトコルだよ
pointXML形式のデータをやり取りするよ
pointHTTPでやり取りするよ
pointPOSTメソッドを使ってやり取りするよ
参考：http://wa3.i-3-i.info/word15419.html
POSTメソッド
まぁ「POSTメソッド」って単語が出てきたら「見えない所に隠してサーバに送るんだな～」と、お考えください。
参考：http://wa3.i-3-i.info/word1496.html
TCP/IP通信のpython実装
client.py
# -*- coding:utf-8 -*- import socket host = "192.168.xxx.xxx" port = 2222 # 適当 client = socket.socket(socket.AF_INET, socket.SOCK_STREAM) #オブジェクトの作成をします client.connect((host, port)) #これでサーバーに接続します client.send("from robonchu") #適当なデータを送信します（届く側にわかるように） response = client.recv(4096) #レシーブは適当な2の累乗にします（大きすぎるとダメ） print response
server.py
# -*- coding:utf-8 -*- import socket host ="192.168.xxx.xxx" port = 2222 # クライアントと揃える serversock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) serversock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) serversock.bind((host,port)) #IPとPORTを指定してバインドします serversock.listen(10) #接続の待ち受けをします（キューの最大数を指定） print 'Waiting for connections...' clientsock, client_address = serversock.accept() #接続されればデータを格納 while True: rcvmsg = clientsock.recv(1024) print 'Received -> %s' % (rcvmsg) if rcvmsg == '': break print 'Type message...' s_msg = raw_input() if s_msg == '': break print 'Wait...' clientsock.sendall(s_msg) #メッセージを返します clientsock.close()
UDP通信のpython実装
参考：Pythonでネットワークプログラミング | saito's memo
TODO
トピックの平均転送速度を求める
$ rostopic bw /turtle1/pose
トピックの配布速度を求める
$ rostopic hz /turtle1/pose
サービスの引数リストの表示
$ rosservice args /turtle1/set_pen
シミュレーターを作ってみる
やりたいこと
簡単な機構のロボットシミュレータをつくってみる
モデル
ls
順運動学を解いてみる
# -*- coding: utf-8 -*- import numpy as np from numpy import sin,cos import matplotlib.pyplot as plt def fk(l1, l2, l3, th1, th2): x2 = l2 * cos(th2) - (l1 + l3) * cos(th1) y2 = l2 * sin(th2) + (l1 + l3) * sin(th1) x1 = -l1 * cos(th1) y1 = l1 * sin(th1) return x1, y1, x2, y2 def fk_frame(l1, l2, l3, l4, l5, l6, l7, th1, th2, x1, y1, x2, y2): x3 = - l4 * cos(th2) - l1 * cos(th1) y3 = - l4 * sin(th2) + l1 * sin(th1) x4 = (l2 + l6) * cos(th2) - (l1 + l3) * cos(th1) - l7 * sin(th2) y4 = (l2 + l6) * sin(th2) + (l1 + l3) * sin(th1) + l7 * cos(th2) x5 = (l2 + l6) * cos(th2) - (l1 + l3) * cos(th1) + l7 * sin(th2) y5 = (l2 + l6) * sin(th2) + (l1 + l3) * sin(th1) - l7 * cos(th2) x_f1 = [0, -l4 * cos(th2), x3, x1] y_f1 = [0, -l4 * sin(th2), y3, y1] x_f2 = [x3, x1 - l5 * sin(th2), x2] y_f2 = [y3, y1 + l5 * cos(th2), y2] x_l = [x2, x4, x5, x2] y_l = [y2, y4, y5, y2] return x_f1, y_f1, x_f2, y_f2, x_l, y_l # グラフの描画 def plot(x, y, x_f1, y_f1, x_f2, y_f2, x_l, y_l): fig = plt.figure() ax = fig.add_subplot(111) # リンクの描画 plt.plot(x, y,"-g",lw=5,label="link") plt.plot(x_f1, y_f1,"-g",lw=5,label="link") plt.plot(x_f2, y_f2,"-g",lw=5,label="link") plt.plot(x_l, y_l,"-g",lw=5,label="link") # 関節の描画 plt.plot(x, y,"or",lw=5, ms=10,label="joint") plt.plot(x_f1, y_f1,"or",lw=5, ms=10,label="joint") plt.plot(x_f2, y_f2,"or",lw=5, ms=10,label="joint") plt.plot(x_l, y_l,"or",lw=5, ms=10,label="joint") plt.xlim(-0.4, 0.4) plt.ylim(-0.1,0.4) plt.grid() plt.show() def main(): # リンクの長さ l1, l2, l3, l4, l5, l6, l7= 0.18, 0.16, 0.0, 0.05, 0.03, 0.07, 0.07 # 第1, 2の関節角度 [th1, th2] = np.radians([90, 0]) # 順運動学の計算 x1, y1, x2, y2 = fk(l1, l2, l3, th1, th2) # ロボットアームのx座標, y座標をリストに格納 x = [0, x1, x2] y = [0, y1, y2] x_f1, y_f1, x_f2, y_f2, x_l, y_l = fk_frame(l1, l2, l3, l4, l5, l6, l7, th1, th2, x1, y1, x2, y2) # ロボットアームの姿勢をグラフにプロット plot(x, y, x_f1, y_f1, x_f2, y_f2, x_l, y_l) if __name__ == '__main__': main()
参考：【Python】2リンクマニピュレータの順運動学シミュレーション | アルゴリズム雑記
解析的に逆運動学を解いてみる
sympyのシンボリック計算を用いて解いてみる
from sympy import * theta1, theta2 = symbols('theta1 theta2') l1, l2 ,l3, x, y = symbols('l1 l2 l3 x y') eq1 = l2 * sin(theta2) + (l1 + l3) * sin(theta1) - y eq2 = l2 * cos(theta2) - (l1 + l3) * cos(theta1) - x init_printing() ans = solve([eq1, eq2], [theta1, theta2]) print ans
計算結果
[(asin((l2*sin(2*atan((2*l2*y - sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2))) - y)/(l1 + l3)) + pi, 2*atan((2*l2*y - sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2))) , (asin((l2*sin(2*atan((2*l2*y + sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2))) - y)/(l1 + l3)) + pi, 2*atan((2*l2*y + sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2))) , (-asin((l2*sin(2*atan((2*l2*y - sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2))) - y)/(l1 + l3)), 2*atan((2*l2*y - sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2))) , (-asin((l2*sin(2*atan((2*l2*y + sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2))) - y)/(l1 + l3)), 2*atan((2*l2*y + sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2)))]
４つ解出てきた。えげつない。。。
解が正しく出ているかチェック
# -*- coding: utf-8 -*- import numpy as np from numpy import sin,cos,pi,sqrt from numpy import arcsin as asin from numpy import arccos as acos from numpy import arctan as atan import matplotlib.pyplot as plt def fk(l1, l2, l3, th1, th2): x2 = l2 * cos(th2) - (l1 + l3) * cos(th1) y2 = l2 * sin(th2) + (l1 + l3) * sin(th1) x1 = -l1 * cos(th1) y1 = l1 * sin(th1) return x1, y1, x2, y2 def fk_frame(l1, l2, l3, l4, l5, l6, l7, th1, th2, x1, y1, x2, y2): x3 = - l4 * cos(th2) - l1 * cos(th1) y3 = - l4 * sin(th2) + l1 * sin(th1) x4 = (l2 + l6) * cos(th2) - (l1 + l3) * cos(th1) - l7 * sin(th2) y4 = (l2 + l6) * sin(th2) + (l1 + l3) * sin(th1) + l7 * cos(th2) x5 = (l2 + l6) * cos(th2) - (l1 + l3) * cos(th1) + l7 * sin(th2) y5 = (l2 + l6) * sin(th2) + (l1 + l3) * sin(th1) - l7 * cos(th2) x_f1 = [0, -l4 * cos(th2), x3, x1] y_f1 = [0, -l4 * sin(th2), y3, y1] x_f2 = [x3, x1 - l5 * sin(th2), x2] y_f2 = [y3, y1 + l5 * cos(th2), y2] x_l = [x2, x4, x5, x2] y_l = [y2, y4, y5, y2] return x_f1, y_f1, x_f2, y_f2, x_l, y_l # グラフの描画 def plot(x, y, x_f1, y_f1, x_f2, y_f2, x_l, y_l): fig = plt.figure() ax = fig.add_subplot(111) # リンクの描画 plt.plot(x, y,"-g",lw=5,label="link") plt.plot(x_f1, y_f1,"-g",lw=5,label="link") plt.plot(x_f2, y_f2,"-g",lw=5,label="link") plt.plot(x_l, y_l,"-g",lw=5,label="link") # 関節の描画 plt.plot(x, y,"or",lw=5, ms=10,label="joint") plt.plot(x_f1, y_f1,"or",lw=5, ms=10,label="joint") plt.plot(x_f2, y_f2,"or",lw=5, ms=10,label="joint") plt.plot(x_l, y_l,"or",lw=5, ms=10,label="joint") plt.xlim(-0.4, 0.4) plt.ylim(-0.1,0.4) plt.grid() plt.show() def main(th1, th2): # リンクの長さ l1, l2, l3, l4, l5, l6, l7= 0.18, 0.16, 0.0, 0.05, 0.03, 0.07, 0.07 # 第1, 2の関節角度 # [th1, th2] = np.radians([110, 15]) # 順運動学の計算 x1, y1, x2, y2 = fk(l1, l2, l3, th1, th2) # ロボットアームのx座標, y座標をリストに格納 x = [0, x1, x2] y = [0, y1, y2] x_f1, y_f1, x_f2, y_f2, x_l, y_l = fk_frame(l1, l2, l3, l4, l5, l6, l7, th1, th2, x1, y1, x2, y2) # ロボットアームの姿勢をグラフにプロット plot(x, y, x_f1, y_f1, x_f2, y_f2, x_l, y_l) if __name__ == '__main__': # リンクの長さ l1, l2, l3, l4, l5, l6, l7= 0.18, 0.16, 0.0, 0.05, 0.03, 0.07, 0.07 x = 0.15 y = 0.15 th = (asin((l2*sin(2*atan((2*l2*y - sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2))) - y)/(l1 + l3)) + pi, 2*atan((2*l2*y - sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2))) th1, th2 = th[0], th[1] main(th1, th2)
あってる。sympy使いやすい。
アニメーションを作ってみる
import numpy as np from numpy import sin,cos,pi,sqrt from numpy import arcsin as asin from numpy import arccos as acos from numpy import arctan as atan import matplotlib.pyplot as plt import matplotlib.animation as animation from matplotlib.animation import FuncAnimation def fk(l1, l2, l3, th1, th2): x2 = l2 * cos(th2) - (l1 + l3) * cos(th1) y2 = l2 * sin(th2) + (l1 + l3) * sin(th1) x1 = -l1 * cos(th1) y1 = l1 * sin(th1) return x1, y1, x2, y2 def fk_frame(l1, l2, l3, l4, l5, l6, l7, th1, th2, x1, y1, x2, y2): x3 = - l4 * cos(th2) - l1 * cos(th1) y3 = - l4 * sin(th2) + l1 * sin(th1) x4 = (l2 + l6) * cos(th2) - (l1 + l3) * cos(th1) - l7 * sin(th2) y4 = (l2 + l6) * sin(th2) + (l1 + l3) * sin(th1) + l7 * cos(th2) x5 = (l2 + l6) * cos(th2) - (l1 + l3) * cos(th1) + l7 * sin(th2) y5 = (l2 + l6) * sin(th2) + (l1 + l3) * sin(th1) - l7 * cos(th2) x_f1 = [0, -l4 * cos(th2), x3, x1] y_f1 = [0, -l4 * sin(th2), y3, y1] x_f2 = [x3, x1 - l5 * sin(th2), x2] y_f2 = [y3, y1 + l5 * cos(th2), y2] x_l = [x2, x4, x5, x2] y_l = [y2, y4, y5, y2] return x_f1, y_f1, x_f2, y_f2, x_l, y_l fig = plt.figure(1) ax = fig.add_subplot(111, aspect='equal', autoscale_on=False, xlim=(-0.4, 0.4), ylim=(-0.1, 0.4)) ax.grid() #th1 = [pi / 4] * 100 #th2 = np.linspace(- pi / 6, pi / 6, 100) l1, l2, l3, l4, l5, l6, l7= 0.18, 0.16, 0.0, 0.05, 0.03, 0.07, 0.07 x0 = np.linspace(0.15, 0.2, 100) x1 = np.linspace(0.2, 0.2, 100) y0 = np.linspace(0.15, 0.2, 100) y1 = np.linspace(0.2, 0.25, 100) x = np.append(x0, x1) y = np.append(y0, y1) ite_len = len(x) # 90 < th1 th1 = asin((l2*sin(2*atan((2*l2*y - sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2))) - y)/(l1 + l3)) + pi print th1 th2 = 2*atan((2*l2*y - sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2)) print th2 # 0 < 90 < th1 # th1 = -asin((l2*sin(2*atan((2*l2*y - sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2))) - y)/(l1 + l3)) # th2 = 2*atan((2*l2*y - sqrt(-l1**4 - 4*l1**3*l3 + 2*l1**2*l2**2 - 6*l1**2*l3**2 + 2*l1**2*x**2 + 2*l1**2*y**2 + 4*l1*l2**2*l3 - 4*l1*l3**3 + 4*l1*l3*x**2 + 4*l1*l3*y**2 - l2**4 + 2*l2**2*l3**2 + 2*l2**2*x**2 + 2*l2**2*y**2 - l3**4 + 2*l3**2*x**2 + 2*l3**2*y**2 - x**4 - 2*x**2*y**2 - y**4))/(-l1**2 - 2*l1*l3 + l2**2 + 2*l2*x - l3**2 + x**2 + y**2)) def two_link(th1, th2): l1, l2, l3, l4, l5, l6, l7= 0.18, 0.16, 0.0, 0.05, 0.03, 0.07, 0.07 # [th1, th2] = np.radians([110, 15]) x1, y1, x2, y2 = fk(l1, l2, l3, th1, th2) x = [0, x1, x2] y = [0, y1, y2] x_f1, y_f1, x_f2, y_f2, x_l, y_l = fk_frame(l1, l2, l3, l4, l5, l6, l7, th1, th2, x1, y1, x2, y2) return x, y, x_f1, y_f1, x_f2, y_f2, x_l, y_l xl, yl, x_f1l, y_f1l, x_f2l, y_f2l, x_ll, y_ll = [], [], [], [], [], [], [], [] for t1, t2 in zip(th1, th2): x, y, x_f1, y_f1, x_f2, y_f2, x_l, y_l = two_link(t1, t2) xl.append(x) yl.append(y) x_f1l.append(x_f1) y_f1l.append(y_f1) x_f2l.append(x_f2) y_f2l.append(y_f2) x_ll.append(x_l) y_ll.append(y_l) line1, = plt.plot([], [],"-g",lw=5,label="link") line2, = plt.plot([], [],"-g",lw=5,label="link") line3, = plt.plot([], [],"-g",lw=5,label="link") line4, = plt.plot([], [],"-g",lw=5,label="link") circle1, = plt.plot([], [],"or",lw=5, ms=10,label="joint") circle2, = plt.plot([], [],"or",lw=5, ms=10,label="joint") circle3, = plt.plot([], [],"or",lw=5, ms=10,label="joint") circle4, = plt.plot([], [],"or",lw=5, ms=10,label="joint") def update(i): line1.set_data(xl[i], yl[i]) line2.set_data(x_f1l[i], y_f1l[i]) line3.set_data(x_f2l[i], y_f2l[i]) line4.set_data(x_ll[i], y_ll[i]) circle1.set_data(xl[i], yl[i]) circle2.set_data(x_f1l[i], y_f1l[i]) circle3.set_data(x_f2l[i], y_f2l[i]) circle4.set_data(x_ll[i], y_ll[i]) ani = animation.FuncAnimation(fig, update, ite_len, blit=False, interval=10, repeat=False) plt.show()
matplotlibのanimation.FuncAnimationを用いて柔軟にアニメーション作成 - Qiita
pythonで二重振り子のアニメーションを作る – into the lambda
参考まとめ
物理計算エンジン
- MoJoco : OpenAI、物理演算エンジン「MuJoCo」を使用したロボットシミュレーション用Pythonライブラリ「mujoco-py」をGitHubにて公開。VR向けコードも公開 | Seamless
倒立振子のシミュレーション(スクラッチ)
simpy
アームリンク
↓是非時間を見つけて一度流したい
パラレルリンク
逆運動学
ROSで逆運動学計算： ROSによるマニピュレータの制御 - 研究紹介ページ
MoveIt!
新しいLinux PCの設定方法まとめ (ラズパイやTX2など)
やりたいこと
新しいLinuxPCをこさえた時に、英語キーボードだったり、sshしにくかったりするので、最低限必要な設定をまとめる。
今後ガンガン追記していく予定
ラズパイ
-
- raspi-configでsshの設定をonに
キーボード:
TX2
キーボード
- sudo dpkg-reconfigure keyboard-configuration : JetsonTX2初期セットアップ(Jetpack)からサンプルのコンパイルまでの手順 - Qiita
-
ubuntu同士なら簡単。「デスクトップの共有」でパスワードの設定。「リモートデスクトップクライアント」で、VNCを選択し、ホスト名 or IPを設定し、接続するとリモートで操作可能に。
参考
TX2全般
- jetson TX2 のセットアップ - 粗大メモ置き場 ←わかりやすい！
sshでXを飛ばす方法
raspi x rosメモ
いつものまにかROSWIKIにraspiでのインストール方法書いてる。最高。
raspi3用 Ubuntu16 + rosのイメージ
raspi zerp用 rosのイメージ
参考
Installing ROS Indigo on the Raspberry Pi | Learning Robotics Using ROS
ラズパイで動くロボット「GoPiGo」をつかって遠隔見守りロボットを作ろう（1） 開発準備編 (1/6)：CodeZine（コードジン）
snowboyで遊んでみる
やりたいこと
Hey , SiriやOK, GoogleのようなHotWordを変更して、音声認識を行いたい
snowboy
install & demo
ubuntu16.04でも動いた♪
install -> https://s3-us-west-2.amazonaws.com/snowboy/snowboy-releases/ubuntu1404-x86_64-1.1.1.tar.bz2
pip install pyaudio
Snowboy Hotword DetectionのCreate Hotwordでtest.pmdlを作成
python demo.py test.pmdl
demo.py
import snowboydecoder import sys import signal interrupted = False def signal_handler(signal, frame): global interrupted interrupted = True def interrupt_callback(): global interrupted return interrupted if len(sys.argv) == 1: print("Error: need to specify model name") print("Usage: python demo.py your.model") sys.exit(-1) model = sys.argv[1] signal.signal(signal.SIGINT, signal_handler) detector = snowboydecoder.HotwordDetector(model, sensitivity=0.5) print('Listening... Press Ctrl+C to exit') detector.start(detected_callback=snowboydecoder.ding_callback, interrupt_check=interrupt_callback, sleep_time=0.03) detector.terminate()
このdetected_callback=snowboydecoder.ding_callbackに自作関数を入れる。
そうすると、HotWordが入った時に関数が呼ばれる。
注意
Do not append () to your callback function: the correct way is to assign detected_callback=your_func instead of detected_callback=your_func(). However, what if you have parameters to assign in your callback functions? Use a lambda function! So your callback would look like: callback=lambda: callback_function(parameters).
参考
tf-pose-estimationのコード理解
やりたいこと
tf-pose-estimationを用いた面白いタスクを作るため、tf-pose-estimationを理解する
いつのまにかROS対応してる。素晴らしい。
rosでの動かし方
sudo apt-get update
sudo apt-get upgrade
sudo apt-get install ros-kinetic-video-stream-opencv
sudo apt-get install ros-kinetic-image-view
git clone https://github.com/ildoonet/ros-video-recorder.git
git clone https://github.com/ildoonet/tf-pose-estimation.git
pip install -U setuptools
pip install -r tf-pose-estimation/requirements.txt
catkin_make
roslaunch tfpose_ros demo_video.launch
Tips
launchのモデルをmobilenetに変更するとCPU only でもかろうじて動く！
msg
BodyPartElm.msg
int32 part_id float32 x float32 y float32 confidence
Person,msg
BodyPartElm[] body_part
Persons.msg
Person[] persons uint32 image_w uint32 image_h Header header
broadcaster_ros.py
TfPoseEstimatorROSのノード
Personsのmsg型に変換
def humans_to_msg(humans):
persons = Persons()
for human in humans:
person = Person()
for k in human.body_parts:
body_part = human.body_parts[k]
body_part_msg = BodyPartElm()
body_part_msg.part_id = body_part.part_idx
body_part_msg.x = body_part.x
body_part_msg.y = body_part.y
body_part_msg.confidence = body_part.score
person.body_part.append(body_part_msg)
persons.persons.append(person)
return persons
以下のようにしてpathを取得することができる
w, h = model_wh(model)
graph_path = get_graph_path(model)
rospack = rospkg.RosPack()
graph_path = os.path.join(rospack.get_path('tfpose_ros'), graph_path)
estimator.py
from estimator import TfPoseEstimator
画像から予測するスクリプトで
class BodyPart:
"""
part_idx : part index(eg. 0 for nose)
x, y: coordinate of body part
score : confidence score
"""
__slots__ = ('uidx', 'part_idx', 'x', 'y', 'score')
def __init__(self, uidx, part_idx, x, y, score):
self.uidx = uidx
self.part_idx = part_idx
self.x, self.y = x, y
self.score = score
def get_part_name(self):
return CocoPart(self.part_idx)
def __str__(self):
return 'BodyPart:%d-(%.2f, %.2f) score=%.2f' % (self.part_idx, self.x, self.y, self.score)
で構成されたhuman情報を返す
新しいpythonの知識
slots： ``__slots__``を使ってメモリを節約 - Qiita
Pythonで__slots__を使う - StoryEdit 開発日誌
@staticmethod： Pythonの「@staticmethod」はどのように役立つのか - モジログ
def str_(self): str()メソッドは、print(x)するときにも呼び出される。
networks.py
from networks import get_graph_path, model_wh
def get_graph_path(model_name):
return {
'cmu_640x480': './models/graph/cmu_640x480/graph_opt.pb',
'cmuq_640x480': './models/graph/cmu_640x480/graph_q.pb',
'cmu_640x360': './models/graph/cmu_640x360/graph_opt.pb',
'cmuq_640x360': './models/graph/cmu_640x360/graph_q.pb',
'mobilenet_thin_432x368': './models/graph/mobilenet_thin_432x368/graph_opt.pb',
}[model_name]
def model_wh(model_name):
width, height = model_name.split('_')[-1].split('x')
return int(width), int(height)
run_webcam.py
tf-pose-estimation/run_webcam.py at master · ildoonet/tf-pose-estimation · GitHub
引数
parser.add_argument('--camera', type=int, default=0) ： カメラ番号の選択
parser.add_argument('--zoom', type=float, default=1.0) ： zoom
parser.add_argument('--model', type=str, default='mobilenet_thin_432x368', help='cmu_640x480 / cmu_640x360 / mobilenet_thin_432x368') : モデルを選択できる。mobilenetが高速。
zoom
if args.zoom < 1.0: canvas = np.zeros_like(image) img_scaled = cv2.resize(image, None, fx=args.zoom, fy=args.zoom, interpolation=cv2.INTER_LINEAR) dx = (canvas.shape[1] - img_scaled.shape[1]) // 2 dy = (canvas.shape[0] - img_scaled.shape[0]) // 2 canvas[dy:dy + img_scaled.shape[0], dx:dx + img_scaled.shape[1]] = img_scaled image = canvas elif args.zoom > 1.0: img_scaled = cv2.resize(image, None, fx=args.zoom, fy=args.zoom, interpolation=cv2.INTER_LINEAR) dx = (img_scaled.shape[1] - image.shape[1]) // 2 dy = (img_scaled.shape[0] - image.shape[0]) // 2 image = img_scaled[dy:image.shape[0], dx:image.shape[1]]
Google AIY Voice Kitで遊んでみる
やりたいこと
設定
ハード： https://yuki-no-yabo.com/how-to-make-google-aiy-voice-kit/
ソフト： https://yuki-no-yabo.com/software-install-for-aiy-voice-kit/
デモ
【Google AIY Voice Kit】Google Assistant APIの設定からデモプログラムの実行まで
Lチカ
Google AIY Voice Kitで電子工作～LEDライトを点灯～
日本語化
AIY Projects kitを試してみよう | スイッチサイエンス マガジン
現状、aiyライブラリの発話関数は日本語対応してなさそう
AIYスピーカーを、ほぼ日本語版Google Homeに【Google Assistant SDKアップデート】
Voice Kitなしで実行
AIY ProjectのVoice KitをVocie Kitなしでやってみた - 知的好奇心 for IoT
GCPサンプル
サンプル アプリケーション | Google Cloud Speech API ドキュメント | Google Cloud Platform
Cloud Speech API で音声をテキストに変換する
HotWord
snowboy/examples/Python at master · Kitt-AI/snowboy · GitHub
Snowboy Hotword DetectionをRaspberry Piで動かす - Qiita
python - How to set up wake word for Google Assistant SDK on a Raspberry Pi - Stack Overflow
