ロボットプログラミングⅡ:第13週 ロボットアーム

arm2-gazebo

arm2-rviz

今週はgazeboを使い2自由度のロボットアームを作り、関節を動かします。2自由度ロボットアームのURDFは参考リンクを参照してください。このサンプルでは参考リンクのrrbot.xacroをベースに作っています。

ソース
Turtlebotをテレオペしたソースコードをベースに ~/catkin_ws/src/armbot2/src/teleop.cppを作成しました。ジョイント1とジョイント2の目標角度をパブリッシュし、/armbot2/joint_statesトピックをサブスクライブしジョイントの現在値を取得しています。この例では、目標角度に追従するのに時間がかかるのでループ毎に1秒スリープしてから現在値を取得しています。

 
// ~/catkin_ws/src/armbot2/src/teleop.cpp
#include "ros/ros.h"  // rosで必要はヘッダーファイル
#include "std_msgs/Float64.h"
#include "sensor_msgs/JointState.h"
#include "nav_msgs/Odometry.h"

using namespace std;
std_msgs::Float64 tmp_joint1, tmp_joint2;
double pos_x, pos_y, pos_z;

void monitorJointState(const sensor_msgs::JointState::ConstPtr& jointstate)
{
  tmp_joint1.data = jointstate->position[0];
  tmp_joint2.data = jointstate->position[1]; 
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "teleop");  // ノードの初期化
  ros::NodeHandle nh; // ノードハンドラ  
  ros::Publisher  pub_joint1, pub_joint2;  // パブリッシャの作成
  ros::Subscriber sub_joints, sub_sensor;  // サブスクライバの作成

  std_msgs::Float64 target_joint1, target_joint2;

  pub_joint1 = nh.advertise<std_msgs::Float64>("/armbot2/joint1_position_controller/command", 100);
  pub_joint2 = nh.advertise<std_msgs::Float64>
("/armbot2/joint2_position_controller/command", 100);
  sub_joints = nh.subscribe("/armbot2/joint_states", 100, monitorJointState);

  target_joint1.data = 0;
  target_joint2.data = 0;

  while (ros::ok()) { // このノードが使える間は無限ループ
    char key;  // 入力キーの値

    ROS_INFO("[Input] j: Joint1++, f: Joint1--, k: Joint2++, d:Joint2--");
    cin >> key; 
    cout << key << endl;

    switch (key) {
    case 'j': target_joint1.data  +=  5 * M_PI/180.0; break;
    case 'f': target_joint1.data  -=  5 * M_PI/180.0; break;
    case 'k': target_joint2.data  +=  5 * M_PI/180.0; break;
    case 'd': target_joint2.data  -=  5 * M_PI/180.0; break;
    default: ROS_INFO("Input j,f,k,d");
    }
      
    pub_joint1.publish(target_joint1); // 角度を送信    
    pub_joint2.publish(target_joint2);
    ROS_INFO("Targe: Joint1=%f Joint2=%f", target_joint1.data, target_joint2.data);

    usleep(1000*1000); // 制御に時間がかかるので1秒寝て待つ
    ros::spinOnce();   // コールバック関数を呼ぶ
    ROS_INFO("Tmp:Joint1=%f Joint2=%f", tmp_joint1.data,    tmp_joint2.data);
  }
  
  return 0;
}

なお、ジョイントのPIDゲインやコントローラーは以下のファイルに記述されています。詳細については参考リンクをご覧ください。
~/catkin_ws/src/armbot2/config/armbot2_control.yaml
準 備

  • $ sudo apt-get update
  • $ sudo apt-get install ros-indigo-gazebo-ros-pkgs ros-indigo-gazebo-ros-control
  • $ sudo apt-get install ros-indigo-gazebo-plugins  ros-indigo-gazebo-ros
  • $ sudo apt-get install ros-indigo-moveit-*
  • $ sudo apt-get install ros-indigo-ros-control ros-indigo-ros-controllers  ros-indigo-joint-state-controller ros-indigo-effort-controllers ros-indigo-position-controllers ros-indigo-joint-trajectory-controller
  • $ sudo apt-get install liburdfdom-tools

プログラムとビルド

  • 以下をホームディレクトリ(/home/ユーザ名)にダウンロードする。
  • $ cd
  • $ cp  armbot2-150108.tgz  ~/catkin_ws/src
  • $ cd  ~/catkin_ws/src
  • $ tar  xvzf  armbot2-150108.tgz
  • $ cd  ~/catkin_ws
  • $ catkin_make

実 行
端末を2つ開き、次のコマンドを実行する。

  • $ roslaunch  armbot2  armbot2.launch
  • $ rosrun armbot2 armbot2
    • armbot2を実行している端末をクリックし、j, k, f, dコマンドを入力する毎にEnterキーを入力するとロボットアームが動く。各コマンドを入力する度にEnterキーを押さなければ動きません。jは第1関節を正方向、fは負方向、 kは第2関節を正方向、dは負方向に目標角度を5度ずつ変化させる。
  • 成功するとrvizとgazeboの2つのウインドウが開く。そのままではrvizにロボットモデルが表示されないので、次の設定を行う。

rvizの設定

rviz-gazebo

  • 次の操作を行う
    1. (1) Global Option -> Fixed Frameをbase_linkに設定
    2. (2) Addボタンをクリックする
    3. (3) RobotModelを追加する

ノードグラフ

ノードグラフは次のとおり。端末を開き、次のコマンドを実行。

  • $ rqt_graph

rqt_graph

参考リンク

 

コメント

タイトルとURLをコピーしました