先週の振り返り

　先週の記事までは、ROS2とWebotsの連携についての調査を一通り行いました。

今週以降の概要

　調査した結果を元に、WebotsとROS2の連携を実装を進める。

連携の実装

　今週から、WebotsとROS2の連携を実装していきます。これには数週間かかるため、この記事を毎週更新することで実装の記録をつけていきます。
　先週まで調査してきたことを元に実装していき、そこで判明したことがあれば一緒に記載していきます。まとめ方としては、毎週ごとに進捗記録を列挙する形になります。
　以下に、開発リポジトリを示します。

github.com

　参考サイトは以下の２つとなっています。

github.com

実装進捗

2022/10/10

　・Wikiに記載されているコードや公式demoを参考に、以下のファイルを作成。
　　・walking_pattern_generator/include/walking_pattern_generator/WebotsRobotController.hpp
　　・walking_pattern_generator/launch/robot_launch.py
　　・walking_pattern_generator/resource/webots_robotis_op2_description.urdf
　　・walking_pattern_generator/src/WebotsRobotController.cpp
　　・walking_pattern_generator/CMakeList.txt
　　・walking_pattern_generator/package.xml
　　・walking_pattern_generator/walking_pattern_generator.xml

　・公式tutorialを参考に、worldファイル（webots_simple_world.wbt）を作成。
　　・公式tutorialの”Create the simulation world in Webots”を完了後、既存のロボットモデルを追加。今回はROBOTIS社のROBOTIS-OP2を使用。Addボタンを押した後に出るポップアップウィンドウから以下のようにして追加すれば良い。(Webots Manual, ROBOTIS' Robotis OP2)
　　　

　　・追加したnodeの設定を少しいじり、結果として以下のようなシミュレーション環境を作成。　　　

　・コンパイルは通ったが、CMakeにLaunchファイルのパスを書いてなかったり、ROS2の初期化コード（〜::init(...)）をどうするかいまいち分かってなかったりと、まだまだ不完全。

　・今後は、各コードの意味をより掘っていきたい。

2022/10/17

　・以下の項目を達成。
　　・ロボットのモータに、関節角度を命令し、操作。
　　・関節に搭載されているPositionSensor(関節角度を観測するセンサ)から値を取得、観測。
　　・他nodeへのPublish。

　・方法
　　・モータの操作
　　　・まず、ヘッダファイルをincludeする必要がある。#include <webots/Motor.hpp>
　　　・initの引数であるnodeからWebotsのAPIのRobotにアクセス。この返り値はモータに限らず、ロボットに搭載されている全てのモジュールにアクセスするために用いる。（型: webots::Robot）
　　　・robot->getMotor("<モータ名>")で特定のモータにアクセス。この返り値を得る。ここから、モータの回転角度や回転速度などを命令する。（型: webots::Motor）
　　　・今回はモータの回転角度を操作したいので、上記の返り値を持つ変数の要素の１つにアクセスし、回転角度[rad]を命令。<変数>->webots::Motor::setPosition(<回転角度[rad]>)。
　　　・以下、今回作成したプログラムからモータ操作に関するコードを抜粋。

// 元プログラム名: WalkingPatternGenerator.hpp
~
            webots::Robot *robot;
            webots::Motor *motor[20];
~

// 元プログラム名: WalkingPatternGenerator.cpp
~
#include  <webots/Motor.hpp>
~
        robot = node->robot();
        motor[0] = robot->getMotor("AnkleL");  // 左足首
~
        motor[0]->webots::Motor::setPosition(1);  // 1[rad]
~

　　・関節角度値の取得
　　　・今回のロボットの場合、関節の角度を測るPositionSensorはモータと同じ場所に同数取り付けられている。得られる値もモータ同様[rad]となっている。
　　　・取得するまでの手順はモータの操作とほぼ同様。値は返り値として得られるため、getValue()で値を得る際は変数に返り値を渡す必要がある。（返り値の型: double）
　　　・また、値を得る前に、値取得を有効（enable）にする必要があり、その際に値取得のサンプリング周期（単位: [ms]）を定める。
　　　　　・注意すべきこととして、有効にして１周期後に初めて値が得られることが挙げられる。有効にしてすぐに値が得られるわけではない。
　　　・以下、関するコードを抜粋。

// 元プログラム名: WalkingPatternGenerator.hpp
~
            webots::Robot *robot;
            webots::PositionSensor *positionSensor[20];
~

// 元プログラム名: WalkingPatternGenerator.cpp
~
#include  <webots/PositionSensor.hpp>
~
        robot = node->robot();
~
        positionSensor[0] = robot->getPositionSensor("AnkleLS");  // 左足首
~
        positionSensor[0]->webots::PositionSensor::enable(100);  // sampling period: 100[ms]
~
        double hage = positionSensor[0]->webots::PositionSensor::getValue();  // 値取得
~

　　・他nodeへのPublish
　　　・他nodeへのPublishは、通常のROS2で作成するPublisherのコードを記載することで実現できる。
　　　・新しいnodeを宣言、定義したり、ROS2を初期化するコードを記述する必要はない。（Pluginだから？）
　　　　・Pythonで書かれた公式のdemoでは、nodeを新しく再生し、initで初期化もしていた。（C++でもできるのか？）
　　　　・恐らく、同様の方法でSubscribeも可能だと思われる。
　　　・今回は実験として、String型のメッセージを"test"という名前のTopicでPublishし、Subscribeしたnodeの方でそのString型のメッセージをscreenに出力した。
　　　　・Subscribeするnodeの名前は、”Test1_Sub”とした。
　　　・以下、Publishに関するコードを抜粋。Subscribeするnodeは、test1_sub.という名前を持つプログラムで定義されており、開発リポジトリで確認できる。

// 元プログラム名: WalkingPatternGenerator.hpp
~
            rclcpp::Publisher<std_msgs::msg::String>::SharedPtr __pub;
~

// 元プログラム名: WalkingPatternGenerator.cpp
~
#include  <webots/PositionSensor.hpp>
~
// (void WalkingPatternGenerator::init()内)
        __pub = node->create_publisher<std_msgs::msg::String>("test", rclcpp::QoS(10));
~
// (void WalkingPatternGenerator::step()内)
        auto datamsg = std::make_shared<std_msgs::msg::String>();
        static int count = 0;
        datamsg->data = "Hello: " + std::to_string(count++);
        __pub->publish(*datamsg);
~

　・非常に有意義な進捗結果が得られ、実装に関する知識がより明確に得られた。
　・まだいくつかやり残していることがあるため、それをこの１週間で済まし、来週には調査記録の確認を完了したい。

2022/10/24

　・以下の項目を達成
　　・ロボットに搭載されている角加速度センサ、角速度センサの値を取得、観測。
　　・webotsのSupervisorから、ロボットの位置や傾きなどを取得、観測。
　　・webotsのバージョンを2022bに、webots_ros2パッケージをv2022.1.2に更新

　・Versionの更新
　　・webotsと、ROS2連携に必要なwebots_ros2のバージョンを最新に更新した。開発中のプログラムの変更点は、worldファイル以外は特にない。worldファイルも、バージョンアップ前と生成環境は変わっていない。

　・センサから値を取得
　　・加速度センサ（Accelerometer）
　　　・ロボットの加速度[m/s²]を得る。ROBOTIS_OP2の場合、加速度を、-3g[m/s²] ~ +3g[m/s²]の範囲を0 ~ 1024の数値（const double）で表現し、それが返り値として得られる。
　　　　・ここで g は、重力加速度9.81[m/s²]を表している。
　　　　・加速度センサの返り値には、シミュレーション環境に対して垂直方向に働いている重力1g[m/s²]が含まれている。
　　・角速度センサ（Gyro）
　　　・ロボットの角速度[rad/s]を得る。ROBOTIS_OP2の場合、角速度を、-1600[deg/sec] ~ +1600[deg/sec]の範囲を0 ~ 1024の数値（const double）で表現し、それが返り値として得られる。
　　　・以下、加速度センサと角速度センサのコード例。

// 元プログラム: WalkingPatternGenerator.cpp
~
// void ~::step()内
~
        accelerometerValue = accelerometer->webots::Accelerometer::getValues();  // 加速度センサの返り値取得。３要素の配列[x, y, z]
        gyroValue = gyro->webots::Gyro::getValues();  // 角速度センサの返り値取得。３要素の配列[x, y, z]
~

　・Supervisorの詳細
　　・Supervisorを使う事前準備
　　　・Supervisorとは、現実世界で人間がおこなうような行動、例えばロボットの位置や動作のリスタートなどをWebots上でおこなうために用いるAPIとなっている。ドキュメントはこちらとなっており、詳細はそこに記されている。
　　　・モータの操作の時と同じように、initの引数であるnodeからWebotsのAPIのRobotにアクセス。それを、webots::Supervisorの型を持つ変数（supervisor）に入れる。（モータの操作の時は、webots::Robotであった。）
　　　・supervisorからgetFromDef()関数を呼び出し、対象のロボット（正確にはノード）のDEF名を引数として渡し、返り値をwebots::Node型の変数（SupervisorNode）で受け取る。この変数から様々なsupervisorを呼び出すことになる。
　　　　・なお、DEF名は（恐らく）事前に設定する必要がある。今回はworldファイル内の一部を、以下のように記述して対応した。（なお、DEF名が存在する意味は分かっていない）

// 元プログラム名: webots_simple_world.wbt
~
DEF ROBOTIS_OP2 RobotisOp2 {  // ここで、DEF名をROBOTIS_OP2に設定

~

　　・以下、事前準備のコード。

// 元プログラム: WalkingPatternGenerator.cpp
~
// void WalkingPattern Generator::init()内
        supervisor = Node->robot();  // Node = node
        SupervisorNode = supervisor->getFromDef("ROBOTIS_OP2");
        if (SupervisorNode == NULL) {  // ちゃんとDEF名が存在するかのチェック
            std::cout << "ERROR!: getFromDef == NULL" << std::endl;
            while(true){  // 存在しなかったら、エラーを吐いて無限ループ
            }
        }
~

　　・ロボットの位置と傾きの取得（Fieldから）
　　　・Fieldから、ロボットの位置と傾きを得る。
　　　・このFieldはロボットに固有に設定されたものであり、ロボット自身が持つ自身の情報を得ることができる。ROBOTIS_OP2のFieldはこのドキュメントに記されている。
　　　・Fieldより、ロボットの位置はtranslation、傾きはrotationから得ることができる。
　　　　・Fieldの活用は、はじめに得たいFieldを関数から指定し、その際に得られた返り値を通じて値を読み書きする。
　　　・以下、コード例。

// 元プログラム: WalkingPatternGenerator.cpp
~
// void ~::init()内
~
        field_translation = SupervisorNode->getField("translation");   // 変数の型はwebots::Field
        field_rotation = SupervisorNode->getField("rotation");  // 引数はField名
~
// void::step()内
~
        translation = field_translation->getSFVec3f();  // グローバル座標系からロボットの位置ベクトル。3要素の行列[x, y, z]。単位[m]
        rotation = field_rotation->getSFRotation;  // 回転角度。3要素の行列[x, y, z]。単位[rad]

　　・ロボットの位置、回転行列、同次変換行列、重心位置、速度、加速度の取得
　　　・グローバル座標系からロボットのローカル座標系への位置ベクトル、回転行列、同次変換行列、重心位置、そしてロボット自身が持つ速度と角速度を得る。
　　　・この場合の位置ベクトルは、Fieldから得たロボットの位置ベクトルと同値であった。
　　　・同次変換行列は、位置ベクトルと回転行列を1つの行列として表したものであり、その要素は別で得られる位置ベクトルや回転行列と同値であった。つまり、同時変換行列の特定の要素を抜き出せば、位置ベクトルと回転行列が得られる。
　　　・以下、コード例。

// 元プログラム: WalkingPatternGenerator.cpp
~
// void ~::step()内
~
        position = SupervisorNode->getPosition();  // 位置ベクトル[x[m], y[m], z[m]]。translationと同値
        orientation = SupervisorNode->getOrientation();  // 回転行列[[x,y,z], [x,y,z], [x,y,z]]
        pose = SupervisorNode->getPose();  // 同次変換行列[[回転行列(3*3), 位置ベクトル(3*1)], [0, 0, 0, 1]]
        centerOfMass = SupervisorNode->getCenterOfMass();  // ロボットの重心位置[x, y, z]。単位[m]
        velocity = SupervisorNode->getVelocity();  // ロボットの速度[m/s]、角速度[rad/s]。各3要素、1行6要素の配列。
~

　・以上で、制御に活用できる今考えられる情報を、ほとんど得ることができた。
　　・接触点を返すgetContactPoint()も利用したかったが、まだうまく使えていない。制御に必須でもない。
　・これで、今できる連携方法の確認は完了したと判断する。

最後に・今後の予定

・３週に渡って、WebotsとROS2の連携を、実際にプログラムを組むことで確認することができました。
・来週からはようやく、本質である制御の理論に挑戦していきたいと思います。
・開発のソースコードは、こちらから確認できます。

参考サイト

・Open Robotics, ROS 2 Documentation: Foxy
・cyberbotics, Webots公式サイト
・cyberbotics, Webots Reference Manual
・cyberbotics, Webots User Guide
・cyberbotics, Webots, GitHub
・cyberbotics, webots_ros2, GitHub
・cyberbotics, webots_ros2 Wiki, GitHub
・@Nek, ROS2導入＆レクチャー, Qiita
　↑ 特にコードの記述の参考にさせていただいたサイト様
・開発リポジトリ, open-rdc, ROS2_Walking_Pattern_Generator, GitHub

ロボット開発記録

ロボット開発の進捗や記録をするブログ

[第４〜６週] ROS2とWebotsの連携実装編 [ヒューマノイド動歩行]