ロボット、電子工作、IoT、AIなどの開発記録

開発したい。開発したい。。。

windows10にROS2をインストールする

公式wikiを見ながら、win10にインストール作業をしました。
  

事前条件

Visual Studio 14.0 2015 Update 3”が入っていて、かつC++も選択してインストールしていること
pythonは2017_12_10時点で最新の3.6.2ではなく3.5の代ではないと途中のtrolliusをインストールするところで入らないのでダウングレードする必要があります!
そして自分はpyenvとか使ってないから一回環境ぶち壊れたよ!(^o^)ノ
 
ダウングレードする場合は

conda install python=3.5

 

Chocolateyのインストール

Windowsのキーを押し検索で” vs2015 x64 native tools command prompt”のアプリを探し、右クリックして[管理者として実行]を選択。
f:id:weekendproject9:20171211175455p:plain

以下のコマンドを張り付けて実行

@"%SystemRoot%\System32\WindowsPowerShell\v1.0\powershell.exe" -NoProfile -InputFormat None -ExecutionPolicy Bypass -Command "iex ((New-Object System.Net.WebClient).DownloadString('https://chocolatey.org/install.ps1'))" && SET "PATH=%PATH%;%ALLUSERSPROFILE%\chocolatey\bin"

そしたら警告が出てきました
警告1.
WARNING: It's very likely you will need to close and reopen your shell
before you can use choco.

インストールしたあと使いたかったら再起動しろと。わかりました。

警告2.
WARNING: You can safely ignore errors related to missing log files when
upgrading from a version of Chocolatey less than 0.9.9.
'Batch file could not be found' is also safe to ignore.
'The system cannot find the file specified' - also safe.

これは無視できるらしい

警告3.
警告: Not setting tab completion: Profile file does not exist at
'C:\Users\\Documents\WindowsPowerShell\Microsoft.PowerShell_profile.ps1'.

これはファイルがないよと言ってますが、別に使わない?ので無視
 
コマンドラインで "choco -v" でバージョン確認
0.10.8
 
一応アップデートする

choco upgrade chocolatey

 

依存関係のインストール

コマンドは vs2015 x64 native tools command promptで実行する
・gitインストール

choco install -y git

環境変数いじったよって言われる
 
・cmakeインストール

choco install -y cmake

環境変数に ”C:\Program Files\CMake\bin” を追加
 
python入れてない人は

choco install -y python

 

Boostインストール

以下からBoostをダウンロード
https://sourceforge.net/projects/boost/files/boost-binaries/1.61.0/boost_1_61_0-msvc-14.0-64.exe/download?use_mirror=jaist

exeでインストールしたあと、環境変数を追加
 BOOST_ROOT    : C:\local\boost_1_61_0
 BOOST_LIBRARYDIR  : C:\local\boost_1_61_0\lib64-msvc-14.0
またpathにも追加
 C:\local\boost_1_61_0\lib64-msvc-14.0

開発者ツールのインストール

 pip install vcstool
 choco install -y curl

 

依存関係をインストールする

Release 2017-04-04-1 · ros2/choco-packages · GitHub
ここに行き、一つずつクリックして以下をダウンロード
・asio.1.10.6.nupkg
・eigen.3.3.3.nupkg
・tinyxml-usestl.2.6.2.nupkg
・tinyxml2.4.0.1.nupkg

以下でインストール。”PATH\TO\DOWNLOADS\” はダウンロードしたフォルダ名に書き換える

choco install -y -s <PATH\TO\DOWNLOADS\> asio eigen tinyxml-usestl tinyxml2

以下pipでインストール

pip install -U setuptools pip
pip install EmPy pyparsing pyyaml
pip install nose coverage mock pytest pytest-cov pytest-runner
pip install flake8 flake8-blind-except flake8-builtins flake8-class-newline flake8-comprehensions flake8-deprecated flake8-docstrings flake8-import-order flake8-quotes pep8 pydocstyle pyflakes

