Dynamixel Workbenchを触ってみた #04

作成日: 2019年7月18日 | 投稿者:みっちー

前回、Dynamixelの情報の読み出しが出来ましたので、次は書き込み!
…ということで、Dynamixelを回して見たいと思います。

dynamixel_workbench_controllersに接続するノードを作ってみる(2)
〜Dynamixelを回してみる〜

サーボを回すには、dynamixel_workbench_controllersに対して、JointTrajectoryという型のメッセージで目標値を制御します。
JointTrajectoryは、単独のジョイントを制御するJointTrajectoryPointという型に、joint名を付加して、複数のjointを一度にコントロールすることが出来る上位の型です。
また、JointTrajectoryPointにはもともと時間軸を持っているため、JointTrajectoryで、複数軸のモーションを制御出来るわけですね

今回はシンプルに、コード上でJointTrajectoryのpositionを指定して、Dynamixelを回してみたいと思います。

// ~/catkin_ws/src/dynamixel_micchy/src/dynamixel_writer.cpp
#include "ros/ros.h"
#include "ros/time.h"

#include "trajectory_msgs/JointTrajectory.h"

ros::Publisher arm_pub;

int main(int argc, char **argv)
{
  ros::init(argc, argv, "dynamixel_writer");  // ノードの初期化
  ros::NodeHandle nh; // ノードハンドラ  

  //パブリッシャの作成
  arm_pub = nh.advertise<trajectory_msgs::JointTrajectory>("/dynamixel_workbench/joint_trajectory",1);

  ros::Rate loop_rate(10);  // 制御周期10Hz
  
  trajectory_msgs::JointTrajectory jtp0;
  
  jtp0.header.frame_id = "base_link";
  jtp0.joint_names.resize(2);
  jtp0.points.resize(1);

  jtp0.points[0].positions.resize(2);

  jtp0.joint_names[0] ="pan";
  jtp0.joint_names[1] ="tilt";

  while(ros::ok()) {
    jtp0.header.stamp = ros::Time::now();
    jtp0.points[0].positions[0] = 0.0;   // 原点のコマンドを送り続けるので起動時にposition0に移動するだけ
    jtp0.points[0].positions[1] = 0.0;   // 原点のコマンドを送り続けるので起動時にposition0に移動するだけ
    jtp0.points[0].time_from_start = ros::Duration(1.0);  //実行時間(1秒かけて移動)

    arm_pub.publish(jtp0);
    ros::spinOnce();   // コールバック関数を呼ぶ
    loop_rate.sleep();
    ROS_INFO("Joint1(%s)= %f | Joint2(%s)= %f ", jtp0.joint_names[0].c_str(),jtp0.points[0].positions[0], jtp0.joint_names[0].c_str(),jtp0.points[0].positions[0]);
  }

  return 0;
}

さっそく実行してみます。

1つめのターミナル
$ roscore

2つめのターミナル
$ roslaunch dynamixel_workbench_controllers dynamixel_controllers.launch

3つめのターミナル
$ rosrun dynamixel_micchy dynamixel_writer

回りましたね!

dynamixel_workbench_controllersに接続するノードを作ってみる3)
〜Dynamixelでマスター&スレーブ〜

読み書きができたら、やりたくなるのはマスター&スレーブの制御ですよね!
2個のサーボで取り組んでいたのは、このためでもあります!
さっそくreaderとwriterのソースを合体します。

// ~/catkin_ws/src/dynamixel_micchy/src/dynamixel2dynamixel.cpp
#include "ros/ros.h"
#include "ros/time.h"

#include "std_msgs/Float64.h"
#include "std_msgs/String.h"

#include "trajectory_msgs/JointTrajectory.h"
#include "sensor_msgs/JointState.h"

std_msgs::String joint1_name, joint2_name;
std_msgs::Float64 joint1_pos, joint2_pos;
std_msgs::Float64 joint1_vel, joint2_vel;
std_msgs::Float64 joint1_eff, joint2_eff;

