二足歩行ロボット研修（kora編）[19] DYNAMIXEL Workbenchでモーションを再生する

こんにちは。koraです。

これまで作ってきた二足歩行ロボットですが、ようやく全身のサーボモータを動かせるようにできました。今回はモーションを再生してみたいと思います。

DYNAMIXEL WorkbenchのROSチュートリアル

参考にするのはROBOTISのROSチュートリアルです。

dynamixel_workbench_operators

前回の記事では、dynamixel_workbench_controllersパッケージを使って特定のサーボモータを指定した角度まで動作させるところまでやりました。今回はdynamixel_workbench_operatorsパッケージを使って動作させます。

モーション用の設定ファイル

まずdynamixel-workbench/dynamixel_workbench_operators/config/motion.yamlファイルを確認します。

joint:
  names: [pan, tilt]
motion:
  names: [right, zero, left]
  right:
    step: [-3.14, -3.14]  # radian
    time_from_start: 2.0  # sec
  zero:
    step: [0.0, 0.0]
    time_from_start: 3.0
  left:
    step: [3.14, 3.14]
    time_from_start: 6.0

まず、動かしたい軸の名前を登録します。dynamixel_workbench_controllersで登録した軸の名前と同じものを選択します。上の例ではpanとtiltですが、ここでは二足歩行ロボットの首を動かしたいのでneck_yawとneck_pitchに変更します。
次に、モーションの名前を登録します。上の例ではrightとzeroとleftです。そして、それぞれのモーションに対して、各々の軸の角度をラジアンで指定し、移動時間を秒で指定します。首が3.14ラジアン（約180度）回ると怖いので、ここでは0.52（約30度）にしてみます。

変更後のmotion.yamlファイルは次のようになります

joint:
  names: [neck_yaw, neck_pitch]
motion:
  names: [right, zero, left]
  right:
    step: [-0.52, -0.52]  # radian
    time_from_start: 2.0  # sec
  zero:
    step: [0.0, 0.0]
    time_from_start: 3.0
  left:
    step: [0.52, 0.52]
    time_from_start: 6.0

モーション再生

準備ができたので、モーションを再生してみます。
まず１つ目のターミナルで

$ roslaunch dynamixel_workbench_controllers dynamixel_controllers.launch

を実行してコントローラを起動します。
次に２つ目のターミナルで

$ roslaunch dynamixel_workbench_operators joint_operator.launch

を実行して、モーションをコントローラに配信するノード（オペレータ）を起動します。成功すると次のような出力が表示されます。

[ INFO] [1609830132.416451927]: motion_name : right, step : -0.520000
[ INFO] [1609830132.420171035]: motion_name : right, step : -0.520000
[ INFO] [1609830132.420747889]: time_from_start : 2.000000
[ INFO] [1609830132.421212650]: motion_name : zero, step : 0.000000
[ INFO] [1609830132.421357688]: motion_name : zero, step : 0.000000
[ INFO] [1609830132.421470244]: time_from_start : 3.000000
[ INFO] [1609830132.421617170]: motion_name : left, step : 0.520000
[ INFO] [1609830132.421751467]: motion_name : left, step : 0.520000
[ INFO] [1609830132.421885986]: time_from_start : 6.000000
[ INFO] [1609830132.429041720]: For now, you can use publish joint trajectory msgs by triggering service(/execution)

最後に３つ目のターミナルで

$ rosservice call /dynamixel_workbench/execution "{}"

を呼ぶことでモーション再生が再生されます。このサービスをオペレータが受け取ると、モーションの内容を/dynamixel_workbench/joint_trajectoryトピックにパブリッシュして、それを受け取ったコントローラがモーションを再生する仕組みです。