OGIMOノート

6歳の娘と、4歳の息子(脳性麻痺)を持った父親エンジニアの備忘録。自作の電子工作おもちゃ/リハビリ器具の製作記録、勉強記録を残していきます

【ROS】 Turtlebot3 のシミュレーション環境を構築してみた

はじめに

自宅で自作ロボットのNavigation環境を構築しようとしているが、モータ周りの動作が安定せず苦戦中。
市販で購入してきたギア内蔵モータのギア比が低めなのか、ちょっと動かすだけで半周したりとなかなかのじゃじゃ馬っぷり。

このままハードウェアの調整だけで時間を使っていくのが勿体ないので、気分転換に市販ロボットで勉強してみようと思う。

とはいえ、Turtlebot3 を購入するのはボーナス前のこの時期にはきついので、とりあえずシミューレション環境でお勉強。ちょうど手元のノートPCを一新して、Windows10とUbuntu16.04をDual Bootできる様にしたので、この環境に構築してみたいと思う。

手順

こちらのサイトを参考に、立ち上げてみる。
demura.net
emanual.robotis.com

$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_gazebo_plugin git

で一通りのTurtlebot3環境をclone。 .bashrcに以下の環境変数を追加。

export TURTLEBOT3_MODEL=burger

そして、catkin_makeをかけてGazebo起動!

$ roslaunch turtlebot3_gazebo  turtlebot3_world.launch 

んっ!? なんかエラーが出て起動しない。。。

