ROS Unity ガジェット シミュレータ ブログ ロボット研究・製作

UnityとROSで仮想myCobotを操作!

本記事では、UnityにロードしたmyCobotの仮想モデルをROSのMoveItから制御する方法を詳しく解説していきます。この記事はチュートリアル形式になっているので、順番に実行すれば再現できるはずです。

この記事を通じて学べること

  • ROSとUnityを使ったロボットシミュレーションの方法
  • Unity Robotics Hubの使い方
  • MoveItによるプランニングの方法

説明する内容は以下の通りです。

  • DockerでROSのデスクトップ環境の作成
  • 各種必要なROSパッケージの導入
  • UnityにmyCobotをロードする方法
  • [執筆中]

DockerでROSのデスクトップ環境の作成

プログラミングでは、使用しているPCのOSや開発環境、バージョンによって予期しないエラーが発生するのが常です。今回は、こういった面倒なエラーを無くし、この記事を追っていくだけでエラーなく再現できるように、Dockerを使用したROSのデスクトップ環境を使用して話を進めていきます。

本記事ではUnityを使用することもあり、Windowsマシンを使用している方が多いと思います。WindowsにDockerを導入する方法は以下の動画で説明していますので、まだDockerをインストールしていない方は、以下の動画に沿って導入してみてください。

Dockerのインストールが済んだ方は、ROSのデスクトップ環境を準備していきましょう。
本記事で使用するROSのディストリビューションはNoeticです。以下のコマンドを実行しましょう。

docker run -p 6080:80 -p 10000:10000 -p 5005:5005 -v /dev:/dev --shm-size=512m tiryoh/ros-desktop-vnc:noetic

上のコマンドにおいて、3つのポートを用意していますが、用途は以下の通りです。

  • 6080:80 ⇒ VNC
  • 10000:10000 ⇒ ROS
  • 5005:5005 ⇒ 予備

6080はVNCでホストマシンのブラウザからROSのデスクトップ環境にアクセスするために、10000はホストPCで動作しているUnityからROSにアクセスするために使用します。5005は予備です。一度起動したDockerコンテナに後からポートを追加することは不可能なので予備のポートは常に用意するように心がけることをお勧めします。

また、本記事ではmyCobotの実機を操作するところまでは説明しませんが、USBで実機とPCを接続すれば動作させることは可能なので、今後のことを考慮して/devをマウントしておきます。

上記のコードを実行したらVNCが自動的に起動しますので、http://127.0.0.1:6080/(←をクリックしたら開きます)アクセスしましょう。

みなさんお馴染みのUbuntuのGUIではないので、一瞬、戸惑うかもしれませんが、必要なものは全て整っています。今の時代、このような便利なものが用意されているのは本当にすごいですね。

ブラウザで開いたときに画面サイズが合わないことがあります。そのような場合は、http://127.0.0.1:6080/resizeにアクセスし、再度http://127.0.0.1:6080/へアクセスしてみてください。画面サイズが変更されているはずです。

それではターミナルを開いてみましょう。ターミナルは下のメニューの最も左のロゴをクリックし、System ToolsからLXTerminalを見つけることができます。

ターミナルが起動したら以下のコマンドを実行し、aptのアップデートとアップグレード、必要なライブラリのインストールをしておきましょう。

sudo apt update 
sudo apt upgrade -y

また、ROSによる開発の決まり文句であるcatkinワークスペースの作成とcatkin buildを実行、bashrcへの追記をしておきます。

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws

catkin build

echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc

以上で、ROSの導入は完了です。

以降で使用する可能性があるので、以下のコードも追加で実行しておきましょう。

sudo apt install python3-pip
pip install pyserial
pip install pymycobot

ROSパッケージの導入

ROSは多くの機能がパッケージとして配布されています。ここでは、

  • UnityとROSを接続するためのパッケージ
  • ロボットを制御するためのパッケージ
  • myCobotを扱うためのパッケージ

を導入します。

UnityとROSを接続するためのパッケージの導入

UnityとROSを接続するための機能は、Unity Robotics Hubで提供されており、ROS-TCP-ConnectorとROS-TCP-Endpointが必要です。ROS-TCP-ConnectorはUnity側に導入し、ROS-TCP-EndpointはROS側に導入します。導入方法は以下の通りです。

# GitHubからROS-TCP-Endpointをクローン
cd ~/catkin_ws/src
git clone https://github.com/Unity-Technologies/ROS-TCP-Endpoint

# ビルド
catkin build
source ~/.bashrc

ダウンロードとビルドが終わったら、動作確認をしてみましょう。以下のコマンドを実行し、エラーが出ないことを確認します。

roslaunch ros_tcp_endpoint endpoint.launch

問題なく実行できれば、0.0.0.0:10000でサーバが起動します(ホストPCからは127.0.0.1:10000でアクセスできる)。

問題なく動作することを確認したらCtrl + Cで実行を終了しましょう。

ロボットを制御するためのパッケージの導入

