ROS講座89 オムニホイールのシミュレーション4（サスペンションを作る）

環境

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

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

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

概要

これまで使ったオムニホイールのシミュレーションでは速度を上げるとタイヤがはねてしまいうまく正しくシミュレーションができません。これを防ぐためにサスペンションを追加します。
実際のロボットにはサスペンションが明確についていませんが、実際は部品の剛性で少したわみます。おそらくこれがうまく走るために必須のようです。シミュレーションの部品は指定しない限り無限に硬いので、このたわみが全くありません。

サスペンションの実現方法

ロボットを記述するURDFには明示的にサスペンションを記述する方法はありません。そこで今回は

• サスペンションの部分を直動のjointにする。
• ros_controlでpid制御をする。
• 目標値を0位置に設定する。

を行います。3つ目の「目標値を0位置に設定する」ですが、これはgazeno上にモデルが出現してから、ros_controlが起動するまでの時間差で、重力によってjointがだんだんと下がって、この位置がros_controlの初期の目標位置になるのでこれを戻すためのものです。

ソースコード

urdf

/sim2_lecture/xacro/odm_omni_wheel_set2.xacro(一部省略)

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">  
  <xacro:macro name="barrel" params="prefix parent dir">
  <!-- 省略 -->
  </xacro:macro>

  <xacro:macro name="motor" params="prefix parent">
　<!-- 省略 -->
  </xacro:macro>

  <xacro:macro name="omni_wheel_set" params="prefix parent">
    <joint name="${prefix}_base_joint" type="fixed">
      <parent link="${parent}"/>
      <child link="${prefix}_base_link"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </joint>
    <link name="${prefix}_base_link" >
      <inertial>
        <mass value="0.010"/>
        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
      </inertial> 
    </link>

    <joint name="${prefix}_sus_joint" type="prismatic">
      <parent link="${prefix}_base_link"/>
      <child  link="${prefix}_sus_link"/>
      <limit lower="-0.02" upper="0.02" effort="100" velocity="1.0" />
      <axis xyz="0 0 1"/>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <dynamics damping="0.01"/>
    </joint>
    <transmission name="${prefix}_sus_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${prefix}_sus_joint">
          <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </joint>
      <actuator name="${prefix}_sus_motor">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        <mechanismReduction>1</mechanismReduction>
      </actuator>
    </transmission>
    <gazebo reference="${prefix}_sus_joint">
      <kp>50000</kp>
      <kd>500</kd>
    </gazebo>
    <link name="${prefix}_sus_link">
      <inertial>
        <mass value="0.010"/>
        <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
      </inertial>
    </link>

    <xacro:motor prefix="${prefix}_motor" parent="${prefix}_sus_link" />

    <joint name="${prefix}_housind_joint" type="fixed">
      <parent link="${prefix}_motor_shaft_link"/>
      <child  link="${prefix}_housing_link"/>
      <origin xyz="0.012 0 0" rpy="0 ${radians(90)} 0"/>
    </joint>
    <link name="${prefix}_housing_link">
      <visual>
        <geometry>
          <mesh filename="package://sim2_lecture/stl/omni_housing.stl" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <material name="gray"/>
      </visual>
      <inertial>
        <mass value="0.025"/>
        <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
      </inertial>  
    </link>
    <gazebo reference="${prefix}_housing_link">
      <material>Gazebo/Gray</material>
    </gazebo>

    <joint name="${prefix}_housing_s_joint" type="fixed">
      <parent link="${prefix}_housing_link"/>
      <child link="${prefix}_housing_s_link"/>
      <origin rpy="0 ${pi} ${pi/4}" xyz="0 0 0"/>
    </joint>
    <link name="${prefix}_housing_s_link">
      <inertial>
        <mass value="0.025"/>
        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
      </inertial>  
    </link>

    <xacro:barrel prefix="${prefix}_barrel_1" parent="${prefix}_housing_link" dir="0"/>
    <xacro:barrel prefix="${prefix}_barrel_2" parent="${prefix}_housing_link" dir="${pi/2}"/>
    <xacro:barrel prefix="${prefix}_barrel_3" parent="${prefix}_housing_link" dir="${pi}"/>
    <xacro:barrel prefix="${prefix}_barrel_4" parent="${prefix}_housing_link" dir="${-pi/2}"/>
    <xacro:barrel prefix="${prefix}_barrel_5" parent="${prefix}_housing_s_link" dir="0"/>
    <xacro:barrel prefix="${prefix}_barrel_6" parent="${prefix}_housing_s_link" dir="${pi/2}"/>
    <xacro:barrel prefix="${prefix}_barrel_7" parent="${prefix}_housing_s_link" dir="${pi}"/>
    <xacro:barrel prefix="${prefix}_barrel_8" parent="${prefix}_housing_s_link" dir="${-pi/2}"/>
  </xacro:macro>
</robot>

prismatic型（直動型）のjointのmotor0_sus_jointを追加しています。またこのjointの根元のlinkにinertialを追加しています。

liftupスクリプト

上記のジョイントを追加しただけだとjointが下がりきったところで固定されてしまいます。$ rostopic pub -1 /odm_robot/sus_controller0/command std_msgs/Float64 "data: 0.0"を送って位置を戻せばよいのですが、下の画像のように急にjointが動いてモデルが跳ねてしまいます。

そこでliftupスクリプトを使ってゆっくりとこれらのjointの位置を変えます。

sim2_lecture/scripts/liftup_sus.py

#!/usr/bin/env python3
import rospy
from std_msgs.msg import Float64

if __name__ == '__main__':
    rospy.init_node('liftup_sus')
    pub0 = rospy.Publisher('/odm_robot/motor0_sus/command', Float64, queue_size=10)
    pub1 = rospy.Publisher('/odm_robot/motor1_sus/command', Float64, queue_size=10)
    pub2 = rospy.Publisher('/odm_robot/motor2_sus/command', Float64, queue_size=10)

    rospy.sleep(5.0)
    HZ=50
    r = rospy.Rate(HZ)
    i = 0
    liftup_step=100
    while not rospy.is_shutdown():
        command_pos= (liftup_step-i) * (0.02/liftup_step)
        pub0.publish(command_pos)
        pub1.publish(command_pos)
        pub2.publish(command_pos)
        #print(command_pos)
        if command_pos<=0:
            break
        i+=1
        r.sleep()
    rospy.loginfo("Liftup Done")

実行

各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。

roslaunch sim2_lecture odm_gazebo4.launch 

サスペンションがゆっくりと上がるのを確認できました。
この状態で走行するとサスペンションが無いときに比べて跳ね返りが少ないことがわかります。

目次ページへのリンク

ROS講座の目次へのリンク