process[rosout-1]: started with pid [4186]
started core service [/rosout]
process[gazebo-2]: started with pid [4194]
process[gazebo_gui-3]: started with pid [4211]
process[spawn_urdf-4]: started with pid [4220]
[ INFO] [1528459620.691972547]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1528459620.692481908]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
Error [parser.cc:581] Unable to find uri[model://sun]
Error [parser.cc:581] Unable to find uri[model://ground_plane]
[ INFO] [1528459622.261795752, 0.022000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.

どうやらmodelファイルが足りないらしい。いろいろと調べたら下記サイトに対策を発見。

How do you completely uninstall old versions of gazebo? - Gazebo: Q&A Forum

サイト通りに従って、
https://bitbucket.org/osrf/gazebo_modelsからosrf-gazebo_models.zipをダウンロード
②解凍ファイルを~/.gazebo/models に移動 (フォルダがなければ新規作成)
③ .bashrcに以下の記述を追加

export GAZEBO_PLUGIN_PATH=~/catkin_ws/src/turtlebot3_gazebo_plugin/build:${GAZEBO_PLUGIN_PATH}
export GAZEBO_MODEL_PATH=~/catkin_ws/src/turtlebot3_gazebo_plugin/models:~/catkin_ws/.gazebo/models:${GAZEBO_MODEL_PATH}

これで再度launchファイルを動かすと、、、起動した!!

ふぅ、焦った。。。 (環境開発はこれだから大変だ。苦行にならずにすみそう)

$  roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

で無事にGazebo上の Turtlebot3 を動かせた!

f:id:motokiinfinity8:20180608214931p:plain

この環境で Rvizが動くかも確認してみた。別ターミナルで以下のコマンドでRviz起動

roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch

f:id:motokiinfinity8:20180608215904p:plain

よし、Rvizでしっかりと LaserScan値も読めています。良い感じ!
シミュレータがあるって、よいねぇ〜



とりあえず、実機の調整に飽きたら、こちらのSimulation環境でNavigationを動かしていこう。

情報量は少なめですが、備忘録がわりとして今日はここまで

【ROS】Nintendo LABO(ニンテンドーラボ) Toy-Con+ESP8266を使ってROS対応二輪ロボットを動かしてみた

完成したもの

出来たものはこちら

youtu.be
f:id:motokiinfinity8:20180524220642j:plain

Nintendo LABOのバイクToy-Conを使って、ロボットを操作できる様にしてみた。
アクセスハンドルをひねるとロボットが前に動き出し、ハンドルを左右に傾けるとロボットも左右に移動する様な使い勝手。
ハンドル加減と曲がり具合はまだ調整中ですが、体験価値としては面白いかと。
娘も楽しそうに遊んでくれてました!(息子にはまだ早いので、息子向けにはToy-Conの見直しから)

ちなみに、ROS上で動かしているので、将来的には自立移動等と組み合わせたりも出来そう♪


作成動機

前回記事
ogimotokin.hatenablog.com
の続きで、「さーて! GW中にencorderからodometryを作成して、いよいよnavigationと結合するか~」と意気込んでいたのですが、
Nintendo LABO発売!
によってすべての計画が崩れてしまいました!
これは面白い!

あっという間に、娘のお手て(マーカー)を追いかけるロボットが出来ました(^-^)
これはただのダンボールで遊ぶおもちゃじゃないですね。子供の成功体験(作った!出来た!動いた!)を積ませて、「上手にできた!楽しい! もっと色々作りたい!」と好奇心を高めさせてくれる素敵な工作教材です(^-^)
ただ簡単に作るだけじゃなく、しっかり原理も学べるという。子供向けに「IRカメラ」とか「振動の早さ(周波数)を変える」とかをかんたんに説明していたりして、これはすごい!
さすが任天堂! 本当に子供心を掴むのがうまいですね!

気が付けばGWも終わり、すっかり計画破綻。
しかし、この体験価値を生かせないだろうか?と考え、「いっそ、娘の作ったToy-CONで実際のロボットも動いたら面白んでないか?」という事で早速トライしてみました。

システム構成(案)

思いついたのは以下の案
f:id:motokiinfinity8:20180524222806p:plain


Arduino対応無線モジュールesp8266(ESP-WROOM-02)マイコンをROS対応して、Nintendo Switch Joy-CONのジャイロセンサ情報をキャプチャーするという手段。Nintendo LABO側はトイコンガレージを使って、2つのジャイロセンサーの角度に応じて赤外線送信命令を送信するプログラムを作成してやればよいので、これからNintendo LABOをROS対応したと言えなくはない。

という訳で、まずはesp8266のROS対応から。
これは Wifi経由でrosserialノードを使う事ができる様だ。下記サイトを参考にしてプログラムを作成してみた。

GitHub - agnunez/espros: ROS serial for ESP8266 over WiFi

serverPortは11411を使用。clientからserverへの接続はesp8266サンプルプログラムと同様。加えて

  ros::NodeHandle nh;
  nh.getHardware()->setConnection(server, serverPort);
  nh.initNode();

を追加するイメージ。その後はrosserialと同じ使い方でpub/sub定義をすれば良い。(完成コードは後述)

課題

順調に制作を進めていたのだが、本家のNintendo LABOの開発が進まず。一番のブロッカーは娘と一緒に遊ぶ時間があわず通常キット5個が完成していない状況。元々娘と一緒に遊ぶ約束で購入したため、私が勝手にトイコンガレージを触ろうとすると
「わたしがまだやってないのに、先にやらんといてーーーー!!」
という始末。これは困った…

システム構成案(改)

という訳で、娘の決済がおりず、やむなくシステム変更。
f:id:motokiinfinity8:20180524222808p:plain

Conの代わりにジャイロセンサ×2を購入して、Toy-Conの段ボールに張り付ける。1つはハンドル部、1つは胴体部。

SODIAL(R) GY-521 MPU-60503軸ジャイロ+3軸加速度センサ
Amazon CAPTCHA

センサーの使い方はこちらのサイトを参考にした
こじ研(ESP センシング編)
https://www.ei.tohoku.ac.jp/xkozima/lab/espTutorial2.html

2つのジャイロセンサーとはI2Cで並列接続。区別できる様に片側のAD0ピンは電源PU抵抗をつけてHにしておく。
胴体部をAD0ピンをPUしてI2Cアドレス0x69、ハンドル部は未実装にしてI2Cアドレス0x68とする。
x,y,z方向の加速度情報を読み出し(読み出しは2byte×2、リトルエンディアンである事に注意)、分解能で割って加速度(G)に変換する。その後、加速度からセンサー対地角度を求める。あとは、ハンドル部(toy_acc)と胴体部(base_acc)との差分をもとにハンドルのひねり量を算出する。

そう、、、お気づきの通り、ここでもう Nintendo LABOは関係なくなってしまった(笑)

作成コード

①esp8266側 toycon_ros.ino

#include <ESP8266WiFi.h>
#include <ros.h>
#include <time.h>
#include <Wire.h>
#include <geometry_msgs/Twist.h>

#define MPU6050_TOY_ADDR 0x68
#define MPU6050_BASE_ADDR 0x69
#define MPU6050_AX  0x3B
#define MPU6050_AY  0x3D
#define MPU6050_AZ  0x3F
#define MPU6050_GX  0x43
#define MPU6050_GY  0x45
#define MPU6050_GZ  0x47

float base_acc_x, base_acc_y, base_acc_z;
float toy_acc_x,  toy_acc_y,  toy_acc_z;
float base_acc_angle_x, base_acc_angle_y, base_acc_angle_z;
float toy_acc_angle_x, toy_acc_angle_y, toy_acc_angle_z;

const char* ssid     = "********************";
const char* password = "******************";
IPAddress server(192,168,11,3);      // Set the rosserial socket ROSCORE SERVER IP address
const uint16_t serverPort = 11411;    // Set the rosserial socket server port

void setupWiFi() {                    // connect to ROS server as as a client
  if(DEBUG){
    Serial.print("Connecting to ");
    Serial.println(ssid);
    WiFi.begin(ssid, password);
  }
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }
  if(DEBUG){
    Serial.println("");
    Serial.println("WiFi connected");
    Serial.println("IP address: ");
    Serial.println(WiFi.localIP());
  }
}

// ROS nodes //
ros::NodeHandle nh;
geometry_msgs::Twist cmdvel_msg; 
ros::Publisher pub_cmdvel("/toycon/cmd_vel", &cmdvel_msg); 
 
void setup() {
  Serial.begin(115200);
  setupWiFi();
  delay(2000);
// Ros objects constructors   
  nh.getHardware()->setConnection(server, serverPort);
  nh.initNode();
  nh.advertise(pub_cmdvel);

  //  i2c as a master
  Wire.begin();
  Wire.beginTransmission(MPU6050_BASE_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission();

  Wire.beginTransmission(MPU6050_TOY_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission();
}

void loop() {
  if (nh.connected()) {
    //  send MPU6050_BASE_ADDR
    //  send start address
    Wire.beginTransmission(MPU6050_BASE_ADDR);
    Wire.write(MPU6050_AX);
    Wire.endTransmission();  
    Wire.requestFrom(MPU6050_BASE_ADDR, 14);

    //  get 14bytes
    short int AccX, AccY, AccZ;
    AccX = Wire.read() << 8;  AccX |= Wire.read();
    AccY = Wire.read() << 8;  AccY |= Wire.read();
    AccZ = Wire.read() << 8;  AccZ |= Wire.read();

    //  X/Y方向を反転
    AccX = AccX * -1.0;
    AccY = AccY * -1.0;

    // 取得した加速度値を分解能で割って加速度(G)に変換する
    base_acc_x = AccX / 16384.0; //FS_SEL_0 16,384 LSB / g
    base_acc_y = AccY / 16384.0;
    base_acc_z = AccZ / 16384.0;

    // 加速度からセンサ対地角を求める
    base_acc_angle_x = atan2(base_acc_x, base_acc_z) * 360 / 2.0 / PI;
    base_acc_angle_y = atan2(base_acc_y, base_acc_z) * 360 / 2.0 / PI;
    base_acc_angle_z = atan2(base_acc_x, base_acc_y) * 360 / 2.0 / PI;


    //  send MPU6050_TOY_ADDR
    //  send start address
    Wire.beginTransmission(MPU6050_TOY_ADDR);
    Wire.write(MPU6050_AX);
    Wire.endTransmission();  
    Wire.requestFrom(MPU6050_TOY_ADDR, 14);

    //  get 14bytes
    AccX = Wire.read() << 8;  AccX |= Wire.read();
    AccY = Wire.read() << 8;  AccY |= Wire.read();
    AccZ = Wire.read() << 8;  AccZ |= Wire.read();

    // 取得した加速度値を分解能で割って加速度(G)に変換する
    toy_acc_x = AccX / 16384.0; //FS_SEL_0 16,384 LSB / g
    toy_acc_y = AccY / 16384.0;
    toy_acc_z = AccZ / 16384.0;

    // 加速度からセンサ対地角を求める
    toy_acc_angle_x = atan2(toy_acc_x, toy_acc_z) * 360 / 2.0 / PI;
    toy_acc_angle_y = atan2(toy_acc_y, toy_acc_z) * 360 / 2.0 / PI;
    toy_acc_angle_z = atan2(toy_acc_x, toy_acc_y) * 360 / 2.0 / PI;

    // cmd_vel判定① : linear.x
    // TOY y角とBASE y角に対してTOY y角が90°以上になった場合、Baseとの差分角分の前進命令を送る
    float max_linear = 0.5;
    float start_angle = 92;
    float max_angle = 145;
    float diff_angle;
    diff_angle = toy_acc_angle_y - base_acc_angle_y;
    if(diff_angle < 0 )  diff_angle = 360 + diff_angle;
    
    if(diff_angle  < start_angle || base_acc_angle_y > 90 || base_acc_angle_y < -90){
      cmdvel_msg.linear.x = 0.0;
    }else if(diff_angle  > max_angle){
      cmdvel_msg.linear.x = max_linear;
    }else{
      cmdvel_msg.linear.x = max_linear * ( diff_angle - start_angle) / (max_angle - start_angle); 
    }

    // cmd_vel判定② :angular.z
    // BASE x角に応じて、左右方向の命令を送る
    if(cmdvel_msg.linear.x == 0.0){
      cmdvel_msg.angular.z = 0.0;
    }else if(base_acc_angle_x >= 35){
      cmdvel_msg.angular.z =0.5 * cmdvel_msg.linear.x /  max_linear;
      cmdvel_msg.linear.x = 0.0;  
    }else if(base_acc_angle_x <= -35){
      cmdvel_msg.angular.z =-0.5 * cmdvel_msg.linear.x /  max_linear;
      cmdvel_msg.linear.x = 0.0;  
    }else if(abs(base_acc_angle_x) < 35){
      cmdvel_msg.angular.z = base_acc_angle_x / 35.0;
    }

    // cmd_vel publish
    pub_cmdvel.publish( &cmdvel_msg );  
  } else {
    Serial.println("Not Connected");
  }
  nh.spinOnce();
  // Loop aprox. every  
  delay(50);  // milliseconds
}

raspberry pi側 aruduono_namual.launch

<?xml version="1.0"?>
<launch>
    <!-- Start joycon -->
    <node pkg="joy" name="joycon" type="joy_node"/>

    <!-- Start alphabot_manual node -->
    <node pkg="alphabot" name="AlphaBotManual" type="AlphaBotManual"  output="screen"/>

    <!-- Set to your serial port of esp8266 -->
    <arg name="toycon_serial_port" default="tcp"/>
    <arg name="toycon_baud_rate" default="57600"/>
    <node pkg="rosserial_python" type="serial_node.py" name="toycon_node" output="screen">
        <param name="port" type="string" value="$(arg toycon_serial_port)"/>
        <param name="baud" type="int" value="$(arg toycon_baud_rate)"/>
    </node>
</launch>
||<</span>

** 最後に
何気に、ESP8266をrosserialで動かす事が出来たのが一番の成果ではなかろうか。

【ROS】ラズパイで始めるROS BOT入門④ ~WaveShare社Alphabotの移動制御対応~

はじめに

ようやくラズパイ環境を構築できたので、いよいよ実物ロボットを使った自律移動を学んでいく。しかし、ここで問題になるのは、どのロボットを購入するかである。
安いと言われているTurtlebot3 Burgerで約7万円、RT社のマイクロマウスでも約5万円。小遣い制の父親エンジニアにはなかなか決裁が取れない様な金額である。

別途、mBot(1.3万円)は持っているのだが、これはモーターエンコーダなしのため、SLAM環境構築には厳しいと考える。
モーターエンコーダ付きの既存二輪走行ロボットでなんか安いのないかなぁー、とAmazonをさまよっていると……1万円以下で発見!!!


AlphaBot - Robotics

WaveShare社のAlphabotなる二輪走行ロボット。サイト情報からは、モーターエンコーダ搭載とあり、お値段はラズパイなしで当時(2017/11)で6500円!
Webサイトで調べるも全然レビュー例がないのではげしく不安だが、「この値段なら失敗してもまだなんとかなる!」と思い購入!

Alphabot組み立て

とりあえず無事に到着したので、組み立ててみる。
メーカーサイトにWikiがあり、組み立て説明書も以下のサイトにある。

AlphaBot - Waveshare Wiki
とりあえず手順に従って組み立てていく。
f:id:motokiinfinity8:20180507022501j:plainf:id:motokiinfinity8:20180508020809j:plain


一旦組み立て完了後の写真。思ってたより大きかった。Raspberry pi3 と Arduino Unoはボディ間にはさむ想定。(ただし、Arduinoはケースを外し、底はテープで絶縁する必要あり)

f:id:motokiinfinity8:20180508021104j:plain

各デバイス(モーター/エンコーダ/ライントレーサー/赤外線センサー)をRaspberry / Arduino のどちらと接続するかをジャンパーで切り替えれる様だ(左側の黄色ジャンパ) これは便利。また、RaspberryとArduino間はUARTで繋がってる様だ。これもスイッチでON/OFFできる。

f:id:motokiinfinity8:20180508021825j:plain

タイヤのエンコーダ。ギアボックス付きモーターにエンコーダ(黒い穴空き円盤)があり、フォトダイオードでパルス検出するタイプ。オシロでデジタル信号を確認したところ、キレイには取れていた!

不満点

便利に感じる面もあるが不満点も多々あり。
・底の補助輪(金属)の回転摩擦が大きく、移動するとガラガラ音が鳴る
・タイヤの付きが甘く外れやすい
・前後の重量バランスがシビアで、後ろ重心になるとタイヤが空回りしてしまう
・ラズパイのUSB電源とHDMIコネクタが内部モーター手前のため、ロボット状態でのケーブル抜き差し不可。
・バッテリーは見た目乾電池だが、乾電池ではなくリチウム電池18650を購入する必要あり。
私はamazonにて、以下のメーカー品を購入したのだが、安全最優先で保護回路付きにしたため、保護回路分電池が長く、無理やり押し込まないと入らない


結果、値段相当ではある印象。
まぁ、これを使いこなしてこそエンジニア!という事で、もう少し頑張ってみたいと思う。

ROSでのcmd_vel対応 (Arduinoマイコンでの左右モーター制御)

まずは走行ロボット制御の基礎である前後左右移動を実装する。操作はPS4コントローラのアナログ(左)スティックで行う。
ブロック図は以下。

f:id:motokiinfinity8:20180511014736j:plain

ラズパイ側では、PS4コントローラのjoyノードから通知されるjoy情報から速度情報(geometry::twist型/cmd_vel)を生成して、arduinoマイコンに通知する。arduinoマイコンはcmd_vel情報から、左右のモーターに必要なPWM値を算出して信号出力するイメージだ。

モーターの必要トルクについては不勉強なため、カット&トライアルを実施。モーターはPWMで制御。現状ではバッテリー80%で80/256くらいの回転数で動き出せる様だ。

Arduinoとのrosserial通信

個人的にROSの非常にありがたい機能であるroserial。これはシリアル通信を使ったROSメッセージを送る汎用プロトコル になり、これがらあれば非力な非OSのマイコンでもclientとしてのROS動作を実現できると非常に使いやすい!

rosserial 導入
sudo apt-get install ros-kinetic-rosserial-arduino ros-kinetic-rosserial
cd <sketchbook>/libraries
rm -rf ros_lib
rosrun rosserial_arduino make_libraries.py .

arduinoWindowsで書きこみたい場合は、作成したros_libをWindows内のarduino/library に置けばオッケー!(ros.hでインクルードエラーが置きなければ良い)

UART使用に向けて

Ubuntu MATE@Raspberry PiではdefaultでUARTが使えないため、以下の設定を実施してUARTを使用可能にする必要がある
①/boot/config.txtの以下の設定を有効にする
 core_freq=250
② /boot/cmdline.txtを以下に変更する
 dwc_otg.lpm_enable=0 console=tty1 root=/dev/mmcblk0p2 rootfstype=ext4 elevator=deadline rootwait quiet splash
③ raspi-configを実行
 「3:Interface Option」→「P5: Serial」→「login shel accessible: No」→「enable:Yes」
④再起動

作成コード

①raspberry側 alphabot_manual.launch

<?xml version="1.0"?>
<launch>
    <!-- Start joycon -->
    <node pkg="joy" name="joycon" type="joy_node"/>

    <!-- Start alphabot_manual node -->
    <node pkg="alphabot" name="AlphaBotManual" type="AlphaBotManual"  output="screen"/>

    <!-- Set to your serial port of Arduino -->
    <arg name="serial_port" default="/dev/ttyS0"/>
    <arg name="baud_rate" default="57600"/>
    <node pkg="rosserial_python" type="serial_node.py" name="serial_node" output="screen">
        <param name="port" type="string" value="$(arg serial_port)"/>
        <param name="baud" type="int" value="$(arg baud_rate)"/>
    </node>
</launch>



②raspberry側 alphabot_manual.cpp

#include <ros/ros.h>
#include <ros/wall_timer.h>
#include <math.h>
#include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>

typedef enum {
	JOY_AXES_MANUAL_VERTICAL     = 1,   // Up or Down     (PS4:左スティック)      [joy_msg.axes]
	JOY_AXES_MANUAL_HORIZONTAL   = 0,   // Right or Left  (PS4:左スティック)      [joy_msg.axes]
} JOY_AXES;

typedef enum {
	JOY_BUTTON_MANUALBOOST        = 0,   // Set Manual Mode  (PS4:□ボタン)  [joy_msg.buttons]
} JOY_BUTTON;

#define UPDATE_TS 0.05

class AlphabotManual{
public:
	AlphabotManual();
	~AlphabotManual();
	void timerCallback(const ros::TimerEvent&);
	void JoyConCallback(const sensor_msgs::Joy &joy_msg);
private:
	ros::Timer timer;
	ros::NodeHandle nh;
	ros::Subscriber joy_sub;
	ros::Publisher twist_pub;
	geometry_msgs::Twist cmd_vel,p_cmd_vel;
};

AlphabotManual::AlphabotManual(){
	timer        = nh.createTimer(ros::Duration(UPDATE_TS), &AlphabotManual::timerCallback, this);
	joy_sub      = nh.subscribe("joy", 10, &AlphabotManual::JoyConCallback, this);
	twist_pub    = nh.advertise<geometry_msgs::Twist>("alphabot/cmd_vel", 10);
}

AlphabotManual::~AlphabotManual(){
}

/***************************************************************************
* PS4コントローラによる操作
***************************************************************************/
void AlphabotManual::JoyConCallback(const sensor_msgs::Joy &joy_msg){ 
	int boost = (joy_msg.buttons[JOY_BUTTON_MANUALBOOST] == 1) ? 3 : 1;
	cmd_vel.linear.x = joy_msg.axes[JOY_AXES_MANUAL_VERTICAL]*0.25*boost;
	cmd_vel.angular.z = joy_msg.axes[JOY_AXES_MANUAL_HORIZONTAL]*0.5*boost;
   	return;
}

void AlphabotManual::timerCallback(const ros::TimerEvent&){
	// cmd_vel更新
	twist_pub.publish(cmd_vel);
	if(p_cmd_vel.linear.x != cmd_vel.linear.x || p_cmd_vel.angular.z != cmd_vel.angular.z ){
		ROS_INFO("[Manual mode]linear.x: %0.2f, angular.z: %0.2f",cmd_vel.linear.x, cmd_vel.angular.z);
		p_cmd_vel = cmd_vel;
	}	
	return;
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "AlphaBotManual");
	AlphabotManual client;
	ros::spin();
	return 0;
}



arduino側 alphabot_cmdvel.ino

#include <ros.h>
#include <geometry_msgs/Twist.h>
//駆動に必要な最低PCMパルス数
#define L_PULSE_START 80 //駆動に必要な最低PCMパルス数
#define R_PULSE_START 80 //駆動に必要な最低PCMパルス数
#define PULSE_1MPS 80 //1.0m/sに必要なPCMパルス数

ros::NodeHandle nh;
int cmdvel_cnt=0;
int l_motor=0, r_motor=0;
std_msgs::Int32 count_msg;
void MotorCmdCallback(const geometry_msgs::Twist& msg){
cmdvel_cnt = 0;

// 進行方向設定
if(msg.linear.x > 0.0){
  l_motor = (int)(L_PULSE_START + PULSE_1MPS * msg.linear.x);
  r_motor = (int)(R_PULSE_START + PULSE_1MPS * msg.linear.x);
}else if(msg.linear.x < 0.0){
  l_motor = (int)(-1*L_PULSE_START + PULSE_1MPS * msg.linear.x);
  r_motor = (int)(-1*R_PULSE_START + PULSE_1MPS * msg.linear.x);
}else{
  l_motor = 0;
  r_motor = 0;
}
// 角度方向設定
if(msg.angular.z > 0.0){
  if(msg.linear.x == 0.0){
    l_motor = (int)(-1*L_PULSE_START - PULSE_1MPS * msg.angular.z * 0.75 /2);
    r_motor = (int)( R_PULSE_START + PULSE_1MPS * msg.angular.z * 0.75 /2); 
  }else if(msg.linear.x > 0.0){
    r_motor += PULSE_1MPS * msg.angular.z;
    l_motor -= PULSE_1MPS * msg.angular.z*0.5;
  }else{
    r_motor -= PULSE_1MPS * msg.angular.z;
    l_motor += PULSE_1MPS * msg.angular.z*0.5;
   }
 }else if(msg.angular.z < 0.0){
    if(msg.linear.x == 0.0){
      l_motor = (int)( L_PULSE_START + PULSE_1MPS * msg.angular.z * -0.75 /2);
      r_motor = (int)( -1 * R_PULSE_START - PULSE_1MPS * msg.angular.z * -0.75 /2); 
    }else if(msg.linear.x > 0.0){
      l_motor += PULSE_1MPS * msg.angular.z * -1;
      r_motor -= PULSE_1MPS * msg.angular.z*0.5 * -1;
  } else{
      l_motor -= PULSE_1MPS * msg.angular.z * -1;
      r_motor += PULSE_1MPS * msg.angular.z*0.5 * -1;
  }
}
  //限度設定
  if(l_motor > 255) l_motor = 255;
  if(l_motor < -255) l_motor = -255;
  if(r_motor > 255) r_motor = 255;
  if(r_motor < -255) r_motor = -255;
return;
}
bool MotorRun(int LS,int RS){ 
  if(LS >= 0 && LS <= 255){
    analogWrite(5, LS); 
    digitalWrite(14, HIGH); 
    digitalWrite(15, LOW);
  }
  if(LS < 0 && LS >= -255){
    analogWrite(5, abs(LS)); 
    digitalWrite(14, LOW); 
    digitalWrite(15, HIGH);
  } 
  if(RS >= 0 && RS <= 255){
    analogWrite(6, RS); 
    digitalWrite(17, HIGH); 
    digitalWrite(16, LOW);
  }
  if(RS < 0 && RS >= -255){
    analogWrite(6, RS); 
    digitalWrite(17, LOW); 
    digitalWrite(16, HIGH);
  }
  if  (RS > 255 || RS < -255 || LS > 255 || LS < -255){
    return false;
  } 
  return true;
}

bool MotorBrake(){ 
  digitalWrite(5,LOW);
  digitalWrite(6,LOW);
}
ros::Subscriber<geometry_msgs::Twist> sub_cmdvel("alphabot/cmd_vel", MotorCmdCallback);
void setup(){
  nh.initNode();
  nh.subscribe(sub_cmdvel);
}

void loop(){
//cmd_vel設定の反映
  if(cmdvel_cnt <= 20){ //cmd_vel命令が 1000ms(50*20)以内の場合
    MotorRun(l_motor, r_motor); 
    cmdvel_cnt++; 
  }else{
    MotorBrake(); 
  }
nh.spinOnce();
delay(50);
}



これで無事にPS4コントローラから二輪走行ロボットを動かすことが出来た。左右のトルクばらつきなどでまっすく進まないケースがあるため、そこについては微調整が必要になりそうだ。次回はOdometryによる自己位置推定のベース作成に行きたいと思う


【簡単IoT】ESP8266(+Amazon echo)で寝室からの呼び出しボタンを作ってみた

はじめに

このGWに二段ベッドを導入してようやく娘が一人で寝れる環境を作ることができた。ただ、残念ながら、娘は毎晩12時頃に目が覚めてしまい、寂しくて泣いてしまうのである。今までなら無言でリビングに出てくるのだが、二段ベッドの上だと寝ぼけてベッドから落ちるのが怖いらしい。かといって、大声で「おとーさーん!」と叫ばれると、下で寝てる息子を起こしかねない。

そこで、今回は寝室からリビングへ呼び出し通知するデバイス(要はナースコール)みたいなのを簡単に作ってみたいと思う。

要件

・寝室ベッドの横に呼び出しボタンを設置。それを押すとリビングで通知音が鳴る事。(寝室では音が鳴らない)
・夜中でも押しやすい様な大きなボタン
・電源から遠いので、コードレス推奨。

amazon echoによる家電制御を導入してからようやく我が家で認められる様になったAlexa! ここらで更に活躍を広げるため、娘の呼び出し通知機能を追加していこうと思う。

ハードウェア

コードレス(無線伝送/電池駆動)かつ超シンプル制御という要件から考えて、今回はArduino対応無線モジュールesp8266を使ってみたいと思う。

【ESP8266

www.switch-science.com
動作確認をするならこれが一番簡単。USB電源供給だしPC書き込み可能だし、書き込みボタンをも搭載されてるし。ただ今回は電池駆動にするため、USBコネクタは冗長になりそうなので、下記のモジュールそのものを使う。
www.switch-science.com
ソフト書き込み時は、ブレッドボード経由でUSBシリアル変換モジュール経由でPCから書き込む。

【ボタン】
娘と近くのコーナリングに行って押しやすそうなボタンを選ぶ。LEDプッシュライト NIT-BP1D で400円くらい。
www.kohnan-eshop.com

【3.3V出力用レギュレータ】
購入したプッシュライトが単4電池×3本だったので、3.6~4.5V→3.3V(マイコン電源)に変換するDC/DCとして、LM3671搭載 3.3V出力DC-DCコンバータを使ってみた。
https://www.switch-science.com/catalog/2638/


接続はシンプルなので文章のみ。
LEDプッシュライトからは、電池電源(3.6~4.5V)とGNDの2本を引っ張り、3.3Vレギュレータで変換した後、ESP8266の電源として使用。
今回の使い方としては、特に他に端子仕様しないので、超シンプル。

完成イメージはこちら。



f:id:motokiinfinity8:20180506174213j:plain
f:id:motokiinfinity8:20180506174201j:plain
f:id:motokiinfinity8:20180506174321j:plain




ソフトウェア

ESP8266に実際に書き込んだソースコードは以下。要は、ボタン押し=マイコン起動したら、リビングにあるラズパイに対してHttpメソッドのGetを投げるだけ。うん、すごく簡単。サンプルコード程度。

#include <ESP8266WiFi.h>
const char* ssid     = ***************;
const char* password = **************;
const char* host = "192.168.0.**";    //リモコン制御用RaspberryのIPアドレス
const char* path = "/bedroom_call";

void setup() {
  Serial.begin(115200);
  delay(100);

  // We start by connecting to a WiFi network
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");  
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
  Serial.print("connecting to ");
  Serial.println(host);
}
  
void loop() {
  // Use WiFiClient class to create TCP connections
  WiFiClient client;
  const int httpPort = 1880;  //Node-REDの使用ポート
  if (!client.connect(host, httpPort)) {
    Serial.println("connection failed");
    return;
  }
   String body = "bedroom_call=1";
     
  // サーバにリクエストを送信(GET)
  client.print(String("GET ") + path +" HTTP/1.1\r\n" +
               "Host: " + host +":1880"+ "\r\n" + 
               "Connection: close\r\n\r\n");

  delay(1000); // Can be changed
  if (client.connected()) { 
    client.stop();  // DISCONNECT FROM THE SERVER
  }

  // 20秒待ち
  delay(20000); // Can be changed
}


本体ラズパイ側では、Node-REDを使用。寝室ボタンからのGETメソッドを受信したら、あらかじめ用意していたwavファイル(「~ちゃんが呼んでるでー!」)を再生するだけ。Google homeでは任意の音声を再生できるのだか、Amazon echoでは現時点でできなそう。text to speechはあるけど機械っぽい発音がいまいちなので。なので、echoをBluetoothスピーカーにしてラズパイから音声再生させる手段にしたいと思う。

なお、音声再生には node-red-contrib-spealkerpiを使用。

npm install node-red-contrib-speakerpi

でインストール可。


ほんま簡単。

ここでトラブル。Amazon echoから音声再生ができない!


Bluetooth経由でAmazon echo と接続。ごちゃごちゃやって、結果として paplay ではechoからの音声再生できたのですが、Node-REDからのSpeaker-Outノードで再生させようとすると、ラズパイ自身(HDMI or Audio Out)からの出力になってしまう。。。困った。

とりあえず、他にやりたい事もあったので、一旦はラズパイ自身に Audio DAC+スピーカを追加して、とりあえずロールアウト。
ちょっと悔しい。

この辺りの解決策、知っている人がいたら、是非とも教えて欲しいものだ。

作成結果

作成後の動画がこちら。

www.youtube.com

ピンポンパンポンの呼び出し音と、優しい関西弁での呼び掛け。text to speechでは出来ないコミカルな呼び出しボタンが出来ました。実際に、寝室の扉を閉めておけば、リビング音は聞こえないので、下の子を起こすこともない!

f:id:motokiinfinity8:20180506152005j:plain

運用を開始しましたが、娘は嬉しそうに使ってくれていて、早速活躍してます! 良かった♪ 「お父さん、こんなに早く出来たん!? すごい!」と尊敬と信頼も勝ち取りました(笑)

作成期間は約2晩(4時間×2日) ソフトは1h程度で、あとはハード構想&改造の時間。esp8266が思った以上に使えそうなのも収穫! これから使っていきます!

【ROS】ラズパイで始めるROS Bot入門③ ~Ubuntu MATEへROSインストール~

はじめに

先日の記事
ogimotokin.hatenablog.com
Ubuntu Server 16.04 LTSを立ち上げて開発していたが、色々と設定をいじっているとデスクトップ環境を壊してしまい、再インストールするはめに…

ただ、現状のUbuntu Server環境では下記の不満があった。
 ・起動が不安定。時々落ちる。
 ・環境インストールが地味に手間がかかる
 ・ VNC環境が構築できず、別クライアントPCからの遠隔開発ができない
ロボット開発という特性上、極力ロボットにHDMIケーブルは刺したくないので、やはりなんとかしておきたい。

という事で、色々と調べてみたら、Raspberry Pi に対してはUbuntu MATEディストリビューションが最近流行りの様だ。VNC環境の構築も簡単そうだしROSも普通に動きそうなので、この機会にUbuntu MATE 16.04 LTSに切り替えてみようと思う。

インストール手順

①公式サイトからUbuntu MATE 16.04 LTSのダウンロード

ubuntu-mate.org
ubuntu-mate-16.04.2-desktop-armhf-raspberry-pi.img.xz をダウンロード。xz形式で圧縮されているので、Windowsであれば「7-ZIP」などの解凍ソフトをつかってimgファイルを抽出。その後は、いつもの「win32diskimager」を使ってSDカードに書き込む。

②ラズパイセットアップ

書き込んだマイクロSDカードをラズパイに設定してAC電源入り。画面指示にしたがって、ネットワーク(無線)設定、言語設定、ユーザー名設定を行っていく。設定を終えたら、デスクトップが起動するまでひたすら待つ。片手間で出来るので楽ちん。

③日本語入力対応

日本語入力は以下のサイトを参考に。
deviceplus.jp
特にはまる事はなかった。

SSH/VNC設定

ssh設定は以下で実施。

sudo apt update
sudo apt upgrade
sudo apt install openssh-server

続いて、sshで使用する22番ポートの解放設定を実施。

sudo ufw allow 22 # sshの再起動
sudo /etc/init.d/ssh restart # 起動時にsshの起動
sudo systemctl enable ssh

他のPCからsshでログインできる事を確認すれば完了

VNC設定は以下で実施。

sudo apt-get install tightvncserver
vncserver :1

これでVNCサーバーが立ち上がるので、クライアントPCのVNCビューワーでIPアドレスにポート5091を指定すれば接続できる。
(この辺りはRasbianで実施した内容と同じ)

自動化する場合は、vncサーバの起動スクリプトの実行スクリプトをOS起動時に実行するため、以下のスクリプトを/etc/init.d/vncbootとして作成する。

#! /bin/sh
# /etc/init.d/vncboot
USER=pi
HOME=/home/pi
export USER HOME
case "$1" in
start)
echo "Starting VNC Server"
#Insert your favoured settings for a VNC session
su $USER -c '/usr/bin/vncserver :1 -geometry 1440x900 -depth 24'
;;
stop)
echo "Stopping VNC Server"
su $USER -c '/usr/bin/vncserver -kill :1'
;;
*)
echo "Usage: /etc/init.d/vncboot {start|stop}"
exit 1
;;
esac
exit 0