↓python3.6は対応していないので注意(trollius 2.1 : Python Package Index

pip install trollius
choco install -y cppcheck

C:\Program Files\CppcheckをPATHに追加する。

opencvインストール

 公式wikiとはちょっとPathを変えてますが以下になります。
 Visual Studio 2015:
  https://github.com/ros2/ros2/releases/download/release-beta2/opencv-2.4.13.2-vc14.VS2015.zip
 解凍して移動→C:\dev\opencv\opencv-2.4.13.2-vc14.VS2015
 環境変数を追加

setx -m OpenCV_DIR C:\dev\opencv\opencv-2.4.13.2-vc14.VS2015

C:\dev\opencv\opencv-2.4.13.2-vc14.VS2015\x64\vc14\bin をPATHに追加

OpenSSLインストール

以下からWin64 OpenSSL v1.0.2をダウンロードします。Win32またはLightバージョンはダウンロードしないでください。
Shining Light Productions - Win32 OpenSSL
exe起動。基本的に進むのみをクリック。最後の寄付はチェックを外してfinish。

setx -m OPENSSL_CONF C:\OpenSSL-Win64\bin\openssl.cfg

また、C:\OpenSSL-Win64\bin\をPATHに追加。
  

ソースの取得

C:直下にファイルを作って移動する

 md \dev\ros2\src
 cd \dev\ros2 

ros2.reposクローンを作成するリポジトリを定義するファイルを取得

curl -sk https://raw.githubusercontent.com/ros2/ros2/release-latest/ros2.repos -o ros2.repos

次に、vcs、ros2.reposにリストされているリポジトリをインポートする

vcs import src < ros2.repos

 

ROS 2コードの作成

フォルダツリーを構築してビルド。”--build-tests”オプションを追加してテストできる
結構時間かかります

cd C:\dev\ros2
python src\ament\ament_tools\scripts\ament.py build
python src\ament\ament_tools\scripts\ament.py build --build-tests

 

実行

一回vs2015 x64 native tools command promptを再起動して以下実行

> call install\local_setup.bat
> ros2 run demo_nodes_py talker

↓以下実行結果↓
C:\dev\ros2>ros2 run demo_nodes_py talker
[INFO] [talker]: Publishing: "Hello World: 0"
[INFO] [talker]: Publishing: "Hello World: 1"
[INFO] [talker]: Publishing: "Hello World: 2"
[INFO] [talker]: Publishing: "Hello World: 3"
[INFO] [talker]: Publishing: "Hello World: 4"
[INFO] [talker]: Publishing: "Hello World: 5"
[INFO] [talker]: Publishing: "Hello World: 6"
[INFO] [talker]: Publishing: "Hello World: 7"
[INFO] [talker]: Publishing: "Hello World: 8"
[INFO] [talker]: Publishing: "Hello World: 9"
[INFO] [talker]: Publishing: "Hello World: 10"
[INFO] [talker]: Publishing: "Hello World: 11"
[INFO] [talker]: Publishing: "Hello World: 12"
[INFO] [talker]: Publishing: "Hello World: 13"
[INFO] [talker]: Publishing: "Hello World: 14"
[INFO] [talker]: Publishing: "Hello World: 15"
[INFO] [talker]: Publishing: "Hello World: 16"
[INFO] [talker]: Publishing: "Hello World: 17"
[INFO] [talker]: Publishing: "Hello World: 18"
[INFO] [talker]: Publishing: "Hello World: 19"
[INFO] [talker]: Publishing: "Hello World: 20"
[INFO] [talker]: Publishing: "Hello World: 21"
[INFO] [talker]: Publishing: "Hello World: 22"
[INFO] [talker]: Publishing: "Hello World: 23"
[INFO] [talker]: Publishing: "Hello World: 24"
[INFO] [talker]: Publishing: "Hello World: 25"

 
f:id:weekendproject9:20171211175612p:plain
 
とりあえずインストールと実行は完了しました。
 

 
「参考」
Windows Development Setup · ros2/ros2 Wiki · GitHub
Troubleshooting

win10にVisualStudio2015,Python3.6,CUDA(gtx1070),Tensorflow環境を入れる

人工知能系の開発環境構築

環境

win10 64bit
gtx1070

VisualStudio2015 Community のダウンロード・インストール

以下URLに行き、ページ下部の”以前のバージョン”をクリック⇒ 2015の項目のダウンロードを選択
ダウンロード | IDE、Code、Team Foundation Server | Visual Studio
すでにログインしている場合は以下から飛べます
Visual Studio Team Services | Sign In

次に
Visual Studio Community 2015 with Update 3 を探して、言語をJapaneseにしてダウンロードします。
ダウンロードしたらexeを起動してインストール開始。
 途中の”インストールの種類”の項目を”カスタム”に変更し、プログラミング言語欄の”C++”,”Python”にチェックを入れてインストールを進めます。

インストール完了後、正常に起動することを確認


Python環境をインストール

Anacondaのサイトから”Python3.6”のバージョンをダウンロード⇒起動してインストール
Downloads | Anaconda
・インストール途中にPathを追加するかの項目があるのでチェックします。
 (非推奨なため問題が起きる時があります。このときは再インストールしてください)
・インストール対象はjust Me
インストールできたらコマンドプロンプトを開き、"python"と打ってインストールできているか確認

VisualStudioにAnaconda紐づけ

VS2015起動してPythonApplicationプロジェクト作成後、ツール→オプション→Python Tools → EnvironmentOptionsを選択
Add Environmentで任意の名前を記入
PathにAnacondaのインストール先を入れる。
標準のまま入れてたら
 Path:”C:\ProgramData\Anaconda3\python.exe”
 WindowsPath:C:\ProgramData\Anaconda3\pythonw.exe
 Library Path:C:\ProgramData\Anaconda3\Lib
 Architecture:x64
 Language Version:3.5
okを押して完了

コードに

print("Hello world")

で実行(F5)して表示されるか確認

CUDA Toolkitをインストール

トップページから行くと最新の9.0のページに飛ぶので、以下の8.0に行きます。
(9.0ですとtensorflowとcuDNNでエラーがあるとの報告があったためインストールを避けました)
CUDA Toolkit 8.0 - Feb 2017 | NVIDIA Developer
ベースインストーラとパッチをダウンロードします。
インストール前にはVisualStudioを落としておきます。
ベースインストーラでインストール後、パッチのexeをあてます。

cuDNNをインストール

NVIDIA cuDNN | NVIDIA Developer
ページ中央”Download”をクリックしてLoginしてください
(登録してない人は登録してください。少し経つと確認メールが来ますが迷惑メールに入る可能性があるので注意)
ログインして入ったら、”I Agree To the Terms of the cuDNN Software License Agreement”にチェックを入れる
"Download cuDNN v6.0 (April 27, 2017), for CUDA 8.0"の項目を開く
”cuDNN v6.0 Library for Windows 10”を選択してダウンロード

展開したら、ファイルがあるのでtoolkitの方に移動する
・展開したファイルbin\cudnn64_6.dll→
C:\Program Files\NVIDIA GPU Computing Toolkit\CUDA\v8.0\binの中に移動
・展開したファイルinclude\cudnn.h→
C:\Program Files\NVIDIA GPU Computing Toolkit\CUDA\v8.0\includeの中に移動
・展開したファイルlib\x64\cudnn.lib→
C:\Program Files\NVIDIA GPU Computing Toolkit\CUDA\v8.0\lib\x64の中に移動

環境変数
”C:\Program Files\NVIDIA GPU Computing Toolkit\CUDA\v8.0”を追加

Tensorflowインストール

コマンドプロンプトでインストール
 ”pip install --upgrade tensorflow-gpu

完了したら、コマンドプロンプトに"python"と打って起動、以下のプログラムが処理できるか確認

import tensorflow as tf
hello = tf.constant('Hello, TensorFlow!')
sess = tf.Session()
print(sess.run(hello))
a = tf.constant(10)
b = tf.constant(32)
sess.run(a + b)
sess.close()

”b'Hello, TensorFlow!'”
"42"
と表示されればok

ロボットを強化学習DQN (Deep Q Network) させる

今回は強化学習DQN (Deep Q Network) を↓で学ばせてもらいました
超シンプルにTensorFlowでDQN (Deep Q Network) を実装してみる 〜導入編〜  | ALGO GEEKS
それで、もう少しどういうものかを見るために自分なりに少し変えてみました。

システムとしては、ロボットがあり、あるターゲットに近づくことができればいいといった感じです。
参考ソースとの差は、
・ロボットを上下左右に移動させる
・ターゲットは初期にランダム配置
・explorationを1.0から始め、学習ごとに減らしていく
・ロボットとターゲットの距離が1になったところで報酬=1
・任意の時間以内にターゲットに近づけなければ報酬=-1

以下15000回学習させた結果↓
青丸:ターゲット
三角:ロボット
f:id:weekendproject9:20171129002539g:plain

dqn_agent.py

from collections import deque
import os

import numpy as np
import tensorflow as tf


class DQNAgent:
    """
    Multi Layer Perceptron with Experience Replay
    """

    def __init__(self, enable_actions, environment_name):
        # parameters
        self.name = "agent"#os.path.splitext(os.path.basename(__file__))[0]
        self.environment_name = environment_name
        self.enable_actions = enable_actions
        self.n_actions = len(self.enable_actions)
        self.minibatch_size = 32
        self.replay_memory_size = 1000
        self.learning_rate = 0.001
        self.discount_factor = 0.9
        self.exploration = 1.0#どのくらいで行動に出るか、大きいと活発に動く 
        self.model_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), "models")
        self.model_name = "{}.ckpt".format(self.environment_name)

        # replay memory
        self.D = deque(maxlen=self.replay_memory_size)

        # model
        self.init_model()

        # variables
        self.current_loss = 0.0

    def init_model(self):
        # input layer (8 x 8)
        #self.x = tf.placeholder(tf.float32, [None, 64, 64])
        self.x = tf.placeholder(tf.float32, [None, 8, 8])

        # flatten (64)
        #x_flat = tf.reshape(self.x, [-1, 512])
        x_flat = tf.reshape(self.x, [-1, 64])

        # fully connected layer (32)
        #W_fc1 = tf.Variable(tf.truncated_normal([512, 512], stddev=0.01))#[64, 64]
        W_fc1 = tf.Variable(tf.truncated_normal([64, 64], stddev=0.01))#[64, 64]
        #b_fc1 = tf.Variable(tf.zeros([512]))#64
        b_fc1 = tf.Variable(tf.zeros([64]))#64
        h_fc1 = tf.nn.relu(tf.matmul(x_flat, W_fc1) + b_fc1)

        # output layer (n_actions)
        #W_out = tf.Variable(tf.truncated_normal([512, self.n_actions], stddev=0.01))
        W_out = tf.Variable(tf.truncated_normal([64, self.n_actions], stddev=0.01))
        b_out = tf.Variable(tf.zeros([self.n_actions]))
        self.y = tf.matmul(h_fc1, W_out) + b_out

        # loss function
        self.y_ = tf.placeholder(tf.float32, [None, self.n_actions])
        self.loss = tf.reduce_mean(tf.square(self.y_ - self.y))

        # train operation
        optimizer = tf.train.RMSPropOptimizer(self.learning_rate)
        self.training = optimizer.minimize(self.loss)

        # saver
        self.saver = tf.train.Saver()

        # session
        self.sess = tf.Session()
        self.sess.run(tf.global_variables_initializer())

    def Q_values(self, state):
        # Q(state, action) of all actions
        return self.sess.run(self.y, feed_dict={self.x: [state]})[0]

    def select_action(self, state, epsilon):
        if np.random.rand() <= epsilon:
            # random
            return np.random.choice(self.enable_actions)
        else:
            # max_action Q(state, action)
            return self.enable_actions[np.argmax(self.Q_values(state))]

    def store_experience(self, state, action, reward, state_1, terminal):
        self.D.append((state, action, reward, state_1, terminal))

    def experience_replay(self):
        state_minibatch = []
        y_minibatch = []

        # sample random minibatch
        minibatch_size = min(len(self.D), self.minibatch_size)
        minibatch_indexes = np.random.randint(0, len(self.D), minibatch_size)

        for j in minibatch_indexes:
            state_j, action_j, reward_j, state_j_1, terminal = self.D[j]
            action_j_index = self.enable_actions.index(action_j)

            y_j = self.Q_values(state_j)

            if terminal:
                y_j[action_j_index] = reward_j
            else:
                # reward_j + gamma * max_action' Q(state', action')
                y_j[action_j_index] = reward_j + self.discount_factor * np.max(self.Q_values(state_j_1))  # NOQA

            state_minibatch.append(state_j)
            y_minibatch.append(y_j)

        # training
        self.sess.run(self.training, feed_dict={self.x: state_minibatch, self.y_: y_minibatch})

        # for log
        self.current_loss = self.sess.run(self.loss, feed_dict={self.x: state_minibatch, self.y_: y_minibatch})

    def load_model(self, model_path=None):
        if model_path:
            # load from model_path
            print(" model_path:", model_path)
            self.saver.restore(self.sess, model_path)
        else:
            # load from checkpoint
            checkpoint = tf.train.get_checkpoint_state(self.model_dir)
            if checkpoint and checkpoint.model_checkpoint_path:
                self.saver.restore(self.sess, checkpoint.model_checkpoint_path)
            print(" model_dir:", checkpoint.model_checkpoint_path)

    def save_model(self):
        self.saver.save(self.sess, os.path.join(self.model_dir, self.model_name))
