LoginSignup
1
0
@IronNyanin株式会社チトセロボティクス

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

Last updated at Posted at 2024-04-09

本記事でできること

image.png

動画

想定シーン

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

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

実行環境

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

使用機材

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

写真：ジョイスティック

image.png

写真：ゲームパッド例

image.png

クルーボについて

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

クルーボの製品サイト：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);
            }
        }
    }
}

おわりに

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

1
0
1

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
0