Docker上で構築したシミュレーション環境で、ロボットHusky + マニピュレータUR3 を用いた物品の持ち上げを試みてみた（Gazebo Link Attacher 使用）

1. はじめに

　前回の記事^[1]では、Docker上で構築したROS Noetic環境において、Huskyというロボットにマニピュレーターを搭載し、その先端にあるグリッパーで物品を把持しようと試みました。しかし、これは失敗に終わりました。その過程で、Gazeboの物理シミュレーションにおいて物品を正しく把持するには、摩擦係数などの設定を細かく調整する必要があり、非常に難しいことがわかりました。
　このシリーズでは、Oyediranらの論文^[2]を参考に、Huskyにマニピュレーターを搭載し、フレームなどの資材を把持・運搬する事例を再現することを目指しています。Oyediranらの論文では、Gazebo Link Attacherというプラグインを使用し、物品を「把持」するというよりも、仮想的にアームと接続する方法が採られていました。

そこで、本記事ではこの方法を用いてシミュレーション上で物品を持ち上げるのをロボットHusky + マニピュレータUR3で試したことについて紹介したいと思います。

2. 実行環境

• CPU: CORE i7 7th Gen
• メモリ: 32GB
• GPU: GeForce RTX 2070
• OS: Ubuntu22.04(WSL2ではなくPCに直接インストール)
• Docker内で構築したROS Noetic環境（シミュレーションした環境は^[1]と^[3]で構築した環境です。構築手順はそちらの記事をご参照ください。）

3. 実行手順

3.1 Gazebo Link Attacher のセットアップ

3.1.1. Gazebo Link Attacher のインストール

terminal(1)

cd ~/catkin_ws/src
git clone -b melodic-devel https://github.com/pal-robotics/gazebo_ros_link_attacher.git
cd ~/catkin_ws
rosdep install --from-paths src --ignore-src -r -y
catkin_make
source devel/setup.bash

3.1.2. Gazebo シミュレーション環境に Link Attacher を組み込む

1. husky_ur3_gazebo/launch/husky_ur3_HRI_lab.launch の修正

<include file=".../empty_world.launch"> の直後に以下を追加しました。

<gazebo>
  <plugin name="link_attacher_plugin" filename="libgazebo_ros_link_attacher.so"/>
</gazebo>

そして、このlaunchファイルはhusky_ur3_HRI_lab2.launchとして以下のように保存しました。ちなみにクローンしてきたリポジトリにはオリジナルのhusky_ur3_HRI_lab2.launchというのもありますが、それを上書きしてしましました。（してしまいましたというのは、たぶんオリジナルのhusky_ur3_HRI_lab2.launchは使わないだろうと思いファイルが増えるのが嫌だったので上書きしました。本当は良くなかったかもしれません。。）

修正後の husky_ur3_HRI_lab2.launch

husky_ur3_HRI_lab2.launch

<?xml version="1.0"?>

<launch>

  <arg name="laser_enabled" default="true"/>
  <arg name="camera_h_enabled" default="true"/>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find husky_ur3_gazebo)/worlds/HRI_lab.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>
  

  <include file="$(find husky_ur3_gazebo)/launch/spawn_husky.launch">
    <arg name="laser_enabled" value="$(arg laser_enabled)"/>
    <arg name="camera_h_enabled" value="$(arg camera_h_enabled)"/>
  </include>
  
  <gazebo>
    <plugin name="link_attacher_plugin" filename="libgazebo_ros_link_attacher.so"/>　#ここを追加
  </gazebo>

</launch>

2. HRI_lab.world にも Plugin を追加

<world> タグの中に以下を追記：

<plugin name="link_attacher_plugin" filename="libgazebo_ros_link_attacher.so"/>

HRI_lab.world

<sdf version='1.6'>
  <world name='default'>
    <plugin name="link_attacher_plugin" filename="libgazebo_ros_link_attacher.so"/> #ここを追加
    <light name='sun' type='directional'>

3. spawn_husky.launch の修正

デフォルト設定では Husky のロボットモデル名が設定されていないため、Gazebo Link Attacher が正常に機能しませんでした。そこで、robot_namespace は default のままにしつつ、model 引数のみ "husky" に明示する形に変更しました。
このようにした理由は、robot_namespace を default から husky に変更すると、トピック名など他の部分にも影響が及び、シミュレーション環境の起動自体がうまくいかなくなったためです。

