Dynamixel Workbenchを触ってみた #04
前回、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& 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モデルとの制御も進めていこうかなという感じです。
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出せるかと思います。
マスタースレーブのyamlファイルとlaunchファイルを公開してもらえますか？
DINAMIXEL Workbench のテストの延長で、5軸の上半身ロボットを作りました。
Githubに上げています。
https://github.com/dreamdrive/upperbody3
該当ファイルはこのあたりですね。
https://github.com/dreamdrive/upperbody3/blob/master/upperbody3_description/launch/dynamixel_controllers.launch
https://github.com/dreamdrive/upperbody3/blob/master/upperbody3_description/config/upperbody3.yaml
長文失礼します。
joint_trajectory_controllerのpythonファイルをみっちーさんの記事をもとにC++ファイルに書き換えようとかんがえているのですが、うまくいきません。pythonで記述したものはうごくので、C++での記述が誤っているのだと考えています。とりあえずpythonで記述したものよりもpointsとモータを減らしています。誤りがあればすこしでもいいので教えてほしいです。
//C++ファイル
#include “ros/ros.h”
#include “ros/time.h”
#include “math.h”
#include “trajectory_msgs/JointTrajectory.h”
ros::Publisher arm_pub;
int main(int argc, char **argv)
{
ros::init(argc, argv, “joint_trajectory_publisher”); // ノードの初期化
ros::NodeHandle nh; // ノードハンドラ
//パブリッシャの作成
arm_pub = nh.advertise(“dtw_robot/arm_robot_trajectory_controller/command”,10);
ros::Rate loop_rate(10); // 制御周期10Hz
trajectory_msgs::JointTrajectory jtp0;
//jtp0.header.frame_id = “base0_link”;
jtp0.joint_names.resize(2);
jtp0.points.resize(2);
jtp0.points[0].positions.resize(2);
jtp0.points[1].positions.resize(2);
jtp0.joint_names[0] =”arm1_joint”;
jtp0.joint_names[1] =”base1_joint”;
while(ros::ok()) {
jtp0.header.stamp = ros::Time::now();
jtp0.points[0].positions[0] = 0; // 原点のコマンドを送り続けるので起動時にposition0に移動するだけ
jtp0.points[0].positions[1] = 0; // 原点のコマンドを送り続けるので起動時にposition0に移動するだけ
jtp0.points[0].time_from_start = ros::Duration(1.0); //実行時間(1秒かけて移動)
jtp0.points[1].positions[0] = 45*M_PI/180; // 原点のコマンドを送り続けるので起動時にposition0に移動するだけ
jtp0.points[1].positions[1] = 0; // 原点のコマンドを送り続けるので起動時にposition0に移動するだけ
jtp0.points[1].time_from_start = ros::Duration(2.0); //実行時間(２秒かけて移動)
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[1].c_str(),jtp0.points[0].positions[0]);
//ros::shutdown();
}
return 0;
}
//pythonファイル（起動確認済み）
#!/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(‘dtw_robot/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”, “base1_joint”, “arm10_joint” ]
msg.points = [JointTrajectoryPoint() for i in range(8)]
msg.points[0].positions = [0.0, 0.0, 0.0]
msg.points[0].time_from_start = rospy.Time(1.0)
msg.points[1].positions = [45*math.pi/180, 0, 0]
msg.points[1].time_from_start = rospy.Time(2.0)
msg.points[2].positions = [45*math.pi/180, -40*math.pi/180, 0]
msg.points[2].time_from_start = rospy.Time(3.0)
msg.points[3].positions = [0, -40*math.pi/180, 0]
msg.points[3].time_from_start = rospy.Time(4.0)
msg.points[4].positions = [-30*math.pi/180, -40*math.pi/180, -0.05]
msg.points[4].time_from_start = rospy.Time(5.0)
msg.points[5].positions = [-30*math.pi/180, -40*math.pi/180, -0.2]
msg.points[5].time_from_start = rospy.Time(6.0)
msg.points[6].positions = [-30*math.pi/180, 0, -0.1]
msg.points[6].time_from_start = rospy.Time(7.0)
msg.points[7].positions = [0, 0, 0]
msg.points[7].time_from_start = rospy.Time(8.0)
pub.publish(msg)
rospy.sleep(0.5)
if __name__ == ‘__main__’:
try:
talker()
except rospy.ROSInterruptException: pass
こんにちは、コメントありがとうございます。
実行してないので、確実なことは言えませんが、ふとソースコードをみて気になるのは、pythonの方は単発のpublishだけど、C++の方はループしていることでしょうか。
しかも制御周期10Hzなので、0.1秒しか間隔がないのに、2秒の実行時間のjointtrajectoryを送ると、実行する時間がないうちにバッファが溢れてしまいそうです。
とりあえず、whileループを外して・・・
subscribeもしてないので、
ros::Rate loop_rate(10); // 制御周期10Hz
ros::spinOnce(); // コールバック関数を呼ぶ
loop_rate.sleep();
の3つも、コメントアウトしても良いかと思います。
それでどうでしょう？
実行したらすぐに終了すると思いますが、1回だけpublishされるかと思います。
実行前に、rqtを立ち上げておけばpublishの内容も確認できるかと思います。
長文申し訳ありません