robot.py

import os
import numpy as np
import matplotlib.pyplot as plt
import math
import copy

class robot:
    def __init__(self):
        fig, ax = plt.subplots(1, 1)

        # parameters
        self.name = os.path.splitext(os.path.basename(__file__))[0]
        hit = 0
        self.fieldX = 8
        self.fieldY = 8
        self.robotX =0
        self.robotY =0
        self.initrobotPositionX = 4
        self.initrobotPositionY = 4
        self.robotMove = 1

        self.enable_actions = (0, 1, 2,3,4)
        self.frame_rate = 15

        self.targetNum = 1
        self.hit = 0
        self.time = 0

        self.around = 1

        # variables
        self.reset()

    def update(self, action):
        """
        action:
            0: do nothing
            1: move up
            2: move right
            3: move down
            4: move left
        """
        # update player position
        if action == self.enable_actions[1]:
            # move up
            self.robotY = max(0, self.robotY - self.robotMove)
        elif action == self.enable_actions[2]:
            # move right
            self.robotX = min(self.robotX + self.robotMove , self.fieldX-1)
        elif action == self.enable_actions[3]:
            # move down
            self.robotY = min(self.fieldY-1, self.robotY + self.robotMove)
        elif action == self.enable_actions[4]:
            # move left
            self.robotX = max(0, self.robotX - self.robotMove)
        else:
            # do nothing
            pass

        # update 
        self.hit = self.targetUpdate(self.initrobotPositionX ,self.initrobotPositionY)


           

        # collision detection
        #reward reset
        self.reward =0
        # end flag
        self.terminal = False
        if(self.time>=5 or self.hit):
         self.terminal = True
         if self.hit:
             # hit
             self.reward = 1
         else:
             # no hit             
             self.reward = -1

    def targetUpdate(self,X,Y):
        i = 0
        self.time+=1
        print("time:",self.time)

        for i in range(self.targetNum):
            #self.rand =np.random.randint(1, 4, self.targetNum)            
            #if self.rand == 1:
            #  self.fannelPointY =  max(0, self.fannelPointY-1)
            #if self.rand == 2:
            #  self.fannelPointY =  min(self.fieldY-1, self.fannelPointY+1)
            #if self.rand == 3:
            #    self.fannelPointX =max(0,   self.fannelPointX-1)
            #if self.rand == 4:
            #    self.fannelPointX = min(self.fannelPointX+1,self.fieldX-1)


            if self.robotX - self.around <= self.fannelPointX and  self.robotX + self.around >= self.fannelPointX:
                if self.robotY - self.around <= self.fannelPointY and self.robotY+self.around >= self.fannelPointY:
                    self.hit = 1

            print("hit:" , self.hit)


            plt.clf()

            plt.plot(-2, -2, "ro")
            plt.plot(10, 10, "ro")            
            plt.plot(self.robotX, self.robotY, c='red',marker='^')
            plt.plot(self.fannelPointX, self.fannelPointY, c='blue',marker='o')
           
            plt.pause(1)

        return self.hit

      

    def draw(self):
        self.screen = np.zeros((self.fieldX, self.fieldY))
        print("robot_position:",self.robotX , "," , self.robotY )
        self.screen[self.robotX, self.robotY] = 1
        print("target_position",self.fannelPointX , "," , self.fannelPointY )

        self.screen[self.fannelPointX, self.fannelPointY] = 2
        print( self.screen)
        print("---")

    def observe(self):
        self.draw()
        return self.screen, self.reward, self.terminal

    def execute_action(self, action):
        self.update(action)

    def reset(self):
        print("robot_init")
        self.hit=0

        # reset other variables
        self.reward = 0
        self.terminal = False

        self.robotX = self.initrobotPositionX
        self.robotY = self.initrobotPositionY
        self.x =np.random.randint(0, self.fieldX, self.targetNum)
        self.y =np.random.randint(0, self.fieldY, self.targetNum)
        rand =np.random.randint(0,(self.fieldX) , self.targetNum)
        i=0

        self.fannelPointX = self.x[i]
        self.fannelPointY = self.y[i]

        if self.robotX - self.around <= self.fannelPointX and  self.robotX + self.around >= self.fannelPointX:
            if self.robotY - self.around <= self.fannelPointY and self.robotY+self.around >= self.fannelPointY:
                if rand >= self.fieldX/2:
                    self.fannelPointX = 0
                    self.fannelPointY = 0
                if rand <= self.fieldX/2:
                    self.fannelPointX = self.fieldX-1
                    self.fannelPointY = self.fieldY-1

        self.time=0

