Unity Robotics Hub 入門 (4) - Pick-and-Place チュートリアル3

以下の記事を参考に、UnityのシミュレータでのPick-and-Placeの実行方法をまとめました。

・Pick-and-Place Tutorial - Part 3: Pick-and-Place In Unity

前回

1. 開発環境の準備

「Unity Robotics Hub 入門 (3) - URDFのインポート」の続きから始めます。

2. TrajectoryPlannerの追加

TrajectoryPlannerの追加手順は、次のとおりです。

(1) Hierarchyウィンドウで「Publisher」を選択し、「Add Component」で「TrajectoryPlanner」を追加。
(2) 「Niryo One」「Target」「TargetPlacement」にHierarchyウィンドウの「niryo_one」「Target」「TargetPlacement」をドラッグ&ドロップ。

3. パブリッシャーボタンの変更

パブリッシャーボタンの変更手順は、次のとおりです。

(1) Hierarchyウィンドウで「Button」を選択し、Inspectorの「Button → On Click()」を「Publisher」の「TrajectoryPlanner → PublishJoints()」に変更。

4. Pick-and-Placeの実行

Pick-and-Placeの実行手順は、次のとおりです。

(1) ROSワークスペースの準備
「Unity Robotics Hub 入門 (1) - デモの実行」と同様です。
(2) roslaunchでROSサービスを起動。

$ roslaunch niryo_moveit part_3.launch

ランチャーでは、以下を起動しています。

・エンドポイントノードの起動
・moverノードの起動
・demo.launchの起動

<launch>
    <!--エンドポイントノードの起動-->
    <rosparam file="$(find niryo_moveit)/config/params.yaml" command="load" />
    <node name="server_endpoint" pkg="ros_tcp_endpoint" type="default_server_endpoint.py" args="--wait" output="screen" respawn="true" />

    <!--moverノードの起動-->
    <node name="mover" pkg="niryo_moveit" type="mover.py" args="--wait" output="screen" />

    <!--demo.launchの起動-->
    <include file="$(find niryo_moveit)/launch/demo.launch" />
</launch>

(3) UnityエディタでPlayボタンを押す。
(4) Publishボタンを押す。
「Niryo One」が動き、「Target」を「TargetPlacement」まで運びます。

5. コードの確認

◎ クライアント (TrajectoryPlanner)
モーションプランニングを実行します。

・Start() : 初期化
・PublishJoints() : サービスへのリクエスト送信 (パブリッシュボタン押下)
　・CurrentJointConfig() : 関節角度の取得。
　・TrajectoryResponse() : サービスのレスポンス受信時の処理。
　　・ExecuteTrajectories() : モーションプランニングの実行。
　　　・OpenGripper() : グリッパーを開く。
　　　・CloseGripper() : グリッパーを閉じる。

サービスでは、リクエストに以下の情報を保持しています。

・NiryoMoveitJoints
　・Jointの角度 (float[])
　・Targetの姿勢 (vector+quaternion)
　・TargetPlacementの姿勢 (vector+quaternion)
・geometry_msgs/Pose pick_pose
・geometry_msgs/Pose place_pose

レスポンスに以下の情報を保持しています。

・moveit_msgs/RobotTrajectory[] trajectories

モーションプランは、次の4つです。

・PreGrasp : つかむ前
・Grasp : つかむ
・PickUp :ピック
・Place : プレース

using System;
using System.Collections;
using System.Linq;
using RosMessageTypes.Geometry;
using RosMessageTypes.NiryoMoveit;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;
using UnityEngine;

// モーションプランナー
public class TrajectoryPlanner : MonoBehaviour
{
    // 定数
    const int k_NumRobotJoints = 6;
    const float k_JointAssignmentWait = 0.1f;
    const float k_PoseAssignmentWait = 0.5f;

    // ROS通信変数
    [SerializeField]
    string m_RosServiceName = "niryo_moveit";
    public string RosServiceName { get => m_RosServiceName; set => m_RosServiceName = value; }

    // 参照
    [SerializeField]
    GameObject m_NiryoOne;
    public GameObject NiryoOne { get => m_NiryoOne; set => m_NiryoOne = value; }
    [SerializeField]
    GameObject m_Target;
    public GameObject Target { get => m_Target; set => m_Target = value; }
    [SerializeField]
    GameObject m_TargetPlacement;
    public GameObject TargetPlacement { get => m_TargetPlacement; set => m_TargetPlacement = value; }

    // つかむ前に、グリッパーが常にキューブの上に配置されていることを確認
    readonly Quaternion m_PickOrientation = Quaternion.Euler(90, 90, 0);
    readonly Vector3 m_PickPoseOffset = Vector3.up * 0.1f;

    // Joint
    ArticulationBody[] m_JointArticulationBodies;
    ArticulationBody m_LeftGripper;
    ArticulationBody m_RightGripper;

    // ROSコネクション
    ROSConnection m_Ros;