その後、以下のコマンドでinit.dで起動する様に修正する。

sudo chmod 755 /etc/init.d/vncboot
sudo update-rc.d vncboot defaults

これで再起動すればOK

★[2018/5/13追記]
無線AP接続設定を変更する場合、表示されるSSIDを選択するだけではうまくいかなかった。「新しいWi-Fiネットワークを作成」メニューに進み、「モード:クライアント」「セキュリティ:WPA & WPA2 Personal」「IPV4設定:DHCP自動 or 手動設定」を選択すれば変更できる。

⑤ROSセットアップ

ROS Wiki
http://wiki.ros.org/ROSberryPi/Installing%20ROS%20Kinetic%20on%20the%20Raspberry%20Pi
にしたがってインストール。

$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
$ sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
$ sudo apt-get update
$ sudo apt-get upgrade
$ sudo apt-get install ros-kinetic-desktop-full
$ sudo rosdep init
$ rosdep update
$ echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc 
$ source ~/.bashrc

完了したらroscoreを起動する事を確認できればOK!

まとめ

Ubutu MATE+ROS環境が簡単に構築できてストレスなし。デスクトップ画面もカッコいいし。
前回に構築したOpen CV+Movidius環境がUbuntu MATEでも動くのかは不明だが、そこは慎重に見て行こう。

