detamamoruのブログ

興味を持ったことや勉強したことに関して記事を書きます。主に低レイヤー寄りの記事を公開。

初めてのロボットアーム作りに挑戦！5-2 PFRLでまずは簡単な動作を学習(後半)

ロボット

出田守です。
アイアンマンを観て、ロボットアームが欲しくなりました。
勉強しながら、初めてのロボットアーム作りに挑戦します。

今回の目標

  1. PFRLを使って簡単な動作を学習

今回はロボットアーム君に簡単な動作をPFRLを使って学習させたいと思います。
長くなったので前半と後半に分けています。
前半は「Raspberry Pi 4のGPIOを使えるようにする」から「カメラ画像をROSでサブスクライブする」まで終わりました。
後半は「ボタン状態をパブリッシュする」から「簡単なタスクを強化学習で学習」まで終わらせたいです。(終わりませんでした・・・)

環境

今回挑戦するタスク

単純にロボットアーム君がボタンを押すという動作を学習させようと思います。
まず全体のブロックチャートを載せます。

f:id:detamamoru:20210106151148p:plain
全体のブロックチャート

できればもっとROSを活用したいですが、まずはPFRLに慣れます。
そして今回のタスクを追加した図を載せます。

f:id:detamamoru:20210106155656p:plain
簡単タスク

スイッチのON/OFF状態をRaspberry Pi 4で受け取り、サービスサーバに保持しておきます。PFRLの報酬計算の際にサービスサーバからスイッチの状態を受け取ります。
スイッチが正しく押された場合は報酬を受け取るという流れです。

ボタン状態のpublisherとsubscliberを作成

ボタンが押された(=0)または離れた(=1)をROSを使ってパブリッシュ/サブスクライブします。

ボタン状態のpublisher

  1. Raspberry Pi 4にログイン
  2. パッケージ作成

    cd catkin_ws/src
catkin_create_pkg button_status std_msgs rospy

    package.xmlを以下のように編集します。

    <?xml version="1.0"?>
<package format="2">
  <name>button_status</name>
  <version>0.1.0</version>
  <description>This package publish the button status</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="jobs4biggieboo@gmail.com">beggieboo</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>MIT</license>


  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/button_status</url> -->
  <url type="repository">https://github.com/BiggieBoo18/mr_robotarm</url>
  <url type="website">https://detamamoru.hateblo.jp</url>


  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
  <author email="jobs4beggieboo@gmail.com">biggieboo</author>

  <!-- The *depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

    CMakeLists.txtの以下の行をアンコメントして編集します。

    catkin_install_python(PROGRAMS
  scripts/button_status_node.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)


  3. publisher作成
    まずはscriptsフォルダを作成します。

    cd catkin_ws/src/button_status
mkdir scripts

    次にpublisherです。ほぼチュートリアルのpublisherと同じですが、型とノード名などは変えています。今回はボタンが押されていれば0を、離れていれば1を配信します。

    #!/usr/bin/env python
import RPi.GPIO as GPIO
import rospy
from std_msgs.msg import Int8

GPIO_PIN = 17