    // スタート
    void Start()
    {
        // ROSコネクションの取得
        m_Ros = ROSConnection.GetOrCreateInstance();

        // サービスのレスポンスのコールバックの登録
        m_Ros.RegisterRosService<MoverServiceRequest, MoverServiceResponse>(m_RosServiceName);

        // Jointの取得
        m_JointArticulationBodies = new ArticulationBody[k_NumRobotJoints];
        var linkName = string.Empty;
        for (var i = 0; i < k_NumRobotJoints; i++)
        {
            linkName += SourceDestinationPublisher.LinkNames[i];
            m_JointArticulationBodies[i] = m_NiryoOne.transform.Find(linkName).GetComponent<ArticulationBody>();
        }
        var rightGripper = linkName + "/tool_link/gripper_base/servo_head/control_rod_right/right_gripper";
        var leftGripper = linkName + "/tool_link/gripper_base/servo_head/control_rod_left/left_gripper";
        m_RightGripper = m_NiryoOne.transform.Find(rightGripper).GetComponent<ArticulationBody>();
        m_LeftGripper = m_NiryoOne.transform.Find(leftGripper).GetComponent<ArticulationBody>();
    }

    // グリッパーを閉じる
    void CloseGripper()
    {
        var leftDrive = m_LeftGripper.xDrive;
        var rightDrive = m_RightGripper.xDrive;

        leftDrive.target = -0.01f;
        rightDrive.target = 0.01f;

        m_LeftGripper.xDrive = leftDrive;
        m_RightGripper.xDrive = rightDrive;
    }

    // グリッパーを開く
    void OpenGripper()
    {
        var leftDrive = m_LeftGripper.xDrive;
        var rightDrive = m_RightGripper.xDrive;

        leftDrive.target = 0.01f;
        rightDrive.target = -0.01f;

        m_LeftGripper.xDrive = leftDrive;
        m_RightGripper.xDrive = rightDrive;
    }

    // Jointの角度の取得
    NiryoMoveitJointsMsg CurrentJointConfig()
    {
        var joints = new NiryoMoveitJointsMsg();
        for (var i = 0; i < k_NumRobotJoints; i++)
        {
            joints.joints[i] = m_JointArticulationBodies[i].jointPosition[0];
        }
        return joints;
    }

    // サービスへのリクエスト送信 (パブリッシュボタン押下)
    public void PublishJoints()
    {
        // リクエストの生成
        var request = new MoverServiceRequest();

        // Jointの角度の指定
        request.joints_input = CurrentJointConfig();

        // Targetの姿勢の指定
        request.pick_pose = new PoseMsg
        {
            position = (m_Target.transform.position + m_PickPoseOffset).To<FLU>(),
            orientation = Quaternion.Euler(90, m_Target.transform.eulerAngles.y, 0).To<FLU>()
        };

        // TargetPlacementの姿勢の指定
        request.place_pose = new PoseMsg
        {
            position = (m_TargetPlacement.transform.position + m_PickPoseOffset).To<FLU>(),
            orientation = m_PickOrientation.To<FLU>()
        };

        // サービスへのリクエスト送信
        m_Ros.SendServiceMessage<MoverServiceResponse>(m_RosServiceName, request, TrajectoryResponse);
    }

    // レスポンス時に呼ばれる
    void TrajectoryResponse(MoverServiceResponse response)
    {
        if (response.trajectories.Length > 0)
        {
            // モーションプランニングの実行
            Debug.Log("Trajectory returned.");
            StartCoroutine(ExecuteTrajectories(response));
        }
        else
        {
            Debug.LogError("No trajectory returned from MoverService.");
        }
    }

    // モーションプランニングの実行
    IEnumerator ExecuteTrajectories(MoverServiceResponse response)
    {
        if (response.trajectories != null)
        {
            // モーションプランの実行 (PreGrasp、Grasp、PickUp、Place)
            for (var poseIndex = 0; poseIndex < response.trajectories.Length; poseIndex++)
            {
                // Jointの角度の指定
                foreach (var t in response.trajectories[poseIndex].joint_trajectory.points)
                {
                    var jointPositions = t.positions;
                    var result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray();

                    // Jointの角度の指定
                    for (var joint = 0; joint < m_JointArticulationBodies.Length; joint++)
                    {
                        var joint1XDrive = m_JointArticulationBodies[joint].xDrive;
                        joint1XDrive.target = result[joint];
                        m_JointArticulationBodies[joint].xDrive = joint1XDrive;
                    }
  
                    // 0.1秒待機
                    yield return new WaitForSeconds(k_JointAssignmentWait);
                }
 
                // Grasp完了時にグリッパークローズ
                if (poseIndex == (int)Poses.Grasp)
                {
                    CloseGripper();
                }

                // 0.1秒待機
                yield return new WaitForSeconds(k_PoseAssignmentWait);
            }

            // Place完了時にグリッパーオープン
            OpenGripper();
        }
    }