次回こそ、BOTの足回りの開発に着手できず、はず!

【簡単IoT】Node-REDを使ってAmazon echo で家電をON/OFFしてみた

動機

今年始めに購入したAmazon echo。残念ながら、天気予報と(ごくたまに)音楽再生する以外に使い道がなく、気がつけば電源すら抜かれてた始末(笑)
こりゃいかん!と思い、家電コントロールスキルを自作しようと企んでたのだが、ROSの勉強を優先させてしまい、放置状態。。

そんな中、昨日にNode-RED入門講座に遭遇して、「あれ、思ってた以上に簡単に作れそうかも!?」な感触を得たので、モチベーションの高いうちに早速取り組んでみた。

やりたいこと

amazon echoからリビングの「蛍光灯」「エアコン」「テレビ」「レコーダ」「扇風機」のON/OFFを音声操作する

・使い方想定1: 就寝直前にリビングの家電を消す場合。眠いのでリモコンを探すのが面倒。
・使い方想定2: 帰宅後にとっさに電気、エアコン等を付けたい場合。帰宅時はたいてい両手がふさがっているので、声で家電をONできると助かる。

実現するためのソフトウェア環境としては、Node-REDを使用する。Node-REDは、Webブラウザ上のGUIで簡単にWebサービスやハードウェアを接続するフローを書けるツール。
ラピッドプロト用ツールとして注目されている様なので、この使い方チュートリアルも踏まえてみた。

