概要

• 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あたりが鍵になりそうです｡
あまり日本語文献がないので､作業が難航しそうです｡