ロボットの制御を行うためのパッケージにMoveItがあります。myCobotでもMoveItで制御するためのスクリプトが用意されています。以下のコマンドを実行して、MoveItで制御するために必要なパッケージを導入しましょう。

sudo apt install -y ros-noetic-moveit
sudo apt install -y ros-noetic-ros-control ros-noetic-ros-controllers
sudo apt install -y ros-noetic-jsk-rviz-plugins

myCobotを扱うためのパッケージの導入

myCobotに関連したデータはmyCobotの開発販売をしているElephant Robotics社の公式リポジトリで公開されているものが使用できます。公式リポジトリを見ると、リポジトリにライセンスがないことに気づきますが、実は中の個々のパッケージにはライセンスが設定されているので、ライセンスを確認したいときは個々のパッケージのフォルダを確認してみてください(mycobot_280_moveitはBSD 2-Clause Licenseになっています)。

cd ~/catkin_ws/src
git clone https://github.com/elephantrobotics/mycobot_ros

catkin build
source ~/.bashrc

mycobot_rosはパッケージというよりはパッケージ群なので、これを導入した時点で、myCobotに関連するパッケージは全て導入できました。試しに以下のコードを実行して、myCobotをRvizから制御できることを確認してみましょう。

roslaunch mycobot_280_moveit demo.launch

もし、launch後にパーツが見つからないという趣旨のエラーが大量に発生したら、ターミナルを再起動してみましょう。そうすれば解決する可能性があります。
少なくとも、私はそれで解決しました。

これらの画面を閉じるときは、roslaunchしたターミナル上でCtrl + Cをしましょう。

余談ですが、「Plan & Execute」したときのノードと流れているトピックをrqt_graphで可視化すると、概略は以下のようになります。

Unityの仮想モデルを動作させるときは、/joint_state_publisherが出力している/joint_stateをサブスクライブして動作させることになります。

UnityにmyCobotをロードする方法

では、UnityにmyCobotの3Dモデルをロードし、ROSから制御するためのメッセージのビルドや簡単なスクリプトの作成を行います。

まず、Unityプロジェクトを作成します。今回は、myCobot 280 M5 MoveItというプロジェクトを作成しました。

Unityパッケージの追加

UnityもROSと同様に便利な機能がパッケージとして配布されています。ROSパッケージの導入のとことで、Unity Robotics HubのROS-TCP-ConnectorはUnity側に導入することを説明した通り、ここではROS-TCP-Connectorを導入します。また、ROSで使用されるロボットファイルをUnityにロードするための機能を提供するURDF Importer(これもUnity Robotics Hubの機能の1つ)も追加します。

ROS-TCP-Connectorの追加

追加手順は以下の通りです。

  • WindowPackage ManagerAdd package from git URL...を選択
  • 入力欄にhttps://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connectorをコピペしEnterを押下

ちゃんとパッケージが追加できるとメニューにRoboticsという項目が追加されます。

ROSで通信するためのアドレスやポートの指定、メッセージファイルのビルドなど諸々はRoboticsの項目から行います。

URDF Importerの追加

ROS-TCP-Connectorの追加手順と同じ方法で、URDF Improterを追加します。追加の際には、以下のリンクを入力欄に入力してください。

https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer

myCobot 280 M5モデルのロードと動作確認

以下のGitHubのリンクからリポジトリをダウンロードして解凍します。とても大きなリポジトリなのでダウンロードと解凍に

時間がかかるかもしれません。

解凍したらエクスプローラで、「mycobot_description」フォルダを探し、それをUnityのProjectタブのAssetsにドラッグアンドドロップします。

Projectタブで、Assets→mycobot_description→urdf→mycobotの順にフォルダを開きます。mycobot_urdfを右クリックして、Import Robot from Selected URDF fileをクリックします。

表示されたタブで、Import URDFをクリックすると、以下のような表示が出ます。

Locate Root Folderをクリックし、Assetsを選択しましょう。

すると、以下のようにmyCobot 280 M5がUnityのシーンに読み込まれます。

Hierarchyでは、myCobot 280 M5がfirefighterと表示されていますが、わかりにくいので、myCobot280M5に書き換えるとよいでしょう。

HierarchyでmyCobot280M5を選択し、右に表示されたInspectorタブを見ると、Controllerというスクリプトが付与されているので、各パラメータを以下のように設定します。

次に、HierarchyでmyCobot280M5→joint1を選択し、inspectorのAdd ComponentでArticulation Bodyを追加します。Articulation Bodyを追加したら、Immovableをクリックします。

ここまで設定ができたら、再生ボタン▶を押し、キーボードの⇅⇄のキーを適当に押してみましょう。myCobot 280 M5の仮想モデルが動作するはずです。

MoveItを使用して動作させる

ROS Messageを追加する

以下のリンクからcommon_msgsをダウンロードします。

ダウンロードしたら解凍します。解答したフォルダを覚えておきましょう。

Unityのメニューから、Robotics→Generate ROS Messages...を選択します。ROS message pathの右にあるBrowseを選択し、先ほどダウンロードし解凍したcommon_msgs(私の場合は、common_msgs-noetic-develとなっています)を選択します。