同様の事はIFTTTを使ってもできる様なのだが、その場合「Alexa、○○○をトリガー」という表現になるらしく、それが個人的には気に入らない。Node-REDスキルの場合、「でんきをつけて」「でんきをけして」という表現になりこちらの方がしっくりくるという理由もある。

ハードウェア

f:id:motokiinfinity8:20180416003411j:plain

ラズベリーパイ

 Node-REDが動いて、極力安価な環境なものを使う。
今回は家に余っていたRaspberry Pi2を使った(環境セットアップ済)
最終的には、コスト&省電力を考えてRaspberry Pi Zeroに移植する想定。

・赤外線送信機
 各家電に送信するための赤外線送信回路。時間短縮のため
既存モジュールを購入する方針。

 候補1: irMagician (大宮技研)
irMagician – 高機能/低価格赤外線リモコン | 大宮技研 合同会社
 よく使われている赤外線送信モジュール。USB(CDC-ACM)でホストに接続、リモコンコードの学習が可能。¥4000程度

候補2: ADSIRE学習リモコン基盤(Bit Trade One)
bit-trade-one.co.jp
 ラズパイ用シールドを想定した赤外線送信モジュール。10個の学習コードをGPIOトリガで送信可能。送信光を9個搭載して広視野角に対応。¥5000程度

 今回は、赤外線送信可能範囲と使いやすさ(GPIO制御のみで送信可)に注目して、【候補2:ADSIRE】を選んだ。

