こんにちは。koraです。
今回はros_controlを使って、これまで製作してきたロボットを動かしたいと思います。
CRANE X7のROSパッケージ
ros_controlに則ったDYNAMIXELサーボ用のパッケージとして、アールティが製作・販売しているアームロボット CRANE X7のROSパッケージ を使うことにします。
crane_x7_controlの確認
ros_controlの実装はcrane_x7_controlパッケージにあります。コントローラを起動する crane_x7_control.launch ファイルを見てみましょう。
<launch> <!-- USB port name for dynamixel_port --> <arg name="port" default="/dev/ttyUSB0" /> <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find crane_x7_description)/urdf/crane_x7.urdf.xacro'" /> <group ns="crane_x7"> <rosparam file="$(find crane_x7_control)/config/crane_x7_control.yaml" command="load"/> <param name="dynamixel_port/port_name" value="$(arg port)"/> <node name="controller_manager" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller arm_controller gripper_controller"/> <node name="crane_x7_control" pkg="crane_x7_control" type="crane_x7_control" output="screen" respawn="false"> <!-- <param name="config_ns" value="/crane_x7"/> --> </node> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen"> <remap from="/joint_states" to="/crane_x7/joint_states" /> </node> </group> </launch>
- 以下の行でサーボと通信するシリアルポートを設定しています。CRANE-X7はUSB-シリアル変換器のU2D2を使用することを想定しているので、デフォルトのシリアルポートのデバイス名は/dev/ttyUSB0になります。このシリアルポートはlaunchファイルを起動する際に変更可能です。
<!-- USB port name for dynamixel_port --> <arg name="port" default="/dev/ttyUSB0" />
<param name="dynamixel_port/port_name" value="$(arg port)"/>
- こちらの行でロボットモデルを読み込んでいます。CRANE-X7のモデルはrobot_descriptionパッケージにあり、xacro形式で記述されています。
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find crane_x7_description)/urdf/crane_x7.urdf.xacro'" />
- 以下の行でControllerの設定ファイル crane_x7_control.yaml を読み込んでいます。このファイルには後でロードするControllerの名前やタイプ、制御対象のジョイント名が記述されています。
<rosparam file="$(find crane_x7_control)/config/crane_x7_control.yaml" command="load"/>
- これらの部分でControllerをロードさせるspawnerを起動しています。joint_state_controller、arm_controller、gripper_controllerはControllerの設定ファイル crane_x7_control.yaml に記述されています。
<node name="controller_manager" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller arm_controller gripper_controller"/>
- 最も重要な、制御ループを回すROSノードを起動します。ロボットのハードウェアを定義するクラスは dxlport_control.h に定義されている DXLPORT_CONTROL で、
ROSノードは hardware.cpp に記述されています。<node name="crane_x7_control" pkg="crane_x7_control" type="crane_x7_control" output="screen" respawn="false"> <!-- <param name="config_ns" value="/crane_x7"/> --> </node>
- 最後に robot_state_publisher ノードを起動します。これはURDFとjoint_statesからtfを出力するノードです。
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen"> <remap from="/joint_states" to="/crane_x7/joint_states" /> </node>
crane_x7_controlの利用
crane_x7_controlはCRANE-X7用に作られたパッケージですが、設定を変更することでDYNAMIXELサーボを使用したロボットにも利用できます。例として二足歩行ロボットの首だけ動かしてみます。
まず、crane_x7_ros リポジトリをGitHubからクローンします。次に、crane_x7_control/config ディレクトリにControllerの設定ファイルを作成します。Controllerの設定と、制御対象のサーボの設定を記述します。
joint_state_controller: type: "joint_state_controller/JointStateController" publish_rate: 250 neck_controller: type: "position_controllers/JointTrajectoryController" publish_rate: 250 joints: - neck_yaw - neck_pitch constraints: goal_time: 0.0 stopped_velocity_tolerance: 1.0 dynamixel_port: baud_rate: 57600 joints: - neck_yaw - neck_pitch neck_yaw: {id: 21, center: 2048, home: 2048, effort_const: 1.00, mode: 3 } neck_pitch: {id: 22, center: 2048, home: 2048, effort_const: 1.00, mode: 3 }
そして、launchファイルを修正します。以前設定したようにシリアルポートを/dev/ttyAMA1に変更し、ロボットのモデルには前回の記事で作成したxacroファイルを指定、Controllerの設定として先程作成したyamlファイルを指定します。
<launch> <!-- USB port name for dynamixel_port --> <arg name="port" default="/dev/ttyAMA1" /> <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find nisoku_kenshu_xyz_description)/urdf/nisoku_kenshu_xyz.xacro'" /> <group ns="crane_x7"> <rosparam file="$(find crane_x7_control)/config/crane_x7_control.yaml" command="load"/> <param name="dynamixel_port/port_name" value="$(arg port)"/> <node name="controller_manager" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller neck_controller"/> <node name="crane_x7_control" pkg="crane_x7_control" type="crane_x7_control" output="screen" respawn="false"> <!-- <param name="config_ns" value="/crane_x7"/> --> </node> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen"> <remap from="/joint_states" to="/crane_x7/joint_states" /> </node> </group> </launch>
動作確認
以上の設定を適用してlaunchファイルを起動すればControllerを立ち上げることができます。neck_controllerにJointTrajectoryControllerを使用しているので、”/crane_x7/neck_controller/command”トピックにJointTrajectoryメッセージを送ることでサーボを動かすことができます。
それでは動作確認のため、DYNAMIXEL Workbenchでモーションを再生したときと同様に、dynamixel_workbench_operatorsを使ってJointTrajectoryを送ってみることにします。ただし、トピック名が変わっているので dynamixel_workbench_operatorsのソースコード の一部を次のように修正する必要があります。
// joint_trajectory_pub_ = node_handle_.advertise<trajectory_msgs::JointTrajectory>("joint_trajectory", 100); joint_trajectory_pub_ = node_handle_.advertise<trajectory_msgs::JointTrajectory>("/crane_x7/neck_controller/command", 100);
修正したコードのビルドができたら、次のコマンドで動作させます。
# 1つ目のターミナルで $ roslaunch crane_x7_control crane_x7_control.launch # 2つ目のターミナルで $ roslaunch dynamixel_workbench_operators joint_operator.launch # 3つ目のターミナルで $ rosservice call /dynamixel_workbench/execution "{}"
実物のロボットの動きがrobot_state_publisherから出力されているので、RViz上のロボットモデルで再現することもできます。
ただし、まだ首の設定しかされていないので、腕や脚は上手く表示されていません。首以外の関節は、次回以降適用させていきたいと思います。