1. はじめに
前回の記事[1]では、Docker上で構築したROS Noetic環境において、Docker上で構築したROS Noetic環境において、HuskyというロボットにマニピュレーターUR３を搭載し、TurtleBot3 Houseにおいて、壁材の把持と自律走行をすることができました。
このシリーズでは、Oyediranらの論文[2]を参考に、Huskyにマニピュレーターを搭載し、フレームなどの資材を把持・運搬する事例を再現することを目指しています。
Oyediranらの研究では、4D BIM（Building Information Modeling）データを活用して、建設スケジュールに基づいたタスク情報を自動でロボットに割り当て、作業を実行させるというアプローチが取られています。私も同様にBIMを利用して自動化を実現したいところですが、現時点ではBIMデータを取り扱うための環境や十分な知識がなく、同様の手法をすぐに再現するのは困難です。
そこで本記事では、よりシンプルな代替手段として、タスク情報を外部のCSVファイルとして用意し、それをロボットが読み込んで「自律移動 → 物品把持 → 自律移動 → 設置 → 戻る」という一連の動作を順に実行する仕組みの構築に挑戦してみました。
2. 実行環境
- CPU: CORE i7 7th Gen
- メモリ: 32GB
- GPU: GeForce RTX 2070
- OS: Ubuntu22.04(WSL2ではなくPCに直接インストール)
- Docker内で構築したROS Noetic環境（シミュレーションした環境は[3]と[4]で構築した環境です。構築手順はそちらの記事をご参照ください。）
3. 実行手順
3.1 タスクの定義ファイルの作成
以下のようなtasks.csvを作成し、~/catkin_ws/src/husky_ur3_simulator/husky_ur3_gripper_moveit_config/scripts/に保存します。
task_id,step,action,object,x,y,yaw_z,yaw_w
A-#1,1,navigate,,0.595,-0.302,-0.441,0.898
A-#1,2,pick,nist_maze_wall_120,0,0,0,0
A-#1,3,navigate,,2.300,-1.015,0.411,0.912
A-#1,4,place,nist_maze_wall_120,0,0,0,0
A-#1,5,navigate,,0.595,-0.302,-0.441,0.898
3.2 Pythonスクリプトの準備
3.1節で用意したcsvファイルを読み込んでロボットにタスクの指示を出す、robot_task_executor.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
import csv
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_object(object_name):
rospy.wait_for_service('/link_attacher_node/attach')
attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach)
req = AttachRequest()
req.model_name_1 = object_name
req.link_name_1 = object_name + "_link"
req.model_name_2 = "husky"
req.link_name_2 = "rh_p12_rn_l2"
attach_srv.call(req)
rospy.loginfo(f"{object_name} attached.")
def detach_object(object_name):
rospy.wait_for_service('/link_attacher_node/detach')
detach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach)
req = AttachRequest()
req.model_name_1 = object_name
req.link_name_1 = object_name + "_link"
req.model_name_2 = "husky"
req.link_name_2 = "rh_p12_rn_l2"
detach_srv.call(req)
rospy.loginfo(f"{object_name} detached.")
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)
arm.go(wait=True)
arm.stop()
rospy.sleep(1)
def navigate_to_goal(x, y, yaw_z, yaw_w):
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
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
client.send_goal(goal)
client.wait_for_result()
def pick_and_lift_object(arm, object_name):
joint_targets = [70, 11, 78, -56, 85, 4]
joint_order = [0, 2, 3, 5, 4, 1]
move_arm_joint_by_joint(arm, joint_targets, joint_order)
control_gripper(1.05)
attach_object(object_name)
lift_joint_targets = joint_targets[:]
lift_joint_targets[1] -= 50
lift_joint_targets[2] -= 30
move_arm_joint_by_joint(arm, lift_joint_targets, joint_order)
def execute_tasks_from_csv(csv_file):
rospy.init_node("robot_task_executor")
moveit_commander.roscpp_initialize(sys.argv)
arm = moveit_commander.MoveGroupCommander("ur3_manipulator")
rospy.sleep(2)
with open(csv_file, 'r') as file:
reader = csv.DictReader(file)
tasks = sorted(reader, key=lambda x: int(x['step']))
current_object = None
for row in tasks:
action = row['action']
x, y = float(row['x']), float(row['y'])
yaw_z, yaw_w = float(row['yaw_z']), float(row['yaw_w'])
object_name = row['object']
if action == 'navigate':
rospy.loginfo(f"Navigating to ({x}, {y})")
navigate_to_goal(x, y, yaw_z, yaw_w)
elif action == 'pick':
rospy.loginfo(f"Picking {object_name}")
current_object = object_name
pick_and_lift_object(arm, current_object)
elif action == 'place':
rospy.loginfo(f"Placing {current_object}")
detach_object(current_object)
control_gripper(0.0)
current_object = None
rospy.loginfo("All tasks completed.")
moveit_commander.roscpp_shutdown()
if __name__ == "__main__":
execute_tasks_from_csv('/home/ubuntu/catkin_ws/src/husky_ur3_simulator/husky_ur3_gripper_moveit_config/scripts/tasks.csv')
このプログラムには以下のコマンドで実行権限を与えました。
chmod +x robot_task_executor.py
3.3 シミュレーション環境の起動
以下の手順でシミュレーション環境を起動しますが、設定変更後に以下のコマンドでビルドして、環境を反映しています。（いつも忘れがちです。）
cd ~/catkin_ws
catkin_make
source devel/setup.bash
3.3.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
物品の把持・持ち上げと自律走行プログラムの起動
rosrun husky_ur3_gripper_moveit_config robot_task_executor.py
4. シミュレーション結果
シミュレーションした結果が以下の動画になります。ロボットHusky + マニピュレータUR3でcsvで作成したタスク表を読み込んで「自律移動 → 物品把持 → 自律移動 → 設置（ほとんど投げ入れている形ですが） → 戻る」という一連の流れを実現できました。
5. まとめ
公開されているリポジトリを活用し、Docker上で構築したROS Noetic環境において、HuskyというロボットにマニピュレーターUR３を搭載し、外部のCSVファイルとして用意したタスク情報をロボットHuskyに読み込ませて「自律移動 → 物品把持 → 自律移動 → 設置 → 戻る」という一連の動作を順に実行することができました。
今後も、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環境） ⬅️ 戻る