こんにちは、アールティのクアンです。今回のブログは初回のCRANE-X7によるピックアンドプレースに続く第二回です。タイトルの通り、本ブログは円弧補間を使ったピックアンドプレースついて解説していこうと思います。よろしくお願いします。
手先の円弧補間
手先の円弧補間によるピックアンドプレースに至った理由は、CRANE-X7のピックアンドプレース動作をより滑らかかつ短時間で実行したい思いから、直線補間を円弧補間に変更します。この修正により、以下の図に示すように、手先の移動距離を短縮することで目標達成が期待できます。ただし、2024年6月現在、アールティが公開しているROSパッケージでは、CRANE-X7の軌道を計画するMoveItの標準プランナーに円弧補間制御は含まれていません。したがって、手先の円弧補間を実装するために、CRANE-X7のMoveItの設定を書き換える必要があります。
直線補間と円弧補間の比較図
MoveItのプランナー
MoveItの設定を書き換える前に、先ずはMoveItのプランナーについてお話ししていきたいと思います。MoveItは、多自由度ロボットの姿勢と軌道を自動的に計画できるプランナーが多く組み込まれています。それらのプランナーはMoveItのplanning_interfaceというインタフェースで切り替えられます。MoveItの主なプランナーは以下のようになります。
- OMPL：多くのサンプリングベースのプランナーをサポート
- Pilz：産業ロボットでよく使用される直線軌道や円弧軌道をサポート
- STOMP：確率的手法を用いた最適化軌道計画をサポート
- CHOMP：勾配による軌道計画の最適化手法をサポート
- SBPL：ダイクストラ法やA*などの探索手法を用いた軌道計画をサポート
ちなみに、MoveItの標準プランナーはOMPL(Open Motion Planning Library)であり、アールティのCRANE-X7用のROSパッケージのデフォルトのプランナーでもあります。前回のブログでのsetTargetPoseやCartesianパスによるピックアンドプレースのデモもOMPLプランナーで軌道を計画しました。ここで気付く人もいるかもしれませんが、各プランナーの概要を読むと、CRANE-X7のプランナーをOMPLからPilzに変更するだけで、円弧補間を実装することができることが分かります。
では、早速MoveItの設定を書き換えましょう。先ず必要なROSパッケージをインストールします。本ブログで使用したROSのバージョンはUbuntu22.04のHumbleであるため、以下のコマンドでダウンロードします。
sudo apt install ros-humble-moveit-resources-prbt-moveit-config
次はcrane_x7_ros/crane_x7_moveit_config/config/の中にpilz_cartesian_limits.yamlファイルを作成します。
cartesian_limits: max_trans_vel: 1 max_trans_acc: 2.25 max_trans_dec: -5 max_rot_vel: 1.57
最後はcrane_x7_ros/crane_x7_moveit_config/launch/run_move_group.launch.pyの中に以下の変更を加えると、CRANE-X7のROSパッケージでPilzプランナーを使用できるようになります。
joint_limits_yaml = load_yaml(
"crane_x7_moveit_config", "config/joint_limits.yaml"
)
# Pilz追加
pilz_cartesian_limits_yaml = load_yaml(
"moveit_resources_prbt_moveit_config", "config/pilz_cartesian_limits.yaml"
)
robot_description_planning = {
"robot_description_planning": {**joint_limits_yaml, **pilz_cartesian_limits_yaml}}
kinematics_yaml = load_yaml('crane_x7_moveit_config', 'config/kinematics.yaml')
# Pilz追加
planning_pipeline_config = {
'default_planning_pipeline': 'ompl',
"planning_pipelines": ["pilz", "ompl"],
'pilz': {
"planning_plugin": "pilz_industrial_motion_planner/CommandPlanner",
"request_adapters": "",
"start_state_max_bounds_erros": 0.1,
},
'ompl': {
'planning_plugin': 'ompl_interface/OMPLPlanner',
'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization \
default_planner_request_adapters/FixWorkspaceBounds \
default_planner_request_adapters/FixStartStateBounds \
default_planner_request_adapters/FixStartStateCollision \
default_planner_request_adapters/FixStartStatePathConstraints',
'start_state_max_bounds_error': 0.1}}
ompl_planning_yaml = load_yaml('crane_x7_moveit_config', 'config/ompl_planning.yaml')
planning_pipeline_config['ompl'].update(ompl_planning_yaml)
上手く修正したら、RViz内でOMPLとPilzを切り替え可能になります。
RViz内でOMPLとPilzを切り替え
Pilzによるの手先の円弧補間
CRANE-X7のROSパッケージ内でPilzプランナーが使用可能になったので、MoveItのC++APIで手先の円弧補間の実装し方について解説していこうと思います。先ずは、MoveItを用いたCRANE-X7のサンプルコードを修正し、以下のコードを追加すると、OMPLとPilzの切り替えを行えます。
std::string planner_name;
// PilzのCIRC（円弧補間）に切り替え
planner_name = "CIRC";
move_group_arm.setPlanningPipelineId("pilz");
move_group_arm.setPlannerId(planner_name);
RCLCPP_INFO(move_group_arm_node->get_logger(), "Using planner: %s", planner_name.c_str());
// OMPLに戻し
planner_name = "RRTConnectkConfigDefault";
move_group_arm.setPlanningPipelineId("ompl");
move_group_arm.setPlannerId(planner_name);
RCLCPP_INFO(move_group_arm_node->get_logger(), "Using planner: %s", planner_name.c_str());
また、C++APIでの手先の円弧補間制御の実装し方は以下のようになります。
// CIRCのinterim_poseで軌道計画
// interim_poseは円弧の上にある点の事で、この点を指定すると円弧軌道の通過点を設定可能
// interim_poseを宣言
geometry_msgs::msg::PoseStamped interim_pose;
interim_pose.header.frame_id = "world";
interim_pose.pose.position.x = 0.3;
interim_pose.pose.position.y = -0.1;
interim_pose.pose.position.z = 0.15;
tf2::Quaternion q;
q.setRPY(angles::from_degrees(0), angles::from_degrees(180), angles::from_degrees(0));
interim_pose.pose.orientation = tf2::toMsg(q);
// 目標位置を指定
geometry_msgs::msg::PoseStamped goal_pose;
goal_pose.header.frame_id = "world";
goal_pose.pose.position.x = 0.3;
goal_pose.pose.position.y = 0.0;
goal_pose.pose.position.z = 0.25;
goal_pose.pose.orientation = tf2::toMsg(q);
move_group_arm.setPoseTarget(goal_pose, "crane_x7_gripper_base_link"); // ロボットのend-effectorのリンク名
// interim_poseで軌道を計画
moveit_msgs::msg::Constraints path_constraints;
path_constraints.name = "interim";
moveit_msgs::msg::PositionConstraint position_constraint;
position_constraint.header.frame_id = interim_pose.header.frame_id;
position_constraint.link_name = "crane_x7_gripper_base_link"; // ロボットのend-effectorのリンク名
position_constraint.constraint_region.primitive_poses.push_back(interim_pose.pose);
position_constraint.weight = 1.0;
path_constraints.position_constraints.push_back(position_constraint);
move_group_arm.setPathConstraints(path_constraints);
// 計画を実行
moveit::planning_interface::MoveGroupInterface::Plan path_plan;
bool success = (move_group_arm.plan(path_plan) == moveit::core::MoveItErrorCode::SUCCESS);
if (success) {
RCLCPP_INFO(move_group_arm_node->get_logger(), "Planning with %s succeeded!", planner_name.c_str());
move_group_arm.execute(path_plan);
} else {
RCLCPP_ERROR(move_group_arm_node->get_logger(), "Planning with %s failed.", planner_name.c_str());
}
手先の円弧補間によるピックアンドプレースのデモは以下のようになります。
これでCRANE-X7でPilzによる手先の円弧補間を実装できました。ただし、ここで一つの問題が発生しました。以下の画像で示した内容のように、PilzのCIRCで計画する時、たまに軌道の加速度が速すぎてCRANE-X7のジョイントのリミットを超えてしまい、軌道を計画できませんでした。CRANE-X7を安全に動かすために、crane_x7_ros/crane_x7_moveit_config/config/joint_limits.yaml設定ファイルの中で各ジョイントの速度と加速度の上限が設定されてあります。このリミットを超えてしまうと、MoveItで計画した軌道が全部キャンセルされます。
自動で生成された加速度が速すぎて軌道計画失敗エラー
この問題を解決するには、CRANE-X7の軌道の速度と加速度を下げる必要があります。MoveItのC++APIで軌道を計画する時、以下のように設定すると、速度や加速度を低く設定できます。また、今回のピックアンドプレースデモで、PilzのCIRCの一番安定な速度と加速度はそれぞれ0.5と0.1でした。Cartesianパスより短い移動距離で高速ピックアンドプレースを行う目的で、Pilzを導入しましたが、思わずところで問題が発生し、目的を達成できませんでした。
MoveGroupInterface move_group_arm(move_group_arm_node, "arm"); move_group_arm.setMaxVelocityScalingFactor(0.5); // Set 0.0 ~ 1.0 move_group_arm.setMaxAccelerationScalingFactor(0.1); // Set 0.0 ~ 1.0
バーチャル壁の設置
円弧補間により、マニピュレータはより滑らかで効率的な動きを実現しました。その結果、速度は低下しましたが、ロボットの安全性が向上しました。Pilzプランナー（Pilz Industrial Motion Planner）は、産業ロボット用に特化した高速かつ高精度の軌道計画が可能なプランナーです。そのため、Pilzを使用した軌道計画では、産業ロボットと同様の危険性があり、さらなる安全対策が必要です。MoveItでは、一般的な産業ロボット用のバーチャルウォール（仮想壁）を設置できます。以下の手順でCRANE-X7のサンプルコードにバーチャルウォールの設定を追加することができます。
#include <moveit/planning_interface/planning_interface.h>
#include "moveit/planning_scene_interface/planning_scene_interface.h"
using PlanningSceneInterface = moveit::planning_interface::PlanningSceneInterface;
// バーチャルウォールを生成する関数
moveit_msgs::msg::CollisionObject create_virtual_wall(const std::string& id, double x, double y, double z,
double roll, double pitch, double yaw,
double width, double depth, double height)
{
// バーチャルウォールのオブジェクトを作成
moveit_msgs::msg::CollisionObject wall;
wall.id = id;
wall.header.frame_id = "world";
// バーチャルウォールの位置と姿勢を代入
geometry_msgs::msg::Pose wall_pose;
wall_pose.position.x = x;
wall_pose.position.y = y;
wall_pose.position.z = z;
tf2::Quaternion q;
q.setRPY(angles::from_degrees(roll), angles::from_degrees(pitch), angles::from_degrees(yaw));
wall_pose.orientation = tf2::toMsg(q);
// バーチャルウォールのオブジェクトの寸法を代入
shape_msgs::msg::SolidPrimitive wall_primitive;
wall_primitive.type = wall_primitive.BOX; // 箱型で壁を作成
wall_primitive.dimensions.resize(3);
wall_primitive.dimensions[0] = width; // width in meters
wall_primitive.dimensions[1] = depth; // depth in meters
wall_primitive.dimensions[2] = height; // height in meters
// バーチャルウォールのオブジェクトを障害物として登録
wall.primitives.push_back(wall_primitive);
wall.primitive_poses.push_back(wall_pose);
wall.operation = wall.ADD;
return wall;
}
// バーチャルウォールの寸法
double wall_width_fb = 0.60; // CRANE-X7の前後の壁の長さ
double wall_width_lr = 1.0; // CRANE-X7の左右の壁の長さ
double wall_depth = 0.02; // 壁の太さ
double wall_height = 0.6; // 壁の高さ
// バーチャルウォールをMoveItのプランニングに登録
PlanningSceneInterface planning_scene_interface;
std::vector<moveit_msgs::msg::CollisionObject> collision_objects;
collision_objects.push_back(create_virtual_wall("wall_front", 0.53, 0.0, (0.6/2.0), 0, 0, 90, wall_width_fb, wall_depth, wall_height));
collision_objects.push_back(create_virtual_wall("wall_back", -0.31, 0.0, (0.6/2.0), 0, 0, 90, wall_width_fb, wall_depth, wall_height));
collision_objects.push_back(create_virtual_wall("wall_left", 0.15, 0.31, (0.6/2.0), 0, 0, 0, wall_width_lr, wall_depth, wall_height));
collision_objects.push_back(create_virtual_wall("wall_right", 0.15, -0.31, (0.6/2.0), 0, 0, 0, wall_width_lr, wall_depth, wall_height));
planning_scene_interface.applyCollisionObjects(collision_objects);
RCLCPP_INFO(this->get_logger(), "バーチャル壁を設定");
上手く設定できると、MoveItを立ち上げる時にRViz内で緑壁が以下の画像のように表示されます。これによって7軸あるCRANE-X7が、想定外の姿勢（人間的に変な姿勢）になって外部のオブジェクトや人間に衝突する危険性が減ると期待できます。
MoveItによるバーチャル壁の設置
おわりに
チーム開発研修の第二回目のブログでは、CRANE-X7のROS2パッケージにPilzプランナーを追加し、手先の円弧補間によるピックアンドプレースを実装しました。さらに、CRANE-X7が動作する際の安全性を高めるために、産業ロボットのバーチャルウォールと同様の安全柵をMoveItで実装しました。次回のブログがシリーズの最終回となります。ぜひお楽しみに。