上の画像のようにフォルダを末端へ辿ると、JointState.msgが見つかると思うので、Build msgを選択します。ビルドが終了すると、Rebuild msgとう表示に変化します。

スクリプトの作成

次に、ROSからトピックを受け取って仮想myCobot280M5を制御するためのプログラムを作成します。

ProjectタブのAssetsフォルダで右クリック→Create→Folderで、Scriptsフォルダを作成します。Scriptsフォルダの中に移動し、右クリック→Create→C# Scriptを選択します。ファイル名は、myCobot280M5Subscriberとします。

ダブルクリックすると、エディタが開くので、以下のコードをコピペして保存してください。

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using JointStateMsg = RosMessageTypes.Sensor.JointStateMsg;

public class myCobot280M5Controller : MonoBehaviour
{
    public ArticulationBody joint2;
    public ArticulationBody joint3;
    public ArticulationBody joint4;
    public ArticulationBody joint5;
    public ArticulationBody joint6;
    public ArticulationBody joint6Flange;

    private ArticulationBody[] joint;
    private ROSConnection ros;


    // Start is called before the first frame update
    void Start()
    {
        ros = ROSConnection.GetOrCreateInstance();
        ros.Subscribe<JointStateMsg>("/move_group/fake_controller_joint_states", Callback);

        joint = new ArticulationBody[6];
        joint[0] = joint2;
        joint[1] = joint3;
        joint[2] = joint4;
        joint[3] = joint5;
        joint[4] = joint6;
        joint[5] = joint6Flange;
    }

    void Callback(JointStateMsg msg)
    {
        for (int i = 0; i < 6; i++)
        {
            ArticulationDrive aDrive = joint[i].xDrive;
            aDrive.target = Mathf.Rad2Deg * (float)msg.position[i];
            joint[i].xDrive = aDrive;
        }
    }
}

スクリプト中のコメントのpoint 1とpoint 2について説明します。point 1は、jointのリストに順番にjointのArticulation Bodyを格納していますが、その格納の順番は、/move_group/fake_controller_joint_statesトピックに記載の順番になっています。rostopic echo /move_group/fake_controller_joint_statesをすると、name部分に、

name: 
  - joint2_to_joint1
  - joint3_to_joint2
  - joint4_to_joint3
  - joint5_to_joint4
  - joint6_to_joint5
  - joint6output_to_joint6

と記載があり、トピックのリストに格納さ入れている順番が確認できるので、その順番と同じになるように格納しています。

スクリプトのアタッチ

Hierarchyタブ内の何もないところで、右クリック→Create Emptyをします。作成された空のオブジェクトの名前をRosSubscriberとします。

RosSubscriberを選択し、InspecterタブのAdd Componentから先ほど作成したスクリプトを追加します。コンポネントを追加したら、Joint 2からJoint 6 Flangeまでの6個のパーツをHierarchyからドラッグアンドドロップします(結果は下図の通り)。

動作確認

ターミナルを開き、以下のコードを実行します。

roslaunch mycobot_280_moveit mycobot_moveit.launch

もう1つターミナルを開き、以下のコードを実行します。

roslaunch ros_tcp_endpoint endpoint.launch

次に、Unityの再生ボタンを押しましょう。

Rviz上で、エンドエフェクタの位置を移動させると、Unityの中のmyCobot 280 M5も動作することが確認できました!

Rvizの仲介なしで実現する

ここまで紹介した方法は、Rvizを使ってmyCobotをMoveItで制御するデモプログラムで流れているトピックをUnityで受け取り制御するものでした。そのため、Unityは表示部分しか担っていません。Unityに表示するだけであれば問題ありませんが、Unityから目標位置を設定し。動作させたい場合は、今の状態では不十分です。

そこで、Unity側にPublisherのスクリプトを作成します。

ROSの時間をUnityの時間と同期させる

Rvizでロボットを表示させるときは、トピックのtimestampにROSの時間が記録されており、MoveItはそれを確認し制御していると思われます。RvizをUnityに置き換えるには、ROSの時間とUnityの時間を同期させ統一する必要があります。代表的な方法は2つあり、ROSの時間をtopicを通じてUnity側に送りUnity側をROSの時間に合わせる方法と、Unityの時間をtopicを通じてROS側に送りROS側をUnityの時間に合わせる方法があります。どちらの方法でもよいですが、シミュレータを使用する利点を活かせる後者を利用することにします。というのも、シミュレータを使用する利点の1つに、シミュレーション速度を変更できることがあげられ、これは強化学習を行いたいときなどにとても重要な機能になります。ROSの時間をシミュレータの時間に合わせれば、シミュレータの時間の進み方を高速化するとROSの時間も進み方を高速化することができます。一方で、シミュレータの時間をROSの時間に合わせる場合、シミュレータの時間の進み方を高速化しても、ROS側の時間の進み方は高速化しないため、rosbugなどでデータを記録して再生する際に問題が発生します。以上の理由より、後者を採用します。

