新入社員の中村です。新人研修として、5週間に渡ってCRANE-X7を使ったピックアンドプレイスシステムを開発します。本記事では、第1週に行ったMoveIt 2を用いたCRANE-X7の操作について報告します。
CRANE-X7を使ったピックアンドプレイスシステムの開発
本研修では、複数の新入社員がチームを組んで、CRANE-X7を使ったピックアンドプレイスシステムを開発します。チームでの開発を通してコミュニケーション能力を向上させたり、工数管理やデータ共有などのチーム開発手法を学んだりすることを目的としています。コミュニケーション能力やチーム開発手法の取得は、研修後にプロジェクトに配属されたとき円滑に作業を進めるために重要です。
CRANE-X7は、弊社が開発・販売する7自由度アームロボットです。教育・研究・開発のためのアームロボットで、サンプルプログラムやドキュメントが充実しています。チーム開発研修を通して、自社製品に対する理解を深めることも重要です。
ピックアンドプレイスシステムが達成すべき課題を以下の図に示します。
ランダムな位置姿勢で中央の箱置き場に置かれた箱をCRANE-X7で把持し、四隅の置き場所に順に箱を置きます。
使用する装置を次の図に示します。
中央の黒い箱を青いボードの四隅に順に移動させます。CRANE-X7の手首にはRealSenseが取り付けられており、箱の上面のArUcoマーカーを読み取って、箱の位置と姿勢を検出することができます。
このピックアンドプレイスシステムは、ROS 2を使用します。CRANE-X7のROS 2パッケージには、ArUcoマーカーを読み取ってピックアンドプレイスを行うサンプルプログラムが含まれています。このサンプルプログラムを少し修正するだけで、課題を達成することができます。
サンプルプログラムの修正で終わってしまうことがないように、本研修では次の観点で評価されます。
- 安全性（衝突時の動作）
- スピード（タスク達成時間の短さ）
- 正確性（置く位置の誤差）
- 丁寧さ（どれだけ加速度や角速度を与えずに動かしたか）
- 賢さ（工夫をした点）
ソフトウェアで可能な安全対策として、衝突検知機能や障害物検知機能が必要となります。スピードの観点では、トルク制御のようなフィードフォワード制御が必要となります。丁寧さの観点では、滑らかな軌道の生成が必要となります。
MoveIt 2を用いたCRANE-X7の操作
第1週では、MoveIt 2を用いたサンプルプログラムを修正して、課題を達成するプログラムを作成します。この節では、サンプルプログラムの詳細とMoveIt 2について説明します。
使用するサンプルプログラムは、CRANE-X7のROS 2パッケージに含まれるaruco_detectionです。以下のコマンドでサンプルを実行することができます。
# USBの設定 sudo chmod 666 /dev/ttyUSB0 sudo chmod a+rw /sys/bus/usb-serial/devices/ttyUSB0/latency_timer sudo echo 1 > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer # CRANE-X7の起動 ros2 launch crane_x7_examples demo.launch.py port_name:=/dev/ttyUSB0 use_d435:=true # 別のターミナルでArUco検知プログラムを実行 ros2 launch crane_x7_examples camera_example.launch.py example:='aruco_detection'
2つ目のlaunchファイルでは、aruco_detection.cppに書かれたArUcoマーカ検出ノードとpick_and_place_tf.cppに書かれたピックアンドプレイスノードが起動します。
pick_and_place_tf.cppにあるPickAndPlaceTfクラスのコンストラクタは次の通りです。
PickAndPlaceTf(
rclcpp::Node::SharedPtr move_group_arm_node,
rclcpp::Node::SharedPtr move_group_gripper_node)
: Node("pick_and_place_tf_node")
{
using namespace std::placeholders;
move_group_arm_ = std::make_shared<MoveGroupInterface>(move_group_arm_node, "arm");
move_group_arm_->setMaxVelocityScalingFactor(0.7);
move_group_arm_->setMaxAccelerationScalingFactor(0.7);
move_group_gripper_ = std::make_shared<MoveGroupInterface>(move_group_gripper_node, "gripper");
move_group_gripper_->setMaxVelocityScalingFactor(1.0);
move_group_gripper_->setMaxAccelerationScalingFactor(1.0);
// SRDFに定義されている"home"の姿勢にする
move_group_arm_->setNamedTarget("home");
move_group_arm_->move();
...
// 関節への負荷が低い撮影姿勢
init_pose();
tf_buffer_ =
std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ =
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
timer_ = this->create_wall_timer(
500ms, std::bind(&PickAndPlaceTf::on_timer, this));
}
以下で各行の解説をします。
move_group_arm_ = std::make_shared<MoveGroupInterface>(move_group_arm_node, "arm");
move_group_arm_->setMaxVelocityScalingFactor(0.7);
move_group_arm_->setMaxAccelerationScalingFactor(0.7);
move_group_gripper_ = std::make_shared<MoveGroupInterface>(move_group_gripper_node, "gripper");
move_group_gripper_->setMaxVelocityScalingFactor(1.0);
move_group_gripper_->setMaxAccelerationScalingFactor(1.0);
ここでMoveGroupInterfaceのオブジェクトを作成します。MoveGroupInterfaceは、MoveIt 2のクラスで、ロボットの複数の関節を動作させるためのインターフェースです。腕の部分とグリッパーの部分でそれぞれMoveGroupInterfaceを作成します。そして、各グループで既定の最高角速度と角加速度に対するスケールを設定します。
// SRDFに定義されている"home"の姿勢にする
move_group_arm_->setNamedTarget("home");
move_group_arm_->move();
次に、MoveIt 2のAPIであるsetNamedTarget()で目標関節角度をcrane_x7_moveit_configパッケージのcrane_x7.srdfで定義されている”home”での関節角度に設定します。次にmove()で軌道生成と軌道追従を行います。
// 関節への負荷が低い撮影姿勢
init_pose();
CRANE-X7の姿勢を関節への負荷が低い姿勢に変更します。特に肘関節のトルクが小さいため、肘関節に負荷がかからない姿勢に変更します。
tf_buffer_ =
std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ =
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
timer_ = this->create_wall_timer(
500ms, std::bind(&PickAndPlaceTf::on_timer, this));
tfメッセージを取得するTransformListenerを作成します。また、タイマー割り込みを作成し、PickAndPlaceTf::on_timer()を500msごとに実行します。
PickAndPlaceTf::on_timer()を次に示します。
void on_timer()
{
// target_0のtf位置姿勢を取得
geometry_msgs::msg::TransformStamped tf_msg;
try {
tf_msg = tf_buffer_->lookupTransform(
"base_link", "target_0",
tf2::TimePointZero);
} catch (const tf2::TransformException & ex) {
RCLCPP_INFO(
this->get_logger(), "Could not transform base_link to target: %s",
ex.what());
return;
}
rclcpp::Time now = this->get_clock()->now();
const std::chrono::nanoseconds FILTERING_TIME = 2s;
const std::chrono::nanoseconds STOP_TIME_THRESHOLD = 3s;
const double DISTANCE_THRESHOLD = 0.01;
tf2::Stamped<tf2::Transform> tf;
tf2::convert(tf_msg, tf);
const auto TF_ELAPSED_TIME = now.nanoseconds() - tf.stamp_.time_since_epoch().count();
const auto TF_STOP_TIME = now.nanoseconds() - tf_past_.stamp_.time_since_epoch().count();
const double TARGET_Z_MIN_LIMIT = 0.04;
// 現在時刻から2秒以内に受け取ったtfを使用
if (TF_ELAPSED_TIME < FILTERING_TIME.count()) {
double tf_diff = (tf_past_.getOrigin() - tf.getOrigin()).length();
// 把持対象の位置が停止していることを判定
if (tf_diff < DISTANCE_THRESHOLD) {
// 把持対象が3秒以上停止している場合ピッキング動作開始
if (TF_STOP_TIME > STOP_TIME_THRESHOLD.count()) {
// 把持対象が低すぎる場合は把持位置を調整
if (tf.getOrigin().z() < TARGET_Z_MIN_LIMIT) {
tf.getOrigin().setZ(TARGET_Z_MIN_LIMIT);
}
picking(tf.getOrigin());
}
} else {
tf_past_ = tf;
}
}
}
500ms毎にtfメッセージの取得を試みます。tfメッセージは、aruco_detection.cppに書かれたArUcoマーカ検出ノードからパブリッシュされます。tfメッセージが取得できた場合は、いくつかの条件に適合する場合、picking()を呼び出し、ピックアンドプレイスを開始します。
picking()を次に示します。
void picking(tf2::Vector3 target_position)
{
const double GRIPPER_DEFAULT = 0.0;
const double GRIPPER_OPEN = angles::from_degrees(60.0);
const double GRIPPER_CLOSE = angles::from_degrees(20.0);
// 何かを掴んでいた時のためにハンドを開閉
control_gripper(GRIPPER_OPEN);
control_gripper(GRIPPER_DEFAULT);
// 掴む準備をする
control_arm(target_position.x(), target_position.y(), target_position.z() + 0.12, -180, 0, 90);
// ハンドを開く
control_gripper(GRIPPER_OPEN);
// 掴みに行く
control_arm(target_position.x(), target_position.y(), target_position.z() + 0.07, -180, 0, 90);
// ハンドを閉じる
control_gripper(GRIPPER_CLOSE);
// 持ち上げる
control_arm(target_position.x(), target_position.y(), target_position.z() + 0.12, -180, 0, 90);
// 移動する
control_arm(0.1, 0.2, 0.2, -180, 0, 90);
// 下ろす
control_arm(0.1, 0.2, 0.13, -180, 0, 90);
// ハンドを開く
control_gripper(GRIPPER_OPEN);
// 少しだけハンドを持ち上げる
control_arm(0.1, 0.2, 0.2, -180, 0, 90);
// 初期姿勢に戻る
// control_arm(0.15, 0.0, 0.3, -180, 0, 90);
init_pose();
// ハンドを閉じる
control_gripper(GRIPPER_DEFAULT);
}
この関数では、control_arm()またはcontrol_gripper()を呼び出し、CRANE-X7を操作します。 control_arm()を次に示します。
void control_arm(
const double x, const double y, const double z,
const double roll, const double pitch, const double yaw)
{
geometry_msgs::msg::Pose target_pose;
tf2::Quaternion q;
target_pose.position.x = x;
target_pose.position.y = y;
target_pose.position.z = z;
q.setRPY(angles::from_degrees(roll), angles::from_degrees(pitch), angles::from_degrees(yaw));
target_pose.orientation = tf2::toMsg(q);
move_group_arm_->setPoseTarget(target_pose);
move_group_arm_->move();
}
まず、tf2::Quaternionで3次元での回転を表すクォータニオンを作成し、setRPY()でクォータニオンの係数をロールピッチヨー角をもとに設定します。toMsg()でtf2::Quaternionをgeometry_msgs::msg::Quaternionに変換し、geometry_msgs::msg::Poseのorientationに代入します。MoveIt 2のsetPoseTargetで目標の姿勢を設定し、move()で軌道生成と軌道追従を行います。
サンプルプログラムの変更
サンプルプログラムを変更して、ボードの四隅に箱を移動させるプログラムを作成します。
void picking(tf2::Vector3 target_position)
{
const double z_grip = 0.18;
const double z_move = 0.20;
const double z_first_move = 0.28;
const double GRIPPER_DEFAULT = 0.0;
const double GRIPPER_OPEN = angles::from_degrees(60.0);
const double GRIPPER_CLOSE = angles::from_degrees(15.0);
const double roll = 0;
const double pitch = 180;
const double yaw = -90;
const struct point points[] = {{0.175, 0.175, 0}, {0.425, 0.175, 0.02}, {0.425, -0.175, 0.02}, {0.175, -0.175, 0}};
// 何かを掴んでいた時のためにハンドを開閉
control_gripper(GRIPPER_OPEN);
control_gripper(GRIPPER_DEFAULT);
control_gripper(GRIPPER_OPEN);
/*-------------------------物体を掴むまで-------------------------*/
//物体の真上まで移動
control_arm(target_position.x()+0.06, target_position.y(), z_first_move, roll, pitch, yaw);
//物体を掴みに行く
control_arm(target_position.x()+0.06, target_position.y(), z_grip, roll, pitch, yaw);
// ハンドを閉じる
control_gripper(GRIPPER_CLOSE);
//物体を持ち上げる
control_arm(target_position.x()+0.06, target_position.y(), z_move, roll, pitch, yaw);
/*-------------------------４点を２周-------------------------*/
const int times = 2;
for (int i = 0; i < times; i++) {
for (int j = 0; j < 4; j++) {
//物体の真上まで移動
control_arm(points[j].x, points[j].y, z_move+points[j].z_add, roll, pitch, yaw);
//物体を掴みに行く
control_arm(points[j].x, points[j].y, z_grip+points[j].z_add, roll, pitch, yaw);
// ハンドを開く
control_gripper(GRIPPER_OPEN);
if(i == times - 1 && j == 3) {
break;
}
else {
// ハンドを閉じる
control_gripper(GRIPPER_CLOSE);
}
//物体を持ち上げる
control_arm(points[j].x, points[j].y, z_move+points[j].z_add, roll, pitch, yaw);
}
}
offset_direction_init_pose();
// ハンドを閉じる
control_gripper(GRIPPER_CLOSE);
}
z座標は、最初に箱をつかみに行くときの高さを表すz_first_moveと箱を掴んで移動する高さを表すz_move、箱を掴む高さを表すz_gripの3種類が定義されています。最初に箱を掴みに行くときは、引数のtarget_positionが表す位置に移動します。以降は、points配列で指定される点に順に移動するようにfor文が組まれています。
変更したプログラムによるピックアンドプレイスを以下の動画に示します。
おわりに
本記事では、CRANE-X7のROS 2パッケージに含まれるaruco_detectionのサンプルプログラムを変更して、ボードの四隅に箱を順に置くプログラムを作成しました。プログラムを変更するために必要なMoveIt 2のAPIやサンプルプログラムの構造について確認しました。次回は、MoveIt 2のより高度な軌道生成や独自の軌道生成について報告します。