train.py

import numpy as np

from robot import robot
from dqn_agent import DQNAgent


def train():
    # parameters
    n_epochs = 15000

    # environment, agent
    env = robot()
    agent = DQNAgent(env.enable_actions, env.name)

    # variables
    win = 0

    for e in range(n_epochs):
        # reset
        frame = 0
        loss = 0.0
        Q_max = 0.0
        env.reset()
        state_t_1, reward_t, terminal = env.observe()

        while not terminal:
            state_t = state_t_1

            # execute action in environment
            action_t = agent.select_action(state_t, agent.exploration)
            env.execute_action(action_t)

            # observe environment
            state_t_1, reward_t, terminal = env.observe()

            # store experience
            agent.store_experience(state_t, action_t, reward_t, state_t_1, terminal)

            # experience replay
            agent.experience_replay()

            # for log
            frame += 1
            loss += agent.current_loss
            Q_max += np.max(agent.Q_values(state_t))
            if reward_t == 1:
                win += 1

        print("EPOCH: {:03d}/{:03d} | WIN: {:03d} | LOSS: {:.4f} | Q_MAX: {:.4f}".format(
            e, n_epochs - 1, win, loss / frame, Q_max / frame))

        agent.exploration = agent.exploration - (( 1.0-0.1)/n_epochs)

        if(n_epochs % 1000):
         agent.save_model()

    # save model
    agent.save_model()

 