ROS側で外部の時間を使用できるようにする

ROSを動作させるターミナルで以下のコードを実行し、外部からの/clockトピックの時間を使用するようにします。

2つのターミナルを開き、1つ目のターミナルでroscoreを実行します。2つ目のターミナルでrosparam set /use_sim_time trueを実行します。

Unityの時間をトピックとして流す

Unityの時間を/clockトピックとして流すためのプログラムは、Unity Robotics Hubのチュートリアルの「Robotics Nav2 SLAM Example」の再利用できます。

具体的に、再利用するプログラムは、以下の2つです。

これらをコピーし、Assets/Scriptsフォルダの中に保存します。コピーしたら、ROSClockPublisher.csの63行目を以下のように修正します。

修正前sec = (int)publishTime→修正後sec = (uint)publishTime

修正する理由は、公開されているコードがROS2用だからです。今回使用しているのはROS1ですので、その表記に変更する必要があるのです。

次に、再程、RosSubscriberを作成した方法と同様の手順で、RosPublisherを作成します。作成後のHierarchyは以下のようになります。

そして、RosPublisherをクリックし、InspectorのAdd ComponentでROSClockPublisherをアタッチします(下図)。

デフォルトでUnityの時間が100Hzでパブリッシュされます。特に変更する必要はないでしょう。

ここで、以下の手順を実施したときにROS側で/clockトピックが正しく見えていれば大丈夫でしょう。

  • Unityの再生ボタンをクリック
  • 1つ目のターミナルでroslaunch ros_tcp_endpoint endpoint.launchを実行
  • 2つ目のターミナルでrostopic echo /clockを実行

もし、/clockトピック自体がなかったり、うまく受け取れていない場合は、時間が表示されません。正しく受け取れている場合は、以下の画像のようにズラズラと時間が流れてくるはずです。

Unityからtf2を流せるようにする

Unityから/tfトピックを流すために先ほどと同じく、Unity Robotics Hubのチュートリアルの「Robotics Nav2 SLAM Example」のプログラムを一部再利用します。

追加で、再利用するプログラムは、以下の4つです。

これらをコピーし、Assets/Scriptsフォルダの中に保存します。

ROS2向けのプログラムなので、何か所か修正する必要があります。

  • TimeStamp.csの12、21、26行目のintuintに変換
  • TransformExtensions.cs
    21行目(変更前):new HeaderMsg(new TimeStamp(timeStamp), tfUnity.parent.gameObject.name)
    21行目(変更後):new HeaderMsg { stamp = new TimeStamp(timeStamp), frame_id = tfUnity.parent.gameObject.name }
  • ROSTransformTreePublisher.cs
    70行目(変更前):new HeaderMsg(new TimeStamp(Clock.time), m_GlobalFrameIds.Last())
    70行目(変更後):new HeaderMsg { stamp = new TimeStamp(Clock.time), frame_id = m_GlobalFrameIds.Last() }
    85行目(変更前):new HeaderMsg(new TimeStamp(Clock.time), m_GlobalFrameIds[i - 1])
    85行目(変更前):new HeaderMsg { stamp = new TimeStamp(Clock.time), frame_id = m_GlobalFrameIds[i - 1] }

変更して、コンパイル後にエラーがなくなったら、Unityに目標位置を指定するための球を用意します。Hierarchyで、右クリック→3D Object→Sphereを選択します。目標位置を表すことが分かりやすいように名前をTargetに変換しましょう。

また、Targetの球のサイズは、Transformでxyzのscaleをすべて0.1にして小さくします。

そして、先ほど用意したROSTransformTreePublisher.csをRosPublisherオブジェクトのコンポネントとして追加します。

myCobotのメッセージ&パッケージの作成

cd ~/catkin_ws/src
catkin_create_pkg mycobot_unity rospy moveit_msgs geometry_msgs message_generation message_runtime 

パッケージを作成したら以下のコマンドを実行します。以下のコマンドでは、myCobotMoveitJointsというメッセージファイルを作成しています。

cd mycobot_unity
mkdir msg
echo -e "float64[6] joints\ngeometry_msgs/Pose goal_pose" > msg/myCobotMoveitJoints.msg

CMakeLists.txtにmyCobotMoveitJointsについて追記します。追記内容は以下の通りです。

add_message_files(
  FILES
  myCobotMoveitJoints.msg
)

generate_messages(
  DEPENDENCIES
  geometry_msgs
  moveit_msgs
  std_msgs
)

以上を書き込んだら、catkin buildをします。

Pythonスクリプトの作成

Unityから送信したターゲットの位置や関節情報を受け取り、プランニング結果をPublishするスクリプトを作成します。スクリプトは以下になります。

#!/usr/bin/env python
# coding: UTF-8

import sys
import rospy
import geometry_msgs.msg
import moveit_commander
from sensor_msgs.msg import JointState
from moveit_msgs.msg import RobotState
from moveit_msgs.msg import RobotTrajectory
from mycobot_unity.msg import myCobotMoveitJoints