void monitorJointState_callback(const sensor_msgs::JointState::ConstPtr&amp; jointstate)
{
  joint1_name.data = jointstate->name[0];    // 名前読み出し
  joint2_name.data = jointstate->name[1];    // 名前読み出し

  joint1_pos.data = jointstate->position[0];    // ポジション読み出し
  joint2_pos.data = jointstate->position[1];    // ポジション読み出し

  joint1_vel.data = jointstate->velocity[0];    // 速度読み出し
  joint2_vel.data = jointstate->velocity[1];    // 速度読み出し

  joint1_eff.data = jointstate->effort[0];    // 負荷読み出し
  joint2_eff.data = jointstate->effort[1];    // 負荷読み出し
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "dynamixel2dynamixel");  // ノードの初期化
  ros::NodeHandle nh; // ノードハンドラ  

  ros::Subscriber sub_joints;  // サブスクライバの作成
  sub_joints = nh.subscribe("/dynamixel_workbench/joint_states", 10, monitorJointState_callback);

  ros::Publisher arm_pub; //パブリッシャの作成
  arm_pub = nh.advertise<trajectory_msgs::JointTrajectory>("/dynamixel_workbench/joint_trajectory",100);

  ros::Rate loop_rate(45);  // 制御周期45Hz(サーボの更新が20msecなのですが、50Hzだと余裕ない感じ)

  trajectory_msgs::JointTrajectory jtp0;
  
  jtp0.header.frame_id = "base_link";
  jtp0.points.resize(1);

  jtp0.joint_names.resize(2);
  jtp0.joint_names[0] ="pan";
  jtp0.joint_names[1] ="tilt";
  
  jtp0.points[0].positions.resize(2);

  jtp0.points[0].effort.resize(2);  // 
  jtp0.points[0].effort[0] = 0.0;   // pan側のeffortを0に設定
//jtp0.points[0].effort[1] = 0.0;   // tilt側は設定しない

  joint1_pos.data = 0.0;  // 初期化
 
  while (ros::ok()) { // 無限ループ

    jtp0.header.stamp = ros::Time::now();
    jtp0.points[0].positions[0] = joint1_pos.data;  // panにpanの値をセット(上書き)
    jtp0.points[0].positions[1] = joint1_pos.data;  // tiltにpanの値をセット(マスター&スレーブ)
    jtp0.points[0].time_from_start = ros::Duration(0.02);  //実行時間20msec (10msecだと動作しない;;)
    arm_pub.publish(jtp0);

    ros::spinOnce();   // コールバック関数を呼ぶ
    loop_rate.sleep();  // 待ち
    ROS_INFO("Joint1(%s)= %f | %f | %f ||| Joint2(%s)= %f | %f | %f ", joint1_name.data.c_str(),joint1_pos.data,joint1_vel.data,joint1_eff.data,joint2_name.data.c_str(),joint2_pos.data,joint2_vel.data,joint2_eff.data);
  }
  
  return 0;
}

下記の通り、実行します。

1つめのターミナル
$ roscore

2つめのターミナル
$ roslaunch dynamixel_workbench_controllers dynamixel_controllers.launch

3つめのターミナル
$ rosrun dynamixel_micchy dynamixel2dynamixel

こちらもバッチリ動きました。
制御周期が45Hzで頭打ちになってしまったのと、若干の遅延は気になりますが、ちゃんとROSを使ってマスタースレーブができました。
制御周期に関しては、ubuntuのタイマの精度を変更すると改善できるのではないかとご意見いただきました。

また、マスタースレーブするたけならメモリ情報を読み書きするだけでもいいのですが、ちゃんと内部で、位置(radian)だけでなく、速度(m/s)、加速度(rad/s)、負荷(Nm)のパラメータを持っているところが素敵ですね。
雰囲気は大事です。(笑)

ちなみに、rqt_graphはこんな感じです。


読み書きができたので、ひとまずはDynamixelを利用できそうです。
制御周期の問題は、姿勢制御をやるならどうかなーって感じですが、瀬戸内ROS勉強会のテーマのビーズセッターに使う分には、十分かなという感じで、そろそろ3Dモデルとの制御も進めていこうかなという感じです。

コメント

  1. ほげ より:

    jtp0.points[0].time_from_start = ros::Duration(0.02);の値を小さくしてもカクカク動き滑らかに動かないのですが、どうすれば滑らかに動くようになるでしょうか?

    • みっちー より:

      コメントありがとうございます。

      これは「DINAMIXEL Workbench controllers」の限界です。
      いろいろ試しましたが、0.02より小さい値を設定しても意味がなく、実質40Hz~50Hz以上の制御周期は出せません。
      それについて一度瀬戸内ROS勉強会で発表したのですが、記事としてまとめられておらずすみません。スライドだけ貼っておきます。
      https://www.slideshare.net/hirokazuonomichi/dynamixel-workbench

      これは、Workbench controllersが、JointTrajectoryのメッセージを使って実装しているため起こる問題です。
      JointTrajectoryは、本来「n秒後にθ度に移動する」という命令をするもので、MoveItと組み合わせて使ったりするぶんには非常に便利なのですが、nを最小限に取るためのメッセージ形式ではありません。

      Workbench controllersに対して、JointStateで値を投げられればベストなのですが、ROBOTISさんに聞いたところそういう機能は実装されていないそうです。

      「n秒後にθ度に移動する」という使い方で良ければ、0.02ではなくもっと大きい値を取ればよいですし、マスタースレーブの様な速い周期で制御したい場合は、「DINAMIXEL SDK」を用いて、自作するしかなさそうです。
      自作すればおそらく1kHz出せるかと思います。

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください

アーカイブ

広告

Top