def button_status():
    pub = rospy.Publisher('button_status', Int8, queue_size=10)
    rospy.init_node('button_status', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        try:
            button_status = GPIO.input(GPIO_PIN) # get the button status
            rospy.loginfo(button_status)
            pub.publish(button_status)
            rate.sleep()
        except:
            pass

if __name__ == '__main__':
    try:
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(GPIO_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP)
        button_status()
    except rospy.ROSInterruptException:
        pass
    finally:
        GPIO.cleanup(GPIO_PIN)

    実行権限を与えます。

    chmod +x scripts/button_status_node.py


  4. 実行

    cd catkin_ws
rosrun button_status button_status_node.py


ボタン状態のsubscliber

  1. パッケージ作成

    cd catkin_ws/src
catkin_create_pkg button_status_receiver std_msgs rospy

    package.xml、CMakeLists.txtはpublisherと説明やファイル名以外一緒なので割愛します。


  2. publisher作成
    まずはscriptsフォルダを作成します。

    cd catkin_ws/src/button_status_receiver
mkdir scripts

    次にpublisherです。

    import rospy
from std_msgs.msg import Int8

def callback(data):
    with open("./button_status.txt", "w+") as fp:
        fp.write(str(data.data))

def receiver():
    rospy.init_node('button_status_receiver_node', anonymous=True)

    rospy.Subscriber("button_status", Int8, callback)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    receiver()

    publisher(button_status_node)からパブリッシュされるボタン状態をサブスクライブして、テキストファイルに保存します。

    cd catkin_ws
catkin_make


  3. 実行

    cd catkin_ws
rosrun button_status_receiver scripts/button_status_receiver_node.py

    実行するとカレントディレクトリにbutton_status.txtというファイルでボタン状態をテキストファイルに保存しています。
    カメラ画像とボタン状態をファイルに保存できたので、強化学習環境でこれらを都度読み込むという算段です。


Windows10からESP32へBluetooth通信

ロボットアーム君を操作するためにWindows10側からESP32マイコン側へBluetooth通信する必要があります。
調べた感じ「bleak」というpythonライブラリがwindowsで使う上ではよさそうです。
インストールはpipで行います。

pip install bleak

まずは、サンプルのdiscover.pyで周辺のBluetooth機器をスキャンします。

"""
Scan/Discovery
--------------
Example showing how to scan for BLE devices.
Updated on 2019-03-25 by hbldh <henrik.blidh@nedomkull.com>
"""

import asyncio
from bleak import discover


async def run():
    devices = await discover()
    for d in devices:
        print(d)


loop = asyncio.get_event_loop()
loop.run_until_complete(run())

接続対象となるBLEのアドレスを覚えておきます。
次に接続し、モータの操作ができるか試します。モータの操作コマンドは以前作成したFs90Controllerを使っています。
motor_cmd.py

import argparse
import asyncio

from bleak import BleakClient

# UART service UUID
CHARACTERISTIC_UUID_RX = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
# BLE MAC ADDRESS
ADDRESS = "F0:08:D1:C8:A1:5E"

async def run(angle, loop):
    async with BleakClient(ADDRESS, loop=loop) as client: # connect to ble device
        connected = await client.is_connected() # is connected?
        print(f"Connected: {connected}") # connected = True or False
        write_value = "1 " + angle
        await client.write_gatt_char(CHARACTERISTIC_UUID_RX, bytearray(write_value.encode()))

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--angle", "-a", type=str, help="motor angle")
    args = parser.parse_args()

    loop = asyncio.get_event_loop()
    loop.run_until_complete(run(args.angle, loop))

python motor_cmd.py --angle 90

外から呼びやすいようにクラスにしました。
ble_motor.py

import argparse
import asyncio

from bleak import BleakClient

# UART service UUID
CHARACTERISTIC_UUID_RX = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
# BLE MAC ADDRESS
ADDRESS = "F0:08:D1:C8:A1:5E"

class BleMotor:
    def __init__(self, loop, address=ADDRESS, write_uuid=CHARACTERISTIC_UUID_RX):
        self.loop       = loop
        self.address    = address
        self.write_uuid = write_uuid
        self.client     = None

    async def connect(self):
        self.client = BleakClient(self.address)
        await self.client.connect()
        connected = await self.client.is_connected() # is connected?
        print(f"Connected: {connected}") # connected = True or False
        return connected

    async def disconnect(self):
        disconnected = await self.client.disconnect()
        print(f"Disconnected: {disconnected}")
        return disconnected

    async def write(self, motor_num, angle):
        write_value = str(motor_num) + " " + str(angle)
        await self.client.write_gatt_char(self.write_uuid, bytearray(write_value.encode()))

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--angle", "-a", type=str, default="90", help="motor angle")
    parser.add_argument("--motor", "-m", type=str, default="1", help="motor number(0~2)")
    args = parser.parse_args()

    loop = asyncio.get_event_loop()
    ble_motor = BleMotor(loop)
    loop.run_until_complete(ble_motor.connect())
    loop.run_until_complete(ble_motor.write(args.motor, args.angle))
    loop.run_until_complete(ble_motor.disconnect())

これでやっと、強化学習の準備はできました(たぶん・・・)。

参考

強化学習環境作成

今回のタスクを学習させるための強化学習環境を作成します。
流れとしては、エージェントはカメラ画像を受け取って、アクションし、ボタン状態によって報酬を受け取り、学習します。カメラ画像はロボットアーム君自身とスイッチが見えるようにします。アクションは、ロボットアーム君の関節の角度を出力します。報酬は、ボタンの状態から正しくスイッチを押せたかどうかを判断し計算します。
今回はロボットアーム君の関節角度を出力する、つまり連続値を出力する必要があるので、連続型の強化学習アルゴリズム「TD3」を選択しました。
まずは1関節を動かして動作を学習させます。
## OpenAIのgymの環境を作成
OpenAIのgymがあれば、学習が楽なので今回用の環境を作成します。以下が環境のソースコードです。
mr_robotarm_env/env_v0.py

import asyncio
import sys
import time

import cv2
import gym
import numpy as np
import gym.spaces

CMD_ANGLE1    = "0"
CMD_ANGLE2    = "1"
CMD_ANGLE3    = "2"
CMD_DUTYCYCLE = "3"
ANGLE_LOW     = 5
ANGLE_HIGH    = 175

CAMERA_IMAGE_PATH  = "catkin_ws/camera_image.jpg"
BUTTON_STATUS_PATH = "catkin_ws/button_status.txt"

class MrRobotarmEnv(gym.Env):
    def __init__(self):
        super().__init__()
        img = self.read_image() # read camera image
        self.action_space      = gym.spaces.Box(low=ANGLE_LOW, high=ANGLE_HIGH, shape=(1,), dtype=np.float32)
        self.observation_space = gym.spaces.Box(low=0, high=255, shape=img.shape, dtype=np.float32) # image size
        self.ble_motor = None
        self.loop      = None
        self.viewer    = None
        self.rewards   = 0

    def set_ble_motor(self, ble_motor, loop):
        self.ble_motor = ble_motor
        self.loop      = loop
        # reset observation
        self.reset()

    def reset(self):
        self.motor_action(CMD_ANGLE2, ANGLE_HIGH)
        observation   = self.read_image() # read the camera image
        return observation

    def step(self, a):
        # action
        self.motor_action(CMD_ANGLE2, a[0])

        # next observation
        observation   = self.read_image() # read the camera image

        # calculate reward
        button_status = self.read_button_status() # read the button status
        reward = -1
        if not button_status:
            reward = 1

        done = False

        return observation, reward, done, {}

    def render(self, mode='human'):
        img = self.read_image() # read the camera image
        if mode == 'rgb_array':
            return img
        elif mode == 'human':
            from gym.envs.classic_control import rendering
            if self.viewer is None:
                self.viewer = rendering.SimpleImageViewer()
            self.viewer.imshow(img)
            return self.viewer.isopen

    def motor_action(self, motor_num, angle):
        self.loop.run_until_complete(self.ble_motor.write("0", 90))          # servo1
        time.sleep(0.5)
        self.loop.run_until_complete(self.ble_motor.write(motor_num, angle)) # servo2
        time.sleep(0.5)
        self.loop.run_until_complete(self.ble_motor.write("2", 0))           # servo3
        time.sleep(1)

    def read_image(self):
        def read_whole_image():
            with open(CAMERA_IMAGE_PATH, 'rb') as f:
                check_chars = f.read()[-2:]
                if check_chars != b'\xff\xd9':
                    return None
                else:
                    return cv2.imread(CAMERA_IMAGE_PATH) # read the camera image

        img = read_whole_image()
        while img is None:
            img = read_whole_image()

        # shrink the image
        h, w = img.shape[:2]
        size = (int(h / 2), int(w / 2))
        img = cv2.resize(img, size, interpolation=cv2.INTER_AREA)

        return img

    def read_button_status(self):
        button_status = open(BUTTON_STATUS_PATH).read() # read the button status
        while not button_status:
            button_status = open(BUTTON_STATUS_PATH).read() # read the button status
        return int(button_status)

そして、gymに環境を登録しておきます。
mr_robotarm_env/__init__.py

from gym.envs.registration import register

register(
    id='mr_robotarm_env-v0',
    entry_point='mr_robotarm_envs.env_v0:MrRobotarmEnv''
)

こうすることで、

import mr_robotarm_env
import gym

env = gym.make("mr_robotarm_env-v0")

とするだけで、環境が作れます。
ボタンが押されたら報酬を+1もらえます。アクションのたびにボタンが押されていたら+1がもらえるので、長押ししていればもらえます。

TD3の学習

PFRLのTD3学習サンプルを改造して、ロボットアーム君学習用にします。
以下がソースです。
train_td3.py

import argparse
import asyncio
import logging
import sys

import gym
import gym.wrappers
import numpy as np
import torch
from torch import nn

import pfrl
from pfrl import experiments, explorers, replay_buffers, utils

from mr_robotarm_envs.env_v0 import MrRobotarmEnv
from ble.ble_motor import BleMotor

def main():

    parser = argparse.ArgumentParser()
    parser.add_argument(
        "--outdir",
        type=str,
        default="results",
        help=(
            "Directory path to save output files."
            " If it does not exist, it will be created."
        ),
    )
    parser.add_argument(
        "--env",
        type=str,
        default="mr_robotarm_env-v0",
        help="Mr Robotarm env to perform algorithm on.",
    )
    parser.add_argument("--seed", type=int, default=0, help="Random seed [0, 2 ** 32)")
    parser.add_argument(
        "--gpu", type=int, default=0, help="GPU to use, set to -1 if no GPU."
    )
    parser.add_argument(
        "--load", type=str, default="", help="Directory to load agent from."
    )
    parser.add_argument(
        "--steps",
        type=int,
        default=10 ** 6,
        help="Total number of timesteps to train the agent.",
    )
    parser.add_argument(
        "--episode_steps",
        type=int,
        default=10,
        help="Number of steps per episode",
    )
    parser.add_argument(
        "--eval-n-runs",
        type=int,
        default=10,
        help="Number of episodes run for each evaluation.",
    )
    parser.add_argument(
        "--eval-interval",
        type=int,
        default=5000,
        help="Interval in timesteps between evaluations.",
    )
    parser.add_argument(
        "--replay-start-size",
        type=int,
        default=10000,
        help="Minimum replay buffer size before " + "performing gradient updates.",
    )
    parser.add_argument("--batch-size", type=int, default=100, help="Minibatch size")
    parser.add_argument("--gray", action="store_true", default=False, help="input gray image")
    parser.add_argument(
        "--render", action="store_true", help="Render env states in a GUI window."
    )
    parser.add_argument(
        "--demo", action="store_true", help="Just run evaluation, not training."
    )
    parser.add_argument("--load-pretrained", action="store_true", default=False)
    parser.add_argument(
        "--pretrained-type", type=str, default="best", choices=["best", "final"]
    )
    parser.add_argument(
        "--monitor", action="store_true", help="Wrap env with gym.wrappers.Monitor."
    )
    parser.add_argument(
        "--log-level", type=int, default=logging.INFO, help="Level of the root logger."
    )
    args = parser.parse_args()

    logging.basicConfig(level=args.log_level)

    args.outdir = experiments.prepare_output_dir(args, args.outdir, argv=sys.argv)
    print("Output files are saved in {}".format(args.outdir))

    # Set a random seed used in PFRL
    utils.set_random_seed(args.seed)

    def make_env(test, ble_motor, loop, max_episode_steps):
        env = gym.make(args.env)
        env.set_ble_motor(ble_motor, loop)
        # wrap TimeLimit
        env = gym.wrappers.TimeLimit(env, max_episode_steps)
        # Unwrap TimeLimit wrapper
        assert isinstance(env, gym.wrappers.TimeLimit)
        env = env.env
        # Use different random seeds for train and test envs
        env_seed = 2 ** 32 - 1 - args.seed if test else args.seed
        env.seed(env_seed)
        # Cast observations to float32 because our model uses float32
        env = pfrl.wrappers.CastObservationToFloat32(env)
        if args.monitor:
            env = pfrl.wrappers.Monitor(env, args.outdir)
        if args.render and not test:
            env = pfrl.wrappers.Render(env)
        if args.gray:
            env = gym.wrappers.GrayScaleObservation(env)
        return env

    # initialize BleMotor
    loop = asyncio.get_event_loop()
    ble_motor = BleMotor(loop)
    loop.run_until_complete(ble_motor.connect())

    env = make_env(test=False, ble_motor=ble_motor, loop=loop, max_episode_steps=args.episode_steps)
    timestep_limit = env.spec.max_episode_steps
    obs_space = env.observation_space
    action_space = env.action_space
    print("Observation space:", obs_space)
    print("Action space:", action_space)
    print(f"{args.steps=}, {timestep_limit=}")

    obs_size = obs_space.low.size
    action_size = action_space.low.size

    policy = nn.Sequential(
        nn.Linear(obs_size, 1500),
        nn.ReLU(),
        nn.Linear(1500, 1000),
        nn.ReLU(),
        nn.Linear(1000, 300),
        nn.ReLU(),
        nn.Linear(300, action_size),
        nn.Tanh(),
        pfrl.policies.DeterministicHead(),
    )
    policy_optimizer = torch.optim.Adam(policy.parameters())

    def make_q_func_with_optimizer():
        q_func = nn.Sequential(
            pfrl.nn.ConcatObsAndAction(),
            nn.Linear(obs_size + action_size, 1500),
            nn.ReLU(),
            nn.Linear(1500, 1000),
            nn.ReLU(),
            nn.Linear(1000, 400),
            nn.ReLU(),
            nn.Linear(400, 300),
            nn.ReLU(),
            nn.Linear(300, 1),
        )
        q_func_optimizer = torch.optim.Adam(q_func.parameters())
        return q_func, q_func_optimizer

    q_func1, q_func1_optimizer = make_q_func_with_optimizer()
    q_func2, q_func2_optimizer = make_q_func_with_optimizer()

    rbuf = replay_buffers.ReplayBuffer(10 ** 6)

    explorer = explorers.AdditiveGaussian(
        scale=0.1, low=action_space.low, high=action_space.high
    )

    def burnin_action_func():
        """Select random actions until model is updated one or more times."""
        return np.random.uniform(action_space.low, action_space.high).astype(np.float32)

    # Hyperparameters in http://arxiv.org/abs/1802.09477
    agent = pfrl.agents.TD3(
        policy,
        q_func1,
        q_func2,
        policy_optimizer,
        q_func1_optimizer,
        q_func2_optimizer,
        rbuf,
        gamma=0.99,
        soft_update_tau=5e-3,
        explorer=explorer,
        replay_start_size=args.replay_start_size,
        gpu=args.gpu,
        minibatch_size=args.batch_size,
        burnin_action_func=burnin_action_func,
    )

    if len(args.load) > 0 or args.load_pretrained:
        # either load or load_pretrained must be false
        assert not len(args.load) > 0 or not args.load_pretrained
        if len(args.load) > 0:
            agent.load(args.load)
        else:
            agent.load(
                utils.download_model("TD3", args.env, model_type=args.pretrained_type)[
                    0
                ]
            )

    eval_env = make_env(test=True, ble_motor=ble_motor, loop=loop, max_episode_steps=args.episode_steps)
    if args.demo:
        eval_stats = experiments.eval_performance(
            env=eval_env,
            agent=agent,
            n_steps=None,
            n_episodes=args.eval_n_runs,
            max_episode_len=timestep_limit,
        )
        print(
            "n_runs: {} mean: {} median: {} stdev {}".format(
                args.eval_n_runs,
                eval_stats["mean"],
                eval_stats["median"],
                eval_stats["stdev"],
            )
        )
        import json
        import os

        with open(os.path.join(args.outdir, "demo_scores.json"), "w") as f:
            json.dump(eval_stats, f)
    else:
        experiments.train_agent_with_evaluation(
            agent=agent,
            env=env,
            steps=args.steps,
            eval_env=eval_env,
            eval_n_steps=None,
            eval_n_episodes=args.eval_n_runs,
            eval_interval=args.eval_interval,
            outdir=args.outdir,
            train_max_episode_len=timestep_limit,
        )
    ble_motor.disconnect()

if __name__ == "__main__":
    main()

今回960GTXという昔に購入したグラボを使用しているので、カラーだとGPUメモリが足りませんでした。なので、グレースケールでかつ、サイズを半分にして読み込みます。
学習する前にRaspberry Pi 4、Windows 10のROS(カメラ画像とボタン状態のノード)を実行しておきます。
学習実行は以下のようにします。

python train_td3.py --gray

試しに実行しましたが、ちゃんと動作している(と思われる)ものの、ポリシーの更新がされず、Q値がnanのままで正しく学習できていない状態です。
以下が学習中のログです。

> python .\train_td3.py --gray
Output files are saved in results\f1e67d056c7a0a3e5973fdadd56f1d22192847c0-00000000-870eec17
INFO:bleak.backends.dotnet.client:Services resolved for BleakClientDotNet (xx:xx:xx:xx:xx:xx)
Connected: True
Observation space: Box(0, 255, (320, 240), uint8)
Action space: Box(5.0, 175.0, (1,), float32)
args.steps=1000000, timestep_limit=10
Premature end of JPEG file
Premature end of JPEG file
INFO:pfrl.experiments.train_agent:outdir:results\f1e67d056c7a0a3e5973fdadd56f1d22192847c0-00000000-870eec17 step:10 episode:0 R:-10
INFO:pfrl.experiments.train_agent:statistics:[('average_q1', nan), ('average_q2', nan), ('average_q_func1_loss', nan), ('average_q_func2_loss', nan), ('average_policy_loss', nan), ('policy_n_updates', 0), ('q_func_n_updates', 0)]
Premature end of JPEG file
INFO:pfrl.experiments.train_agent:outdir:results\f1e67d056c7a0a3e5973fdadd56f1d22192847c0-00000000-870eec17 step:20 episode:1 R:-4
INFO:pfrl.experiments.train_agent:statistics:[('average_q1', nan), ('average_q2', nan), ('average_q_func1_loss', nan), ('average_q_func2_loss', nan), ('average_policy_loss', nan), ('policy_n_updates', 0), ('q_func_n_updates', 0)]
Premature end of JPEG file
INFO:pfrl.experiments.train_agent:outdir:results\f1e67d056c7a0a3e5973fdadd56f1d22192847c0-00000000-870eec17 step:30 episode:2 R:-6
INFO:pfrl.experiments.train_agent:statistics:[('average_q1', nan), ('average_q2', nan), ('average_q_func1_loss', nan), ('average_q_func2_loss', nan), ('average_policy_loss', nan), ('policy_n_updates', 0), ('q_func_n_updates', 0)]
・
・
・
INFO:pfrl.experiments.train_agent:outdir:results\f1e67d056c7a0a3e5973fdadd56f1d22192847c0-00000000-870eec17 step:3880 episode:387 R:-8
INFO:pfrl.experiments.train_agent:statistics:[('average_q1', nan), ('average_q2', nan), ('average_q_func1_loss', nan), ('average_q_func2_loss', nan), ('average_policy_loss', nan), ('policy_n_updates', 0), ('q_func_n_updates', 0)]
Premature end of JPEG file
INFO:pfrl.experiments.train_agent:outdir:results\f1e67d056c7a0a3e5973fdadd56f1d22192847c0-00000000-870eec17 step:3890 episode:388 R:-10
INFO:pfrl.experiments.train_agent:statistics:[('average_q1', nan), ('average_q2', nan), ('average_q_func1_loss', nan), ('average_q_func2_loss', nan), ('average_policy_loss', nan), ('policy_n_updates', 0), ('q_func_n_updates', 0)]
INFO:pfrl.experiments.train_agent:outdir:results\f1e67d056c7a0a3e5973fdadd56f1d22192847c0-00000000-870eec17 step:3900 episode:389 R:-8
INFO:pfrl.experiments.train_agent:statistics:[('average_q1', nan), ('average_q2', nan), ('average_q_func1_loss', nan), ('average_q_func2_loss', nan), ('average_policy_loss', nan), ('policy_n_updates', 0), ('q_func_n_updates', 0)]

・・・ということで、延長戦突入です。新しいグラボが欲しい！
ここまでの物をgithubに上げてます。
github.com

参考

初めてのロボットアーム作りに挑戦！5-1 PFRLでまずは簡単な動作を学習(前半)

ロボット

出田守です。
アイアンマンを観て、ロボットアームが欲しくなりました。
勉強しながら、初めてのロボットアーム作りに挑戦します。

今回の目標

  1. PFRLを使って簡単な動作を学習

今回はロボットアーム君に簡単な動作をPFRLを使って学習させたいと思います。
長くなったので前半と後半に分けています。

環境

今回挑戦するタスク

単純にロボットアーム君がボタンを押すという動作を学習させようと思います。
まず全体のブロックチャートを載せます。

f:id:detamamoru:20210106151148p:plain
全体のブロックチャート

できればもっとROSを活用したいですが、まずはPFRLに慣れます。
そして今回のタスクを追加した図を載せます。

f:id:detamamoru:20210117164358p:plain
簡単タスク

スイッチのON/OFF状態をRaspberry Pi 4で受け取り、テキストファイルに記録しておきます。PFRLの報酬計算の際にテキストファイルからスイッチの状態を受け取ります。
スイッチが正しく押された場合は報酬を受け取るという流れです。

RPi.GPIOライブラリを使えるようにする

Ubuntu 18.04をインストールしたRaspberry Pi 4上でGPIOを読み書きをできるようにします。私はよく使われる「RPi.GPIO」を使用することにしました。
インストール自体は簡単で、

sudo apt update
sudo apt upgrade
sudo apt install python-pip python-dev
pip install RPi.GPIO

だけです。
しかし、ライブラリをインポートして使おうとすると、RuntimeError: Not running on a RPi!というエラーが発生するので、ubuntuユーザがgpioを触る権限を以下のように与えます。

ls -l /dev/gpiomem
sudo chown root:$USER /dev/gpiomem
sudo chmod g+rw /dev/gpiomem

あるいは以下でもいけるかもしれません。

sudo adduser username gpio

エラー時の参考URL

GPIO17を使って、まずはOUTPUTできるかテストします。テストコードは以下です。

import RPi.GPIO as GPIO

GPIO.setmode(GPIO.BCM)
GPIO.setup(17, GPIO.OUT)

GPIO.output(17, 1)
try:
    while True:
        pass
except:
    pass
finally:
    GPIO.cleanup(17)

テスターなどでGPIO17の電圧が3.3Vになっていることを確認します。Ctrl-cでストップします。
次にINPUTできるかもテストします。

import RPi.GPIO as GPIO
import time

GPIO.setmode(GPIO.BCM)
GPIO.setup(17, GPIO.IN, pull_up_down=GPIO.PUD_UP)

try:
    while True:
        print(GPIO.input(17))
        time.sleep(1)
except:
    pass
finally:
    GPIO.cleanup(17)

ターミナル上に「1」が表示されるはずなので、GPIO17にGroundを接続してあげると「0」に切り替わることを確認します。
以上でGPIOを使えるようになりました。

ROSのPublisherとSubscriberを作成

この時点で気付いてしまったのですが、ROSはPython2をPFRLはPython3を使うので、実行バージョンが合っていないことになります。ROS2をインストールするべきだったかもしれません・・・。
とりあえず、今回はROSからパブリッシュされる画像と ボタンの状態(On/Off)をサブスクライブし保存して、PFRLから都度読み込むという形でやろうと思います。
cv_cameraですでにpublisherがあるので、残りは、画像のsubscliberとボタン状態のpublisherとsubscliberを作ります。

ワークスペース作成

まずはワークスペースを作成します。

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
cd ../
catkin_make
source devel/setup.bash

windowsでは、

mkdir %HOME%\catkin_ws\src
cd %HOME%\catkin_ws\src
catkin_init_workspace
cd ..\
catkin_make
devel\setup.bat

画像のsubscliber

  1. パッケージを作成します。

    cd catkin_ws\src
catkin_create_pkg save_image std_msgs roscpp


  2. package.xmlとCMakeLists.txtを編集
    package.xmlは以下のように編集しました。

    <?xml version="1.0"?>
<package format="2">
  <name>save_image</name>
  <version>0.1.0</version>
  <description>This package stores the image data published by cv_camera.</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="jobs4beggieboo@gmail.com">biggieboo</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>MIT</license>


  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/save_image</url> -->
  <url type="repository">https://github.com/BiggieBoo18/mr_robotarm</url>
  <url type="website">https://detamamoru.hateblo.jp</url>


  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
  <author email="jobs4beggieboo@gmail.com">biggieboo</author>

  <!-- The *depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>cv_camera</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>cv_camera</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>cv_camera</exec_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

    CMakeLists.txtは以下のように編集しました。

    cmake_minimum_required(VERSION 3.0.2)
project(save_image)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  cv_camera
)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   std_msgs
# )

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES save_image
#  CATKIN_DEPENDS roscpp std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
#add_library(${PROJECT_NAME}
#  src/${PROJECT_NAME}/save_image.cpp
#)

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node src/save_image_node.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
)

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_save_image.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)


  3. 依存パッケージのインストール
    cv_cameraパッケージをインストールします。
    まずはsave_image.rosinstallというファイルを作成して、以下のように記述します。

    - git: {local-name: cv_camera, uri: https://github.com/OTL/cv_camera.git, version: master}

    vcsを使ってインポートします。

    cd catkin_ws
vcs import src < src\save_image\save_image.rosinstall
rosdep install -i --from-paths src\cv_camera
    参考


  4. 画像のsubscriberノードの作成
    catkin_ws/src/save_image/src/save_image_node.cppというファイルで画像保存用のノードを作成します。
    コードはcv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImagesを参考に、以下のようにしました。

    #include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

static const std::string OPENCV_WINDOW = "Image window";

class ImageConverter
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;

public:
  ImageConverter()
    : it_(nh_)
  {
    // Subscrive to input video feed and publish output video feed
    image_sub_ = it_.subscribe("/cv_camera/image_raw", 1,
      &ImageConverter::imageCb, this);
    // image_pub_ = it_.advertise("/image_converter/output_video", 1);

    // cv::namedWindow(OPENCV_WINDOW);
  }

  ~ImageConverter()
  {
    // cv::destroyWindow(OPENCV_WINDOW);
  }

  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    // Draw an example circle on the video stream
    // if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
    //   cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));

    // Save image
    cv::imwrite("camera_image.jpg", cv_ptr->image);

    // Update GUI Window
    // cv::imshow(OPENCV_WINDOW, cv_ptr->image);
    // cv::waitKey(3);

    // Output modified video stream
    // image_pub_.publish(cv_ptr->toImageMsg());
  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "save_image_node");
  ImageConverter ic;
  ros::spin();
  return 0;
}

    catkin_wsフォルダでビルドします。

    catkin_make

    ビルドが完了したら、以下のように実行します。

    rosrun save_image save_image_node

    実行すると、catkin_wsフォルダにcamera_image.jpgというファイルが作成されて、カメラ画像が保存されます。