class myCobotMover:
    def __init__(self, subTopicName, pubTopicName):
        self.sub = rospy.Subscriber(subTopicName, myCobotMoveitJoints, self.callback)
        self.pub = rospy.Publisher(pubTopicName, RobotTrajectory, queue_size=1)
        self.move_group = moveit_commander.MoveGroupCommander('arm_group')
        self.joint_state = JointState()
        self.joint_state.name = ['joint2_to_joint1', 'joint3_to_joint2', 'joint4_to_joint3', 'joint5_to_joint4', 'joint6_to_joint5', 'joint6output_to_joint6']
        self.robot_state = RobotState()
        self.pose_goal = geometry_msgs.msg.Pose()

    def callback(self, msg):
        plan = self.plan_trajectory(self.move_group, msg.goal_pose, msg.joints)
        self.move_group.stop()
        self.move_group.clear_pose_targets()

        if type(plan) is tuple:
            print("success>>>")
        else:
            print("fail>>>")

        self.pub.publish(plan[1])

    def plan_trajectory(self, move_group, pose_target, start_joints):
        self.joint_state.position = start_joints
        self.robot_state.joint_state = self.joint_state
        self.move_group.set_start_state(self.robot_state)

        self.pose_goal.position = pose_target.position
        self.pose_goal.orientation = pose_target.orientation
        self.move_group.set_joint_value_target(self.pose_goal, True)

        return self.move_group.plan()

def main():
    rospy.init_node('mycobot_mover')
    node = myCobotMover("mycobot_joints", "moveit_response")
    rospy.spin()

if __name__ == "__main__":
    main()

launchファイルの作成

~/catkin_ws/src/mycobot_unity/にlaunchフォルダを作成し、mycobot_moveit_unity.launchという名前で保存します。

<launch>
  <!-- Unityの時間を使用するためにrosparamとして/use_sim_timeをtrueにする -->
  <param name="/use_sim_time" value="true" />

  <!-- UnityのROS-TCP-Endpointを起動 -->
  <include file="/home/ubuntu/catkin_ws/src/ROS-TCP-Endpoint/launch/endpoint.launch"/>

  <!-- myCobotのURDF、SRDF、その他の設定ファイルをパラメータサーバにロードする-->
  <include file="/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_280/mycobot_280_moveit/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <!-- シミュレーションで使用する軌道制御器 -->
  <!-- 実機がある場合は実機付属の制御器が行う作業なので偽のコントローラと呼ばれているらしい -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="false"/>
    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
  </node>

  <!-- 公開された関節状態が与えられたら、ロボットリンクのtfを公開する -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
 
  <!-- MoveIt! のメインとなる実行ファイルを実行-->
  <include file="/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_280/mycobot_280_moveit/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="false"/>
  </include>

  <!-- mover.pyを起動 -->
  <node name="mover" pkg="mycobot_unity" type="mover.py" args="--wait" output="screen" />
</launch>

最低限の機能だけをつけているので、必要があれば機能を増やしてください。

保存したら、catkin buildsource ~/.bashrcを実行してください。

Unity側の

メッセージのビルド

今、VNCを使用しているDockerコンテナからmycobot_unityフォルダをホストPCに取り出します。取り出したら、UnityのAssetにドラッグアンドドロップしましょう。

ホストPCでubuntuのターミナルを追加で起動し、docker psコマンドでここまで開発で使用したコンテナのIDを確認します。

docker cp コンテナID:/home/ubuntu/catkin_ws/src/mycobot_unity .

現在、自分がいるフォルダ内にコピーされました。explorer.exe .コマンドを実行し、現在いるフォルダをエクスプローラで表示します。

先ほど、myCobotMoveitJoints.msgというメッセージフィルを作成しました。

ファイルに既述した内容は以下の通りです。

float64[6] joints
geometry_msgs/Pose goal_pose

これと全く同じファイルをUnityが動作しているホストPC内に作成し、UnityのC#スクリプトから使用できるようにビルドします。

ダウンロードフォルダなどに作成して、Robotics→Generate ROS Messages...→ROS message pathから上のメッセージファイルが格納されているフォルダを指定しビルドします。

目標位置をGUIから指定するためのオブジェクトを用意

目標位置と現在のロボットの間接角度をバブリッシュするスクリプトの追加

MoveItでモーションをプランニングするには、先端位置の目標位置とプランニング開始するための現在の間接角度をtopicとして流す必要があります。そのスクリプトはすでに公式が用意してくれているのでこちらを参考にmyCobot用に書き換えます(SourceDestinationPublisher.cs)。新たに作成するスクリプトの名前をmyCobotSourceDestinationPublisher.csとします。

using RosMessageTypes.Geometry;
using RosMessageTypes.MycobotUnity;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;
using Unity.Robotics.UrdfImporter;
using UnityEngine;

public class myCobotSourceDestinationPublisher : MonoBehaviour
{
    private static readonly string TopicName = "/mycobot_joints";
    private static readonly Quaternion PickOrientation = Quaternion.Euler(90, 90, 0);

