ロボット開発記録

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

[第3週] ROS2とWebotsの連携 調査編2 [ヒューマノイド動歩行]

先週の振り返り

 先週の記事では、WebotsとROS2の連携方法と、その連携に関する調査を開始したところでした。

今週の概要

 先週に引き続き、WebotsとROS2に関する調査の報告
<追記> ・2022/10/6: URDFの記述に関する説明の修正。

連携に関する調査

 先週に引き続き、WebotsとROS2の連携に関して調査を進めていきます。この2つの連携は、Webots公式から配布されているパッケージを使うことで実現できます。demoもあるため実装後すぐに連携を体験できます。
 しかし自分自身でWebotsと連携するROS2パッケージを作成するとなると、その連携の仕組みを知る必要があります。そのために必要な情報を調査し、まとめていくことを先週から行っています。今回はその現状報告となります。
 なお今回の情報源は、webots_ros2のWikiWebots User Guide、そしてWebots Reference Manualです。ここを参照すれば十分なのですが、ここでは今回の開発で必要な箇所のみに絞って調査しています。(調査の備忘録

 調査対象は、大きく分けて以下の3つです。
 ・Webots上に既に存在するロボットをROS2で認識する方法
 ・Webots上のロボットをROS2から制御する方法
 ・上記2つのことをするために必要なヘッダと関数(C++

注意!:この記録が正しいとは限りません。間違いを見つけ次第できる限り 修正していきます。

1.WebotsにあるロボットをROS2で認識する方法

・Webotsに既にあるロボットをROS2で認識する場合、URDFに説明を記述する必要があります。以下、公式のdemoから引用です。

webots_ros2/webots_ros2_universal_robot/resource/webots_abb_description.urdf

<?xml version="1.0" ?>
<robot name="ABB Webots">
    <webots>
        <plugin type="webots_ros2_control::Ros2Control" />
    </webots>

    <ros2_control name="WebotsControl" type="system">
        <hardware>
            <plugin>webots_ros2_control::Ros2ControlSystem</plugin>
        </hardware>

        <!-- ABB -->
        <joint name="A motor">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="B motor">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="C motor">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="D motor">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="E motor">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="F motor">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>

        <!-- ROBOTIQ 3F Gripper -->
        <joint name="palm_finger_1_joint">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_1_joint_1">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_1_joint_2">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_1_joint_3">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="palm_finger_2_joint">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_2_joint_1">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_2_joint_2">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_2_joint_3">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_middle_joint_1">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_middle_joint_2">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_middle_joint_3">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>

    </ros2_control>
</robot>

引用元:GitHub, cyberbotics, webots_ros2/webots_ros2_universal_robot/resource/webots_abb_description.urdf (観覧日: 2022/10/4)

github.com

 ・~はじめ2つのpluginは、Webotsと連携させるために必要なものとなっています。~
  ・webots_ros2_control::Ros2Control:ros2_controlの実装。タグ内に記述(追記2022/10/9: コレがなくても、対応するような独自プラグインを作成すれば連携は可能。公式demo,mavicを参照)
   ・タグ:webots関連の構成全体がこのタグ下で宣言される。
  ・webots_ros2_control::Ros2ControlSystem:ros2_controlの使用に必要
   ・joint_nameは元のロボットモデルに合わせる必要がある。モデルのjoint名は、既存のものであれば公式からドキュメントが公開されているため、そちらを見れば良いだろう。以下、例で用いているロボットのドキュメントのリンク。

cyberbotics.com

cyberbotics.com

 ・なお、Webots上のロボットに搭載されているセンサもURDFに説明を記述してやることで扱えるようになります。以下は、URDFの記述方法を記した公式Wikiのページと、例として参考になる公式demoのコードへのリンクです。

github.com

github.com

 ・上記以外にも、独自のプラグインを適用することもできます。このプラグインはROS2からの制御に使うことができます。以下、例として公式demoからの引用です。以下のようにプラグイン名(プラグイン記述プログラムとしてはclass名)を記述する必要があります。

webots_ros2/webots_ros2_tesla/resource/tesla_webots.urdf

<?xml version="1.0" ?>
<robot name="Tesla Webots">
    <webots>
        <plugin type="webots_ros2_tesla.tesla_driver.TeslaDriver" />
    </webots>
</robot>

このプラグインソースコード:webots_ros2_tesla/tesla_driver.py 引用元:GitHub, cyberbotics, webots_ros2/webots_ros2_tesla/resource/tesla_webots.urdf (観覧日: 2022/10/4)

github.com


2.WebotsにあるロボットをROS2から制御する方法

・制御方法の1つとして、独自の制御プラグインを作成して使うことが考えられます。プラグインを適用するには、上記で示した独自プラグインの適用方法に沿う必要があります。
 ・このプラグインはROS2のnode作成に加え、WebotsのAPIを使うためのclassを継承する必要があります。以下、公式demoから、プラグインとURDFのコードの引用です。

webots_ros2/webots_ros2_mavic/webots_ros2_mavic/mavic_driver.py

# Copyright 1996-2021 Cyberbotics Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""ROS2 Mavic 2 Pro driver."""

import math
import rclpy
from geometry_msgs.msg import Twist


K_VERTICAL_THRUST = 68.5    # with this thrust, the drone lifts.
K_VERTICAL_P = 3.0          # P constant of the vertical PID.
K_ROLL_P = 50.0             # P constant of the roll PID.
K_PITCH_P = 30.0            # P constant of the pitch PID.
K_YAW_P = 2.0
K_X_VELOCITY_P = 1
K_Y_VELOCITY_P = 1
K_X_VELOCITY_I = 0.01
K_Y_VELOCITY_I = 0.01
LIFT_HEIGHT = 1


def clamp(value, value_min, value_max):
    return min(max(value, value_min), value_max)


class MavicDriver:
    def init(self, webots_node, properties):
        self.__robot = webots_node.robot
        self.__timestep = int(self.__robot.getBasicTimeStep())

        # Sensors
        self.__gps = self.__robot.getDevice('gps')
        self.__gyro = self.__robot.getDevice('gyro')
        self.__imu = self.__robot.getDevice('inertial unit')

        # Propellers
        self.__propellers = [
            self.__robot.getDevice('front right propeller'),
            self.__robot.getDevice('front left propeller'),
            self.__robot.getDevice('rear right propeller'),
            self.__robot.getDevice('rear left propeller')
        ]
        for propeller in self.__propellers:
            propeller.setPosition(float('inf'))
            propeller.setVelocity(0)

        # State
        self.__target_twist = Twist()
        self.__vertical_ref = LIFT_HEIGHT
        self.__linear_x_integral = 0
        self.__linear_y_integral = 0

        # ROS interface
        rclpy.init(args=None)
        self.__node = rclpy.create_node('mavic_driver')
        self.__node.create_subscription(Twist, 'cmd_vel', self.__cmd_vel_callback, 1)

    def __cmd_vel_callback(self, twist):
        self.__target_twist = twist

    def step(self):
        rclpy.spin_once(self.__node, timeout_sec=0)

        roll_ref = 0
        pitch_ref = 0

        # Read sensors
        roll, pitch, _ = self.__imu.getRollPitchYaw()
        _, _, vertical = self.__gps.getValues()
        roll_acceleration, pitch_acceleration, twist_yaw = self.__gyro.getValues()
        velocity = self.__gps.getSpeed()
        if math.isnan(velocity):
            return

        # Allow high level control once the drone is lifted
        if vertical > 0.2:
            # Calculate velocity
            velocity_x = (pitch / (abs(roll) + abs(pitch))) * velocity
            velocity_y = - (roll / (abs(roll) + abs(pitch))) * velocity

            # High level controller (linear velocity)
            linear_y_error = self.__target_twist.linear.y - velocity_y
            linear_x_error = self.__target_twist.linear.x - velocity_x
            self.__linear_x_integral += linear_x_error
            self.__linear_y_integral += linear_y_error
            roll_ref = K_Y_VELOCITY_P * linear_y_error + K_Y_VELOCITY_I * self.__linear_y_integral
            pitch_ref = - K_X_VELOCITY_P * linear_x_error - K_X_VELOCITY_I * self.__linear_x_integral
            self.__vertical_ref = clamp(
                self.__vertical_ref + self.__target_twist.linear.z * (self.__timestep / 1000),
                max(vertical - 0.5, LIFT_HEIGHT),
                vertical + 0.5
            )
        vertical_input = K_VERTICAL_P * (self.__vertical_ref - vertical)

        # Low level controller (roll, pitch, yaw)
        yaw_ref = self.__target_twist.angular.z

        roll_input = K_ROLL_P * clamp(roll, -1, 1) + roll_acceleration + roll_ref
        pitch_input = K_PITCH_P * clamp(pitch, -1, 1) + pitch_acceleration + pitch_ref
        yaw_input = K_YAW_P * (yaw_ref - twist_yaw)

        m1 = K_VERTICAL_THRUST + vertical_input + yaw_input + pitch_input + roll_input
        m2 = K_VERTICAL_THRUST + vertical_input - yaw_input + pitch_input - roll_input
        m3 = K_VERTICAL_THRUST + vertical_input - yaw_input - pitch_input + roll_input
        m4 = K_VERTICAL_THRUST + vertical_input + yaw_input - pitch_input - roll_input

        # Apply control
        self.__propellers[0].setVelocity(-m1)
        self.__propellers[1].setVelocity(m2)
        self.__propellers[2].setVelocity(m3)
        self.__propellers[3].setVelocity(-m4)

引用元:GitHub, cyberbotics, webots_ros2/webots_ros2_mavic/webots_ros2_mavic/mavic_driver.py (観覧日: 2022/10/4)

webots_ros2/webots_ros2_mavic/resource/mavic_webots.urdf

<?xml version="1.0" ?>
<robot name="Mavic Webots">
    <webots>
        <device reference="gps" type="GPS">
            <ros>
                <enabled>true</enabled>
                <alwaysOn>true</alwaysOn>
            </ros>
        </device>

        <plugin type="webots_ros2_driver::Ros2IMU">
            <enabled>true</enabled>
            <topicName>/imu</topicName>
            <alwaysOn>true</alwaysOn>
            <frameName>imu_link</frameName>
            <inertialUnitName>inertial unit</inertialUnitName>
            <gyroName>gyro</gyroName>
        </plugin>

        <plugin type="webots_ros2_mavic.mavic_driver.MavicDriver" />
    </webots>
</robot>

引用元:GitHub, cyberbotics, webots_ros2/webots_ros2_mavic/resource/mavic_webots.urdf (観覧日: 2022/10/4)

github.com

 ・このプログラムは、ドローンのDJI' Mavic 2 PROWebotsドキュメントを制御するプログラムである。classのMavicDriverがプラグインとしてURDFに登録する必要がある。

 ・結構長く書かれているが、重要なのはinit関数とstep関数、そしてこのclassが継承をしているということである。
  ・classの継承:WebotsのAPIを使うために、WebotsNodeを継承する必要がある。これはwebots_ros2/webots_ros2_driverから提供されており、継承元classのコードもそこから参照できる。
  ・init関数:WebotsNodeで定義されている関数をオーバーライド。ROS2のnodeの定義やWebotsのAPIを使うための処理、各変数の宣言や定義がされている。webots_node.robotから、Webots上のロボットにアクセスできる。これはWebotsのAPIの1つであるRobot(公式ドキュメント)に対応している。ここを通じてロボットのセンサデータを得たりデバイスの操作が可能となる。そのデバイスもURDFで記述しておく必要がある。
  ・step関数:WebotsNodeで定義されている関数をオーバーライド。この関数内に書かれたコードを繰り返し実行する。

 ・1つの疑問として、プラグインには、nodeを直接実行するコード(main関数)は記述されていない。そのため、プラグインが実行されるタイミングがわからない。ただ予想として、Launchファイルが実行されてWebotsが完全に立ち上がった最後に実行されていることが考えられる。

 以上のような構成となっています。この他にもLaunchファイルやPythonであればsetup.pyを記述する必要があります。
 以上のこれらのことは公式Wikiにも書かれています。以下、そのドキュメントです。

github.com

github.com


3.必要なヘッダと関数(C++

・以上まで、制御するために必要なものを特定しました。しかし公式demoはPythonで書かれており、今回使うC++とは大きく記述が異なります。ここでは、C++で記述する場合のコードを調査します。

 ・C++の場合であっても必要なことや処理の流れは変わらず、WebotsNodeクラスを継承する必要がある。異なるのは記述と、ビルドの設定を記述するファイルがsetup.pyではなくCMakeFileであること、そしてプラグインの記述に、classを宣言するプログラム(ヘッダファイル:.hpp)とclassを定義するプログラム(.cpp)が必要であること。
  ・このclassの書き方はROS2で行われるものと同様。ヘッダファイルで宣言されるclassが継承するclassを、webots_ros2_driver::PluginInterfaceとすれば良い。このPluginInterfaceはWebotsNodeも含んでいるため、これを継承することでWebotsのAPIを扱うことができる。

 C++の記述のテンプレートやCMakeFileの記述例は、公式Wikiの方に書かれています。そちらを参考にすれば良いと考えています。

github.com


 今回の調査では、以上のことまで把握することができました。これらすべては、Webots公式の方から公開されているドキュメントを参考にしています。そちらも参照してください。(リンクは参考文献に記載)


終わりに・今後の予定

 前回から今回に渡って、ROS2とWebotsの連携について、ドキュメントを元に調査をしてきました。次回からはこの調査を元に、実際にプログラムを書きながらロボットを動かすことに挑戦しようと考えています。

 ここまで読んでいただき、ありがとうございました。また来週も、よろしくお願いします。

参考文献

Webots User Guide
Webots Reference Manual
Home · cyberbotics/webots_ros2 Wiki · GitHub (最終観覧日:2022/10/4)