文量が多く、はてなブログがとてつもなく重くなったので、一旦ここまでとします。

参考

初めてのロボットアーム作りに挑戦！4: PFRL + Raspberry pi 4 + Raspberry pi camera v2 + ROS(melodic)

ロボット

出田守です。
アイアンマンを観て、ロボットアームが欲しくなりました。
勉強しながら、初めてのロボットアーム作りに挑戦します。

今回の目標

  1. PFRLをインストール・サンプル実行
  2. Raspberry pi 4にUbuntu 18.04をインストール
  3. Raspberry pi camera v2の動作確認
  4. ROS(melodic)をインストール・動作確認

今回は盛りだくさんです。
ここまでで作ってきたロボットアームの動作をカメラからの画像を入力し強化学習を用いて動作を学習させたいと思いました。
そのための環境構築を行います。

使用した環境

PFRL

友人からPreferred NetworksさんがPFRLという強化学習のライブラリを公開したよと教えてもらいました。
Chainer RLの後継版？で、Chainer RLのコードとも互換性があるようです。
早速インストールします。前提として、Python 3.7.7以上、PyTorchがインストールされていること。

pip install pfrl

私の環境ではPyTorchが入っていないと、エラーが出ました。なので
PyTorch のインストール（Windows 上）
を参考に事前にPyTorchをインストールしました。ちなみにGPUを載せていますので、GPU用をインストールしました。