    public UrdfJointRevolute[] jointArticulationBodies;
    public GameObject target;
    private ROSConnection m_Ros;
    private MyCobotMoveitJointsMsg jointMsg;

    void Start()
    {
        m_Ros = ROSConnection.GetOrCreateInstance();
        m_Ros.RegisterPublisher<MyCobotMoveitJointsMsg>(TopicName);
        jointMsg = new MyCobotMoveitJointsMsg();
    }

    public void Publish()
    {
        for (var i = 0; i < jointArticulationBodies.Length; i++)
        {
            jointMsg.joints[i] = jointArticulationBodies[i].GetPosition();
        }

        jointMsg.goal_pose = new PoseMsg
        {
            position = target.transform.position.To<FLU>(),
            orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To<FLU>()
        };

        m_Ros.Publish(TopicName, jointMsg);
    }
}

UnityとROSの座標系は異なるため値の送信には注意が必要です。Unityの座標系は左手座標系、ROSの座標系は右手座標系です。UnityからROSに位置や回転を送信するときは左手座標系表現から右手座標系表現に変換する必要があります。とはいえ、簡単に変換できるようにTo<FLU>()というメソッドが用意されています。

このスクリプトをHiererchyのRosPublisherのコンポネントとして追加して、下図のようになるようドラッグアンドドロップします。

GUIにパブリッシュボタンを用意

Publishボタンを追加します。追加方法は、Hierechyで右クリック→UI→Buttonです。

Canvasというオブジェクトが追加されるので、その中のTextと書かれたオブジェクトのInspectorからボタンの名前をPublishに変更します。ButtonのInspectorのOn Click()において、左側にRosPublisherをドラッグアンドドロップし、右側にmyCobotSourceDestinationPublisherのPublish関数を追加します。

動作計画の結果を受け取りmyCobotを動作させるスクリプト

myCobotMover.csを作成。以下の通り。

using System.Collections;
using RosMessageTypes.Moveit;
using Unity.Robotics.ROSTCPConnector;
using UnityEngine;

public class myCobotMover : MonoBehaviour
{
    private static readonly string TopicName = "/moveit_response";
    private static readonly Quaternion PickOrientation = Quaternion.Euler(90, 90, 0);

    public ArticulationBody[] jointArticulationBodies;
    private ROSConnection m_Ros;

    void Start()
    {
        m_Ros = ROSConnection.GetOrCreateInstance();

        m_Ros.Subscribe<RobotTrajectoryMsg>(TopicName, Callback);
    }

    void Callback(RobotTrajectoryMsg trajectory)
    {
        if (trajectory!= null && trajectory.joint_trajectory.points.Length > 0) 
        {
            print("success>>>");
            StartCoroutine(ExecuteTrajectories(trajectory));
        }
        else
        {
            print("failed>>>");
        }
    }

    IEnumerator ExecuteTrajectories(RobotTrajectoryMsg trajectory)
    {
        foreach (var t in trajectory.joint_trajectory.points)
        {
            float[] result = new float[6];
            for (var i = 0; i < t.positions.Length; i++)
            {
                result[i] = (float)t.positions[i] * Mathf.Rad2Deg;
            }
            for (var i = 0; i < jointArticulationBodies.Length; i++)
            {
                var joint1XDrive = jointArticulationBodies[i].xDrive;
                joint1XDrive.target = result[i];
                jointArticulationBodies[i].xDrive = joint1XDrive;
            }

            yield return new WaitForSeconds(0.1f);
        }

        yield return new WaitForSeconds(0.1f);
    }
}

動かしてみる

そのまえに、、、

TF_OLD_DATA ignoring data from the past for frame・・・TF_REPEATED_DATA ignoring data with redundant timestamp for frame・・・というエラーが発生することがあります。

これは、タイムスタンプがシミュレータの時間が使われているのにもかかわらず別の時間のタイムスタンプが付与されたトピックが送られてきたときに出るもののようです。rostopic echo /tfとして表示してみると、stampのsec部分にUnityの時間とは異なるものが混ざっていました。正直、対処法が良くわかりません。今のところ大きな問題は発生していないので放置しようと思います。予想ですが、古いほうのtfが悪さしている可能性があります。今回は、tf2を使用しているので特に問題になることはないでしょう。ただ、Warningが鬱陶しいので解決できるかな解決したいところではあります。

また、目標位置に到達しないことがあるため、別のスクリプトを用意して、moveitから出力される最終的な位置が目標いちに到達できているかを判定し、そうでない場合は到達するまでプランニングさせるプログラムを作成する。もちろん、failedの場合は実施しない。

Pythonスクリプトの作成

Publishなしで常にTargetを追従し続けるようにする

MoveItによるプランニングはなんか不安な感じです。Publishを押しても、正しくターゲットに到達してくれないことが多いです。ターゲットに到達しなかった場合に、毎回、Publishを押すのはちょっと使いずらいですし、現実的ではありません。そこで、エンドエフェクタとTargetの位置を常に監視し、もし、一定以上の差があった場合は自動的にプランニングを回すというプログラムに変更しようと思います。このプログラムに変更すれば、エンドエフェクタがターゲットに到達するまでプランニングが実行されますし、常にターゲットを追従することが可能となるはずです。

