ROS講座103 armロボットのsim環境を作る

環境

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

項目	値
CPU	Core i5-8250U
Ubuntu	16.04
ROS	Kinetic
Gazebo	7.0.0

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

概要

armロボット用のsim環境を構築をします。アームの逆運動学を解くには数学的には6degだと必要十分に解けますが、moveitでは5、6、7自由度が解くには良いらしいです。

アームの形を確認する

ソースコード

urdfファイル：arm_robot.urdf

形の確認

rvizでの形状の確認

 roscd arm_lecture/urdf/
roslaunch urdf_tutorial display.launch model:=arm_robot.urdf 

画像のように6軸のアームです。また最上部に位置指定用にtarget_linkがpitchを90度まわして設置しています。

• (1)yaw軸：台座（青灰）とリンク（灰）
• (2)pitch軸：リンク（灰）とリンク（青灰）
• (3)pitch軸：リンク（青灰）とリンク（灰）
• (4)pitch軸：リンク（灰）とリンク（青灰）
• (5)roll軸：リンク（青灰）とリンク（灰）
• (6)yaw軸：リンク（灰）とリンク（赤）

ros_controlの設定

回転軸は今まではros_controlの速度コントローラーでまわしていましたが、アームではeffort_controllers/JointTrajectoryControllerというインターフェイスを使ってすべての軸をまとめて1つのロボットとして動かします。また単純なtopicのIFとfollow_joint_trajectoryという名前のactionlibのIFの2つが発生します。

ソースコード

urdfファイル：arm_robot.urdf
先ほどのurdfにros_control関連のファイルを追加したものです。

arm_lecture/config/trajectory_controller.yaml

arm_robot_trajectory_controller:
  type: effort_controllers/JointTrajectoryController
  joints:
    - arm1_joint
    - arm2_joint
    - arm3_joint
    - arm4_joint
    - arm5_joint
    - arm6_joint
  gains:
    arm1_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
    arm2_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
    arm3_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
    arm4_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
    arm5_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
    arm6_joint: {p: 100, d: 1, i: 1, i_clamp: 1}

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

arm_lecture/launch/arm_robot_sim.launch

<?xml version="1.0" ?>
<launch>
  <arg name="model" default="$(find arm_lecture)/urdf/arm_robot_sim.urdf" />
  <param name="robot_description" textfile="$(arg model)" />

  <!-- gazebo world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
  </include>

  <!-- gazebo model -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model arm_robot" />
  <rosparam file="$(find arm_lecture)/config/trajectory_controller.yaml" command="load"/>
  <node name="arm_robot_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="arm_robot_trajectory_controller joint_state_controller"/>
</launch>

実行

roslaunch arm_lecture arm_robot_sim.launch 

初期状態はまっすぐ上のはずですが、大体これぐらい曲がったところで止まっています。これはgazeboにモデルが出てくるのとros_controlがロードされるのにタイムラグがあるために、その間に軸が自重で落ちるからです。

軸に指令を送る。

前述したJointTrajectoryControllerの型で軸に指令を送ってみます。

ソースコード

arm_lecture/scripts/joint_trajectory_publisher.py

#!/usr/bin/env python
# license removed for brevity
import rospy
import math
from std_msgs.msg import String
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

def talker():
    rospy.init_node('joint_trajectory_publisher', anonymous=True)
    pub = rospy.Publisher('arm_robot_trajectory_controller/command', JointTrajectory, queue_size=10)
    rospy.sleep(0.5)

    msg = JointTrajectory()
    msg.header.stamp = rospy.Time.now()
    msg.joint_names = [ "arm1_joint", "arm2_joint", "arm3_joint", "arm4_joint", "arm5_joint", "arm6_joint" ]
    msg.points = [JointTrajectoryPoint() for i in range(6)]
    msg.points[0].positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    msg.points[0].time_from_start = rospy.Time(1.0)
    msg.points[1].positions = [0.0, math.pi/6.0, math.pi/6.0, math.pi/6.0, 0.0, 0.0]
    msg.points[1].time_from_start = rospy.Time(2.0)
    msg.points[2].positions = [0.5, math.pi/6.0, math.pi/6.0, math.pi/6.0, 0.5, 0.0]
    msg.points[2].time_from_start = rospy.Time(3.0)
    msg.points[3].positions = [-0.5, math.pi/6.0, math.pi/6.0, math.pi/6.0, -0.5, 0.0]
    msg.points[3].time_from_start = rospy.Time(4.0)
    msg.points[4].positions = [0.0, math.pi/6.0, math.pi/6.0, math.pi/6.0, 0.0, 0.0]
    msg.points[4].time_from_start = rospy.Time(5.0)
    msg.points[5].positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    msg.points[5].time_from_start = rospy.Time(6.0)

    pub.publish(msg)
    rospy.sleep(0.5)

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException: pass

実行

1つ目のターミナル

roslaunch arm_lecture arm_robot_sim.launch 

2つ目のターミナル

rosrun arm_lecture joint_trajectory_publisher.py 

参考

joint_trajectory_controllerのリファレンス

目次ページへのリンク

ROS講座の目次へのリンク