2軸ロボットアームの制御
2軸ロボットアームを制御します．
目的
これまでに扱ってきたロボットは2輪走行車でした．
今回は軸数の少ない簡易的なロボットアームを使用し，ローレベルな制御を学びます．
講義の前半ではスライド資料を用いて運動学について説明し，講義の後半では本Webページを用いて実習を行います．
準備
3リンク2ジョイントの簡易的なロボットアームrrbot(Revolute-Revolute Manipulator Robot)を使用するために準備します．
パッケージをgithubのリポジトリからクローンし，依存するパッケージをインストールします．
1
2
3
4
5
6
7
8
9
10
|
$ sudo apt install python3-rosdep2
$ sudo rosdep init
$ sudo apt install ros-humble-controller-manager
$ mkdir -p ~/ros2_ws/src
$ cd ~/ros2_ws/src
$ git clone https://github.com/ros-controls/ros2_control_demos -b humble
$ cd ~/ros2_ws/
$ rosdep update --rosdistro=$ROS_DISTRO
$ sudo apt-get update
$ rosdep install --from-paths ./ -i -y --rosdistro ${ROS_DISTRO}
|
ros-humble-gazebo-rosパッケージのインストールに失敗したというエラーが発生しますが，この後の処理に支障がないため進めてください．
これでパッケージをビルドする準備が整いました．
1
2
3
|
$ source ~/.bashrc
$ cd ~/ros2_ws/
$ colcon build
|
rrbotの起動
それではロボットアームrrbotを起動しましょう．
下記のlaunchより起動できます．
1
2
3
|
$ cd /ros2_ws
$ source install/setup.bash
$ ros2 launch ros2_control_demo_example_1 rrbot.launch.py
|
rviz2が立ち上がり，3リンク2ジョイントのロボットが表示されます．
また，ロボットを動かすためのコントローラーも起動します．
トピックの確認
どのようなトピックがアクティブであるかを確認します．
1
2
3
4
5
6
7
8
9
10
11
12
13
14
|
$ ros2 topic list
/clicked_point
/dynamic_joint_states
/forward_position_controller/commands
/forward_position_controller/transition_event
/goal_pose
/initialpose
/joint_state_broadcaster/transition_event
/joint_states
/parameter_events
/robot_description
/rosout
/tf
/tf_static
|
たくさんのトピックが配信/購読されていますが，コントローラが購読するトピックは/forward_position_controller/commandsです．
下記のコマンドで使用するメッセージ型を調べましょう．
1
2
3
4
|
$ ros2 topic info /forward_position_controller/commands
Type: std_msgs/msg/Float64MultiArray
Publisher count: 0
Subscription count: 1
|
std_msgs/msg/Float64MultiArrayであることがわかりました．
実際にどのようなデータを扱っているかを調べます．
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
|
$ ros2 interface show std_msgs/msg/Float64MultiArray
# This was originally provided as an example message.
# It is deprecated as of Foxy
# It is recommended to create your own semantically meaningful message.
# However if you would like to continue using this please use the equivalent in example_msgs.
# Please look at the MultiArrayLayout message definition for
# documentation on all multiarrays.
MultiArrayLayout layout # specification of data layout
#
#
MultiArrayDimension[] dim #
string label #
uint32 size #
uint32 stride #
uint32 data_offset #
float64[] data # array of data
|
float64のリストとして表現されるdataを受け渡していることがわかります．
std_msgs/msg/Float64MultiArrayは汎用的なメッセージの型であるため，リストの大きさが定義されていませんが，
rrbotの場合にはリストの要素のサイズは2となり，それぞれの値はラジアンで表現される各ジョイントの変位(角度)を表します．
pubコマンドで関節角度を配信し，ロボットが制御できるかを確認しましょう．
1
2
3
|
$ ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data:
- 0.5
- 0.5"
|
2つのジョイントの関節角度をそれぞれ0.5としてトピックを配信します．
トピックを配信後，ロボットがゆっくりと動作を開始し，下記のような姿勢になります．
プログラム
先ほどはpubコマンドでトピックを配信しましたが，プログラムで配信します．
トピック通信の実装にて使用したサンプルプログラムをベースとしたプログラムになっています．
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
|
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
import time
import math
class rrbotControl(Node):
def __init__(self):
super().__init__('rrbot_control_node')
self.ps_pub = self.create_publisher(Float64MultiArray, 'forward_position_controller/commands', 1)
time.sleep(1.0)
def setPose(self, p1, p2):
pose = Float64MultiArray()
pose.data = p1, p2
self.ps_pub.publish(pose)
def main(args=None):
rclpy.init(args=args)
node = rrbotControl()
node.setPose(math.pi * 0.5, math.pi * 0.5)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
|
サンプルプログラムでは，2つの関節角度をmath.pi * 0.5，度数法で表すと90度としてトピックを配信しています．
サンプルプログラムを実行すると下記のような結果が得られます．
課題
- サンプルプログラムにおける関節角度の値を変えてプログラムを実行しなさい．
- 順運動学により手先位置を計算し，ターミナルに出力しなさい．
- 手先位置を与え，逆運動学により関節角度を計算してターミナルに出力しなさい．