メッセージファイルの作成

先ほど、以下のようなメッセージファイルを使用しました。

float64[6] joints
geometry_msgs/Pose goal_pose

これに類似した以下のようなメッセージファイルをmyCobotTrackingTarget.msgという名前でROS側とUnity側の両方に作成しビルドします。

float64[6] joints
geometry_msgs/Pose end_effector_pose
geometry_msgs/Pose goal_pose

これを作成したら、mycobot_unityパッケージのCMakeLists.txtのadd_message_filesに追記します。

C#スクリプトの作成

以下のスクリプトをmyCobotTargetEndEffectorPosePublisher.csとして保存します。

using RosMessageTypes.Geometry;
using RosMessageTypes.MycobotUnity;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.Core;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;
using Unity.Robotics.UrdfImporter;
using UnityEngine;

public class myCobotTargetEndEffectorPosePublisher : MonoBehaviour
{
    private static readonly string TopicName = "/mycobot_tracking";
    private static readonly Quaternion PickOrientation = Quaternion.Euler(0, 0, 0);

    public UrdfJointRevolute[] jointArticulationBodies;
    public GameObject target;
    public GameObject endEffector;

    [SerializeField]
    double m_PublishRateHz = 1f;

    double m_LastPublishTimeSeconds;

    double PublishPeriodSeconds => 1.0f / m_PublishRateHz;

    bool ShouldPublishMessage => Clock.NowTimeInSeconds > m_LastPublishTimeSeconds + PublishPeriodSeconds;

    private ROSConnection m_Ros;
    private MyCobotTrackingTargetMsg jointMsg;

    void Start()
    {
        m_Ros = ROSConnection.GetOrCreateInstance();
        m_Ros.RegisterPublisher<MyCobotTrackingTargetMsg>(TopicName);
        jointMsg = new MyCobotTrackingTargetMsg();

        m_LastPublishTimeSeconds = Clock.time + PublishPeriodSeconds;
    }

    public void Publish()
    {
        // 現在の間接角度を格納
        for (var i = 0; i < jointArticulationBodies.Length; i++)
        {
            jointMsg.joints[i] = jointArticulationBodies[i].GetPosition();
        }
        // 目標位置を格納
        jointMsg.goal_pose = new PoseMsg
        {
            position = target.transform.position.To<FLU>(),
            orientation = Quaternion.Euler(target.transform.eulerAngles.x, 
                                           target.transform.eulerAngles.y, 
                                           target.transform.eulerAngles.z).To<FLU>()
        };
        // エンドエフェクタの位置を格納
        jointMsg.end_effector_pose = new PoseMsg
        {
            position = endEffector.transform.position.To<FLU>(),
            orientation = Quaternion.Euler(endEffector.transform.eulerAngles.x, 
                                           endEffector.transform.eulerAngles.y, 
                                           endEffector.transform.eulerAngles.z).To<FLU>()
        };

        m_Ros.Publish(TopicName, jointMsg);
        m_LastPublishTimeSeconds = Clock.FrameStartTimeInSeconds;
    }

    //常にパブリッシュし続ける
    private void Update()
    {
        if (ShouldPublishMessage)
        {
            Publish();
        }
    }
}

保存したら、RosPublisherにAdd Componentします。それぞれ必要なパーツをドラッグアンドドロップしたときの状態は以下の画像の通りです。送信する周期は1秒にしましょう。周期が短くなるとプランニング結果で振動することが増えてしまいますし、一方で長すぎると追従がぎこちなくなってしまいます。

Pythonスクリプトの作成

以下のプログラムを、tracking_target.pyとして、mycobot_unity/scripts直下に配置します。

#!/usr/bin/env python
# coding: UTF-8

import sys
import rospy
import geometry_msgs.msg
import moveit_commander
from sensor_msgs.msg import JointState
from moveit_msgs.msg import RobotState
from moveit_msgs.msg import RobotTrajectory
from mycobot_unity.msg import myCobotTrackingTarget

