🛸
PyBulletでドローンを飛ばす その１
強化学習の勉強でもしてみようかと調査をしていたら、深層強化学習用の物理シミュレータとしてPyBulletというオープンソースのシミュレータが良く利用されるということをちょくちょく見聞きしました。
事例としては、URDF形式のファイルを読み込んでアーム型のロボット等の挙動をシミュレートするというものが多いようです。
強化学習にもシミュレーションにも疎いもので、PyBulletでどのようなことが出来るのかイメージが湧かず、「ロボットみたいに関節があるモデルの物理演算に特化したものなのかなぁ」と勝手に想像していたのですが、実際にはそんなことはないみたいですね。
以下のようなドローンの挙動をシミュレーションする事例もありました。
原著が英語の論文なので、肝心な中身を良く理解できていませんが、今後のAI研究を促すためにと、ドローンの強化学習の一例としてソースコードを公開してくださっているようです。素晴らしい。ありがとうございます。
こんな応用が利くのであれば、個々の案件に合わせた様々なケースへの適用も、物理モデルさえ定義すれば可能になるような気がします。面白そうだ。
ということで、強化学習のお勉強は一旦保留して、まずはPyBulletの使い方を勉強することにしました。
教材は前述のドローンのリポジトリです。こちらの内容をトレースして、とりあえずPyBullet上でドローンの挙動をシミュレーションするところをやってみたいと思います。
１．ドローンのURDFファイル
オリジナルのリポジトリでは、リアルなドローンのメッシュを張り付けたURDFファイルがモデルとして利用されていました。ここではURDFの勉強も兼ねて、ほんの少しだけ内容を修正した以下のようなファイルを用意しました。
assets/frone_p_01.urdf
<?xml version="1.0" ?>
<robot name="cf2_p_variant">
<properties arm="0.0397" kf="3.16e-10" km="7.94e-12" thrust2weight="2.25" max_speed_kmh="30" gnd_eff_coeff="11.36859" prop_radius="2.31348e-2" drag_coeff_xy="9.1785e-7" drag_coeff_z="10.311e-7" dw_coeff_1="2267.18" dw_coeff_2=".16" dw_coeff_3="-.11" />
<link name="base_link">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.027"/>
<inertia ixx="2.3951e-5" ixy="0.0" ixz="0.0" iyy="2.3951e-5" iyz="0.0" izz="3.2347e-5"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 -0.005"/>
<geometry>
<box size="0.04 0.04 0.015"/>
</geometry>
<material name="grey">
<color rgba=".5 .5 .5 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder radius=".06" length=".025"/>
</geometry>
</collision>
</link>
<link name="prop0_link">
<inertial>
<origin rpy="0 0 0" xyz="0.0397 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.0397 0 0.005"/>
<geometry>
<cylinder radius=".02" length=".015"/>
</geometry>
<material name="light_green">
<color rgba="0.0 1.0 0.0 0.5"/>
</material>
</visual>
</link>
<joint name="prop0_joint" type="fixed">
<parent link="base_link"/>
<child link="prop0_link"/>
</joint>
<link name="prop1_link">
<inertial>
<origin rpy="0 0 0" xyz="0 0.0397 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0.0397 0.005"/>
<geometry>
<cylinder radius=".02" length=".015"/>
</geometry>
<material name="light_red">
<color rgba="1.0 0.0 0.0 0.5"/>
</material>
</visual>
</link>
<joint name="prop1_joint" type="fixed">
<parent link="base_link"/>
<child link="prop1_link"/>
</joint>
<link name="prop2_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.0397 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.0397 0 0.005"/>
<geometry>
<cylinder radius=".02" length=".015"/>
</geometry>
<material name="light_red">
<color rgba="1.0 0.0 0.0 0.5"/>
</material>
</visual>
</link>
<joint name="prop2_joint" type="fixed">
<parent link="base_link"/>
<child link="prop2_link"/>
</joint>
<link name="prop3_link">
<inertial>
<origin rpy="0 0 0" xyz="0 -0.0397 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 -0.0397 0.005"/>
<geometry>
<cylinder radius=".02" length=".015"/>
</geometry>
<material name="light_red">
<color rgba="1.0 0.0 0.0 0.5"/>
</material>
</visual>
</link>
<joint name="prop3_joint" type="fixed">
<parent link="base_link"/>
<child link="prop3_link"/>
</joint>
<link name="center_of_mass_link">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<joint name="center_of_mass_joint" type="fixed">
<parent link="base_link"/>
<child link="center_of_mass_link"/>
</joint>
</robot>
このファイルをPyBulletで読み込むと以下のようなモデルになります。
うーん、しょぼいですね。
しかしまあ、実際にプロペラを回転して航空力学的な挙動をシミュレーションする訳ではないので、おおよその雰囲気とドローンの向きが伝わればとりあえず良しとしておきます。
２．ドローンのプロパティの読み込み
前述のURDFファイルの前半の方、<properties arm="0.0397" kf="3.16e-10" ... />の部分に、今回のドローンのシミュレーションを行う上で重要な、ドローンの重量、慣性モーメントといった物理量（定数）が記載されています。
この<properties />部分の情報は、PyBulletで通常のURDFファイルのように読み込んでもシミュレーションに反映されないので、独自に解析してデータとして取り込んであげる必要があります。
これらの処理がオリジナルのリポジトリにも当然組み込まれているのですが、今回は自前のプログラムを流用しました（以前に似たようなものを作ったことがあったので）。
参考までにURDFファイルとともに下記に上げております。
本題から外れるので説明は省きますが、以下のようにしてドローンのプロパティを読み込めるようになっています。
from util.file_tools import DroneUrdfAnalyzer
file = 'assets/drone_p_01.urdf'
urdf_a = DroneUrdfAnalyzer()
# 返り値（params）はdataclass型です。次項で詳細を説明。
parms = urdf_a.parse(file)
print('drone weight:' parms.m) # -> drone weight: 0.027
このようにして読み込んだドローンのプロパティを格納するために、以下のようなdataclassを定義しました。
読み込んだドローンの物理量を用いて作成しなければいけない演算用の行列などがあるのですが、それらもこのdataclassに読み込んだ時点で予め計算するようにしています。
util/data_definition.py
@dataclass
class DroneProperties(object):
"""
The drone parameters.
kf : It is the proportionality constant for thrust, and thrust is proportional to the square of rotation speed.
km : It is the proportionality constant for torque, and torque is proportional to the square of rotation speed.
"""
type: int = 1 # The drone type 0:OTHER 1:QUAD_PLUS 2:QUAD_X # ドローンの形状タイプ
g: float = 9.8 # gravity acceleration # 重力加速度
m: Optional[float] = None # ドローンの重量.
l: Optional[float] = None # ドローンの重心からロータの設置位置までの距離
thrust2weight_ratio: Optional[float] = None
# イナーシャ関連
ixx: float = 0
iyy: float = 0
izz: float = 0
J: np.ndarray = np.array([])
J_inv: np.ndarray = np.array([])
# ローターの回転数の2乗に比例するスラスト定数、トルク定数
kf: Optional[float] = None # The proportionality constant for thrust.
km: Optional[float] = None # The proportionality constant for torque.
collision_h: Optional[float] = None
collision_r: Optional[float] = None
collision_shape_offsets: List[float] = field(default_factory=list)
collision_z_offset: float = None
max_speed_kmh: Optional[float] = None
gnd_eff_coeff: Optional[float] = None
prop_radius: Optional[float] = None
drag_coeff_xy: float = 0
drag_coeff_z: float = 0
drag_coeff: np.ndarray = None
dw_coeff_1: Optional[float] = None
dw_coeff_2: Optional[float] = None
dw_coeff_3: Optional[float] = None
# compute after determining the drone type
gf: float = 0 # gravity force
hover_rpm: float = 0
max_rpm: float = 0
max_thrust: float = 0
max_xy_torque = 0
max_z_torque = 0
grand_eff_h_clip = 0 # The threshold height for ground effects.
A: np.ndarray = np.array([])
inv_A: np.ndarray = np.array([])
B_coeff: np.ndarray = np.array([])
Mixer: np.ndarray = np.ndarray([]) # use for PID control
def __post_init__(self):
self.J = np.diag([self.ixx, self.iyy, self.izz])
self.J_inv = np.linalg.inv(self.J)
self.collision_z_offset = self.collision_shape_offsets[2]
self.drag_coeff = np.array([self.drag_coeff_xy, self.drag_coeff_xy, self.drag_coeff_z])
self.gf = self.g * self.m
self.hover_rpm = np.sqrt(self.gf / (4 * self.kf))
self.max_rpm = np.sqrt((self.thrust2weight_ratio * self.gf) / (4 * self.kf))
self.max_thrust = (4 * self.kf * self.max_rpm ** 2)
if self.type == 2: # QUAD_X
self.max_xy_torque = (2 * self.l * self.kf * self.max_rpm ** 2) / np.sqrt(2)
self.A = np.array([[1, 1, 1, 1], [1 / np.sqrt(2), 1 / np.sqrt(2), -1 / np.sqrt(2), -1 / np.sqrt(2)],
[-1 / np.sqrt(2), 1 / np.sqrt(2), 1 / np.sqrt(2), -1 / np.sqrt(2)], [-1, 1, -1, 1]])
self.Mixer = np.array([[.5, -.5, -1], [.5, .5, 1], [-.5, .5, -1], [-.5, -.5, 1]])
elif self.type in [0, 1]: # QUAD_PLUS, OTHER
self.max_xy_torque = (self.l * self.kf * self.max_rpm ** 2)
self.A = np.array([[1, 1, 1, 1], [0, 1, 0, -1], [-1, 0, 1, 0], [-1, 1, -1, 1]])
self.Mixer = np.array([[0, -1, -1], [+1, 0, 1], [0, 1, -1], [-1, 0, 1]])
self.max_z_torque = 2 * self.km * self.max_rpm ** 2
self.grand_eff_h_clip = 0.25 * self.prop_radius * np.sqrt(
(15 * self.max_rpm ** 2 * self.kf * self.gnd_eff_coeff) / self.max_thrust)
self.inv_A = np.linalg.inv(self.A)
self.B_coeff = np.array([1 / self.kf, 1 / (self.kf * self.l), 1 / (self.kf * self.l), 1 / self.km])
３．まとめ
PyBulletとは直接関係がないURDFファイル読み込みだけの内容になってしまいましたが、とりあえず初回はここまでにしておきます。
今後は何回かに分けて、PyBulletでドローンを動かすところを書いていきたいと思っています。
イメージとしてはこんな感じです。
次の記事はこちら
Discussion