ROS講座27 ros_controlでjointを動かす

環境

この記事は以下の環境で動いています。

項目	値
CPU	Core i5-8250U
Ubuntu	20.04
ROS	Noetic
Gazebo	11.9.0

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

前回はただの箱を表示するだけで、動く要素がありませんでした。sdfでモデルを書いているときと同様にgazeboプラグインを使って軸の駆動を実電することは可能です。しかしurdfでモデルを書いているときは、ros_controlというプラグインを使うこともできます。今回はこれを使うためのURDFとlaunchで設定をして、1軸で動くアームロボットのシミュレーションをします。

インストール

追加で以下のパッケージが必要です。

sudo apt install ros-noetic-gazebo-ros
sudo apt install ros-noetic-gazebo-ros-control 
sudo apt install ros-noetic-ros-control
sudo apt install ros-noetic-ros-controllers 

ソースコード

urdf

gazebo_urdf_lecture/urdf/arm_robot.urdf

<?xml version="1.0"?>
<robot name="arm_robot">
  <link name="world"/>

  <joint name="base_joint" type="fixed">
    <parent link="world"/>
    <child link="base_link"/> 
  </joint>
  <link name="base_link">
    <inertial>
      <origin xyz="0 0 0.5" rpy="0 0 0"/>
      <mass value="1.0"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0.5" rpy="0 0 0"/>
      <geometry>
        <box size="0.1 0.1 1.0"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0.5" rpy="0 0 0"/>
      <geometry>
        <box size="0.1 0.1 1.0"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="base_link">
    <material>Gazebo/Red</material>
  </gazebo>

  <joint name="arm1_joint" type="continuous">
    <parent link="base_link"/>
    <child link="arm1_link"/>
    <axis xyz="0 1 0"/>
    <origin xyz="0 0.1 1.0" rpy="0 0 0"/>
    <dynamics damping="0.1"/>
  </joint>
  <transmission name="arm1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="arm1_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="arm1_motor">
       <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
       <mechanismReduction>1</mechanismReduction>
    </actuator>
  </transmission>
  <link name="arm1_link">
    <inertial>
      <mass value="1.0"/>
      <origin xyz="0 0 0.25" rpy="0 0 0"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0.25" rpy="0 0 0"/>
      <geometry>
        <box size="0.1 0.1 0.5"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0.25" rpy="0 0 0"/>
      <geometry>
        <box size="0.1 0.1 0.5"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="arm1_link">
    <material>Gazebo/Blue</material>
  </gazebo>

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/arm_robot</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      <legacyModeNS>true</legacyModeNS>
    </plugin>
  </gazebo>
</robot>

新しく出てきた要素を解説していきます。

• 'world'という名前のリンクは特別なリンクで、gazeboワールドの原点を表します。したがってこれとfixedで接続するとgazeboの中で地面と固定された物体になります。こうすると前回の赤い箱のようにgazebo上でツールで移動させることはできません。
• <joint>要素内の<dynamics damping="0.1"/>は軸のダンピングの係数を決めます。これは減衰係数とも言い

F[N]=K \cdot V[m/s]

で表されます。この係数は軸の特性を決めるのもありますが、シミュレーションの振動を防止して安定化させることを主な目的としています。適当に今回は0.1と入れました。

• <transmission>要素：軸を駆動するアクチュエーターの設定です。

  1. <transmission>要素のname属性：他と被らない名前です。特にほかで使うことはあまりありません。
  2. <joint>要素のname属性：制御する対象のjointの名前を入れます。
  3. <joint>要素の<hardwareInterface>要素：基本的にhardware_interface/EffortJointInterfaceで固定です（これ以外はgazeboでサポートされていない）。
  4. <actuator>要素のname属性：他と被らない適当な値です。
  5. <actuator>要素の<hardwareInterface>要素：何種類かありますが、基本的にhardware_interface/EffortJointInterfaceを使います固定です。
  6. <actuator>要素の<mechanismReduction>要素：1で大丈夫です。

• <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">あたり：これはROS controlをロードするための記述です。<robotNamespace>要素の中身はurdf2行目のrobot nameと同じものを入れます。2019年の春当たりのアップデート以降では、ここで<robotSimType>と<legacyModeNS>の2つを入れる必要があります。

launch

以下のように

• gazeboワールドの起動
• urdfの出現
• ros_controlの起動

の3つを行います。