無事インストールできたら、
pfrl/quickstart.ipynb at master · pfnet/pfrl · GitHub
を実行し、動作確認を行います。

Raspberry pi 4にUbuntu 18.04をインストールと設定

1. ARM用Ubuntu 18.04 serverをダウンロード

まず、ARM用のUbuntu 18.04 serverをダウンロードします。

f:id:detamamoru:20201206202436p:plain
Ubuntu 18.04 server

2. Raspberry Pi Imagerをダウンロード・インストール

Raspberry Pi Imagerをダウンロードし、インストールします。

3. Ubuntu 18.04 serverを書込み・インストール

Rasberry Pi Imagerを起動し、

  1. 「Operating System」→「Use custom」→Ubuntu 18.04 serverイメージを選択
  2. 「SD Card」→書込み対象のSDカードを選択
  3. 「WRITE」

これでSDカードへUbuntu 18.04 serverが書き込まれます。あとは、SDカードをRaspberry pi 4へ差し込み、準備完了です。

4. 初回起動

Raspberry pi 4にモニタやキーボード、マウスを接続します。次に電源を接続し、モニタに起動画面が表示されるのを確認します。初回はUbuntuユーザのパスワードを求められますので、良いパスワードを設定します。しばらくするとログイン画面に進みます。
<注意>電源接続時はまだネットワークは接続しません！