環境構築

Node-RED環境を構築する。こちらのサイトを参考。
ここを見ればほとんどやりたい事ができる。

「アレクサ、LEDつけて」でRaspberry PiからLEDを点灯する – oh-maker

ここにある
1. Alexa Home Skill Bridgeのアカウント作成 (web)
2. 制御対象となるデバイスを登録 (Web)
3. Node-REDのAlexaスキルの有効化 (スマホのAlexaアプリ)
4.Node-REDにAlexa Home Skill Bridgeノードを追加 (ラズパイ)
を実施していく。

はまったのは4の部分。Node-REDのGUI画面でどんだけ頑張っても、
f:id:motokiinfinity8:20180415234708p:plain
Alexa Home Skill Bridgeが見えない。。。

調べた結果、どうやらNode-REDのバージョンが古かった模様

update-nodejs-and-nodered 

でNode-REDを入れなおして、再度4を実施すれば無事に環境構築完了!

ちなみに、「○○をつけて」の○○はスマホのAlexaアプリから変更できます。
例えば、私は上記2のデバイス登録で「ライト」と名前をつけましたが、Alexaアプリでは「でんき」と変更しました。
これで、「Alexa、でんきをつけて」「Alexa、でんきをけして」という命令に反応してくれます。

ソフトウェア実装