gazebo_urdf_lecture/launch/arm_robot.launch

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="model" default="$(find gazebo_urdf_lecture)/urdf/arm_robot.urdf"/>   
  <param name="robot_description" command="$(find xacro)/xacro $(arg model)"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch"/>

  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model arm_robot" />

  <rosparam>
    arm_robot:      
      arm1_joint_position_controller:
        type: effort_controllers/JointPositionController
        joint: arm1_joint
        pid: {p: 2.0, i: 1.0, d: 0.1, i_clamp_min: -1.0, i_clamp_max: 1.0}
      joint_state_controller:
        type: joint_state_controller/JointStateController
        publish_rate: 10  
  </rosparam>
  <node name="controller_spawner" pkg="controller_manager"
        type="spawner" respawn="false"
        output="screen" ns="/arm_robot"
        args="arm1_joint_position_controller joint_state_controller"/>
</launch>

新しい要素であるros_controlの起動を解説していきます。
<rosparam>~</rosparam>ではros_control用のrosparamを設定しています。ここで設定するパラメーターは以下のようになります。

• ルートの名前空間としてロボット名を入れます。これはこの次に起動する/controller_spawnerのns属性で入れる名前と対応させます。
• 次に「ros_controlの名前」を入れます。1つの軸につき1つの名前を付けます。この名前は他と被らなければ何でも大丈夫です。
• typeはコントローラーの名前です。後述します。
• jointはurdfのjointの名前です。
• pidでは制御のPIDゲインを設定します。

この次ではROSノードの/controller_spawnerを起動しています。最後のarg属性では使用するros_controlの名前を入力します。

実行

以下のコマンドでシミュレーションを実行します。
各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。

シミュレーションの実行

roslaunch gazebo_urdf_lecture arm_robot.launch

軸の駆動

シミュレーションを実行すると軸はまっすぐの状態で固定されています。

軸の駆動（別のターミナルで）

rostopic pub -1 /arm_robot/arm1_joint_position_controller/command std_msgs/Float64 "data: 0.5"

rqtを使った軸への指令

上のようにrostopic pubを使うのではなくrqtを使って指令を送ってみます。

rqtの起動

rosrun rqt_gui rqt_gui

メニューバーの「plugins」->「Topics」->「Message Publisher」を選択します。

出てくる画面で「Topic：/arm_robot/arm1_joint_position_controller/command」と「Type：std_msgs/Float64」を埋めて緑のプラスボタンを押します。
チェックボックスを入れて、「expression」のところに値を入れるとgazeboでjointが動きます。
ここではiというパラメーターを入れることができます。例えば「rate」に20、「expression」にsin(1/10)と入れるとjointがゆっくり振動します。

PIDパラメーターの変更

jointを駆動するコントローラーはPID制御で制御されています。これらのPIDのゲインはrosparamで指定されているので実行中に変更することができます。

rqtの起動

rosrun rqt_reconfigure rqt_reconfigure 

以下の画像のように左側のメニューを開いてarm_robot->arm1_joint_position_controller->pidと開きます。
このguiでpidのゲインを変えることができます。試しにdの値を10から80に変更します。

PIDゲインを変更したので下の画像のように軸の挙動が変わりました。

inertialを入れなければならないlinkのルール

センサーの位置用のlinkなどinertialを入れたくないlinkがこの先発生します。まだ完全にわかっていないのですが、以下の2つは確実です。

• fixedのjointでつながれているlinkはそのうちの1つにinertialが書いてあればよい（下記の場合を除く）。
• fixed以外のjointでつながれているlinkは親と子の両方ともinertialが書いてないといけない（要検証？）。

コントローラータイプとハードウェアインターフェイスについて

コントローラータイプとはlaunchファイルのの中でtype:の部分で指定していたものです。ハードウェアインターフェイスとはurdfの<transmission>要素の<hardwareInterface>の中で指定しているものです。用途によって以下のように使い分けます。今回は一番上の組み合わせを使いました。

ros_controlへの指令	gazeboへの指令		コントローラータイプ	hardwareInterface/*
位置	力	->	effort_controllers/JointPositionController	EffortJointInterface
速度	力	->	effort_controllers/JointVelocityController	EffortJointInterface
力	力	->	effort_controllers/JointEffortController	EffortJointInterface
位置	位置	->	velocity_controllers/JointPositionController	VelocityJointInterface
速度	速度	->	velocity_controllers/JointVelocityController	VelocityJointInterface
位置	位置	->	position_controllers/JointPositionController	PositionJointInterface

#参考

• GazeboをROSに繋ぐ (ROS Control 編)
• ROSwiki URDFのtransmission

目次ページへのリンク

ROS講座の目次へのリンク