5. 初期設定

Ubuntuにログインしたら、

  1. 有線ネットワークを接続(無線ネットワーク接続出来るのならそれでも良い)
  2. パッケージを更新

    sudo apt update
sudo apt upgrade


  3. 初期設定
    Ubuntu18.04 インストール後の初期設定メモ - Qiitaを参考に、必要な設定を行いました。

  4. zram-configのインストール
    Rasbian用のメモリをUbuntu用にスワップするためにインストールしておきます。

    sudo apt install zram-config


  5. Ubuntuのデスクトップ環境をインストール

    sudo apt install ubuntu-desktop


  6. デスクトップ環境を起動

    startx


6. RAMの割当て

Raspberry pi 4のRAMをUbuntuに適切に割り当てるために、zram-configを使用します。

sudo vim /usr/bin/init-zram-swapping

そして以下の赤線部分のように「* 3」を加えます。こうすることで約5.5GBのメモリスワップを確保します。

f:id:detamamoru:20201218133422p:plain
「* 3」を加える

7. VNC環境構築

キーボードやマウスを毎回接続するのは面倒なのでVNC環境を構築します。

sudo apt update

xfaceデスクトップ環境を使います。gnomeだとsystemdでgnome-sessionのidがうまく取得できず、画面が真っ暗になって正常動作しなかったためです。

