専門実験2025ロボット制御Ⅰ：第１週実習１実機での操作（ROS2）

実習１：実機での操作 (ROS2)

このページは以下のRobotis社e-manualにベースに作成したものです．以下の説明を良く読んで、そのとおり実習を進めてください。

OpenMANIPULATOR-X e-manual

ロボットを動かす準備
- 本実験ではOpenMANIPULATOR-XのコントローラとしてU2D2 (デバイス名：ttyUSB）を使います．
- まず，U2D2とノートPCをUSBケーブルで接続する。
- 次に，U2D2 Power Hub BoardにACアダプタ(SMPS 12V5A) の電源ケーブルを接続して，そのPower switch（電源スイッチ）を入れる．下図のTTL Cableはすでに接続されている。本実験の器材での接続は下の写真を参照（U2D2 Power Hub  BoardとU2D2が一体化している）。なお、１週目の実験では使わないが、2週目の実験ではロボットのカメラを使うので、写真にはカメラのUSBケーブルがUSBハブに接続されている。
  　　　　　システム構成
- ノートPCの電源を入れてUbuntuを起動する。
- ログイン時にユーザ名とパスワードを聞かれるので以下を入力する．なお，パスワードを入力しても、打ったキーなど何も見えないが気にせず入力してからEnterキーを押す．
  - ユーザ名: kit
  - パスワード:  ILoveKIT
- ユーザがデバイスを使えるように，chmodコマンドでファイルのパーミッション（権限）を変更する．777は全てのユーザに読み，書き，実行権を与える意味．パスワードを聞かれるので、先ほどのパスワードを入力する。
  - $  sudo chmod 777 /dev/ttyUSB0
- ctrl, alt, tキーを同時に押して端末を開いて，次のlsコマンドを入力してttyUSB0というファイルがあるか確認する．先頭の$はプロンプト（コマンド入力の目印）なので打ち込まない．lsコマンドはファイルを表示するコマンド．ttyUSB0が表示されていれば，U2D2がコンピュータに認識されている．なければ認識されていないので，U2D2とコンピュータとの接続とU2D2 Power Hub Boardの電源が入っているか確認する．
  - $  ls /dev
Dockerの起動とターミナルの使い方
- ターミナル上で，作業フォルダへ移動
  - $ cd ~/docker-foxy2024-main
- Dockerの実行
  - $ sudo ./run-docker-container.sh (/run-docker-container.shの前にドット.がある）
- ターミナルTerminatorの起動
  - $ terminator
- エディタVimの起動
  - $ vim
- Vimの使い方
  - 起動と終了
    - Vimを開くには、ターミナルで「vim ファイル名」と入力します。新しいファイルを作成する場合も同様です。
    - 終了するには、まず「Esc」キーを押してコマンドモードに戻り、:wで保存、:wqで保存して終了、または :q!で保存せずに終了します。
  - モードの切り替え
    - インサートモード（編集モード）：iキーを押してテキストを入力します。終了するには「Esc」を押してコマンドモードに戻ります。
    - コマンドモード：ファイルの保存や終了、カーソル移動などが行えるモードです。
  - 基本操作
    - 保存：:w と入力してエンターを押すと保存されます。
    - カーソル移動：矢印キーを使って移動できます。
    - 行の削除：ddでカーソル位置の行を削除します。
    - コピーとペースト：yyで行をコピーし、pで貼り付けます。
  - ヘルプ
    - より詳しい使い方を知りたい場合は、:help と入力してエンターを押すとVimのヘルプが表示されます。
本実験では，実験環境としてDockerを用いた仮想環境を利用する．
キーボードでロボットを動かそう
では，ロボットを動かしてみよう！

Docker上で2分割したターミナルの上側で次のコマンドを実行してデバイスファイルttyUSB0があるか確認する．ttyUSB0がない場合は，ケーブルの接続，U2D2 Power Hubボードの電源，机上のテーブルタップの電源スイッチが入っているか確認する．それらが問題ない場合はコンピュータを再起動する．先ほど、DockerのホストOSで確認したが、仮想環境であるDockerでもデバイスファイルが見えないとロボットは動かない。
-  $ ls /dev
OpenMANIPULATOR-X コントローラの起動．上の説明で既に実行している場合は次の作業1は不要．

$ ros2 run dynamixel_sdk_examples read_write_trapezoidal_node

下のターミナルを開き，次のコマンドでディレクトリを移動し、そこでプログラムを作成していきます。

$ cd /home/root/colcon_ws/src/Professional_Experiment_2024/DynamixelSDK

　実験用ノートPCの/home/kit/DO_NOT_DELETE/keyboard.pyをUbuntuのアプリFilesでダブルクリックして開く。そのプログラムをマウスでドラッグしてコピーした後に、ctrl+shift+vでVimにペーストする。インサートモードでないとペーストできないので注意。その場合は、iキーを押してインサートモードにする。

$ vim keyboard.py
vimの操作方法
- 保存して終了 :wq

プログラムの起動．

$ python3 keyboard.py

　4. 操作方法(左：プラス，右：マイナス)

ｘ軸方向の操作：ｑ、ｗキー
ｙ軸方向の操作：a、ｓキー
ｚ軸方向の操作：ｚ、ｘキー
手先角の操作：ｃ、ｖキー

入力する際の例

一段階ｘ軸方向に動作したい場合の入力：ｑキーを押してエンター

複数段階ｘ軸方向に動作したい場合の入力：ｑｑ…と任意の回数入力してエンター

キーボード操作のプログラム keyboard.py

“””
2023/11/14 KAWASUMI Ren
“””

import rclpy
from rclpy.node import Node
from dynamixel_sdk_custom_interfaces.msg import SetPosition
import numpy as np
import warnings
import time
from geometry_msgs.msg import PoseArray, Pose
warnings.simplefilter(‘ignore’)

class OpenManipulatorX(Node):
def __init__(self):
super().__init__(‘OpenManipulatorX_node’)
self.L1 = 0.077
self.L2 = 0.13
self.L3 = 0.124
self.L4 = 0.126
self.L5 = 0.024
self.L6 = 0.128
self.OPEN_DEGREE = -50.0
self.CLOSE_DEGREE = 0.0
self.ORIGIN = 2048
self.ID = [11, 12, 13, 14, 15]
self.DT = 2.0
self.joint_angles = [0, 0, 0, 0, 0]
self.publisher = self.create_publisher(SetPosition, ‘/set_position’, 10)
self.create_subscription(Pose, ‘/aruco_poses_from_world’, self.pose_callback, 10)

def pose_callback(self, msg):
self.marker_pose = Pose()
bias_x = 0.0
bias_y = 0.0
bias_z = 0.0
self.marker_pose.position.x = msg.position.x + bias_x
self.marker_pose.position.y = msg.position.y + bias_y
self.marker_pose.position.z = msg.position.z + bias_z
print(f’ARマーカを認識しました pose:{self.marker_pose.position}’)

def move(self, eef_pose, velocity = 100, acceleration = 10):
“””
OpenManipulator-Xを目標姿勢に動かす.

引数
eef_pose : エンドエフェクタの目標姿勢 [x(m), y(m), z(m), pitch(degree)]
velocity : アームの各関節モータの最大速度
acceleration : アームの各関節モータの最大加速度
“””
self.inverseKinematics(eef_pose)
for i in range(4):
self.revolution(self.ID[i], self.joint_angles[i], velocity, acceleration)
time.sleep(self.DT)

def initPose(self, velocity = 50, acceleration = 5):
“””
OpenManipulator-Xを初期姿勢に動かす.

引数
velocity : アームの各関節モータの最大速度
acceleration : アームの各関節モータの最大加速度
“””
self.joint_angles = [0, 0, 0, 0, 0]
time.sleep(self.DT)
for i in range(5):
self.revolution(self.ID[i], self.joint_angles[i], velocity, acceleration)
time.sleep(self.DT)

def openGripper(self, velocity = 100, acceleration = 20):
“””
gripperを開く.

引数
velocity : gripperモータの最大速度
acceleration : gripperモータの最大加速度
“””
self.revolution(self.ID[4], self.OPEN_DEGREE, velocity, acceleration)
time.sleep(self.DT)

def closeGripper(self, velocity = 100, acceleration = 20):
“””
gripperを閉じる.

引数
velocity : gripperモータの最大速度
acceleration : gripperモータの最大加速度
“””
self.revolution(self.ID[4], self.CLOSE_DEGREE, velocity, acceleration)
time.sleep(self.DT)

def revolution(self, motor_id, degree, velocity, acceleration):
“””
指定したIDのモータを位置＋速度台形制御で回転させる.

引数
motor_id     : モータのID
degree       : モータの目標角度[deg]
velocity     : モータの最大速度
acceleration : モータの最大加速度
“””
msg = SetPosition()
msg.id = motor_id
msg.position = int(self.ORIGIN + degree / 0.088)
msg.velocity = int(velocity)
msg.acceleration = int(acceleration)
self.publisher.publish(msg)

def inverseKinematics(self, eef_pose):
“””
OpenManipulatar-Xの逆運動学を解く.

引数
eef_pose : エンドエフェクタの目標姿勢 [x(m), y(m), z(m), pitch(degree)]
“””
theta_1 = np.arctan(eef_pose[1] / eef_pose[0])
h = eef_pose[2] – self.L1 + self.L4 * np.sin(np.deg2rad(eef_pose[3]))
k = np.hypot(eef_pose[0], eef_pose[1]) – self.L4 * np.cos(np.deg2rad(eef_pose[3]))
phi = np.arctan2(h, k)
psi = np.arctan2(self.L5, self.L6)
v = np.hypot(h, k)
alpha = np.arccos((self.L2**2 + v**2 – self.L3**2) / (2.0 * self.L2 * v))
theta_2 = np.pi / 2.0 – alpha – phi – psi
beta = np.arccos(
(self.L2**2 + self.L3**2 -v**2) / (2.0 * self.L2 * self.L3)
)
theta_3 = np.pi – beta – np.pi / 2.0 + psi
theta_4 = np.deg2rad(eef_pose[3]) – (theta_2 + theta_3)
if self.checkAngleValid(theta_1, theta_2, theta_3, theta_4) == True:
self.joint_angles = [np.rad2deg(theta_1) , np.rad2deg(theta_2), np.rad2deg(theta_3), np.rad2deg(theta_4), 0.0]
else:
print(‘\033[31m’+’OpenMANIPULATOR-Xでは実現できないeef_poseが設定されました。’+’\033[0m’)
return “error”

def checkAngleValid(self, angle1, angle2, angle3, angle4, show=False):
“””
inverseKinematicsで計算された各関節角度にnanが含まれるか確認する.
引数    angle1~4 : モータの目標関節角度[rad]
返り値
True     : 計算が正常に行われてnanが含まれていない.
False    : nanが含まれている.
“””
if show == True:
print(‘theta_1 :’, np.rad2deg(angle1))
print(‘theta_2 :’, np.rad2deg(angle2))
print(‘theta_3 :’, np.rad2deg(angle3))
print(‘theta_4 :’, np.rad2deg(angle4))
if(np.isnan(angle1)): return False
if(np.isnan(angle2)): return False
if(np.isnan(angle3)): return False
if(np.isnan(angle4)): return False
return True

def main(args=None):
rclpy.init(args=args)
open_manipulator_x = OpenManipulatorX()
vel = 25
accel = 5
pos_x = 0.1
pos_y = 0.0
pos_z = 0.1
pitch = 90.0

open_manipulator_x.initPose()
open_manipulator_x.openGripper()

while rclpy.ok():
#rclpy.spin_once(open_manipulator_x)
open_manipulator_x.closeGripper()
try:
prev_pos = [pos_x,pos_y,pos_z,pitch]
#キーボード入力
keyboard_input = input(“・x軸方向:q,w\n\n・y軸方向:a,s\n\n・z軸方向:z,x\n\n・手先ピッチ角:c,v\n”)
if “q” in keyboard_input:
for i in range(keyboard_input.count(“q”)):
pos_x += 0.02

elif “w” in keyboard_input:
for i in range(keyboard_input.count(“w”)):
pos_x -= 0.02

elif “a” in keyboard_input:
for i in range(keyboard_input.count(“a”)):
pos_y += 0.02

elif “s” in keyboard_input:
for i in range(keyboard_input.count(“s”)):
pos_y -= 0.02

elif “z” in keyboard_input:
for i in range(keyboard_input.count(“z”)):
pos_z += 0.015

elif “x” in keyboard_input:
for i in range(keyboard_input.count(“x”)):
pos_z -= 0.015

elif “c” in keyboard_input:
for i in range(keyboard_input.count(“c”)):
pitch += 3

elif “v” in keyboard_input:
for i in range(keyboard_input.count(“v”)):
pitch -= 3

print(“x:”,pos_x,”y:”,pos_y,”pos_z:”,pos_z,”pitch:”,pitch)

error = open_manipulator_x.move([pos_x, pos_y, pos_z, pitch], vel, accel)
print(“error_flag:”,error)
if error == 0:
pos_x = prev_pos[0]
pos_y = prev_pos[1]
pos_z = prev_pos[2]
pitch = prev_bos[3]

except:
pass

#open_manipulator_x.move([0.1, 0.1, 0.1, 90.0], vel, accel)
#open_manipulator_x.move([0.1, 0.1, 0.05, 90.0], vel, accel)
#open_manipulator_x.openGripper()

#time.sleep(2.0)

#open_manipulator_x.initPose()
#open_manipulator_x.openGripper()

rclpy.shutdown()

if __name__ == ‘__main__’:
main()

終わり