1. はじめに
前回の記事[1]では、Docker上で構築したROS Noetic環境において、HuskyというロボットにマニピュレーターUR３を搭載し、デフォルトとは異なるシミュレーション環境下において、走行用地図の生成と自律走行をすることができました。
このシリーズでは、Oyediranらの論文[2]を参考に、Huskyにマニピュレーターを搭載し、フレームなどの資材を把持・運搬する事例を再現することを目指しています。Oyediranらの論文では、マニピュレーターを搭載したHuskyが物品を把持しながら自律走行をして作業しています。
そこで、本記事ではロボットHusky + マニピュレータUR3で自律走行を物品を把持しながら自律走行することを試したことについて紹介したいと思います。
2. 実行環境
- CPU: CORE i7 7th Gen
- メモリ: 32GB
- GPU: GeForce RTX 2070
- OS: Ubuntu22.04(WSL2ではなくPCに直接インストール)
- Docker内で構築したROS Noetic環境（シミュレーションした環境は[3]と[4]で構築した環境です。構築手順はそちらの記事をご参照ください。）
3. 実行手順
3.1 持ち上げる物品の選定
持ち上げる物品としては、Oyediranらの論文[2]と同じように窓枠を運びたかったのですが、見つけることができませんでしたので、以下のリンクで公開されているボードを運ぶことにしました。
👆のサイトからモデルをダウンロードし、隠しファイルとなっている.gazebo/modelの中にダウンロードしてきたファイルを解凍のうえ保存します。
3.2 Gazebo Link Attacherのworldファイルへの反映とセットアップ
前節で選定したボードをシミュレーション環境に反映するためと、Gazebo Link Attacherのセットアップするためにturtlebot3_house.worldに以下を追加しました
<plugin name="link_attacher_plugin" filename="libgazebo_ros_link_attacher.so"/>
修正したturtlebot3_house.worldの全体は以下のようになります。
<sdf version='1.4'>
<world name='default'>
<plugin name="link_attacher_plugin" filename="libgazebo_ros_link_attacher.so"/>
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<!-- A ground plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<physics type="ode">
<real_time_update_rate>1000.0</real_time_update_rate>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<ode>
<solver>
<type>quick</type>
<iters>150</iters>
<precon_iters>0</precon_iters>
<sor>1.400000</sor>
<use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
</solver>
<constraints>
<cfm>0.00001</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
<contact_surface_layer>0.01000</contact_surface_layer>
</constraints>
</ode>
</physics>
<!-- A turtlebot symbol -->
<include>
<uri>model://turtlebot3_house</uri>
</include>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>true</shadows>
</scene>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>0.0 0.0 17.0 0 1.5708 0</pose>
<view_controller>orbit</view_controller>
</camera>
</gui>
</world>
</sdf>
3.3 Gazebo Link Attacherと持ち上げる物品のlaunchファイルへの反映
前回の記事[1]で作成したhusky_in_tb3_house.launchをコピーして、以下の変更を加えhusky_in_tb3_house_wall.launchとして同じディレクトリ（~/catkin_ws/src/husky_ur3_simulator/husky_ur3_gazebo/launch）に保存しました。
修正したhusky_in_tb3_house_wall.launchの全体は以下のようになります。
<?xml version="1.0"?>
<launch>
<arg name="laser_enabled" default="true"/>
<arg name="camera_h_enabled" default="true"/>
<!-- TurtleBot3 House環境の起動 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_house.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>
<!-- Husky + UR3をスポーン -->
<include file="$(find husky_ur3_gazebo)/launch/spawn_husky_tb3house_world.launch">
<arg name="laser_enabled" value="$(arg laser_enabled)"/>
<arg name="camera_h_enabled" value="$(arg camera_h_enabled)"/>
</include>
<!-- Gazebo Link Attacher プラグイン読み込み -->
<gazebo>
<plugin name="link_attacher_plugin" filename="libgazebo_ros_link_attacher.so"/>
</gazebo>
<!-- SDFモデルをパラメータサーバに読み込み -->
<param name="nist_maze_wall_description"
command="cat $(env HOME)/.gazebo/models/nist_maze_wall_120/model.sdf"/>
<!-- Gazeboに持ち上げる物品のモデルをスポーンする -->
<node name="spawn_nist_maze_wall" pkg="gazebo_ros" type="spawn_model" output="screen"
args="-param nist_maze_wall_description
-sdf
-model nist_maze_wall_120
-x -1.2 -y 1.5 -z -0.5
-R 1.5708 -P 0 -Y 0"/>
</launch>
3.4 自律走行機能について
以下の記事で作成したhusky_ur3_in_tb3_house_amcl.launchをそのまま使いました。
3.5 シミュレーション環境の起動
以下の手順でシミュレーション環境を起動しますが、設定変更後に以下のコマンドでビルドして、環境を反映しています。（いつも忘れがちです。）
cd ~/catkin_ws
catkin_make
source devel/setup.bash
3.5.1 シミュレーション環境の起動
Gazebo起動
roslaunch husky_ur3_gazebo husky_in_tb3_house_wall.launch
MoveIt＋RViz起動
roslaunch husky_ur3_gripper_moveit_config Omni_control.launch
ナビゲーション（自律走行）起動
roslaunch husky_ur3_navigation husky_ur3_in_tb3_house_amcl.launch
3.5.2 物品の把持・持ち上げと自律走行プログラム
以下の内容でpick_lift_carry_wall.pyを作成し、~/catkin_ws/src/husky_ur3_simulator/husky_ur3_gripper_moveit_config/scripts内に保存しました。
#!/usr/bin/env python3
import sys
import rospy
import actionlib
import moveit_commander
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from std_msgs.msg import Float64
from gazebo_ros_link_attacher.srv import Attach, AttachRequest
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_wall():
rospy.wait_for_service('/link_attacher_node/attach')
try:
attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach)
req = AttachRequest()
req.model_name_1 = "nist_maze_wall_120"
req.link_name_1 = "nist_maze_wall_120_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 wall 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)
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 navigate_to_goal(x, y, yaw_z, yaw_w):
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
rospy.loginfo("🚗 Waiting for move_base action server...")
client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.orientation.z = yaw_z
goal.target_pose.pose.orientation.w = yaw_w
rospy.loginfo(f"🚗 Sending goal: x={x}, y={y}")
client.send_goal(goal)
client.wait_for_result()
if client.get_state() == actionlib.GoalStatus.SUCCEEDED:
rospy.loginfo("Navigation succeeded!")
else:
rospy.logerr("Navigation failed!")
sys.exit(1)
def pick_and_lift_wall(arm):
joint_targets = [70, 11, 78, -56, 85, 4]
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 False
rospy.loginfo("Closing gripper...")
control_gripper(1.05)
rospy.sleep(1)
rospy.loginfo("Attaching wall using Gazebo Link Attacher...")
attach_wall()
rospy.sleep(1)
rospy.loginfo("Lifting wall...")
lift_joint_targets = joint_targets[:]
lift_joint_targets[1] -= 50
lift_joint_targets[2] -= 30
if not move_arm_joint_by_joint(arm, lift_joint_targets, joint_order):
return False
rospy.loginfo("Wall picked and lifted successfully")
return True
def main():
rospy.init_node("husky_pick_lift_carry_wall")
moveit_commander.roscpp_initialize(sys.argv)
arm = moveit_commander.MoveGroupCommander("ur3_manipulator")
rospy.sleep(2)
# 1. 最初の位置まで自律移動
navigate_to_goal(0.595, -0.302, -0.441, 0.898)
# 2. 壁を把持し持ち上げる
if not pick_and_lift_wall(arm):
rospy.logerr("Failed to pick and lift wall.")
sys.exit(1)
# 3. 壁を持ち上げたまま目的地まで自律移動
navigate_to_goal(2.616, -0.715, 0.411, 0.912)
rospy.loginfo("Task completed successfully!")
moveit_commander.roscpp_shutdown()
if __name__ == "__main__":
main()
またこのプログラムを実行するのに以下のコマンドで、権限を付与を付与しました。
chmod +x pick_lift_carry_wall.py
そしてプログラム実行します。
rosrun husky_ur3_gripper_moveit_config pick_lift_carry_wall.py
4. シミュレーション結果
シミュレーションした結果が以下の動画になります。ロボットHusky + マニピュレータUR3で壁材を把持・持ち上げたままで、自律走行させることができました。
5. まとめ
公開されているリポジトリを活用し、Docker上で構築したROS Noetic環境において、HuskyというロボットにマニピュレーターUR３を搭載し、デフォルトとは異なるシミュレーション環境下において、壁材の把持と自律走行をすることができました。
今回成功したことは、以下のとおりです。
- TurtleBot3 Houseを使って、デフォルトにはない物品をシミュレーション環境に再現
- Gazebo Link AttacherとAMCL＋Move Baseを利用し、物体の把持と持ち上げながらの自律走行まで実現
今後も、Oyediranらの論文[2]の再現を目指して取り組んでいきたいと考えています。今後は工程との連携や安全評価などに取り組むことを考えています。
参考記事やサイト
[1]Docker上で構築したシミュレーション環境で、ロボットHusky + マニピュレータUR3 のロボットモデルで自律走行を試してみた ⬅️ 戻る
[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 + マニピュレータUR3 を用いた物品の把持を試みてみた（ROS Noetic環境） ⬅️ 戻る
[4]Docker で ロボットHusky にマニピュレータを搭載して動かしてみた（ROS Noetic環境） ⬅️ 戻る