メモ: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; }
今日の本文
大きなテレビが安い。
ソニー 65V型地上・BS・110度CSデジタル4K対応 LED液晶テレビ(別売USB HDD録画対応)Android TV 機能搭載BRAVIA KJ-65X8000G
- 発売日: 2019/05/22
- メディア: エレクトロニクス
ソニー 4Kチューナー BS/CS4K 地上デジタル 裏録対応 ダブルチューナー HDMIケーブル付属 DST-SHV1
- 発売日: 2018/11/10
- メディア: エレクトロニクス