<arg name="robot_namespace" default="/"/> <!-- そのまま -->
...
<node name="spawn_husky_model" pkg="gazebo_ros" type="spawn_model"
    args="-x $(arg x)
          -y $(arg y)
          -z $(arg z)
          -Y $(arg yaw)
          -unpause
          -urdf
          -param robot_description
          -model husky" /> <!-- ← 明示 -->

全体的には以下のように修正しました。

spawn_husky.launch

<?xml version="1.0"?>
<launch>

  <arg name="multimaster" default="false"/>

  <arg name="robot_namespace" default="/"/> <!-- そのまま -->

  <arg name="x" default="0.0"/>
  <arg name="y" default="0.0"/>
  <arg name="z" default="0.0"/>
  <arg name="yaw" default="0.0"/>

  <arg name="laser_enabled" default="$(optenv HUSKY_LMS1XX_ENABLED false)"/>
  <arg name="camera_h_enabled" default="$(optenv HUSKY_UR5_ENABLED false)"/>
  <arg name="urdf_extras" default="$(optenv HUSKY_URDF_EXTRAS)"/>


  <group ns="$(arg robot_namespace)">

    <group if="$(arg multimaster)">
      <include file="$(find husky_ur3_gazebo)/launch/description.launch" >
        <arg name="robot_namespace" value="$(arg robot_namespace)"/>
        <arg name="laser_enabled" default="$(arg laser_enabled)"/>
       	<arg name="camera_h_enabled" default="$(arg camera_h_enabled)"/>
        <arg name="urdf_extras" default="$(arg urdf_extras)"/>
      </include>
        
      <include file="$(find multimaster_launch)/launch/multimaster_gazebo_robot.launch">
        <arg name="gazebo_interface" value="$(find husky_ur3_gazebo)/config/gazebo_interface.yaml" />
        <arg name="robot_namespace" value="$(arg robot_namespace)"/>
      </include>

      <!-- For multimaster bringup, need to load the controller config -->
      <rosparam command="load" file="$(find husky_ur3_gazebo)/config/control.yaml" />
    </group>

    <!-- For single master bringup, run robot control on the gazebo master -->
    <!-- Note that husky_description/description.launch is already included in husky_control/control.launch. -->
    <group unless="$(arg multimaster)">
      <include file="$(find husky_ur3_gazebo)/launch/control.launch">
        <arg name="multimaster" value="$(arg multimaster)"/>
        <arg name="laser_enabled" value="$(arg laser_enabled)"/>      
	      <arg name="camera_h_enabled" value="$(arg camera_h_enabled)"/>
        <arg name="urdf_extras" value="$(arg urdf_extras)"/>
      </include>
    </group>
    


    <group if="$(arg camera_h_enabled)">

      <!-- Include poincloud_to_laserscan if simulated Kinect is attached -->
      <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan" output="screen">

          <remap from="cloud_in" to="camera/depth/points"/>
          <remap from="scan" to="scan"/>
          <rosparam>
              target_frame: base_link # Leave empty to output scan in the pointcloud frame
              tolerance: 1.0
              min_height: 0.05
              max_height: 1.0

              angle_min: -0.52 # -30.0*M_PI/180.0
              angle_max: 0.52 # 30.0*M_PI/180.0
              angle_increment: 0.005 # M_PI/360.0
              scan_time: 0.3333
              range_min: 0.45
              range_max: 4.0
              use_inf: true

              # Concurrency level, affects number of pointclouds queued for processing and number of threads used
              # 0 : Detect number of cores
              # 1 : Single threaded
              # 2->inf : Parallelism level
              concurrency_level: 1
          </rosparam>
      </node>

    </group>

    <!-- Spawn robot in gazebo -->
    <node name="spawn_husky_model" pkg="gazebo_ros" type="spawn_model"
        args="-x $(arg x)
              -y $(arg y)
              -z $(arg z)
              -Y $(arg yaw)
              -unpause
              -urdf
              -param robot_description
              -model husky" /> <!-- ← 明示 -->

  </group>
  