sudo apt install xfce4 xfce4-goodies

tigervncを使うことにしました。特に理由はありません。

sudo apt install tigervnc-standalone-server tigervnc-common

初期起動時はVNCパスワードの設定を行います。このパスワードはVNCクライアントから接続する際に必要なので覚えておいてください。

vncserver

すでに起動しているvncserverをkillします。

vncserver -kill :*

xstartupを既存のものから変更します。

mv ~/.vnc/xstartup ~/.vnc/xstartup.bkp
vim ~/.vnc/xstartup
#!/bin/bash
xrdb $HOME/.Xresources
startxfce4 &

実行権限を与えます。

sudo chmod +x ~/.vnc/xstartup

systemdのサービスファイルを作成します。

sudo vim /etc/systemd/system/tigervnc@1.service
[Unit]
Description=tigervnc (Remote access)
After=syslog.target network.target

[Service]
Type=forking
User=ubuntu
Group=ubuntu
PIDFile=/home/ubuntu/.vnc/%H:%i.pid
WorkingDirectory=/home/ubuntu
ExecStartPre=/usr/bin/vncserver -kill :%i > /dev/null 2>&1
ExecStart=/usr/bin/vncserver -localhost :%i
ExecStop=/usr/bin/vncserver -kill :%i

[Install]
WantedBy=multi-user.target

systemdに作成したサービスを登録します。

sudo systemctl daemon-reload
sudo systemctl enable tigerservice@1.service
sudo systemctl start tigerservice@1.service

下記のURLを参考にインストールしました。
Install TigerVNC on Ubuntu 18.04 - JournalDev
Ubuntu 20 .04にVNC をインストールして構成 する方法 | DigitalOcean

8. OpenCV 4.5.0をインストール

使うと思うのでOpenCV 4.5.0(2020/12/17時点の最新バージョン)をインストールしておきます。

  1. 必要なライブラリをインストール

    sudo apt update
