cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

メモ:ur_rtdeに独自関数を追加したいときの方法

URロボットの通信用インターフェースの一つであるRTDEでは、urscriptやその入出力の変数を自前で定義して、呼び出してやる必要があります。URは、入出力のデータの並びの定義をrecipeと呼んでいます。わかってしまえば、たいして難しくないものの、その仕組みはちょっとヤヤコシイので、ur_rtdeに自前の関数を追加したいときのメモを残しておくことにします。

編集する必要があるファイルは、
1. rtde_control.script
2. rtde.h
3. rtde_control.h
4. rtde_control.cpp
です。
rtde_control.scriptにはURScript形式で記述したロボットコマンドを追加します。
rtde.hには、RobotCommandのTypeとRecipeの定義を追加します。
rtde_control.hには、自前スクリプトの呼び出し関数の定義を追加し、rtde_control.cppには、自前スクリプトの呼び出し関数の定義を記述します。入力変数と、出力変数の受け取りも記述します。
定義や関数を追加した後に、cmakeからur_rtdeの再ビルドを行います。

is_within_safety_limits()をコマンド33に追加するときの例

rtde_control.script

    def process_cmd():
        cmd = read_input_integer_register(0)

        if cmd == 1:
            textmsg("movej")
            q = [0, 0, 0, 0, 0, 0]
            q[0] = read_input_float_register(0)

~中略~

        elif cmd == 33:
            textmsg("is_within_safety_limits")
            x = p[0, 0, 0, 0, 0, 0]
            x[0] = read_input_float_register(0)
            x[1] = read_input_float_register(1)
            x[2] = read_input_float_register(2)
            x[3] = read_input_float_register(3)
            x[4] = read_input_float_register(4)
            x[5] = read_input_float_register(5)
            
            val = is_within_safety_limits(x)
            if val == False:
                        write_output_integer_register(1, 0)
            else:
                        write_output_integer_register(1, 1)
            end
            textmsg("is_within_safety_limits done")
        elif cmd == 255:
            textmsg("Received stop!")
        end

        return cmd != 255
    end

rtde.h

  class RobotCommand
  {
   public:
    enum Type
    {
      NO_CMD = 0,

~中略~

      GET_INVERSE_KINEMATICS = 31,
      PROTECTIVE_STOP = 32,
      IS_WITHIN_SAFETY_LIMITS = 33, ///// added
      STOP = 255
    };

    enum Recipe
    {
      RECIPE_1 = 1,

~中略~

      RECIPE_11 = 11,
      RECIPE_12 = 12 ///// added
    };

rtde_control.h

~前略~

  RTDE_EXPORT std::vector<double> getInverseKinematics(const std::vector<double> &x, const std::vector<double> &qnear,
      double max_position_error=1e-10, double max_orientation_error=1e-10);

  RTDE_EXPORT int getIsWithinSafetyLimits(const std::vector<double> &x); ///// added

  /**
    * @brief Triggers a protective stop on the robot. Can be used for testing and debugging.
    */
  RTDE_EXPORT bool triggerProtectiveStop();

~中略~
  private:
    int getIsWithinSafetyLimitsValue(); ///// added

~後略~

rtde_control.cpp

RTDEControlInterface::RTDEControlInterface(std::string hostname, int port) : hostname_(std::move(hostname)), port_(port)
{
 ~前略~

  // Setup input recipes

  ~中略~

  // Recipe_12
  std::vector<std::string> is_within_safety_limits_input = {
      "input_int_register_0",    "input_double_register_0", "input_double_register_1", "input_double_register_2",
      "input_double_register_3", "input_double_register_4", "input_double_register_5"};
  rtde_->sendInputSetup(is_within_safety_limits_input);

 ~後略~
  }
}


bool RTDEControlInterface::reconnect()
{
~前略~

  // Setup input recipes
  // Recipe 1

~中略~

  // Recipe_12
  std::vector<std::string> is_within_safety_limits_input = {
      "input_int_register_0",    "input_double_register_0", "input_double_register_1", "input_double_register_2",
      "input_double_register_3", "input_double_register_4", "input_double_register_5"};
  rtde_->sendInputSetup(is_within_safety_limits_input);

  // Init Robot state
 ~後略~
}

int RTDEControlInterface::getIsWithinSafetyLimitsValue()
{
  if (robot_state_ != nullptr)
  {
    return robot_state_->getOutput_int_register_1();
  }
  else
  {
    throw std::logic_error("Please initialize the RobotState, before using it!");
  }
}

int RTDEControlInterface::getIsWithinSafetyLimits(const std::vector<double> &x)
{
  RTDE::RobotCommand robot_cmd;
  robot_cmd.type_ = RTDE::RobotCommand::Type::IS_WITHIN_SAFETY_LIMITS;
  robot_cmd.recipe_id_ = RTDE::RobotCommand::Recipe::RECIPE_12;
  robot_cmd.val_ = x;
  if (sendCommand(robot_cmd))
  {
    return getIsWithinSafetyLimitsValue();
  }
  else
  {
    return -1;
  }
}

double型のベクターを渡したいだけなのに、頭に "input_int_register_0" がついているのは、ベクターの大きさを暗黙のうちに渡すような仕組みになっているからです。明示的には渡しません。

実行例

#include <ur_rtde/rtde_control_interface.h>
#include <thread>
#include <chrono>
#include <iostream>

using namespace ur_rtde;

int main(int argc, char* argv[])
{
  RTDEControlInterface rtde_control("192.168.0.1");

  // int movec_mode = 0;
  std::vector<double> tcp_pose1 = {-0.143, -0.435, 0.20, -0.001, 3.12, 0.04};
  std::vector<double> tcp_pose2 = {-0.143, -0.51, 0.21, -0.001, 3.12, 0.04};
  std::vector<double> tcp_pose3 = {0.4, 0.4, 10.0, 0.0, 3.14159, 0.0};

  //rtde_control.getInverseKinematics();
  std::cout << "getIsWithinSafetyLimits" << std::endl;
  bool result;
  result = rtde_control.getIsWithinSafetyLimits(tcp_pose1);
  std::cout << result << std::endl;
  result = rtde_control.getIsWithinSafetyLimits(tcp_pose2);
  std::cout << result << std::endl;
  result = rtde_control.getIsWithinSafetyLimits(tcp_pose3);
  std::cout << result << std::endl;

  return 0;
}

今日の本文

大きなテレビが安い。

今なら同じSONYの4kチューナーを買って、ソニーに申請すると3万円返ってくる。実質4Kチューナーをタダでもらえたうえで300円得する計算になります。

www.sony.jp