</launch>

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

3.2.1. Husky+UR3をGazeboで起動

terminal(1)

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

3.2.2. MoveIt（モーションプランニング）＋RVizを起動

terminal(2)

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

3.2.3. 物品をグリッパーにアタッチして持ち上げるプログラム

以下の内容で pick_place_R2.py を作成し、~/catkin_ws/src/husky_ur3_simulator/husky_ur3_gripper_moveit_config/scripts に保存：

#!/usr/bin/env python3
import sys
import rospy
import moveit_commander
from std_msgs.msg import Float64
from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse


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 attach_object():
    """Gazebo Link Attacher で coke_can をグリッパーにアタッチする"""
    rospy.wait_for_service('/link_attacher_node/attach')
    try:
        attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach)
        req = AttachRequest()
        req.model_name_1 = "coke_can"
        req.link_name_1 = "link"
        req.model_name_2 = "husky"
        req.link_name_2 = "rh_p12_rn_l2"  # グリッパーのリンク名を指定
        res = attach_srv.call(req)
        if res.ok:
            rospy.loginfo("✅ Successfully attached coke_can to gripper")
        else:
            rospy.logwarn("⚠️ Attach service returned False")
    except rospy.ServiceException as e:
        rospy.logerr(f"❌ Service call failed: {e}")


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)  # deg -> rad
        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)
    arm = moveit_commander.MoveGroupCommander("ur3_manipulator")
    rospy.sleep(2)

    # Step 1: アームを grasp position へ移動
    joint_targets = [70, 11, 78, -56, 85, 4]  # deg
    joint_order = [0, 2, 3, 5, 4, 1]

    rospy.loginfo("Moving arm to pre-grasp pose...")
    if not move_arm_joint_by_joint(arm, joint_targets, joint_order):
        return

    # Step 2: グリッパーを閉じて物体を掴む
    rospy.loginfo("Closing gripper...")
    control_gripper(1.05)
    rospy.sleep(1)

    # Step 3: Gazebo Link Attacherでアタッチ
    rospy.loginfo("Attaching object using Gazebo Link Attacher...")
    attach_object()
    rospy.sleep(1)

    # Step 4: アームを持ち上げ位置へ移動
    rospy.loginfo("Lifting object...")
    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

    rospy.loginfo("✅ Pick and lift sequence completed")
    moveit_commander.roscpp_shutdown()


if __name__ == "__main__":
    pick_and_place()

実行権限付与と実行方法

terminal(3)

cd ~/catkin_ws/src/husky_ur3_simulator/husky_ur3_gripper_moveit_config/scripts
chmod +x pick_place_R2.py
cd ~/catkin_ws
source devel/setup.bash
rosrun husky_ur3_gripper_moveit_config pick_place_R2.py

4. シミュレーション結果

シミュレーションした結果が以下の動画になります。coke_canをグリッパーにアタッチして持ち上げることができました。

5. まとめ

公開されているリポジトリとプラグインを活用し、Docker上で構築したROS Noetic環境において、HuskyというロボットにマニピュレーターUR３を搭載し、その先端についているグリッパーで物品を持ち上げるシミュレーションを試みることができました。

その結果、得られた知見は以下のとおりです。

• gazebo_ros_link_attacher を pluginとして正しく組み込む必要がある（worldまたはlaunch）
• husky の model name は spawn_model の -model 引数で明示し、URDF上のロボット名と一致させる必要がある
• トラブルの多くは model/link 名の不一致と plugin の定義忘れに起因していた。

今後も、Oyediranらの論文^[2]を再現することを目指します。ただし、自分のマシンスペックには制約があるため、完全な再現は難しく、簡易的なシミュレーションになる見込みです。その中でも、シミュレーション環境を作業に適した形へと改良し、運搬対象をドアや窓枠など、論文で扱われていたものに近いものへ変更しながら、再現を進めていきたいと考えています。

参考記事やサイト

[1]Docker上で構築したシミュレーション環境で、ロボットHusky + マニピュレータUR3 を用いた物品の把持を試みてみた（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　⬅️ 戻る
[3]Docker で ロボットHusky にマニピュレータを搭載して動かしてみた（ROS Noetic環境）　⬅️ 戻る