    enum Poses
    {
        PreGrasp,
        Grasp,
        PickUp,
        Place
    }
}

◎ サーバー (niryo_moveit)

#!/usr/bin/env python
# coding: UTF-8
from __future__ import print_function
import rospy
import sys
import copy
import math
import moveit_commander
import moveit_msgs.msg
from moveit_msgs.msg import Constraints, JointConstraint, PositionConstraint, OrientationConstraint, BoundingVolume
from sensor_msgs.msg import JointState
from moveit_msgs.msg import RobotState
import geometry_msgs.msg
from geometry_msgs.msg import Quaternion, Pose
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
from niryo_moveit.srv import MoverService, MoverServiceRequest, MoverServiceResponse
joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']

# MelodicとNoeticの間で、plan()の戻り型が変更
if sys.version_info >= (3, 0):
    def planCompat(plan):
        return plan[1]
else:
    def planCompat(plan):
        return plan

# ロボットのJointの角度と目標姿勢からモーションプランを作成
def plan_trajectory(move_group, destination_pose, start_joint_angles): 
    # スタート状態の生成
    current_joint_state = JointState()
    current_joint_state.name = joint_names
    current_joint_state.position = start_joint_angles
    moveit_robot_state = RobotState()
    moveit_robot_state.joint_state = current_joint_state

    # 開始状態の指定
    move_group.set_start_state(moveit_robot_state)

    # 姿勢目標の指定
    move_group.set_pose_target(destination_pose)

    # モーションプランニングの実行
    plan = move_group.plan()

    # モーションプランに失敗
    if not plan:
        exception_str = """
            Trajectory could not be planned for a destination of {} with starting joint angles {}.
            Please make sure target and destination are reachable by the robot.
        """.format(destination_pose, destination_pose)
        raise Exception(exception_str)

    return planCompat(plan)


"""
    以下の4つの状態を使用して、モーションプランを作成。
    
    1. Pre Grasp - グリッパーをターゲットの真上に移動
    2. Grasp - グリッパーをターゲットまで下げる
    3. Pick Up - グリッパーを元の高さまで上げる
    4. Place - グリッパーをターゲットプレースメントまで移動

    グリッパーの動作は、このモーションプランの範囲外で処理される。
        - Grasp完了時にグリッパークローズ
        - Place完了時にグリッパーオープン

    https://github.com/ros-planning/moveit/blob/master/moveit_commander/src/moveit_commander/move_group.py
"""
def plan_pick_and_place(req):
    response = MoverServiceResponse()

    group_name = "arm"
    move_group = moveit_commander.MoveGroupCommander(group_name)

    current_robot_joint_configuration = req.joints_input.joints

    # Pre grasp - グリッパーをターゲットの真上に移動
    pre_grasp_pose = plan_trajectory(move_group, req.pick_pose, current_robot_joint_configuration)
    
    if not pre_grasp_pose.joint_trajectory.points:
        return response

    previous_ending_joint_angles = pre_grasp_pose.joint_trajectory.points[-1].positions

    # Grasp - グリッパーをターゲットまで下げる
    pick_pose = copy.deepcopy(req.pick_pose)
    pick_pose.position.z -= 0.05
    grasp_pose = plan_trajectory(move_group, pick_pose, previous_ending_joint_angles)
    
    if not grasp_pose.joint_trajectory.points:
        return response

    previous_ending_joint_angles = grasp_pose.joint_trajectory.points[-1].positions

    # Pick Up - グリッパーを元の高さまで上げる
    pick_up_pose = plan_trajectory(move_group, req.pick_pose, previous_ending_joint_angles)
    
    if not pick_up_pose.joint_trajectory.points:
        return response

    previous_ending_joint_angles = pick_up_pose.joint_trajectory.points[-1].positions

    # Place - グリッパーをターゲットプレースメントまで移動
    place_pose = plan_trajectory(move_group, req.place_pose, previous_ending_joint_angles)

    if not place_pose.joint_trajectory.points:
        return response

    # モーションプランがすべて機能した場合は、レスポンスにプランを追加
    response.trajectories.append(pre_grasp_pose)
    response.trajectories.append(grasp_pose)
    response.trajectories.append(pick_up_pose)
    response.trajectories.append(place_pose)

    # 姿勢目標のクリア
    move_group.clear_pose_targets()

    return response

# サーバー
def moveit_server():
    # moveit_commanderの初期化
    moveit_commander.roscpp_initialize(sys.argv)

    # ノードの生成
    rospy.init_node('niryo_moveit_server')

    # サーバーのリクエスト受信時のコールバックの登録
    s = rospy.Service('niryo_moveit', MoverService, plan_pick_and_place)
    print("Ready to plan")

    # ノード終了まで待機
    rospy.spin()

# メイン
if __name__ == "__main__":
    moveit_server()

次回

Unityではじめる ROS・人工知能 ロボットプログラミング実践入門