ジョイスティックで産業用ロボットを操縦する方法

本記事でできること

動画

想定シーン

産業用ロボットをジョイスティックで操縦する

産業用ロボットはとてもパワフルな機械であるため、ロボットアーム本体から離れた位置から操縦したいという場合があります。通常の教示作業（ティーチング）の際には、ロボットに付属のティーチングペンダントで操作しますが、いかんせん動作速度が遅く、また、手動モード（教示モード）担っているために、他のアプリケーションとの連動ができません。そこで、本記事では、ゲームなどに使用されるジョイスティックやゲームコントローラを使用してロボットを遠隔操縦する方法を掲載します。

実行環境

• OS：Ubuntu 22.04 LTS
• 言語：C++
• クルーボ：v5.1.0

使用機材

• 産業用ロボット（お好きなメーカ）
• ジョイスティック、ゲームパッド

写真：ジョイスティック

写真：ゲームパッド例

クルーボについて

ロボットアプリケーション開発には、株式会社チトセロボティクスのロボット制御ソフトウェア「クルーボ」を使用します。本記事のプログラムは、クルーボがインストールされた制御コンピュータ上で動作します。

クルーボの製品サイト：https://chitose-robotics.com/product

Ubuntuでジョイスティック、ゲームパッドを扱えるようにする

joystick, jstest のインストール

$ sudo apt install joystick
$ sudo apt install jstest-gtk
$ jstest /dev/input/js0

これで端末でジョイスティックの動きを確認することができます。

もし、GUIのソフトウェアで確認したい場合には、qjoypad を使用することをおすすめします。

$ sudo apt install qjoypad

全体プログラム

joystick.h

#pragma once

#include <iostream>
#include <vector>

namespace crewbo::joystick {
class JoyStick {
public:
    JoyStick(const char* device_file, const int max_abs_value = 1);
    ~JoyStick();
    void readValue();
    std::vector<int> axisValue();
    std::vector<int> buttonValue();

private:
    int joy_fd;
    int num_of_axis;
    int num_of_buttons;
    float max_abs_value_for_percentage;
    char name_of_joystick[80];
    std::vector<int> joy_axis;
    std::vector<int> joy_button;
};

}  // namespace crewbo::joystick

joystick.cpp

#include <fcntl.h>
#include <linux/joystick.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <iomanip>
#include <iostream>

#include "project_lib/joystick.h"

namespace crewbo::joystick {

JoyStick::JoyStick(const char* device_file, const int max_abs_value) {
    if ((this->joy_fd = open(device_file, O_RDONLY)) < 0) {
        std::cerr << "Error in crewbo::joystick::JoyStick::JoyStick(): デバイスをオープンできません。" << device_file;
        return;
    }

    this->num_of_axis = 0;
    this->num_of_buttons = 0;
    if (max_abs_value > 1) {
        this->max_abs_value_for_percentage = static_cast<float>(max_abs_value) / 100.f;
    } else {
        this->max_abs_value_for_percentage = 1;
    }

    ioctl(this->joy_fd, JSIOCGAXES, &this->num_of_axis);
    ioctl(this->joy_fd, JSIOCGBUTTONS, &this->num_of_buttons);
    ioctl(this->joy_fd, JSIOCGNAME(80), &this->name_of_joystick);

    this->joy_button.resize(num_of_buttons, 0);
    this->joy_axis.resize(num_of_axis, 0);

    std::cout << "Joystick: " << this->name_of_joystick << std::endl;
    std::cout << this->num_of_axis << " - axis, " << this->num_of_buttons << " - buttons" << std::endl;
    fcntl(joy_fd, F_SETFL, O_NONBLOCK);  // using non-blocking mode
}

JoyStick::~JoyStick() { close(this->joy_fd); }

void JoyStick::readValue() {
    js_event js;

    read(this->joy_fd, &js, sizeof(js_event));

    switch (js.type & ~JS_EVENT_INIT) {
        case JS_EVENT_AXIS:
            if ((int)js.number >= this->joy_axis.size()) {
                std::cerr << "err:" << (int)js.number << std::endl;
            }
            this->joy_axis[(int)js.number] =
                    static_cast<int>(static_cast<float>(js.value) / this->max_abs_value_for_percentage);
            break;
        case JS_EVENT_BUTTON:
            if ((int)js.number >= this->joy_button.size()) {
                std::cerr << "err:" << (int)js.number << std::endl;
            }
            this->joy_button[(int)js.number] = js.value;
            break;
        default:
            break;
    }

    usleep(100);
}

std::vector<int> JoyStick::axisValue() { return this->joy_axis; }

std::vector<int> JoyStick::buttonValue() { return this->joy_button; }

}  // namespace crewbo::joystick

robot_control_with_gamepad.cpp

#include <fmt/format.h>
#include <iostream>
#include <optional>
#include <string>
#include "crewbo/crewbo.h"
#include "project_lib/joystick.h"
#include "project_lib/vision_assistant.h"

