1. はじめに

前回の記事^[1]ではDocker上で構築したROS Noetic環境にてHuskyというロボットにマニピュレーターを搭載し、アームを動かすことやグリッパーを開閉することをシミュレーションできました。このシリーズの取り組みではOyediranらの論文^[2]において、Huskyというロボットにマニピュレーターを搭載し、フレームなどの資材を把持して運搬する事例を再現していこうと考えています。それに向けてグリッパーで何か物をつかむことができないかと取り組んだ内容を紹介したいと思います。

この記事で実施したこと

MoveIt を用いたアームの制御
物体の把持と持ち上げ（これは失敗）

2. 実行環境

CPU: CORE i7 7th Gen
メモリ: 32GB
GPU: GeForce RTX 2070
OS: Ubuntu22.04(WSL2ではなくPCに直接インストール)
Docker内で構築したROS Noetic環境
Nvidia Driver: 535

3. 実行手順

3.1 シミュレーション環境の起動

Husky + UR3 シミュレーションの起動

terminal(1)

cd ~/catkin_ws
source devel/setup.bash
roslaunch husky_ur3_gazebo husky_ur3_HRI_lab.launch

MoveIt + RViz の起動

terminal(2)

cd ~/catkin_ws
source devel/setup.bash
roslaunch husky_ur3_gripper_moveit_config Omni_control.launch

3.2. Gazebo で把持対象物を設置

GazeboのGUI から coke_can を手動で以下の図のように設置しました。

3.3 coke_canを把持して持ち上げようとするプログラム

本来であれば、画像認識技術を用いてcoke_canの位置を認識し、その位置に合わせてアームを制御し、把持するのが最適な方法です。しかし、今回は簡略化のため、coke_canが常に特定の位置にあることを前提とし、画像認識は行わず、アームを予め設定された位置へ移動させ、移動完了後に把持する方法としました。

以下の pick_place.py を作成し、~/catkin_ws/src/husky_ur3_simulator/husky_ur3_gripper_moveit_config/scripts に保存します。

#!/usr/bin/env python3
import sys
import rospy
import moveit_commander
import actionlib
from std_msgs.msg import Float64
from tf.transformations import quaternion_from_euler

def control_gripper(position):
    pub = rospy.Publisher('/rh_p12_rn_position/command', Float64, queue_size=10)
    rospy.sleep(1)
    pub.publish(Float64(data=position))
    rospy.sleep(2)

def move_arm_joint_by_joint(arm, joint_values, order):
    for i in order:
        joint_goal = arm.get_current_joint_values()
        joint_goal[i] = joint_values[i] * (3.14159 / 180)  # 度数法をラジアンに変換
        arm.set_start_state_to_current_state()
        arm.set_joint_value_target(joint_goal)
        success = arm.go(wait=True)
        arm.stop()
        if not success:
            rospy.logerr(f"Failed to move joint {i} to {joint_values[i]} degrees")
            return False
        rospy.sleep(1)
    return True

def pick_and_place():
    rospy.init_node("husky_ur3_pick_place")

    moveit_commander.roscpp_initialize(sys.argv)
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    arm = moveit_commander.MoveGroupCommander("ur3_manipulator")

    rospy.sleep(2)

    # アームの関節角度（安全な順番で動かす）
    joint_targets = [
        70,   # shoulder_pan_joint
        11,   # shoulder_lift_joint
        78,   # elbow_joint
        -56,  # wrist_1_joint
        85,   # wrist_2_joint
        4     # wrist_3_joint
    ]
    joint_order = [0, 2, 3, 5, 4, 1]  # 物体を倒さない順番で調整

    if not move_arm_joint_by_joint(arm, joint_targets, joint_order):
        return

    # グリッパーを閉じる（把持）
    control_gripper(0.8)
    rospy.sleep(2)

    # 物体を持ち上げる
    lift_joint_targets = joint_targets[:]
    lift_joint_targets[1] -= 10  # 肩の角度を少し上げる
    lift_joint_targets[2] -= 10  # 肘を少し伸ばす
    
    if not move_arm_joint_by_joint(arm, lift_joint_targets, joint_order):
        return

    # グリッパーを開く（開放）
    control_gripper(0.0)

    moveit_commander.roscpp_shutdown()

if __name__ == "__main__":
    pick_and_place()

👆のpick_place.pyのプログラムに以下のコマンドで実行権限を付与します。

terminal(3)

cd ~/catkin_ws/src/husky_ur3_simulator/husky_ur3_gripper_moveit_config/scripts
chmod +x pick_place.py

そして以下のコマンドでプログラムを実行します。

cd ~/catkin_ws
source devel/setup.bash
rosrun husky_ur3_gripper_moveit_config pick_place.py

そして把持しようとしたときの動画が以下になります。coke_canをつかむことはできますが、持ち上げることができませんでした。またアームを予め設定された位置へ移動させても位置が微妙にずれcoke_canをつかめないこともありました。

4. 発生した課題と改善点についてのまとめ

今回のシミュレーションでは以下の問題がありました。問題のまとめと次回に向けた改善点を示します。

❌ 物体が滑って持ち上がらない

グリッパーの力が弱く、物体が滑って持ち上がらない。
今後の改善案：
調査したのですがGazeboでの物理シミュレーションで物を把持するのは、摩擦係数などの設定調整をする必要があり非常に難しいことがわかりました。また参考文献^[2]ではGazebo Link Attacherというプラグインを使って物品を「把持」（というよりも仮想的にアームと接続）していることが分かりました。次回はこれを使った試みをしたいと思います。

❌ 動的な位置調整がない

アームとグリッパーを適切な位置に移動させることができず物品を把持することができない
今後の改善案：
- RealSenseなどのカメラを活用し、把持対象の物品の位置を検出する
- 把持対象の物品を検出した位置を基にアームとグリッパーの位置をリアルタイムで調整する

参考記事やサイト

[1]Docker でロボットHusky にマニピュレータを搭載して動かしてみた（ROS Noetic環境）　⬅️ 戻る
[2]Integration of 4D BIM and Robot Task Planning: Creation and Flow of Construction-Related Information for Action-Level Simulation of Indoor Wall Frame Installation　⬅️ 戻る