実装したノードの全体イメージは以下
f:id:motokiinfinity8:20180416005716j:plain


一例として、光灯をつけるノードの拡大。
f:id:motokiinfinity8:20180416005744j:plain

1: Alexa-homeからのdeviceノード(この場合「ライト」)を措定
2: switchノードで msg.payload==true時と msg.payload==false時の分岐を記述
3: triggerノードでパルスを作成。本ボードでは赤外線送信は Lトリガのため、 0パルスで設定
4: rpi-gpio outノードで該当するGPIOピンにつなげる

こんな感じで比較的直感的に書ける!

学習基板の10個のリモコンコードはあらかじめ個別に学習させておく。
学習基板の取説通りの手順で、
SW1:電気ONコード
SW2:電気OFFコード
SW3:エアコンONコード ※季節に応じて「暖房」「冷房」を学習し直す必要あり
SW4:エアコンOFFコード
SW5 : テレビ電源ボタンコード
SW6 : レコーダ再生ボタンコード
SW7 : 扇風機電源ボタンコード

を配置してそれに対応したGPIOを4で制御する。
テレビ/扇風機などはON/OFFが同じ電源ボタンのため、ONとOFFの区別がつかない(つまり、テレビの電源が付いている状態で「テレビをつけて」というとテレビは消えてしまう)がそこは今回は妥協する。

