[ロボ実験記録] ROS2の勉強3: 物体を掴んで移動する
概要
ROS2と教育用ロボアーム CRANE+ V2を使って何かを掴むプログラムを作ります
これまでの振り返り(dockerを使ったロボットアームの立ち上げまで)
こちらの書籍・レポジトリを参考にコードを書いていきます。
実装
アーム制御ライブラリの作成
ロボットアームの姿勢は「逆運動学」というものに基づいて計算するようです。
ロボットが専門ではない筆者は、一旦、このあたりの理論をすべてスキップすることにしました。
上記書籍のchapter 6で登場するcommander5.pyでは、逆運動学に基づく実装がなされているようです。
準備された関数を使うと、アームの座標を指定するだけで、必要な動作を自動計算してくれます。
こちらを改造して、アーム制御プログラムを作ります。
コードが長くなると嫌なので、アーム制御をするためのクラスを抜粋・pyファイルとして分離します。
Commander.py (commander1.pyなどと同じ階層に作成)
import rclpy
from rclpy.node import Node
from rclpy.duration import Duration
from tf2_ros import LookupException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from tf_transformations import euler_from_quaternion, quaternion_from_euler
from geometry_msgs.msg import TransformStamped
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from math import pi
# CRANE+ V2用のトピックへ指令をパブリッシュし,tfを利用するノード
class Commander(Node):
def __init__(self):
super().__init__('commander')
self.joint_names = [
'crane_plus_joint1',
'crane_plus_joint2',
'crane_plus_joint3',
'crane_plus_joint4']
self.publisher_joint = self.create_publisher(
JointTrajectory,
'crane_plus_arm_controller/joint_trajectory', 10)
self.publisher_gripper = self.create_publisher(
JointTrajectory,
'crane_plus_gripper_controller/joint_trajectory', 10)
self._tf_publisher = StaticTransformBroadcaster(self)
self.send_static_transform()
self._tf_buffer = Buffer()
self._tf_listener = TransformListener(self._tf_buffer, self)
def publish_joint(self, q, time):
msg = JointTrajectory()
msg.header.stamp = self.get_clock().now().to_msg()
msg.joint_names = self.joint_names
msg.points = [JointTrajectoryPoint()]
msg.points[0].positions = [
float(q[0]), float(q[1]), float(q[2]), float(q[3])]
msg.points[0].time_from_start = Duration(
seconds=int(time), nanoseconds=(time-int(time))*1e9).to_msg()
self.publisher_joint.publish(msg)
def publish_gripper(self, gripper, time):
msg = JointTrajectory()
msg.header.stamp = self.get_clock().now().to_msg()
msg.joint_names = ['crane_plus_joint_hand']
msg.points = [JointTrajectoryPoint()]
msg.points[0].positions = [float(gripper)]
msg.points[0].time_from_start = Duration(
seconds=int(time), nanoseconds=(time-int(time))*1e9).to_msg()
self.publisher_gripper.publish(msg)
def get_endtip_position(self):
try:
when = rclpy.time.Time()
trans = self._tf_buffer.lookup_transform(
'crane_plus_base',
'crane_plus_endtip',
when,
timeout=Duration(seconds=1.0))
except LookupException as e:
self.get_logger().info(e)
return None
tx = trans.transform.translation.x
ty = trans.transform.translation.y
tz = trans.transform.translation.z
rx = trans.transform.rotation.x
ry = trans.transform.rotation.y
rz = trans.transform.rotation.z
rw = trans.transform.rotation.w
roll, pitch, yaw = euler_from_quaternion([rx, ry, rz, rw])
return tx, ty, tz, roll, pitch, yaw
def send_static_transform(self):
st = TransformStamped()
st.header.stamp = self.get_clock().now().to_msg()
st.header.frame_id = 'crane_plus_link4'
st.child_frame_id = 'crane_plus_endtip'
st.transform.translation.x = 0.0
st.transform.translation.y = 0.0
st.transform.translation.z = 0.090
qu = quaternion_from_euler(0.0, -pi/2, 0.0)
st.transform.rotation.x = qu[0]
st.transform.rotation.y = qu[1]
st.transform.rotation.z = qu[2]
st.transform.rotation.w = qu[3]
self._tf_publisher.sendTransform(st)
publish_jointはジョイントの位置、publish_gripperはグリッパの位置を指定する関数、get_endtip_positionは状態の取得関数のようです。
前回に作成したcommander1_2.pyを書き換えます。
a,z,s,x,d,c,g,bキーでアームの先端やグリッパの状態を変更できるプログラムです。
import time
import threading
from crane_plus_commander.kbhit import KBHit
from crane_plus_commander.kinematics import (
forward_kinematics, from_gripper_ratio, gripper_in_range,
inverse_kinematics, joint_in_range, to_gripper_ratio)
from .Commander import Commander
import rclpy
def main():
# ROSクライアントの初期化
rclpy.init()
# ノードクラスのインスタンス
commander = Commander()
# 別のスレッドでrclpy.spin()を実行する
thread = threading.Thread(target=rclpy.spin, args=(commander,))
thread.start()
# 最初の指令をパブリッシュする前に少し待つ
time.sleep(2.0)
# 初期ポーズへゆっくり移動させる
joint = [0.0, -1.16, -2.01, -0.73]
gripper = 0
dt = 1
commander.publish_joint(joint, dt)
commander.publish_gripper(gripper, dt)
# 逆運動学の解の種類
elbow_up = True
# キー読み取りクラスのインスタンス
kb = KBHit()
print('a, z, s, x, d, c, g, bキーを押して手先を動かす')
print('スペースキーを押して起立状態にする')
print('Escキーを押して終了')
# Ctrl+cでエラーにならないようにKeyboardInterruptを捕まえる
try:
while True:
# 順運動学
[x, y, z, pitch] = forward_kinematics(joint)
ratio = to_gripper_ratio(gripper)
# 変更前の値を保持
joint_prev = joint.copy()
gripper_prev = gripper
elbow_up_prev = elbow_up
# 目標関節値とともに送る目標時間
dt = 0.2
# キーが押されているか?
if kb.kbhit():
c = kb.getch()
# 押されたキーによって場合分けして処理
if c == 'a':
x += 0.01
elif c == 'z':
x -= 0.01
elif c == 's':
y += 0.01
elif c == 'x':
y -= 0.01
elif c == 'd':
z += 0.01
elif c == 'c':
z -= 0.01
elif c == 'g':
ratio += 0.1
elif c == 'b':
ratio -= 0.1
elif c == ' ': # スペースキー
joint = [0.0, -1.16, -2.01, -0.73]
gripper = 0
dt = 1.0
elif ord(c) == 27: # Escキー
break
# 逆運動学
if c in 'azsxdcfve':
joint = inverse_kinematics([x, y, z, pitch], elbow_up)
if joint is None:
print('逆運動学の解なし')
joint = joint_prev.copy()
elif c in 'gb':
gripper = from_gripper_ratio(ratio)
# 指令値を範囲内に収める
if not all(joint_in_range(joint)):
print('関節指令値が範囲外')
joint = joint_prev.copy()
elbow_up = elbow_up_prev
if not gripper_in_range(gripper):
print('グリッパ指令値が範囲外')
gripper = gripper_prev
# 変化があればパブリッシュ
publish = False
if joint != joint_prev:
print((f'joint: [{joint[0]:.2f}, {joint[1]:.2f}, '
f'{joint[2]:.2f}, {joint[3]:.2f}]'))
commander.publish_joint(joint, dt)
publish = True
if gripper != gripper_prev:
print(f'gripper: {gripper:.2f}')
commander.publish_gripper(gripper, dt)
publish = True
# パブリッシュした場合は,設定時間と同じだけ停止
if publish:
time.sleep(dt)
position = commander.get_endtip_position()
if position is not None:
x, y, z, roll, pitch, yaw = position
print((f'x: {x:.3f}, y: {y:.3f}, z: {z:.3f}, '
f'roll: {roll:.3f}, pitch: {pitch:.3f}, '
f'yaw: {yaw:.3f}'))
time.sleep(0.01)
except KeyboardInterrupt:
pass
# 終了ポーズへゆっくり移動させる
#joint = [0.0, 0.0, 0.0, 0.0]
gripper = 0
dt = 5
commander.publish_joint(joint, dt)
commander.publish_gripper(gripper, dt)
rclpy.shutdown()
print('終了')
このプログラムを利用し、キーボードを操作しながら、以下の状態を作り出します。
(初期状態)
1. 目的物の座標までアームを移動
2. グリッパでものを掴む
3. 持ち上げる
4. アームを他の地点まで移動
5. グリッパを離す
6. 初期状態
アームを動かすたびに
x: 0.106, y: 0.049, z: 0.159, roll: 0.000, pitch: -0.051, yaw: 0.435
といった表記が出力されるので、それらをメモっておいて、1-5の状態を記録しておきます。
以下のように、commander1_2.pyを書き換えます。
inv_pose_dictに各姿勢を記録しておき、キーボードで「1」を押したときに、指定した座標に移動するプログラムにしました。
import time
import threading
from crane_plus_commander.kbhit import KBHit
from crane_plus_commander.kinematics import (
forward_kinematics, from_gripper_ratio, gripper_in_range,
inverse_kinematics, joint_in_range, to_gripper_ratio)
from .Commander import Commander
import rclpy
init_pose=[[0.0, -1.16, -2.01, -0.73],-0.69]
inv_pose_dict={
"1move":{"x": 0.203, "y": 0.039, "z": 0.076, "pitch": 0.067,"gripper":-0.69},
"2grip":{"x": 0.203, "y": 0.039, "z": 0.076, "pitch": 0.067,"gripper":0.5},
"3up":{"x": 0.018, "y": 0.132, "z": 0.205, "pitch": 0.164,"gripper":0.5},
"4down":{"x": -0.005, "y": 0.132, "z": 0.111, "pitch": 0.175,"gripper":0.5},
"5detatch":{"x": -0.005, "y": 0.132, "z": 0.111, "pitch": 0.175,"gripper":-0.69},
}
def main():
# ROSクライアントの初期化
rclpy.init()
# ノードクラスのインスタンス
commander = Commander()
# 別のスレッドでrclpy.spin()を実行する
thread = threading.Thread(target=rclpy.spin, args=(commander,))
thread.start()
# 最初の指令をパブリッシュする前に少し待つ
time.sleep(2.0)
# 初期ポーズへゆっくり移動させる
joint, gripper = init_pose
dt = 1
commander.publish_joint(joint, dt)
commander.publish_gripper(gripper, dt)
# 逆運動学の解の種類
elbow_up = True
# キー読み取りクラスのインスタンス
kb = KBHit()
print('a, z, s, x, d, c, g, bキーを押して手先を動かす')
print('スペースキーを押して起立状態にする')
print('Escキーを押して終了')
# Ctrl+cでエラーにならないようにKeyboardInterruptを捕まえる
try:
while True:
# 順運動学
[x, y, z, pitch] = forward_kinematics(joint)
ratio = to_gripper_ratio(gripper)
# 変更前の値を保持
joint_prev = joint.copy()
gripper_prev = gripper
elbow_up_prev = elbow_up
# 目標関節値とともに送る目標時間
dt = 0.2
# キーが押されているか?
if kb.kbhit():
c = kb.getch()
# 押されたキーによって場合分けして処理
if c == 'a':
x += 0.01
elif c == 'z':
x -= 0.01
elif c == 's':
y += 0.01
elif c == 'x':
y -= 0.01
elif c == 'd':
z += 0.01
elif c == 'c':
z -= 0.01
elif c == 'g':
ratio += 0.1
elif c == 'b':
ratio -= 0.1
elif c == ' ': # スペースキー
joint = [0.0, -1.16, -2.01, -0.73]
gripper = 0
dt = 1.0
elif c=="1":
for k in inv_pose_dict:
print(k)
inv_pose=inv_pose_dict[k]
joint = inverse_kinematics([inv_pose["x"],
inv_pose["y"],
inv_pose["z"],
inv_pose["pitch"],
], elbow_up)
gripper= (inv_pose["gripper"])
#移動
dt=2
commander.publish_joint(joint, dt)
commander.publish_gripper(gripper, dt)
time.sleep(dt)
elif ord(c) == 27: # Escキー
break
# 逆運動学
if c in 'azsxdcfve':
joint = inverse_kinematics([x, y, z, pitch], elbow_up)
if joint is None:
print('逆運動学の解なし')
joint = joint_prev.copy()
elif c in 'gb':
gripper = from_gripper_ratio(ratio)
# 指令値を範囲内に収める
if not all(joint_in_range(joint)):
print('関節指令値が範囲外')
joint = joint_prev.copy()
elbow_up = elbow_up_prev
if not gripper_in_range(gripper):
print('グリッパ指令値が範囲外')
gripper = gripper_prev
# 変化があればパブリッシュ
publish = False
if joint != joint_prev:
print((f'joint: [{joint[0]:.2f}, {joint[1]:.2f}, '
f'{joint[2]:.2f}, {joint[3]:.2f}]'))
commander.publish_joint(joint, dt)
publish = True
if gripper != gripper_prev:
print(f'gripper: {gripper:.2f}')
commander.publish_gripper(gripper, dt)
publish = True
# パブリッシュした場合は,設定時間と同じだけ停止
if publish:
time.sleep(dt)
position = commander.get_endtip_position()
if position is not None:
x, y, z, roll, pitch, yaw = position
print((f'x: {x:.3f}, y: {y:.3f}, z: {z:.3f}, '
f'roll: {roll:.3f}, pitch: {pitch:.3f}, '
f'yaw: {yaw:.3f}'))
time.sleep(0.01)
except KeyboardInterrupt:
pass
# 終了ポーズへゆっくり移動させる
joint,gripper=init_pose
dt = 5
commander.publish_joint(joint, dt)
commander.publish_gripper(gripper, dt)
rclpy.shutdown()
print('終了')
動作の様子はこちら
かなりぎこちないですが、基本的な動きはできていると思います。
次回は、カメラとの連動ができるようになりたいところです。
tf2やMoveIt2 Pythonあたりが鍵になりそうです。
あまり日本語文献がないので、作業が難航しそうです。
この記事が気に入ったらサポートをしてみませんか?