int main() {
    system("start_melfa");

    // カメラに接続する
    crewbo::camera::RealSense camera;
    va::Display display("sample");

    // ジョイスティックに接続する。
    const char* device_file = "/dev/input/js0";
    const int max_abs_value = 32767 - 1;
    crewbo::joystick::JoyStick JoyStick(device_file, max_abs_value);
    std::vector<int> joystick_axis;
    std::vector<int> joystick_button;

    // ロボットアームに接続する。
    crewbo::robot::melfa::base_kit::MelfaRv4frl robot;
    using namespace crewbo::literals;

    SPDLOG_INFO("ロボットを操作できます。ボタン[13]で終了します。");
    SPDLOG_INFO("左スティック = X,Y / J2, J1");
    SPDLOG_INFO("右スティック = Z,C / J3, J4");
    SPDLOG_INFO("十字操作キー = A,B / J5, J6");
    SPDLOG_INFO("ボタン1: 低速モード, ボタン2: 高速モード, ボタン3: POSEモード, ボタン4: JOINTモード");
    bool is_operatable = true;
    float override_value = 0.1f;
    bool pose_mode = true;

    while (is_operatable == true) {
        if (pose_mode == true) {
            crewbo::robot::concurrent_updater::ConcurrentPoseUpdater updater(&robot, 0.2f, 1e-3f);

            while (true) {
                const cv::Mat camera_image = camera.fetchSingleFrame_();
                display.show(camera_image);

                crewbo::Pose current_pose = robot.getCurrentPose_();
                std::cout << "POSE : " << current_pose << std::endl;

                JoyStick.readValue();
                joystick_axis = JoyStick.axisValue();
                joystick_button = JoyStick.buttonValue();

                if (joystick_button[12]) {
                    is_operatable = false;
                    break;
                }
                if (joystick_button[0]) {
                    override_value = 0.1f;
                    std::cout << "override : " << override_value << std::endl;
                }
                if (joystick_button[1]) {
                    override_value = 0.5f;
                    std::cout << "override : " << override_value << std::endl;
                }
                if (joystick_button[2]) {
                    pose_mode = true;
                    break;
                }
                if (joystick_button[3]) {
                    pose_mode = false;
                    break;
                }

                const float error_x = static_cast<float>(-joystick_axis[1]);
                const float error_y = static_cast<float>(joystick_axis[0]);
                const float error_z = static_cast<float>(-joystick_axis[2]);
                const float error_ra = static_cast<float>(joystick_axis[3]);
                const float error_rb = static_cast<float>(joystick_axis[5]);
                const float error_rc = static_cast<float>(joystick_axis[4]);
                const crewbo::Degrees error_ra_degree{error_ra};
                const crewbo::Degrees error_rb_degree{error_rb};
                const crewbo::Degrees error_rc_degree{error_rc};

                const float p_gain_for_position = 0.05f * override_value;
                const float input_x = p_gain_for_position * error_x;
                const float input_y = p_gain_for_position * error_y;
                const float input_z = p_gain_for_position * error_z;

                const float p_gain_for_rotation = 0.01f * override_value;
                const crewbo::Degrees input_ra = p_gain_for_rotation * error_ra_degree;
                const crewbo::Degrees input_rb = p_gain_for_rotation * error_rb_degree;
                const crewbo::Degrees input_rc = p_gain_for_rotation * error_rc_degree;

                const crewbo::Pose input_rel_pose{input_x, input_y, input_z, input_ra, input_rb, input_rc};
                updater.updateGoalByRelPose_(input_rel_pose);
            }
        } else {
            crewbo::robot::concurrent_updater::ConcurrentJointUpdater updater(&robot, 0.2f, 1e-3f);

            while (true) {
                const cv::Mat camera_image = camera.fetchSingleFrame_();
                display.show(camera_image);

                crewbo::Joint current_joint = robot.getCurrentJoint_();
                std::cout << "JOINT: " << current_joint << std::endl;

                JoyStick.readValue();
                joystick_axis = JoyStick.axisValue();
                joystick_button = JoyStick.buttonValue();

                if (joystick_button[12]) {
                    is_operatable = false;
                    break;
                }
                if (joystick_button[0]) {
                    override_value = 0.1f;
                    std::cout << "override : " << override_value << std::endl;
                }
                if (joystick_button[1]) {
                    override_value = 0.5f;
                    std::cout << "override : " << override_value << std::endl;
                }
                if (joystick_button[2]) {
                    pose_mode = true;
                    break;
                }
                if (joystick_button[3]) {
                    pose_mode = false;
                    break;
                }

                const float error_j1 = static_cast<float>(joystick_axis[0]);
                const float error_j2 = static_cast<float>(joystick_axis[1]);
                const float error_j3 = static_cast<float>(joystick_axis[2]);
                const float error_j4 = static_cast<float>(joystick_axis[3]);
                const float error_j5 = static_cast<float>(joystick_axis[5]);
                const float error_j6 = static_cast<float>(joystick_axis[4]);
                const crewbo::Degrees error_j1_degree{error_j1};
                const crewbo::Degrees error_j2_degree{error_j2};
                const crewbo::Degrees error_j3_degree{error_j3};
                const crewbo::Degrees error_j4_degree{error_j4};
                const crewbo::Degrees error_j5_degree{error_j5};
                const crewbo::Degrees error_j6_degree{error_j6};

                const float p_gain_for_rotation = 0.01f * override_value;
                const crewbo::Degrees input_j1 = p_gain_for_rotation * error_j1_degree;
                const crewbo::Degrees input_j2 = p_gain_for_rotation * error_j2_degree;
                const crewbo::Degrees input_j3 = p_gain_for_rotation * error_j3_degree;
                const crewbo::Degrees input_j4 = p_gain_for_rotation * error_j4_degree;
                const crewbo::Degrees input_j5 = p_gain_for_rotation * error_j5_degree;
                const crewbo::Degrees input_j6 = p_gain_for_rotation * error_j6_degree;

                const crewbo::Joint input_rel_joint{input_j1, input_j2, input_j3, input_j4, input_j5, input_j6};
                updater.updateGoalByRel_(input_rel_joint);
            }
        }
    }
}

おわりに

人手作業をロボットアームで自動化するために、カメラを使ったロボット制御＝ビジュアルフィードバック制御が大切です。さらに今回は、ロボットを操縦できるようにするプログラムを作成しました。カメラの入力の代わりにジョイスティックを使用することで、ロボットアプリケーションの実装の幅を広げることができます。