Edited at

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


環境

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

項目

CPU
Core i5-8250U

Ubuntu
16.04

ROS
Kinetic

Gazebo
7.14.0

インストールについてはROS講座02 インストールを参照してください。

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


概要

オムニホイールのシミュレーションをしていると、速度を上げるとタイヤがはねてしまってうまくシミュレーションできないことが発生します。これを防ぐためにサスペンションを追加します。

実際のロボットにはサスペンションが明確についていなくても、部品の剛性で少したわみます。おそらくこれがうまく走るために必須のようです。シミュレーションの部品は指定しない限り無限に硬いので、このたわみが全くありません。


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

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


  • サスペンションの部分を直動のjointにする。

  • ros_controlでpid制御をする。

  • 目標値を0位置に設定する。

を行います。3つ目の「目標値を0位置に設定する」ですが、これはurdfをspawnした時はjointが初期位置(0)にありますが、ros_controlが起動するまでに時間差があり、その間に重力でだんだんと下がってしまいます。そのために重力で落ちた位置が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"を送って位置を戻せばよいのですが、下の画像のように跳ねてしまいます。

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


sim2_lecture/scripts/liftup_sus.py

#!/usr/bin/env python

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")



実行

roslaunch sim2_lecture odm_gazebo4.launch 

サスペンションがゆっくりと上がるのを確認できました。

この状態で走行するとサスペンションが無いときに比べて跳ね返りが少ないことがわかります。


目次ページへのリンク

ROS講座の目次へのリンク