class myCobotTracker:
    def __init__(self, subTopicName, pubTopicName):
        self.sub = rospy.Subscriber(subTopicName, myCobotTrackingTarget, self.callback)
        self.pub = rospy.Publisher(pubTopicName, RobotTrajectory, queue_size=1)
        self.move_group = moveit_commander.MoveGroupCommander('arm_group')
        self.joint_state = JointState()
        self.joint_state.name = ['joint2_to_joint1', 'joint3_to_joint2', 'joint4_to_joint3', 'joint5_to_joint4', 'joint6_to_joint5', 'joint6output_to_joint6']
        self.robot_state = RobotState()
        self.pose_goal = geometry_msgs.msg.Pose()

    def callback(self, msg):
        start_joints = msg.joints

        # ターゲットとエンドエフェクタの誤差を計算
        delta = abs(msg.end_effector_pose.position.x - msg.goal_pose.position.x) \
              + abs(msg.end_effector_pose.position.y - msg.goal_pose.position.y) \
              + abs(msg.end_effector_pose.position.z - msg.goal_pose.position.z) \
              + abs(msg.end_effector_pose.orientation.x - msg.goal_pose.orientation.x) \
              + abs(msg.end_effector_pose.orientation.y - msg.goal_pose.orientation.y) \
              + abs(msg.end_effector_pose.orientation.z - msg.goal_pose.orientation.z) \
              + abs(msg.end_effector_pose.orientation.w - msg.goal_pose.orientation.w)

        # 絶対誤差の累積が0.2よりも大きかった場合はプランニングを実施しロボットを動作させる
        if delta > 0.02:
            plan = self.plan_trajectory(self.move_group, msg.goal_pose, msg.joints)
            self.move_group.stop()
            self.move_group.clear_pose_targets()

            if plan[0]:
                print("success>>>")
                # プランニングが成功した場合はパブリッシュ
                self.pub.publish(plan[1])
            else:
                print("fail>>>")

    def plan_trajectory(self, move_group, pose_target, start_joints):
        self.joint_state.position = start_joints
        self.robot_state.joint_state = self.joint_state
        self.move_group.set_start_state(self.robot_state)

        self.pose_goal.position = pose_target.position
        self.pose_goal.orientation = pose_target.orientation
        self.move_group.set_joint_value_target(self.pose_goal, True)

        return self.move_group.plan()

def main():
    rospy.init_node('mycobot_tracker')
    node = myCobotTracker("mycobot_tracking", "moveit_response")
    rospy.spin()

if __name__ == "__main__":
    main()

念のためcatkin buildを実行しておきましょう。

launchファイルの編集

先ほど作成した、mycobot_moveit_unity.launchの下から2行目に、type="mover.py"がありますが、そこを、type="tracking_target.py"に変更します。

実行してみよう

以上を行った後に実行してみましょう。ちゃんと追従していますでしょうか???

[動画を挿入]

動作不可能エリア?

を考慮したプランニング

MoveItのPython APIから情報を確認する

>>> move_group.get_current_pose()
header: 
  seq: 0
  stamp: 
    secs: 1660786736
    nsecs:  17536163
  frame_id: "joint1"
pose: 
  position: 
    x: 0.04560003143886354
    y: -0.06462102695479831
    z: 0.41113976263572166
  orientation: 
    x: -0.49999999999325395
    y: 0.5000018365991791
    z: -0.49999816339407505
    w: 0.5000000000067462
import rospy
import moveit_commander
from math import pi
import tf
from geometry_msgs.msg import Quaternion, Vector3, Pose

# moveit_commander.roscpp_initialize()

rospy.init_node("mycobot_unity")
move_group = moveit_commander.MoveGroupCommander("arm_group")

pose_goal = Pose()
pose_goal.position = Vector3(0.1, 0.0, 0.1)
q = tf.transformations.quaternion_from_euler(0, 0, 0)
pose_goal.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
move_group.set_pose_target(pose_goal)
move_group.go(wait=True)

# pose_goal.position = Vector3(1e-6, 1e-6, 1e-6)
# q = tf.transformations.quaternion_from_euler(1e-6, 1e-6, 1e-6)

def move(x, y, z, a,b,c,d):
    pose_goal = Pose()
    pose_goal.position = Vector3(x, y, z)
    pose_goal.orientation = Quaternion(a, b, c, d)
    move_group.set_pose_target(pose_goal)
    move_group.go(wait=True)

実機と連動させる

WSL2を使用している方は、WSL2からUSBデバイスが使用できるようにする必要があります。以下の記事が参考になります。

https://qiita.com/baggio/items/28c13ed8ac09fc7ebdf1

2台のmyCobotを制御する方法

2台のmyCobotを制御する方法を説明します。誰でも最初に思いつくかもしれないですが、同じroslaunchを2回やればいいのか??と思うかもしれませんが、topicが衝突してしまうので、個別に動かせません。そこで、2つのネームスペースを作成し、各ネームスペースの中でroslaunchをすることになります。

ここでは具体例として、各々"left"と"right"というネームスペースのもとで、2台のmyCobotを制御することにします。ネームスペースの指定は、groupタグを使用します。そして、各ネームスペースの中で既存のlaunchファイルをincludeで呼び出します。

<launch>
  <group ns="left">
    <include file="$(find mycobot_280_moveit)/launch/demo.launch"/>
  </group>
  <group ns="right">
    <include file="$(find mycobot_280_moveit)/launch/demo.launch"/>
  </group>
</launch>

  • この記事を書いた人
管理人

管理人

このサイトの管理人です。 人工知能や脳科学、ロボットなど幅広い領域に興味をもっています。 将来の目標は、人間のような高度な身体と知能をもったパーソナルロボットを開発することです。 最近は、ロボット開発と強化学習の勉強に力を入れています(NOW)。

-ROS, Unity, ガジェット, シミュレータ, ブログ, ロボット研究・製作

PAGE TOP