環境
この記事は以下の環境で動いています。
| 項目 | 値 |
|---|---|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |
| Gazebo | 11.9.0 |
インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。
概要
今回はSDFでmodelを記述して車輪ロボットをgazebo上に出現させます。またgazeboプラグインを使って車輪ロボットを動かします。
ソースコード
差動2輪ロボットのGazeboモデルを作成します。
gazebo1_lecture/models/wheel_robot_simple/model.config
<?xml version="1.0" ?>
<model>
<name>wheel_robot_simple</name>
<version>1.0</version>
<sdf version="1.6">model.sdf</sdf>
<author>
<name></name>
<email></email>
</author>
<description></description>
</model>
gazebo1_lecture/models/wheel_robot_simple/model.sdf
<?xml version='1.0'?>
<sdf version="1.4">
<model name="wheel_robot_simple">
<link name="body_link">
<pose>-0.05 0 0.1 0 0 0</pose>
<inertial>
<mass>5.0</mass>
<inertia>
<ixx>0.042</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.042</iyy>
<iyz>0.0</iyz>
<izz>0.075</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.3 0.3 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.3 0.3 0.1</size>
</box>
</geometry>
</visual>
</link>
<joint name='back_ball_joint' type='fixed'>
<parent>body_link</parent>
<child>back_ball_link</child>
<pose>0 0 0 0 0 0</pose>
</joint>
<link name="back_ball_link">
<pose>-0.15 0 0.05 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.000001</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.000001</iyy>
<iyz>0.0</iyz>
<izz>0.000001</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
<surface>
<friction>
<ode>
<mu>0.01</mu>
<mu2>0.01</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>100000</kp>
<kd>1</kd>
</ode>
</contact>
</surface>
</collision>
<visual name="visual">
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
</visual>
</link>
<joint name='left_wheel_joint' type='revolute'>
<parent>body_link</parent>
<child>left_wheel_link</child>
<pose>0 0 0 0 0 0</pose>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<link name="left_wheel_link">
<pose>0 0.18 0.072 1.5707 0 0</pose>
<inertial>
<mass>0.2</mass>
<inertia>
<ixx>0.00026</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00026</iyy>
<iyz>0.0</iyz>
<izz>0.00052</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<cylinder>
<radius>0.072</radius>
<length>0.04</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.5</mu>
<mu2>0.5</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>100000</kp>
<kd>1</kd>
</ode>
</contact>
</surface>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.072</radius>
<length>0.04</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Red</name>
</script>
</material>
</visual>
</link>
<joint name='right_wheel_joint' type='revolute'>
<parent>body_link</parent>
<child>right_wheel_link</child>
<pose>0 0 0 0 0 0</pose>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<link name="right_wheel_link">
<pose>0 -0.18 0.072 1.5707 0 0</pose>
<inertial>
<mass>0.2</mass>
<inertia>
<ixx>0.00026</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00026</iyy>
<iyz>0.0</iyz>
<izz>0.00052</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<cylinder>
<radius>0.072</radius>
<length>0.04</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.5</mu>
<mu2>0.5</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>100000</kp>
<kd>1</kd>
</ode>
</contact>
</surface>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.072</radius>
<length>0.04</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Red</name>
</script>
</material>
</visual>
</link>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<commandTopic>/cmd_vel</commandTopic>
<odometryTopic>/odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<robotBaseFrame>base_link</robotBaseFrame>
<publishWheelTF>false</publishWheelTF>
<publishOdomTF>true</publishOdomTF>
<publishWheelJointState>false</publishWheelJointState>
<wheelSeparation>0.36</wheelSeparation>
<wheelDiameter>0.144</wheelDiameter>
<wheelAcceleration>30</wheelAcceleration>
<wheelTorque>5.0</wheelTorque>
<updateRate>40</updateRate>
<odometrySource>encoder</odometrySource>
<leftJoint>left_wheel_joint</leftJoint>
<rightJoint>right_wheel_joint</rightJoint>
<publishTf>true</publishTf>
</plugin>
</model>
</sdf>
link-joint構造
以下の記述でlinkとlinkを接続することが出来ます。この例ではbody_linkの子リンクとしてback_ball_jointを介してback_ball_linkを接続します。
モデルの一部
<link name="body_link">
</link>
<joint name='back_ball_joint' type='fixed'>
<parent>body_link</parent>
<child>back_ball_link</child>
<pose>0 0 0 0 0 0</pose>
</joint>
<link name="back_ball_link">
</link>
今回のモデルは全体では以下のようなリンク構造です。
- body_link
- (fixed)back_ball_joint--back_ball_link: 後輪のキャスターに相当
- (revolute)left_wheel_joint--left_wheel_link: 左の動輪
- (revolute)right_wheel_joint--right_wheel_link: 右の動輪
fixedは2つのlinkを固定で接続します。revoluteは2つのlinkは軸を中心に回転します。
衝突演算パラメーター設定
collision要素の一部
<surface>
<friction>
<ode>
<mu>0.5</mu>
<mu2>0.5</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>100000</kp>
<kd>1</kd>
</ode>
</contact>
</surface>
-
frictionは摩擦係数の設定です。基本的にmu=mu2の値にします。ボールキャスター等の滑るものは0.01~0.1程度、動輪のタイヤなどのグリップするものなら0.5程度にします。 -
contactは表面の硬さに関するパラメーターです。- kpは表面の「めり込み」についての反力です。めり込み量[m]xkp[N/m]=反力[N] となります。小型ロボットではkp=1e5~7程度になります。kdは速度に関する反力です。影響はそこまで大きくないので1にしておきます。
- このようなSDFの要素はSDFフォーマットリファレンスに載っています。
プラグインの設定
差動2輪のタイヤを回すプラグインであるdiff_driveプラグインを使用します。このプラグインはGazebo ModelプラグインなのでModelタグの直下に書きます。
modelの一部
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<commandTopic>/cmd_vel</commandTopic>
<odometryTopic>/odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<robotBaseFrame>base_link</robotBaseFrame>
<publishWheelTF>false</publishWheelTF>
<publishOdomTF>true</publishOdomTF>
<publishWheelJointState>false</publishWheelJointState>
<wheelSeparation>0.36</wheelSeparation>
<wheelDiameter>0.144</wheelDiameter>
<wheelAcceleration>30</wheelAcceleration>
<wheelTorque>5.0</wheelTorque>
<updateRate>40</updateRate>
<odometrySource>encoder</odometrySource>
<leftJoint>left_wheel_joint</leftJoint>
<rightJoint>right_wheel_joint</rightJoint>
<publishTf>true</publishTf>
</plugin>
ROSトピックで受信したTwistを受けてタイヤを回すlibgazebo_ros_diff_drive.soの設定をします。
- 駆動側
- commandTopic: Twistのトピック名
- leftJoint: 左タイヤのjoint名
- wheelSeparation: 左右輪の間の距離
- wheelDiameter: 車輪の直径
- leftJoint: 左タイヤのjoint名
- rightJoint: 右タイヤのjoint名
- wheelAcceleration: タイヤの最大加速度
- wheelTorque: タイヤの最大トルク
- odom出力側
- odometryTopic: Odomトピック名
- updateRate: 出力レート
- odometryFrame: odomフレーム名
- robotBaseFrame: baseフレーム名
- odometrySource: 出力される位置のタイプ(encoder or world)
- joint_state出力側
- publishWheelJointState: 出力enable
- tf出力側
- publishOdomTF: jointのtfの出力enable
- publishWheelTF: odom->baseのtfの出力enable
- publishTf: tfの出力enable
実行
モデルの確認
各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。
roslaunch gazebo_ros empty_world.launch world_name:=wheel_robot_simple.world
SDFで定義したように差動2輪ロボットが出現します。
ここで「view」->「Inertials」、「Contacts」をチェックします。そうするとイナーシャ相当の直方体や接地点が表示されます。ここで
- Inertials: ピンクの図形が、おおむね表示されている図形と一致しているか確認します。ずれが大きい場合は、タグを修正します。
- Contacts: 接地点が安定しているか確認します。振動して接地点が現れたり消えたりする場合はタグの中身を修正してください。
駆動
rvizも起動します。のちのパートで作るものですが、twist panel(plugin編で作成)を起動します。これを使って/cmd_velにTwistをpublishします。
以下のようなコマンドでもTwistをpublishできます。
cmd_velのpublish
rostopic pub -1 /cmd_vel geometry_msgs/Twist "linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.5"
参考
ROSwiki urdf/link
Gazebo + ROS で自分だけのロボットをつくる
SDFフォーマットリファレンス