今後はこうしていきたいですね↓
f:id:weekendproject9:20171129002636p:plain

[参考]
超シンプルにTensorFlowでDQN (Deep Q Network) を実装してみる 〜導入編〜  | ALGO GEEKS
[Python]強化学習(DQN)を実装しながらKerasに慣れる - Qiita
DQNをKerasとTensorFlowとOpenAI Gymで実装する

arduinoエラーについて

arduino開発で突如出たエラーについてメモ。

以下、エラー内容

/arduino/hardware/tools/avr/bin/../lib/gcc/avr/4.9.2/../../../../avr/lib/avr5/crtatmega328p.o:(.init9+0x0): undefined reference to `main'

この時はarduinoIDEのウインドウを開きすぎなので一度全部閉じましょう。
また、タブを作ってやるのもだめらしい。。

pythonで3リンク逆運動学を解いて表示する

pythonで3リンク逆運動学を計算して2次元で表示します。

f:id:weekendproject9:20171117134827p:plain

環境

win7 64bit
python 3.5.2
matplotlib 1.5.3

プログラム

・3リンク問題を逆運動学で算出してmatplotlibで表示しています
・Px,Pyが手先目標座標なので、使用するときは変更してください
・L1,L2,L3のリンクの長さは直接指定しているので、使用するときは変更してください
・tht3は直接指定しているので、使用するときは変更してください
・エラー系は実装していません

# -*- coding: utf-8 -*-
import math
import numpy
import matplotlib.pyplot as plt
 
fig = plt.figure()

#アーム手先座標
Px = 15
Py = 20

#Linkの長さ 
L1 = 10
L2 = 10
L3 = 10


#逆運動学計算↓
#手先座標に対し
# x =L1cos(tht1)+L2cos(tht1+th2)+L3cos(tht1+tht2+tht3)
# y =L1sin(tht1)+L2sin(tht1+th2)+L3sin(tht1+tht2+tht3)
# tht = tht1+tht2+tht3
#が成り立つ

# L3の角度指定。(x軸とのなす角)
tht = math.radians(90)

#tht1の算出
A = Py - (L3*math.sin(tht))
B = Px - (L3*math.cos(tht))
C = (pow((Py - (L3*math.sin(tht))),2) + pow((Px - (L3*math.cos(tht))),2) + pow(L1,2) - pow(L2,2)) / (2*L1)
tht1= math.atan2(A,B) - math.atan2(math.sqrt((pow(A,2)+pow(B,2)-pow(C,2))),C)

#tht2の算出
D = (pow((Py - L3*math.sin(tht)),2) + pow((Px - L3*math.cos(tht)),2) - pow(L1,2) + pow(L2,2)) / (2*L2)
tht2 = math.atan2(math.sqrt((pow(A,2)+pow(B,2)-pow(C,2))),C) - math.atan2(math.sqrt((pow(A,2)+pow(B,2)-pow(D,2)))*-1,D)

#tht3の算出
tht3 = tht-tht2-tht1


#表示のために順運動学計算する
#リンク1の座標算出
#数式↓
#x =L1cos(tht1)
#y =L2sin(tht1)
#プログラム↓
x1 = L1 * numpy.cos(tht1)
y1 = L1 * numpy.sin(tht1)

#リンク2の座標算出
#数式↓
#x2 =L1cos(tht1)+L2cos(tht1+th2)
#y2 =L1sin(tht1)+L2sin(tht1+th2)
#プログラム↓
x2 = x1 + (L2 * numpy.cos((tht1+tht2)))
y2 = y1 + (L2 * numpy.sin((tht1+tht2)))
 
#リンク3の座標算出
#数式↓
#x3 =L1cos(tht1)+L2cos(tht1+th2)+L3cos(tht1+tht2+tht3)
#y3 =L1sin(tht1)+L2sin(tht1+th2)+L3sin(tht1+tht2+tht3)
#プログラム↓
x3 = x2 + (L3 * numpy.cos((tht1+tht2+tht3)))
y3 = y2 + (L3 * numpy.sin((tht1+tht2+tht3)))

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

#表示
#リンクの座標表示↓
plt.text(x1, y1, x1, verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
plt.text(x1, y1-0.8, y1,verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
plt.text(x2, y2, x2,verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
plt.text(x2, y2-0.8, y2, verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
plt.text(x3, y3, x3,verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
plt.text(x3, y3-0.8, y3, verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
#線分表示↓
plt.plot(x,y,"r-")

#表示実行
plt.show()



pythonで2リンク逆運動学を解いて表示する

pythonで2リンク逆運動学を計算して2次元で表示します。

f:id:weekendproject9:20171117013028p:plain

環境

win7 64bit
python 3.5.2
matplotlib 1.5.3

プログラム

・2リンク問題を逆運動学で算出してmatplotlibで表示しています
・Px,Pyが手先目標座標なので、使用するときは変更してください
・L1,L2のリンクの長さは直接指定しているので、使用するときは変更してください
・計算結果はprint文とグラフ上に表示されます
・エラー系は実装していません

import math
import numpy
import matplotlib.pyplot as plt
 
fig = plt.figure()

#アーム手先座標
Px = 6
Py = 7.5

#Linkの長さ 
L1 = 10
L2 = 10

#逆運動学計算↓
L3 = math.sqrt((Px*Px) + (Py*Py))
fai2 = math.acos(((L1*L1) + (L2*L2) - ( L3 * L3)) / (2*L1*L2))
tht2 = math.pi - fai2
fai1 = math.acos(((L1*L1) + (L3 * L3) - (L2*L2)) / (2*L1*L3))
fai0 = math.atan(Py / Px)
tht1 = math.atan2(Py , Px) - fai1

#角度計算↓
#deg1 = math.degrees(tht1)
#deg2 = math.degrees(tht2)

#表示のために順運動学計算する
#リンク1の座標算出
#数式↓
#x =L1cos(tht1)
#y =L2sin(tht1)
#プログラム↓
x1 = L1 * numpy.cos(tht1)
y1 = L1 * numpy.sin(tht1)

#リンク1の座標算出
#数式↓
#x2 =L1cos(tht1)+L2cos(tht1+th2)
#y2 =L1sin(tht1)+L2sin(tht1+th2)
#プログラム↓
x2 = x1 + L2 * numpy.cos((tht1+tht2))
y2 = y1 + L2 * numpy.sin((tht1+tht2))
 
#計算結果表示↓
print(x1)
print(x2)
print(y1)
print(y2)


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

#表示
#リンクの座標表示↓
plt.text(x1, y1, x1, verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
plt.text(x1, y1-0.8, y1,verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
plt.text(x2, y2, x2,verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
plt.text(x2, y2-0.8, y2, verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
#線分表示↓
plt.plot(x,y,"r-")

#表示実行
plt.show()


pythonで2リンク順運動学を解いて表示する

pythonで2リンク順運動学を計算して2次元で表示します。

f:id:weekendproject9:20171117005404p:plain

環境

win7 64bit
python 3.5.2
matplotlib 1.5.3

プログラム

・2リンク問題を順運動学で算出してmatplotlibで表示しています。
・L1,L2のリンクの長さは直接指定しているので使用するときは変更してください
・deg1,deg2はリンクの角度なので使用するときは変更してください
・計算結果はprint文とグラフ上に表示されます
・エラー系は実装していません

import math
import numpy
import matplotlib.pyplot as plt
 
fig = plt.figure()

#Linkの長さ 
L1 = 10
L2 = 10

#各リンクの角度指定
deg1 = 30
deg2 = 50

#リンク1の座標算出
#数式↓
#x =L1cos(tht1)
#y =L2sin(tht1)
#プログラム↓
x1 = L1 * numpy.cos(math.radians(deg1))
y1 = L1 * numpy.sin(math.radians(deg1))

#リンク1の座標算出
#数式↓
#x2 =L1cos(tht1)+L2cos(tht1+th2)
#y2 =L1sin(tht1)+L2sin(tht1+th2)
#プログラム↓
x2 = x1 + L2 * numpy.cos(math.radians(deg1+deg2))
y2 = y1 + L2 * numpy.sin(math.radians(deg1+deg2))
 
#計算結果表示↓
print(x1)
print(x2)
print(y1)
print(y2)


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

#表示
#リンクの座標表示↓
plt.text(x1, y1, x1, verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
plt.text(x1, y1-0.8, y1,verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
plt.text(x2, y2, x2,verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
plt.text(x2, y2-0.8, y2, verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8)
#線分表示↓
plt.plot(x,y,"r-")

#表示実行
plt.show()