sudo apt upgrade
sudo apt install build-essential cmake gcc g++ git unzip pkg-config
sudo apt install libjpeg-dev libpng-dev libtiff-dev
sudo apt install libavcodec-dev libavformat-dev libswscale-dev
sudo apt install libgtk2.0-dev libcanberra-gtk*
sudo apt install libxvidcore-dev libx264-dev
sudo apt install python3-dev python3-numpy python3-pip
sudo apt install python-dev python-numpy
sudo apt install libtbb2 libtbb-dev libdc1394-22-dev
sudo apt install libv4l-dev v4l-utils
sudo apt install libopenblas-dev libatlas-base-dev libblas-dev
sudo apt install liblapack-dev gfortran libhdf5-dev
sudo apt install libprotobuf-dev libgoogle-glog-dev libgflags-dev
sudo apt install protobuf-compiler

  2. OpenCV 4.5.0をダウンロード

    cd ~
wget -O opencv.zip https://github.com/opencv/opencv/archive/4.5.0.zip
wget -O opencv_contrib.zip https://github.com/opencv/opencv_contrib/archive/4.5.0.zip
unzip opencv.zip
unzip opencv_contrib.zip
mv opencv-4.5.0 opencv
mv opencv_contrib-4.5.0 opencv_contrib
cd opencv
mkdir build
cd build
    make -j4
sudo make install
sudo ldconfig
sudo apt-get update


Raspberry Pi Camera V2の接続とテスト

  1. 接続
    下記写真のように、両側の黒いピンを押し上げて、隙間に差し込みます。

    f:id:detamamoru:20201220180038j:plain
    接続前
    f:id:detamamoru:20201220180139j:plain
    接続後


  2. テスト
    OpenCVで確認してみます。
    テストコードは以下。VideoCaptureの引数は私の環境では0でした。

    #test_camera.py
import cv2

capture = cv2.VideoCapture(0)