実装結果

ここまでで約2時間で完成!

無事に、リビング電灯、エアコン、テレビ、レコーダをAmazon echoで操作できた!

早速、外出先からの帰宅時に使ってみたが、息子を抱っこしながら「Alexa、電気を付けて!」と言ったら即座に電気がつくとなんだか嬉しかった!
おもわずAlexaに「ありがとう!」と言うと、「よかったー♪」と嬉しそうに答えてくれて、ちょっと距離が近づいた気持ちになりました。

これからも使っていこう!

【アーム制御】CRANE+を使ったROSアーム制御②

ogimotokin.hatenablog.com

前回はMove it!を使ってアームを任意座標に持ってくるプログラムを作成したが、
残念ながら応答速度が非常に遅い事 & 移動できない座標位置が非常に多く現状使い物にならなかった。

なので、Move It!を使わずにCRANE+に適した形で逆運動学を解いてサーボモータ角度を直接制御するアプローチをトライしてみた。

CRANE+に対する逆運動学アプローチ

物理系は素人なので、まずは逆運動学(Inverse Kinematics)の算出方法の勉強から。

tajimarobotics.com

•順運動学 … 関節サーボの目標角度から指先の座標がどの様になるかを算出する問題
•逆運動学 … 指先の目標座標から間接サーボの角度を算出する問題

という事。解き方としては幾何学的にN個の連立方程式を解く問題が一般的な様ですね。当然ながら、複数解は発生するし、関節の多いアームほど計算が複雑になってくる訳で、なかなか素人では難しい。

しかし、このCRANE+はアーム部は4軸と自由度が少なめなので、これを利用して簡略化して考えてみた。アームの構成をおさらいすると、以下のサイトを参照頂きたい。

www.rt-shop.jp

まず、水平方向の制御はアーム根本のサーボモータ(Joint1)のみなので、水平方向(xy平面)の座標軸からサーボモータ角度は一意に決まる。一方、垂直方向を決めるサーボモータ数は3軸(つまり3変数)だが、2軸(Joint2/Joint3)をアーム位置制御用、1軸(Joint4)をアーム角度制御用という様に役割分担をすれば、単純化できそう!

という事でトライ!

f:id:motokiinfinity8:20180405235358j:plain
f:id:motokiinfinity8:20180405235653j:plain
f:id:motokiinfinity8:20180405235754j:plain



高校数学で勉強した余弦定理を使えば、とりあえず計算出来た!

これをコード実装した。一部抜粋。

#define L_B2 0.076	// base_link〜Shoulder(id2)のLink長さ
#define L_23 0.0825	// Shoulder(id2)~Elbow(id3)のLink長さ
#define L_34 0.094	// Elbow(id3)〜Wrist(id4)のLink長さ
#define L_4G 0.10	// Wrist(id4)〜Gripper_link先のLink長さ

void ArmCtrlTrack::timerCallback(const ros::TimerEvent&){
	// JOYCONからの操作情報をアップデート(ARM)
	if(pos_delta.x !=0 || pos_delta.y != 0 || pos_delta.z != 0){
		arm_pose.pose.position.x += pos_delta.x;
		arm_pose.pose.position.y += pos_delta.y;
		arm_pose.pose.position.z += pos_delta.z;
		ROS_WARN("[Manual]Move Arm_position[%f, %f, %f]", arm_pose.pose.position.x, arm_pose.pose.position.y, arm_pose.pose.position.z);
	}
	arm_MoveIK(arm_pose.pose.position, wrist_arg, &servo_arg);
}

void ArmCtrlTrack::arm_MoveIK(const geometry_msgs::Point gripper_position, const float wrist_arg, CraneARM_Servo_Arg *servo_arg){
	
	float theta_2d, theta_3d, theta_4d;
	float R_24 = sqrt(pow(gripper_position.x,2)+pow(gripper_position.y,2))-L_4G*cos(wrist_arg);
	float Z_24 = gripper_position.z + L_4G*sin(wrist_arg)- L_B2;

	theta_2d = acos((pow(L_23,2)-pow(L_34,2)+pow(R_24,2)+pow(Z_24,2))/(2*L_23*sqrt(pow(R_24,2)+pow(Z_24,2))));
	theta_3d = acos((pow(L_34,2)-pow(L_23,2)+pow(R_24,2)+pow(Z_24,2))/(2*L_34*sqrt(pow(R_24,2)+pow(Z_24,2))));
	theta_4d = atan2(Z_24, R_24);
	servo_arg->id2.data = theta_2d + atan2(Z_24, R_24)-M_PI/2;
	servo_arg->id3.data = theta_2d + theta_3d;
	servo_arg->id4.data = theta_4d-theta_3d+wrist_arg;

	servo_arg->id1.data = atan2(gripper_position.y, gripper_position.x);
	//ROS_WARN("[calc] id2:%f, id3:%f, id4:%f", servo_arg->id2.data, servo_arg->id3.data, servo_arg->id4.data);
	//ROS_WARN("[calc] id1:%f", servo_arg->id1.data);

	// サーボ角度Publish
	if(servo_arg->id4.data >= -M_PI/2 ||servo_arg->id4.data <= M_PI/2) joint4_pub.publish(servo_arg->id4);
	if(servo_arg->id3.data >= -M_PI   ||servo_arg->id3.data <= M_PI)   joint3_pub.publish(servo_arg->id3);
	if(servo_arg->id2.data >= -M_PI/2 ||servo_arg->id2.data <= M_PI/2) joint2_pub.publish(servo_arg->id2);
	joint1_pub.publish(servo_arg->id1);

	return;
}

解けない場合は servo_arg->idx.data = nan になるので、その場合はPublishさせない様にしないといけない点に注意。

実機動作結果

動画は以下
youtu.be

おぉ、MoveIt!使用時に比べて、座標指定に対する応答が早いっ!
PS4コントローラのアナログスティックの動きに追従してくれるので、もう少し作り込めばUFOキャッチャーみたいな操作感になりそう!


次回は、カメラによる物体認識機能も加えて、ペットボトルの位置を自動検出してボトルを把持するプログラムを作ってみよう!