while(True):
    ret, frame = capture.read()
    windowsize = (800, 600)
    frame = cv2.resize(frame, windowsize)

    cv2.imshow('test cameara',frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

capture.release()
cv2.destroyAllWindows()

    python3 test_camera.py

    ちゃんとカメラ動画が映れば準備完了です。


ROS(melodic)のインストール

Ubuntu 18.04にインストール

Ubuntu 18.04にROS(melodic)をmelodic/Installation/Ubuntu - ROS Wikiに従ってインストールします。
Keyが変わったりするので、必ずwikiを見てインストールします。
今回はros-melodic-desktop-fullをインストールします。
インストールが完了したら、ROSのワークスペースを作成・初期化しておきます。

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
cd ../
catkin_make
echo "source ${HOME}/devel/setup.bash" >> ~/.bashrc

roscoreコマンドを実行してマスターが起動するか確認します。

roscore
...
started core service [/rosout]

Windows 10 64bitにインストール

併せて、Windows 10マシンにもROS(melodic)をInstallation/Windows - ROS Wikiに従ってインストールします。
ただし、最初のChocolateyのインストールがうまくいかなかったので、Chocolatey Software | Installing Chocolateyに従ってインストールします。
PowerShellを管理者権限で起動して、以下を実行します。ただし、コマンドは上記したページをコピーすること！

Set-ExecutionPolicy Bypass -Scope Process -Force; [System.Net.ServicePointManager]::SecurityProtocol = [System.Net.ServicePointManager]::SecurityProtocol -bor 3072; iex ((New-Object System.Net.WebClient).DownloadString('https://chocolatey.org/install.ps1'))

こちらもroscoreコマンドを実行してマスターが起動するか確認します。

roscore
...
started core service [/rosout]

ROSからカメラ画像データを送信

ROSパッケージのcv_cameraを使ってカメラ画像を送ってみます。

cv_cameraパッケージのインストール

cd ~/catkin_ws/src
git clone https://github.com/OTL/cv_camera.git
cd ../
catkin_make

動作確認

まずはローカル内で動かしてみます。

roscore
rosparam set cv_camera/device_id 0
rosrun cv_camera cv_camera_node

rvizで確認します。

rviz

起動後「Displays」→「Image」を追加し、「Image Topic」に「/cv_camera/image_raw」を選択するとカメラ画像が表示されます。

別マシン上でカメラ画像を確認します。
Windows 10側をペアレントとし、Raspberry Pi 4側をチャイルドとします。(ちなみに最近マスター/スレーブという呼び方は差別的として改められているようです。なのでここではマスターをペアレント、スレーブをチャイルドと呼ぶことにします。)
まず、Windows 10側、Raspberry Pi 4側どちらにも環境変数「ROS_HOSTNAME」と「ROS_MASTER_URI」を設定します。
Windows 10側はROS_HOSTNAMEに「自身のIPアドレス」を、ROS_MASTER_URIに「http://自身のIPアドレス:11311」を設定します。
Windows 10側はROS_HOSTNAMEに「自身のIPアドレス」を、ROS_MASTER_URIに「http://Windows 10側のIPアドレス:11311」を設定します。

次に、Windows 10側でroscoreを起動します。さらに、rvizも起動しておきましょう。
Raspberry Pi 4側では先ほどと同様にcv_cameraノードを起動します。
rviz上にImageを追加して、カメラ画像が表示されていれば成功です。

もし、この時何もトピック名が表示されない場合は、Windows 10側のファイアウォールを疑ってください。私の場合受信規則のpythonのパブリックを許可していなかったため、TCP接続できない状態でした。

以上で、環境構築は完了です。

参考

マジなロボットアームが届きました。

ロボット

出田守です。
日本Androidの会秋葉原支部ロボット部 - connpassにてロボットアームの発表をしたところ、
メンバーの方から7軸の真面目なロボットアームをお借りできることになりました。
今回はそのロボットアームの動作確認を行いたいと思います。
内容は以下に沿って進めます。


配線まではやっていただいていたのでスキップします。

火入れ

とりあえずACアダプタを繋いでみました。
すでにサンプルプログラムが書き込まれているのか水平に保たれています。
ただ、やはりエンドエフェクタのモータから無理してる音がします。
とりあえず、サンプルプログラムを見てみて、再度書き込んでみます。

サンプルプログラム書込み

ArduinoとPCを接続し、サンプルプログラムを書き込んでみます。
サンプルプログラムは以下の通りです。
github.com
Arduino IDEの設定で、以下のリンクを参照しようとしたところ、リンク切れでした。
http://wiki.tsukarm.com/Arduio%E9%96%8B%E7%99%BA%E7%92%B0%E5%A2%83%E6%A7%8B%E7%AF%89
とりあえず、Arduino Dueなので以下のURLを参考にしてボードマネージャからインストールします。

そして、TsukArm-LibraryをCloneしてtsukarm_libraryをZIP圧縮し、
Arduino IDEの[スケッチ] -> [ライブラリをインクルード] -> [.ZIP形式のライブラリをインストール]でライブラリをインストールしました。
その後書込みを行って、アームが水平に保たれていることが確認できました。

初めてのロボットアーム作りに挑戦！3: 3リンクの運動学とアーム製作

ロボット

出田守です。
アイアンマンを観て、ロボットアームが欲しくなりました。
勉強しながら、初めてのロボットアーム作りに挑戦します。

今回の目標

3リンクアームを製作する。

材料

  1. サーボモータ FS90 3個
  2. ESP32DevKitCv4 1個
  3. 単3×4電池ボックス 1個
  4. 単3電池 4本
  5. マイクロUSBケーブル 1本

3リンクにおける順運動学

f:id:detamamoru:20201123164657j:plain
3リンクの角度と位置

\begin{aligned} x &= (L_2cos(\theta_2) + L_3cos(\theta_2 + \theta_3))cos(\theta_1)\\ y &= (L_2cos(\theta_2) + L_3cos(\theta_2 + \theta_3))sin(\theta_1)\\ z &= L_2sin(\theta_2) + L_3sin(\theta_2 + \theta_3) + L_1 \end{aligned}

3リンクにおける逆運動学

以下は、余弦定理を用いて導出しました。

\begin{aligned} \overline{OP} &= \sqrt{x^2 + y^2}\\ \alpha &= cos^{-1}(\frac{L_2^2 + L_3^2 - \overline{OP}^2}{2L_2L_3})\\ \beta &= cos^{-1}(\frac{L_2^2 + \overline{OP}^2 - L_3^2}{2L_2\overline{OP}})\\ \gamma &= sin^{-1}(\frac{z - L_1}{\overline{OP}})\\ \theta_1 &= tan^{-1}(\frac{y}{x})\\ \theta_2 &= \gamma \mp\beta\\ \theta_3 &= \pm(\pi - \alpha) \end{aligned}

3リンクアームシミュレータ

gist.github.com

3リンクアームの製作

今回は、アプリで3D操作をどうするか分からなかったので、単純に座標x, y, zを指定して送信するようにしました。
github.com

今回は動画無しです。

参考

初めてのロボットアーム作りに挑戦！2: 2リンクの運動学とアーム製作

ロボット

出田守です。
アイアンマンを観て、ロボットアームが欲しくなりました。
勉強しながら、初めてのロボットアーム作りに挑戦します。

今回の目標

2リンクアームを製作する。

材料

  1. サーボモータ FS90 2個
  2. ESP32DevKitCv4 1個
  3. 単3×4電池ボックス 1個
  4. 単3電池 4本
  5. マイクロUSBケーブル 1本

2リンクにおける順運動学

f:id:detamamoru:20200928085416j:plain
2リンクの角度と位置

2リンクの順運動学は、1リンクの順運動学の式と2リンク目における角度を足し合わせた式になります。

\begin{aligned} x &= L1cos\theta_1+L2cos(\theta_1+\theta_2) \\ y &= L1sin\theta_1+L2sin(\theta_1+\theta_2) \\ \end{aligned}

2リンクにおける逆運動学

2リンクの逆運動学は順運動学の式から変形する方法や、余弦定理を用いる方法などいくつかあるようです。
以下のサイトでは、順運動学の式から変形し導出しています。

以下の本では、余弦定理から導出していました。

逆運動学の式は以下です。

\begin{aligned} \theta_1 &= \pm{cos^{-1}(\frac{x^2+y^2+L_1^2-L_2^2}{2L_1\sqrt{x^2+y^2}})}+tan^{-1}(\frac{y}{x}) \\ \theta_2 &= tan^{-1}(\frac{y-L_1sin\theta_1}{x-L_1cos\theta_1})-\theta_1 \\ \end{aligned}
1リンクの時とは違い複雑です。

2リンク運動学シミュレータ

逆運動学の動きを見るために、Jupyter notebookを使って、シミュレータみたいなものを作りました。
gist.github.com

2リンクアーム製作

今回は、アプリ側でタッチされた座標をもとに逆運動学で入力する角度を算出し、それをサーボモータFS90(2つ)に入力することにします。

f:id:detamamoru:20200928174311j:plain
アプリとサーボモータの入出力

前々回作成したアプリケーションに機能を追加しました。
github.com

Demo動画

youtu.be

参考

初めてのロボットアーム作りに挑戦！1: 1リンクの運動学

ロボット

出田守です。
アイアンマンを観て、ロボットアームが欲しくなりました。
勉強しながら、初めてのロボットアーム作りに挑戦します。

今回の目標

1リンクにおける運動学を理解する。

運動学(Kinematics)

運動学とは、マニピュレータやリンクの状態(角度や長さなど)と手先状態(位置と姿勢)の関係を図形的に理解することです。

f:id:detamamoru:20200916132836j:plain
1リンクの角度と位置

運動学は、順運動学と逆運動学の2つに分類できます。

順運動学(Forward Kinematics)

順運動学は、マニピュレータの変位(直動関節の場合は長さ l、回転関節の場合は角度 θ)から、手先位置や姿勢を求めます。
以下は回転関節で、順運動学を計算する式です。

\begin{aligned} x &= Lcos\theta \\ y &= Lsin\theta \\ \end{aligned}

逆運動学(Inverse Kinematics)

逆運動学は、順運動学とは逆に手先位置から変位を求めます。
以下は回転関節で手先位置 x,yが与えられている場合の逆運動学を計算する式です。

\theta = tan^{-1}(\frac{y}{x})
今回はリンクが1つしかないので簡単に求められますが、リンクが増えていくにつれて逆運動学の式がめちゃくちゃ複雑になっていくらしいです。

前回のAndroidアプリに順運動学を用いた手先位置表示を追加しました。
github.com

次回は2リンクアームを製作します。