diff --git a/.github/workflows/build_test.yaml b/.github/workflows/build_test.yaml index a8bf936..6431838 100644 --- a/.github/workflows/build_test.yaml +++ b/.github/workflows/build_test.yaml @@ -2,13 +2,15 @@ name: BuildAndTest on: push: + branches: + - main paths-ignore: - '**.md' pull_request: + branches: + - main paths-ignore: - - '**.md' - schedule: - - cron: "0 2 * * 0" # Weekly on Sundays at 02:00 + - '**.md' jobs: lint: diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml new file mode 100644 index 0000000..377ba28 --- /dev/null +++ b/.github/workflows/industrial_ci.yaml @@ -0,0 +1,28 @@ +name: industrial_ci + +on: + push: + paths-ignore: + - 'docs/**' + - '**.md' + pull_request: + paths-ignore: + - 'docs/**' + - '**.md' + schedule: + - cron: "0 2 * * 0" # Weekly on Sundays at 02:00 + +jobs: + industrial_ci: + strategy: + matrix: + env: + - { ROS_DISTRO: foxy, ROS_REPO: ros } + - { ROS_DISTRO: galactic, ROS_REPO: ros } + - { ROS_DISTRO: humble, ROS_REPO: ros } + - { ROS_DISTRO: rolling, ROS_REPO: ros } + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: "ros-industrial/industrial_ci@master" + env: ${{ matrix.env }} \ No newline at end of file diff --git a/README.md b/README.md index 52ea67b..379fd06 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # RTマニピュレータC++ライブラリとサンプル集（rt_manipulators_cpp） -[![BuildAndTest](https://github.com/rt-net/rt_manipulators_cpp/actions/workflows/build_test.yaml/badge.svg)](https://github.com/rt-net/rt_manipulators_cpp/actions/workflows/build_test.yaml) +[![industrial_ci](https://github.com/rt-net/rt_manipulators_cpp/actions/workflows/industrial_ci.yaml/badge.svg?branch=ros2)](https://github.com/rt-net/rt_manipulators_cpp/actions/workflows/industrial_ci.yaml) 本リポジトリは、株式会社アールティが販売している [アームロボット**CRANE-X7(クラインエックスセブン)**](https://rt-net.jp/products/crane-x7/) @@ -11,87 +11,25 @@ [](https://rt-net.jp/products/crane-x7/) [](https://rt-net.jp/products/sciurus17) -[![gravity_compensation](https://rt-net.github.io/images/crane-x7/x7_s17_gravity_compensation.gif)](./samples/samples03/README.md) - ## 動作環境 -- CMake (>= 3.1.0) -- g++ (>= 7.5.0) -- [DYNAMIXEL SDK](https://github.com/ROBOTIS-GIT/DynamixelSDK) (Linux 64bit向けにビルド&インストール) -- [yaml-cpp (>= 0.5.0)](https://github.com/jbeder/yaml-cpp) -- Eigen (>= 3.3.4) -- Linux OS - - Ubuntu 18.04 - - Ubuntu 20.04 -- ロボット - - [CRANE-X7](https://rt-net.jp/products/crane-x7/) - - [Sciurus17](https://rt-net.jp/products/sciurus17/) +- ROS Foxy +- ROS Galactic +- ROS Humble +- ROS Rolling ## インストール方法 -### DYNAMIXEL SDKのインストール - -[DYNAMIXEL SDKのe-manual](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/library_setup/cpp_linux/#cpp-linux) -を参考に、Linux 64bit環境用にDYNAMIXEL SDKをビルド＆インストールします。 - -```sh -$ sudo apt install build-essential -$ cd ~ -$ git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git -$ cd ~/DynamixelSDK/c++/build/linux64 -$ make -$ sudo make install -``` - -### その他依存関係のインストール - -次のコマンドを実行します。 - -```sh -$ sudo apt install libyaml-cpp-dev libeigen3-dev cmake -``` - ### RTマニピュレータC++ライブラリのビルド&インストール -詳細は[rt_manipulators_lib/README.md](./rt_manipulators_lib/README.md)を参照してください。 - ```sh -$ cd ~ -$ git clone https://github.com/rt-net/rt_manipulators_cpp -$ cd rt_manipulators_cpp/rt_manipulators_lib -$ ./build_install_library.bash +$ cd ~/ros2_ws/src # workspaceを~/ros2_wsに作成している場合 +$ git clone -b ros2 https://github.com/rt-net/rt_manipulators_cpp +$ rosdep install -r -y --from-paths . --ignore-src +$ cd .. +$ colcon build --symlink-install ``` -## サンプルプログラムの実行 - -詳細は[samples/README.md](./samples/README.md)を参照してください。 - -```sh -$ cd rt_manipulators_cpp/samples/samples01 -$ ./build_samples.bash - -$ cd bin/ -$ ./x7_onoff -CRANE-X7のトルクをON/OFFするサンプルです. -CRANE-X7(ポート:/dev/ttyUSB0 ボーレート:3000000)に接続します. -コンフィグファイル:../config/crane-x7.yamlを読み込みます. -Config file '../config/crane-x7.yaml' loaded. -arm - joint1, id:2, mode:3 - joint2, id:3, mode:3 - joint3, id:4, mode:3 - joint4, id:5, mode:3 - joint5, id:6, mode:3 - joint6, id:7, mode:3 - joint7, id:8, mode:3 -hand - joint_hand, id:9, mode:3 -サーボグループ:armのトルクをONにします. -... -``` - -[![](https://img.youtube.com/vi/cA_3HU3HfcM/sddefault.jpg)](https://youtu.be/cA_3HU3HfcM) - ## 免責事項 当該製品および当ソフトウェアの使用中に生じたいかなる損害も株式会社アールティでは一切の責任を負いかねます。 diff --git a/rt_manipulators_examples/CMakeLists.txt b/rt_manipulators_examples/CMakeLists.txt new file mode 100644 index 0000000..e8b53da --- /dev/null +++ b/rt_manipulators_examples/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.5) +project(rt_manipulators_examples) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rt_manipulators_cpp REQUIRED) + +add_executable(x7_forward_kinematics src/x7_forward_kinematics.cpp) +ament_target_dependencies(x7_forward_kinematics + rclcpp + rt_manipulators_cpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +install(TARGETS x7_forward_kinematics + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/rt_manipulators_examples/README.md b/rt_manipulators_examples/README.md new file mode 100644 index 0000000..1993e63 --- /dev/null +++ b/rt_manipulators_examples/README.md @@ -0,0 +1,11 @@ +# RT Manipulators Examples + +## CRANE-X7 Forward Kinematics + +これは +[rt_manipulators_cppのサンプル](https://github.com/rt-net/rt_manipulators_cpp/blob/v1.1.2/samples/samples02/src/x7_forward_kinematics.cpp) +をROS 2用に変換したものです + +```sh +$ ros2 launch rt_manipulators_examples example.launch.py example:=x7_forward_kinematics +``` diff --git a/samples/samples01/config/crane-x7.yaml b/rt_manipulators_examples/config/crane-x7.yaml similarity index 100% rename from samples/samples01/config/crane-x7.yaml rename to rt_manipulators_examples/config/crane-x7.yaml diff --git a/samples/samples02/config/crane-x7_links.csv b/rt_manipulators_examples/config/crane-x7_links.csv similarity index 99% rename from samples/samples02/config/crane-x7_links.csv rename to rt_manipulators_examples/config/crane-x7_links.csv index e20e7a6..645bd88 100644 --- a/samples/samples02/config/crane-x7_links.csv +++ b/rt_manipulators_examples/config/crane-x7_links.csv @@ -1,13 +1,13 @@ -リンク名称,ID,,,,ワールド座標系ホームポジション位置,,,ワールド座標系リンク間相対距離,,,ワールド座標系ホームポジション姿勢,,,質量[g],ローカル座標系質量位置,,,ローカル座標系慣性テンソル(重心),,,,,,ハードウェア可動範囲,,角度制限設定済可動範囲,,ホームポジション角度,回転軸方向,サーボモータ種類,トルク定数,サーボモータID,サーボモータ可動範囲,,通信速度,Return Delay Time,Operation Mode,Pゲイン,Iゲイン,Dゲイン -,自分リンク,妹リンク,子リンク,親リンク,x[mm],y[mm],z[mm],x[mm],y[mm],z[mm],X[deg],Y[deg],Z[deg],m[g],x[mm],y[mm],z[mm],Ixx[gmm^2],Ixy[gmm^2],Iyy[gmm^2],Ixz[gmm^2],Iyz[gmm^2],Izz[gmm^2],θ1[deg],θ2[deg],θ1[deg],θ2[deg],サーボ指令値[deg],±XYZ,型番,[Nm/A],ID,θ1[-],θ2[-],bps,-,,-,-,- -CRANE-X7_Base,1,-,2,-,0,0,0,0,0,0,0,0,0,388,-6.734,-0.001,17.921,324812.981,1.195,482993.163,-47122.898,19.475,585569.734,-,-,-,-,-,-,-,,-,-,-,-,-,,-,-,- -CRANE-X7_Link1,2,-,3,1,0,0,41,0,0,41,0,0,0,253,0.243,-0.105,46.95,178706.106,-73.234,176299.332,888.676,623.013,89586.08,-160,160,-157,157,180,Z+,XM430-W350,1.783,2,262,3834,3Mbps,0,3,1000,300,200 -CRANE-X7_Link2,3,-,4,2,0,0,105,0,0,64,90,0,0,136,0.031,34.172,0.019,168092.477,20.51,96197.568,-400.417,62.417,134475.028,-90,90,-90,90,180,Y-,XM540-W270,2.409,3,1024,3072,3Mbps,0,3,2200,500,500 -CRANE-X7_Link3,4,-,5,3,0,0,170,0,0,65,0,0,0,321,-13.722,-0.081,95.202,1819759.341,753.977,1883280.126,-37299.429,4147.003,158644.809,-160,160,-157,157,180,Z+,XM430-W350,1.783,4,262,3834,3Mbps,0,3,3000,800,1000 -CRANE-X7_Link4,5,-,6,4,0,0,355,0,0,185,90,0,0,222,-9.388,80.39,0.064,488905.787,-3904.567,114431.658,82.949,2823.368,502575.787,-160,0,-160,0,180,Y-,XM430-W350,1.783,5,228,2048,3Mbps,0,3,1600,400,400 -CRANE-X7_Link5,6,-,7,5,0,0,476,0,0,121,0,0,0,207,-0.084,0.462,63.718,290227.705,100.249,257036.585,-476.631,-16861.834,87512.332,-160,160,-157,157,180,Z+,XM430-W350,1.783,6,262,3834,3Mbps,0,3,800,200,200 -CRANE-X7_Link6,7,-,8,6,0,0,605,0,0,129,90,0,0,140,6.1,-3.549,0.826,40139.362,-2908.265,61345.454,639.429,-489.787,73485.846,-90,90,-90,90,180,Y-,XM430-W350,1.783,7,1024,3072,3Mbps,0,3,800,100,200 -CRANE-X7_Link7,8,-,9,7,0,0,624,0,0,19,0,0,0,121,0.108,-1.034,20.848,32035.813,-94.503,41433.837,261.225,388.481,42899.559,-170,170,-167,167,180,Z+,XM430-W350,1.783,8,148,3948,3Mbps,0,3,800,100,200 -CRANE-X7_HandA,9,10,-,8,-12,0,648,-12,0,24,90,0,0,15.8,-2.647,26.904,3.541,10056.667,-197.178,4954.751,-98.854,536.803,6239.775,0,90,-5,90,180,Y-,XM430-W350,1.783,9,1991,3072,3Mbps,0,3,-,-,- -CRANE-X7_HandB,10,-,-,8,12,0,648,12,0,24,90,0,0,13.9,3.008,30.851,2.845,7483.7,361.284,4088.581,63.385,228.226,4372.991,0,90,-5,90,180,Y+,-,,-,-,-,-,-,,-,-,- +リンク名称,ID,,,,ワールド座標系ホームポジション位置,,,ワールド座標系リンク間相対距離,,,ワールド座標系ホームポジション姿勢,,,質量[g],ローカル座標系質量位置,,,ローカル座標系慣性テンソル(重心),,,,,,ハードウェア可動範囲,,角度制限設定済可動範囲,,ホームポジション角度,回転軸方向,サーボモータ種類,トルク定数,サーボモータID,サーボモータ可動範囲,,通信速度,Return Delay Time,Operation Mode,Pゲイン,Iゲイン,Dゲイン +,自分リンク,妹リンク,子リンク,親リンク,x[mm],y[mm],z[mm],x[mm],y[mm],z[mm],X[deg],Y[deg],Z[deg],m[g],x[mm],y[mm],z[mm],Ixx[gmm^2],Ixy[gmm^2],Iyy[gmm^2],Ixz[gmm^2],Iyz[gmm^2],Izz[gmm^2],θ1[deg],θ2[deg],θ1[deg],θ2[deg],サーボ指令値[deg],±XYZ,型番,[Nm/A],ID,θ1[-],θ2[-],bps,-,,-,-,- +CRANE-X7_Base,1,-,2,-,0,0,0,0,0,0,0,0,0,388,-6.734,-0.001,17.921,324812.981,1.195,482993.163,-47122.898,19.475,585569.734,-,-,-,-,-,-,-,,-,-,-,-,-,,-,-,- +CRANE-X7_Link1,2,-,3,1,0,0,41,0,0,41,0,0,0,253,0.243,-0.105,46.95,178706.106,-73.234,176299.332,888.676,623.013,89586.08,-160,160,-157,157,180,Z+,XM430-W350,1.783,2,262,3834,3Mbps,0,3,1000,300,200 +CRANE-X7_Link2,3,-,4,2,0,0,105,0,0,64,90,0,0,136,0.031,34.172,0.019,168092.477,20.51,96197.568,-400.417,62.417,134475.028,-90,90,-90,90,180,Y-,XM540-W270,2.409,3,1024,3072,3Mbps,0,3,2200,500,500 +CRANE-X7_Link3,4,-,5,3,0,0,170,0,0,65,0,0,0,321,-13.722,-0.081,95.202,1819759.341,753.977,1883280.126,-37299.429,4147.003,158644.809,-160,160,-157,157,180,Z+,XM430-W350,1.783,4,262,3834,3Mbps,0,3,3000,800,1000 +CRANE-X7_Link4,5,-,6,4,0,0,355,0,0,185,90,0,0,222,-9.388,80.39,0.064,488905.787,-3904.567,114431.658,82.949,2823.368,502575.787,-160,0,-160,0,180,Y-,XM430-W350,1.783,5,228,2048,3Mbps,0,3,1600,400,400 +CRANE-X7_Link5,6,-,7,5,0,0,476,0,0,121,0,0,0,207,-0.084,0.462,63.718,290227.705,100.249,257036.585,-476.631,-16861.834,87512.332,-160,160,-157,157,180,Z+,XM430-W350,1.783,6,262,3834,3Mbps,0,3,800,200,200 +CRANE-X7_Link6,7,-,8,6,0,0,605,0,0,129,90,0,0,140,6.1,-3.549,0.826,40139.362,-2908.265,61345.454,639.429,-489.787,73485.846,-90,90,-90,90,180,Y-,XM430-W350,1.783,7,1024,3072,3Mbps,0,3,800,100,200 +CRANE-X7_Link7,8,-,9,7,0,0,624,0,0,19,0,0,0,121,0.108,-1.034,20.848,32035.813,-94.503,41433.837,261.225,388.481,42899.559,-170,170,-167,167,180,Z+,XM430-W350,1.783,8,148,3948,3Mbps,0,3,800,100,200 +CRANE-X7_HandA,9,10,-,8,-12,0,648,-12,0,24,90,0,0,15.8,-2.647,26.904,3.541,10056.667,-197.178,4954.751,-98.854,536.803,6239.775,0,90,-5,90,180,Y-,XM430-W350,1.783,9,1991,3072,3Mbps,0,3,-,-,- +CRANE-X7_HandB,10,-,-,8,12,0,648,12,0,24,90,0,0,13.9,3.008,30.851,2.845,7483.7,361.284,4088.581,63.385,228.226,4372.991,0,90,-5,90,180,Y+,-,,-,-,-,-,-,,-,-,- 右手系、正面はX方向,,,,,,,,,, ,,,,,,,,,,,,,,,,, c,,,,,,,,,,,,, \ No newline at end of file diff --git a/rt_manipulators_examples/launch/example.launch.py b/rt_manipulators_examples/launch/example.launch.py new file mode 100644 index 0000000..5395e3a --- /dev/null +++ b/rt_manipulators_examples/launch/example.launch.py @@ -0,0 +1,58 @@ +# Copyright 2022 RT Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + declare_example_name = DeclareLaunchArgument( + 'example', default_value='x7_forward_kinematics', + description=('Set an example executable name: ' + '[x7_forward_kinematics]') + ) + + config_file_path = os.path.join( + get_package_share_directory('rt_manipulators_examples'), + 'config', + 'crane-x7.yaml' + ) + + link_file_path = os.path.join( + get_package_share_directory('rt_manipulators_examples'), + 'config', + 'crane-x7_links.csv' + ) + + example_node = Node(name=[LaunchConfiguration('example')], + package='rt_manipulators_examples', + executable=LaunchConfiguration('example'), + output='screen', + parameters=[{ + 'port_name': '/dev/ttyUSB0', + 'baudrate': 3000000, + 'config_file_path': config_file_path, + 'link_file_path': link_file_path, + }]) + + ld = LaunchDescription() + ld.add_action(declare_example_name) + ld.add_action(example_node) + + return ld diff --git a/rt_manipulators_examples/package.xml b/rt_manipulators_examples/package.xml new file mode 100644 index 0000000..1831a6b --- /dev/null +++ b/rt_manipulators_examples/package.xml @@ -0,0 +1,23 @@ + + + + rt_manipulators_examples + 0.0.0 + Examples for RT Manipulators C++ Library + RT Corporation + Apache License 2.0 + + ShotaAk + + ament_cmake + + rclcpp + rt_manipulators_cpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rt_manipulators_examples/src/x7_forward_kinematics.cpp b/rt_manipulators_examples/src/x7_forward_kinematics.cpp new file mode 100644 index 0000000..af5ffcb --- /dev/null +++ b/rt_manipulators_examples/src/x7_forward_kinematics.cpp @@ -0,0 +1,125 @@ +// Copyright 2022 RT Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// これはrt_manipualtors_cppのサンプルをROS 2用に変換したものです +// https://github.com/rt-net/rt_manipulators_cpp/blob/v1.1.2/samples/samples02/src/x7_forward_kinematics.cpp + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rt_manipulators_cpp/hardware.hpp" +#include "rt_manipulators_cpp/kinematics.hpp" +#include "rt_manipulators_cpp/kinematics_utils.hpp" +#include "rt_manipulators_cpp/link.hpp" + +using namespace std::chrono_literals; + +class X7ReadPosition : public rclcpp::Node +{ +public: + X7ReadPosition() + : Node("x7_read_position") + { + timer_ = this->create_wall_timer(10ms, std::bind(&X7ReadPosition::timer_callback, this)); + + this->declare_parameter("port_name", "/dev/ttyUSB0"); + this->declare_parameter("baudrate", 3000000); + this->declare_parameter("config_file_path", "config/crane-x7.yaml"); + this->declare_parameter("link_file_path", "config/crane-x7_links.csv"); + } + + ~X7ReadPosition() + { + if (hardware_) { + hardware_->stop_thread(); + hardware_->disconnect(); + } + } + + bool init(void) + { + const auto port_name = this->get_parameter("port_name").get_value(); + const auto baudrate = this->get_parameter("baudrate").get_value(); + const auto config_file_path = this->get_parameter("config_file_path").get_value(); + const auto link_file_path = this->get_parameter("link_file_path").get_value(); + + hardware_ = std::make_shared(port_name); + if (!hardware_->connect(baudrate)) { + RCLCPP_ERROR(this->get_logger(), "Failed to connect a robot."); + return false; + } + + if (!hardware_->load_config_file(config_file_path)) { + RCLCPP_ERROR(this->get_logger(), "Failed to read a config file."); + return false; + } + std::vector group_names = {"arm", "hand"}; + if (!hardware_->start_thread(group_names, std::chrono::milliseconds(10))) { + RCLCPP_ERROR(this->get_logger(), "Failed to start a thread."); + return -1; + } + + links_ = kinematics_utils::parse_link_config_file(link_file_path); + return true; + } + +private: + void timer_callback() + { + std::vector positions; + if (hardware_->get_positions("arm", positions)) { + set_arm_joint_positions(links_, positions); + kinematics::forward_kinematics(links_, 1); + + int target_link = 8; + auto pos_xyz = links_[target_link].p; + RCLCPP_INFO( + this->get_logger(), "Pos x:%f, y:%f, z:%f", + pos_xyz[0], pos_xyz[1], pos_xyz[2]); + auto euler_zyx = kinematics_utils::rotation_to_euler_ZYX(links_[target_link].R); + RCLCPP_INFO( + this->get_logger(), "Rot z:%f, y:%f, x:%f", + euler_zyx[0], euler_zyx[1], euler_zyx[2]); + } + } + + void set_arm_joint_positions( + std::vector & links, + std::vector positions) + { + int start_id = 2; // Link1 + for (std::size_t i = 0; i < positions.size(); i++) { + links[start_id + i].q = positions[i]; + } + } + + rclcpp::TimerBase::SharedPtr timer_; + std::shared_ptr hardware_; + kinematics_utils::links_t links_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + if (node->init()) { + rclcpp::spin(node); + } + rclcpp::shutdown(); + return 0; +} diff --git a/rt_manipulators_lib/CMakeLists.txt b/rt_manipulators_lib/CMakeLists.txt index 0eb86ed..d3cbbe0 100644 --- a/rt_manipulators_lib/CMakeLists.txt +++ b/rt_manipulators_lib/CMakeLists.txt @@ -7,4 +7,61 @@ project(rt_manipulators_cpp VERSION 1.0) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED True) -add_subdirectory(src) +# add_subdirectory(src) +find_package(ament_cmake REQUIRED) +find_package(dynamixel_sdk REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3) +find_package(yaml_cpp_vendor REQUIRED) + +set(library_name ${CMAKE_PROJECT_NAME}) + +add_library(${library_name} + SHARED + src/hardware.cpp + src/joint.cpp + src/hardware_joints.cpp + src/hardware_communicator.cpp + src/kinematics.cpp + src/kinematics_utils.cpp + src/config_file_parser.cpp + src/dynamixel_x.cpp + src/dynamixel_xm430.cpp + src/dynamixel_xm540.cpp + src/dynamixel_xh430.cpp + src/dynamixel_xh540.cpp + src/dynamixel_p.cpp + src/dynamixel_ph42.cpp +) +target_include_directories(${library_name} + PUBLIC + $ + $) +ament_target_dependencies(${library_name} + dynamixel_sdk + Eigen3 + yaml_cpp_vendor +) + +ament_export_targets(export_${library_name} HAS_LIBRARY_TARGET) +ament_export_dependencies( + dynamixel_sdk + eigen3_cmake_module + Eigen3 + yaml_cpp_vendor +) +install( + DIRECTORY include/ + DESTINATION include/${library_name} +) + +install( + TARGETS ${library_name} + EXPORT export_${library_name} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +ament_package() diff --git a/rt_manipulators_lib/package.xml b/rt_manipulators_lib/package.xml new file mode 100644 index 0000000..9c9232a --- /dev/null +++ b/rt_manipulators_lib/package.xml @@ -0,0 +1,26 @@ + + + + rt_manipulators_cpp + 1.0.0 + RT Manipulators C++ Library + RT Corporation + Apache License 2.0 + + ShotaAk + + ament_cmake + + ament_lint_auto + ament_lint_common + + dynamixel_sdk + eigen + eigen3_cmake_module + yaml-cpp + yaml_cpp_vendor + + + ament_cmake + + diff --git a/samples/README.md b/samples/README.md deleted file mode 100644 index b655062..0000000 --- a/samples/README.md +++ /dev/null @@ -1,42 +0,0 @@ -# サンプル集 - -このディレクトリは、RTマニピュレータC++ライブラリの使い方を知るためのサンプルプログラムを提供します。 - -## サンプル集01 ロボットのサーボモータを動かす - -次のコマンドを実行して、サンプル集をビルドします。 - -```sh -$ cd samples01 -$ ./build_samples.bash -``` - -ビルドに成功すると`samples01/bin/`ディレクトリに実行ファイルが生成されます。 - -サンプルプログラムの使い方については、[samples01/README.md](./samples01/README.md)を参照してください。 - -## サンプル集02 リンクの位置・姿勢を変更する - -次のコマンドを実行して、サンプル集をビルドします。 - -```sh -$ cd samples02 -$ ./build_samples.bash -``` - -ビルドに成功すると`samples02/bin/`ディレクトリに実行ファイルが生成されます。 - -サンプルプログラムの使い方については、[samples02/README.md](./samples02/README.md)を参照してください。 - -## サンプル集03 解析的な逆運動学、トルク制御 - -次のコマンドを実行して、サンプル集をビルドします。 - -```sh -$ cd samples03 -$ ./build_samples.bash -``` - -ビルドに成功すると`samples03/bin/`ディレクトリに実行ファイルが生成されます。 - -サンプルプログラムの使い方については、[samples03/README.md](./samples03/README.md)を参照してください。 diff --git a/samples/samples01/.clang-format b/samples/samples01/.clang-format deleted file mode 100644 index 7ca5230..0000000 --- a/samples/samples01/.clang-format +++ /dev/null @@ -1,2 +0,0 @@ -BasedOnStyle: Google -ColumnLimit: 100 \ No newline at end of file diff --git a/samples/samples01/CMakeLists.txt b/samples/samples01/CMakeLists.txt deleted file mode 100644 index 984d497..0000000 --- a/samples/samples01/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ -cmake_minimum_required(VERSION 3.10) - -# set the project name and version -project(samples01 VERSION 1.0) - -# specify the C++ standard -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED True) - -# 生成された実行ファイルをソースのbinディレクトリに出力する -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) - -# サンプルのビルド -set(list_samples - x7_onoff - s17_onoff - x7_read_position - s17_read_position - x7_write_position - s17_write_position - x7_thread - s17_thread - x7_read_present_values - s17_read_present_values - x7_write_velocity - s17_write_velocity - x7_write_current - s17_write_current -) -foreach(sample IN LISTS list_samples) - message("${sample}") - add_executable(${sample} - src/${sample}.cpp - ) - target_link_libraries(${sample} PRIVATE rt_manipulators_cpp) -endforeach() diff --git a/samples/samples01/README.md b/samples/samples01/README.md deleted file mode 100644 index e283f0d..0000000 --- a/samples/samples01/README.md +++ /dev/null @@ -1,871 +0,0 @@ -# サンプル集01 ロボットのサーボモータを動かす - -- [サンプル集01 ロボットのサーボモータを動かす](#サンプル集01-ロボットのサーボモータを動かす) - - [サンプルのビルド](#サンプルのビルド) - - [通信設定](#通信設定) - - [サーボモータのトルクをON/OFFする](#サーボモータのトルクをonoffする) - - [動画](#動画) - - [解説](#解説) - - [サーボモータの現在角度を読み取る](#サーボモータの現在角度を読み取る) - - [動画](#動画-1) - - [解説](#解説-1) - - [サーボモータの目標角度を書き込む](#サーボモータの目標角度を書き込む) - - [動画](#動画-2) - - [解説](#解説-2) - - [スレッドでサーボモータの角度を読み書きする](#スレッドでサーボモータの角度を読み書きする) - - [動画](#動画-3) - - [解説](#解説-3) - - [サーボモータの速度、電流、入力電圧、温度を読み取る](#サーボモータの速度電流入力電圧温度を読み取る) - - [動画](#動画-4) - - [解説](#解説-4) - - [サーボモータの目標速度を書き込む](#サーボモータの目標速度を書き込む) - - [動画](#動画-5) - - [解説](#解説-5) - - [サーボモータの目標電流を書き込む](#サーボモータの目標電流を書き込む) - - [動画](#動画-6) - - [解説](#解説-6) - -## サンプルのビルド - -次のコマンドを実行して、サンプル集をビルドします。 - -```sh -$ ./build_samples.bash -``` - -ビルドに成功すると`samples01/bin/`ディレクトリに実行ファイルが生成されます。 - -## 通信設定 - -USB通信ポートのアクセス権限を変更するため次のコマンドを実行します。 - -```sh -$ sudo chmod 666 /dev/ttyUSB0 -``` - -永続的なアクセス権限を付与する場合は次のコマンドを実行します。 - -```sh -$ sudo usermod -aG dialout $USER -$ reboot -``` - -ロボットとの通信遅延を最小にするため次のコマンドを実行します。 - -```sh -$ sudo chmod a+rw /sys/bus/usb-serial/devices/ttyUSB0/latency_timer -$ sudo echo 1 > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer -``` - -Sciurus17を使用する場合は、 -[Sciurus17入門ガイド](https://rt-net.jp/products/sciurus17/)に記載されている手順で -Sciurus17制御基板の通信タイムアウト機能を解除してください。 - -解除しない場合はサンプル実行中のスリープ処理によって通信タイムアウト機能が働き、サーボが脱力します。 - -## サーボモータのトルクをON/OFFする - -次のコマンドを実行します。 -サーボモータのトルクがON / OFFします。 - -```sh -# CRANE-X7の場合 -$ cd bin/ -$ ./x7_onoff -# Sciurus17の場合 -$ ./s17_onoff -``` - -実行結果（CRANE-X7の場合） - -```sh -CRANE-X7のトルクをON/OFFするサンプルです. -CRANE-X7(ポート:/dev/ttyUSB0 ボーレート:3000000)に接続します. -コンフィグファイル:../config/crane-x7.yamlを読み込みます. -Config file '../config/crane-x7.yaml' loaded. -arm - joint1, id:2, mode:3 - joint2, id:3, mode:3 - joint3, id:4, mode:3 - joint4, id:5, mode:3 - joint5, id:6, mode:3 - joint6, id:7, mode:3 - joint7, id:8, mode:3 -hand - joint_hand, id:9, mode:3 -ジョイントグループ:armのトルクをONにします. -10秒間スリープします. -サーボモータの制御モードが位置制御モードのとき、ロボットに触れるとトルクがONになっていることがわかります. -ジョイントグループ:armのトルクをOFFにします. -CRANE-X7との接続を解除します. -``` - -### 動画 - -[![](https://img.youtube.com/vi/cA_3HU3HfcM/sddefault.jpg)](https://youtu.be/cA_3HU3HfcM) - -### 解説 - -RTマニピュレータC++ライブラリを使用する場合は`rt_manipulators_cpp/hardware.hpp`をincludeします。 - -```cpp -#include "rt_manipulators_cpp/hardware.hpp" -``` - -ロボットと通信するために、`rt_manipulators_cpp::Hardware(port_name)`クラスのインスタンスを生成します。 -インスタンスの引数には通信ポート名を入力します。 - -```cpp -rt_manipulators_cpp::Hardware hardware("/dev/ttyUSB0"); -``` - -ロボットと接続するために、`Hardware.connect(baudrate)`を実行します。 -引数には通信ボーレート（bps）を入力します。 - -```cpp -hardware.connect(3000000); -``` - -ロボットのサーボモータ構成を読み込むために、`Hardware.load_config_file(file_path)`を実行します。 -引数にはコンフィグファイルのパスを入力します。 - - -```cpp -hardware.load_config_file("../config/crane-x7.yaml"); -``` - -コンフィグファイルはYAMLフォーマットで次のように作成します。 - -```yaml -joint_groups: - ジョイントグループ名(1): - joints: - - ジョイント名(1) - - ジョイント名(2) - - ジョイント名(3) - ジョイントグループ名(2): - joints: - - ジョイント名(4) - - ジョイント名(5) - -ジョイント名(1): { id : 0, dynamixel: "XM430", operating_mode: 3 } -ジョイント名(2): { id : 1, dynamixel: "XM430", operating_mode: 3 } -ジョイント名(3): { id : 2, dynamixel: "XM430", operating_mode: 3 } -ジョイント名(4): { id : 3, dynamixel: "XM540", operating_mode: 3 } -ジョイント名(5): { id : 4, dynamixel: "XM540", operating_mode: 3 } -``` - -トルクをONするために`Hardware.torque_on(group_name)`を実行します。 -引数にはジョイントグループ名を入力します。 -ジョイントグループのすべてのモータのトルクがONされます。 - -```cpp -hardware.torque_on("arm"); -``` - -トルクをOFFするために`Hardware.torque_off(group_name)`を実行します。 -引数にはジョイントグループ名を入力します。 -ジョイントグループのすべてのモータのトルクがFFされます。 - -```cpp -hardware.torque_off("arm"); -``` - -ロボットとの接続を解除するために`Hardware.disconnect()`を実行します。 - -```cpp -hardware.disconnect(); -``` - -## サーボモータの現在角度を読み取る - -次のコマンドを実行します。 -サーボモータの現在角度がターミナルに表示されます。 - -```sh -# CRANE-X7の場合 -$ cd bin/ -$ ./x7_read_position -# Sciurus17の場合 -$ ./s17_read_position -``` - -実行結果（CRANE-X7の場合） - -```sh -... -ID:2のサーボ角度は1.491029radです. -joint_handのサーボ角度は1.259398radです. -armグループの0番目のサーボ角度は1.491029radです. -armグループの1番目のサーボ角度は-0.466330radです. -armグループの2番目のサーボ角度は0.087437radです. -armグループの3番目のサーボ角度は-1.848447radです. -armグループの4番目のサーボ角度は-0.044485radです. -armグループの5番目のサーボ角度は0.233165radです. -armグループの6番目のサーボ角度は0.961806radです. -... -``` - -### 動画 - -[![](https://img.youtube.com/vi/c0dBK2sj2Dw/sddefault.jpg)](https://youtu.be/c0dBK2sj2Dw) - -### 解説 - -サーボモータの現在角度を取得するため、 -コンフィグファイルのジョイントグループに`sync_read:position`を追加します。 - -```yaml -joint_groups: - ジョイントグループ名(1): - joints: - - ジョイント名(1) - - ジョイント名(2) - - ジョイント名(3) - sync_read: - - position - ジョイントグループ名(2): - joints: - - ジョイント名(4) - - ジョイント名(5) - sync_read: - - position -``` - -サーボモータの現在情報を読み取るため、`Hardware.sync_read(group_name)`を実行します。 -引数にはジョイントグループ名を入力します。 - -この関数を実行すると、ロボットとの通信が発生します。 - -```cpp -hardware.sync_read("arm"); -``` - -サーボモータの現在角度を取得するため、`Hardware.get_position(id, position)`を実行します。 -引数にはサーボモータのIDと、角度(radian)の格納先を入力します。 - -```cpp -double position; -hardware.get_position(2, position); -``` - -ジョイント名で指定することも可能です。 - -```cpp -double position; -hardware.get_position("joint1", position); -``` - -ジョイントグループの現在角度を一括で取得する場合は、`Hardware.get_positions(group_name, positions)`を実行します。 -引数にはジョイントグループ名と、角度の格納先を入力します。 - -```cpp -std::vector positions; -hardware.get_positions("arm", positions); -``` - -## サーボモータの目標角度を書き込む - -次のコマンドを実行します。 -CRANE-X7は垂直姿勢に移行し、ハンドを開閉します。 -Sciurus17は胴体が正面を向く姿勢に移行し、右ハンドを開閉します。 - -***安全のためロボットの周りに物や人を近づけないでください。*** - -```sh -# CRANE-X7の場合 -$ cd bin/ -$ ./x7_write_position -# Sciurus17の場合 -$ ./s17_write_position -``` - -実行結果（CRANE-X7の場合） - -```sh -... -armグループのサーボ最大加速度を0.5pi rad/s^2、最大速度を0.5pi rad/sに設定します. -handグループのサーボ最大加速度を0.5pi rad/s^2、最大速度を0.5pi rad/sに設定します. -arm、handグループのサーボ位置制御PIDゲインに(800, 0, 0)を書き込みます. -armグループのサーボ目標角度に0.0 radを書き込みます. -5秒後にX7が垂直姿勢へ移行するため、X7の周りに物や人を近づけないで下さい. -5秒間スリープして動作完了を待ちます. -ID:5のサーボ目標角度に-2.094395 radを書き込みます. -5秒間スリープして動作完了を待ちます. -joint_handジョイントのサーボ目標角度に0.523599 radを書き込みます. -5秒間スリープして動作完了を待ちます. -joint_handジョイントのサーボ目標角度に0.000000 radを書き込みます. -5秒間スリープして動作完了を待ちます. -arm、handグループのサーボ位置制御PIDゲインに(5, 0, 0)を書き込み、脱力させます. -10秒間スリープします. -CRANE-X7との接続を解除します. -``` - -### 動画 - -[![](https://img.youtube.com/vi/JLKXTyUJo1A/sddefault.jpg)](https://youtu.be/JLKXTyUJo1A) - -### 解説 - -サーボモータに目標角度を書き込むため、 -コンフィグファイルのジョイントグループに`sync_write:position`を追加します。 - -ジョイントの`operating_mode`は`3`に設定します. -コンフィグファイル読み込み時に、サーボモータ内部のパラメータである`Operating Mode`に -`3 (位置制御モード)`が書き込まれます. - -```yaml -joint_groups: - ジョイントグループ名(1): - joints: - - ジョイント名(1) - - ジョイント名(2) - - ジョイント名(3) - sync_write: - - position - ジョイントグループ名(2): - joints: - - ジョイント名(4) - - ジョイント名(5) - sync_write: - - position - -ジョイント名(1): { id : 0, dynamixel: "XM430", operating_mode: 3 } -ジョイント名(2): { id : 1, dynamixel: "XM430", operating_mode: 3 } -ジョイント名(3): { id : 2, dynamixel: "XM430", operating_mode: 3 } -ジョイント名(4): { id : 3, dynamixel: "XM540", operating_mode: 3 } -ジョイント名(5): { id : 4, dynamixel: "XM540", operating_mode: 3 } -``` - -サーボモータの最大動作加速度を設定するため、`Hardware.write_max_acceleration_to_group(group_name, acceleration)`を実行します。 -引数にはジョイントグループ名と、加速度(radian / second^2)を入力します。 - -```cpp -hardware.write_max_acceleration_to_group("arm", 0.5 * M_PI); -``` - -サーボモータの最大動作速度を設定するため、`Hardware.write_max_velocity_to_group(group_name, velocity)`を実行します。 -引数にはジョイントグループ名と、速度(radian / second)を入力します。 - -```cpp -hardware.write_max_velocity_to_group("arm", 0.5* M_PI); -``` - -サーボモータの位置制御PIDゲインを設定するため、`Hardware.write_position_pid_gain_to_group(group_name, p, i, d)`を実行します。 -引数にはジョイントグループ名と、PIDゲインを入力します。 - -```cpp -hardware.write_position_pid_gain_to_group("arm", 800, 0, 0); -``` - -指定したサーボモータにPIDゲインを設定する場合は、`Hardware.write_position_pid_gain(id, p, i, d)`を実行します。 - -```cpp -hardware.write_position_pid_gain(2, 800, 0, 0); -``` - -ジョイント名で指定することも可能です。 - -```cpp -hardware.write_position_pid_gain("joint1", 800, 0, 0); -``` - -サーボモータの目標角度を設定するため、`Hardware.set_position(id, position)`を実行します。 -引数にはサーボモータのIDと、目標角度(radian)を入力します。 -設定した目標角度をサーボモータへ書き込むためには、`Hardware.sync_write()`を実行します。 - -```cpp -double position = 0.0; -hardware.set_position(2, position); -``` - -ジョイント名で指定することも可能です。 - -```cpp -double position = 0.0; -hardware.set_position("joint1", position); -``` - -ジョイントグループの目標角度を一括で設定する場合は、`Hardware.set_positions(group_name, positions)`を実行します。 -引数にはジョイントグループ名と、目標角度を入力します。 - -```cpp -std::vector positions(7, 0.0); -hardware.get_positions("arm", positions); -``` - -サーボモータへ目標値を書き込むため、`Hardware.sync_write(group_name)`を実行します。 -引数にはジョイントグループ名を入力します。 - -この関数を実行すると、ロボットとの通信が発生します。 - -```cpp -hardware.sync_write("arm"); -``` - -## スレッドでサーボモータの角度を読み書きする - -次のコマンドを実行します。 -CRANE-X7は、肘の現在角度に合わせてハンドを開閉します。 -Sciurus17は、右肘の現在角度に合わせて右ハンドを開閉します。 - -***安全のためロボットの周りに物や人を近づけないでください。*** - -```sh -# CRANE-X7の場合 -$ cd bin/ -$ ./x7_thread -# Sciurus17の場合 -$ ./s17_thread -``` - -実行結果（CRANE-X7の場合） - -```sh -handジョイントグループのトルクをONにします. -read/writeスレッドを起動します. -5秒後にX7のハンドが開くので、ハンドに触れないでください. -armグループの0番目のサーボ角度は-1.070719radです. -armグループの1番目のサーボ角度は-1.632156radです. -armグループの2番目のサーボ角度は0.897379radです. -armグループの3番目のサーボ角度は-2.686000radです. -armグループの4番目のサーボ角度は-0.684155radです. -armグループの5番目のサーボ角度は1.141282radです. -armグループの6番目のサーボ角度は-0.954136radです. -handグループの0番目のサーボ角度は-0.007670radです. -armグループの0番目のサーボ角度は-1.070719radです. -armグループの1番目のサーボ角度は-1.632156radです. -armグループの2番目のサーボ角度は0.897379radです. -armグループの3番目のサーボ角度は-2.686000radです. -armグループの4番目のサーボ角度は-0.684155radです. -... -``` - -### 動画 - -[![](https://img.youtube.com/vi/r7ssEHS-jIk/sddefault.jpg)](https://youtu.be/r7ssEHS-jIk) - -### 解説 - -サーボモータのデータを読み書きするスレッドを起動するため、`Hardware.start_thread(group_names, update_cycle_ms)`を実行します。 -引数にはジョイントグループ名と、スレッドの更新周期(msec)を入力します。 - -スレッド内では`Hardware.sync_read()`と`Hardware.sync_write()`が実行されます。 - -更新周期を短くする場合は[通信設定](#通信設定)を参考にUSB通信ポートの遅延を最小にしてください。 - -```cpp -std::vector group_names = {"arm", "hand"}; -hardware.start_thread(group_names, std::chrono::milliseconds(10)); -``` - -スレッドを停止する場合は、`Hardware.stop_thread()`を実行します。 - -```cpp -hardware.stop_thread(); -``` - -## サーボモータの速度、電流、入力電圧、温度を読み取る - -次のコマンドを実行します。サーボモータの現在角度、速度、電流、入力電圧、温度がターミナルに表示されます。 - -```sh -# CRANE-X7の場合 -$ cd bin/ -$ ./x7_read_present_values -# Sciurus17の場合 -$ ./s17_read_present_values -``` - -実行結果（CRANE-X7の場合） - -```sh -... -サーボモータの電流値を観察しやすくするため、 -5秒後にhandグループのトルクがONします. -arm: index, position[rad], velocity[rad/s], current[A], voltage[V], temperature[deg] -0, 0.619728, 0.000000, 0.000000, 12.200000, 32 -1, 1.561592, 0.000000, 0.000000, 12.100000, 32 -2, -1.965029, 0.000000, -0.002690, 12.000000, 32 -3, -2.442097, 0.000000, -0.008070, 11.800000, 32 -4, -1.922078, -0.023981, 0.000000, 11.700000, 34 -5, 1.217981, -0.071942, 0.000000, 11.700000, 35 -6, -0.358952, -0.023981, -0.008070, 11.700000, 35 -hand: index, position[rad], velocity[rad/s], current[A], voltage[V], temperature[deg] -0, 0.354350, 0.000000, 0.040350, 11.700000, 37 -ID:9: position[rad], velocity[rad/s], current[A], voltage[V], temperature[deg] -0.354350, 0.000000, 0.040350, 11.700000, 37 -joint_hand: position[rad], velocity[rad/s], current[A], voltage[V], temperature[deg] -0.354350, 0.000000, 0.040350, 11.700000, 37 -... -``` - -### 動画 - -[![](https://img.youtube.com/vi/NbhPIo9yL1A/sddefault.jpg)](https://youtu.be/NbhPIo9yL1A) - -### 解説 - -サーボモータの現在値を取得するため、 -コンフィグファイルのジョイントグループに`sync_read:{position, velocity, current, voltage, temperature}`を追加します。 - -読み取りたいデータ項目を`sync_read`に追加してください。 - -```yaml -joint_groups: - ジョイントグループ名(1): - joints: - - ジョイント名(1) - - ジョイント名(2) - - ジョイント名(3) - sync_read: - - position - - velocity - - current - - voltage - - temperature - ジョイントグループ名(2): - joints: - - ジョイント名(4) - - ジョイント名(5) - sync_read: - - current - - temperature -``` - -サーボモータの現在情報を読み取るため、`Hardware.sync_read(group_name)`を実行します。 -引数にはジョイントグループ名を入力します。 - -この関数を実行すると、ロボットとの通信が発生します。 - -```cpp -hardware.sync_read("arm"); -``` - -サーボモータの速度を取得するため、`Hardware.get_velocity(id, velocity)`を実行します。 -引数にはサーボモータのIDと、速度(radian / second)の格納先を入力します。 - -```cpp -double velocity; -hardware.get_velocity(2, velocity); -``` - -サーボモータの電流(ampere)は、`Hardware.get_current(id, current)`で取得できます。 - -```cpp -double current; -hardware.get_current(2, current); -``` - -サーボモータの入力電圧(volt)は、`Hardware.get_voltage(id, voltage)`で取得できます。 - -```cpp -double voltage; -hardware.get_voltage(2, voltage); -``` - -サーボモータの温度(degree Celsius)は、`Hardware.get_temperature(id, temperature)`で取得できます。 - -```cpp -int8_t temperature; -hardware.get_temperature(2, temperature); -``` - -ジョイント名で指定することも可能です。 - -```cpp -double velocity; -double current; -double voltage; -int8_t temperature; -hardware.get_velocity("joint1", velocity); -hardware.get_current("joint1", current); -hardware.get_voltage("joint1", voltage); -hardware.get_temperature("joint1", temperature); -``` - -ジョイントグループの現在値を一括で取得することも可能です。 - -```cpp -std::vector velocities; -std::vector currents; -std::vector voltages; -std::vector temperatures; -hardware.get_velocities("arm", velocities); -hardware.get_currents("arm", currents); -hardware.get_voltages("arm", voltages); -hardware.get_temperatures("arm", temperatures); -``` - -## サーボモータの目標速度を書き込む - -次のコマンドを実行します。 -CRANE-X7は前腕を回転させます。**前腕が浮くようにCRANE-X7の腕を持ち上げて下さい** - -Sciurus17は両腕の手首を回転させます。 - -```sh -# CRANE-X7の場合 -$ cd bin/ -$ ./x7_write_velocity -# Sciurus17の場合 -$ ./s17_write_velocity -``` - -実行結果（CRANE-X7の場合） - -```sh -... -wristグループのサーボ最大加速度を5pi rad/s^2に設定します. -wristグループのサーボ速度制御PIゲインに(100, 1920)を書き込みます. -read/writeスレッドを起動します. -5秒後に手先が動き出すため、手先の周りに物や人を近づけないで下さい. -set velocity:0.314159 rad/s -set velocity:-0.314159 rad/s -... -set velocity:2.82743 rad/s -joint5ジョイントの現在角度が限界角度に到達しました、goal_velocityを0で上書きします. -joint6ジョイントの現在角度が限界角度に到達しました、goal_velocityを0で上書きします. -... -スレッドを停止します. -wristグループにはvelocityのsync_writeが設定されています. -安全のため, stop_thread()関数内で目標速度 0 rad/sを書き込みます. -CRANE-X7との接続を解除します. -wristグループにはvelocityのsync_writeが設定されています. -安全のため, disconnect()関数内で目標速度 0 rad/sを書き込みます. -``` - -### 動画 - -[![](https://img.youtube.com/vi/JxJ_BFnzJqc/sddefault.jpg)](https://youtu.be/JxJ_BFnzJqc) - -### 解説 - -サーボモータに目標速度を書き込むため、 -コンフィグファイルのジョイントグループに`sync_write:velocity`と`sync_read:position`を追加します. -`sync_read:position`は可動範囲超過を検出するために必要です. - -ジョイントの`operating_mode`は`1`に設定します. -コンフィグファイル読み込み時に、サーボモータ内部のパラメータである`Operating Mode`に -`1 (速度制御モード)`が書き込まれます. - -ジョイントの`pos_limit_margin`は、 -サーボモータ内部に設定された可動範囲（`Max/Min Position Limit`）から、 -どれくらいの角度余裕(radian)を設けるかというパラメータです. - -**サーボモータが速度制御モードの時、サーボモータ内部の角度制限機能が動作しません。** -そのため、`Hardware`クラスのスレッド内部で、 -サーボモータの現在角度を観察し、可動範囲を超える場合は目標速度`0 radian / second`を書き込む処理を実施しています. - -安全のためサーボモータ内部の可動範囲パラメータ（`Max/Min Position Limit`）が適切に設定されているかご確認ください。 - -サーボモータの現在角度が可動範囲の限界値付近にあるとき、 -目標速度`0 radian / second`を書き込んでも回転速度が下がりきらず、 -現在角度が可動範囲を超える場合があります. -安全ため、コンフィグファイルの`pos_limit_margin`に0以上の数値を設定し、 -可動範囲を狭くしてください. - -```yaml -joint_groups: - ジョイントグループ名(1): - joints: - - ジョイント名(1) - - ジョイント名(2) - - ジョイント名(3) - sync_write: - - velocity - sync_read: - - position - -ジョイント名(1): { id : 0, dynamixel: "XM430", operating_mode: 1, pos_limit_margin: 0.5} -ジョイント名(2): { id : 1, dynamixel: "XM430", operating_mode: 1, pos_limit_margin: 0.5} -ジョイント名(3): { id : 2, dynamixel: "XM430", operating_mode: 1, pos_limit_margin: 0.5} -``` - -サーボモータの速度制御PIゲインを設定するため、`Hardware.write_velocity_pi_gain_to_group(group_name, p, i)`を実行します。 -引数にはジョイントグループ名と、PIゲインを入力します。 - -```cpp -hardware.write_velocity_pi_gain_to_group("arm", 100, 1920); -``` - -指定したサーボモータにPIゲインを設定する場合は、`Hardware.write_velocity_pi_gain(id, p, i)`を実行します。 - -```cpp -hardware.write_velocity_pi_gain(2, 100, 1920); -``` - -ジョイント名で指定することも可能です。 - -```cpp -hardware.write_velocity_pi_gain("joint1", 100, 1920); -``` - -サーボモータに目標速度を書き込む準備として、 -`Hardware.start_thread(group_names, update_cycle_ms)`を実行し、スレッドを起動します。 - -```cpp -std::vector group_names = {"arm", "hand"}; -hardware.start_thread(group_names, std::chrono::milliseconds(10)); -``` - -サーボモータの目標速度を設定するため、`Hardware.set_velocity(id, velocity)`を実行します。 -引数にはサーボモータのIDと、目標速度(radian / second)を入力します。 - -```cpp -double velocity = 3.14; -hardware.set_velocity(2, velocity); -``` - -ジョイント名で指定することも可能です。 - -```cpp -double velocity = 0.0; -hardware.set_velocity("joint1", velocity); -``` - -ジョイントグループの目標速度を一括で設定する場合は、`Hardware.set_velocities(group_name, velocities)`を実行します。 -引数にはジョイントグループ名と、目標速度を入力します。 - -```cpp -std::vector velocities(7, 3.14); -hardware.get_velocities("arm", velocities); -``` - -速度制御時は、安全のため`Hardware.stop_thread()`と`Hardware.disconnect()`の内部で -目標速度`0 radian / second`が書き込まれます。 - -```cpp -hardware.stop_thread(); -hardware.disconnect(); -``` - -## サーボモータの目標電流を書き込む - -次のコマンドを実行します。 -CRANE-X7は前腕を回転させます。**前腕が浮くようにCRANE-X7の腕を持ち上げて下さい** - -Sciurus17は両腕の手首を回転させます。 - -```sh -# CRANE-X7の場合 -$ cd bin/ -$ ./x7_write_current -# Sciurus17の場合 -$ ./s17_write_current -``` - -実行結果（CRANE-X7の場合） - -```sh -... -read/writeスレッドを起動します. -5秒後に手先が動き出すため、手先の周りに物や人を近づけないで下さい. -set current:0.005 A -set current:-0.005 A -set current:0.01 A -set current:-0.01 A -... -set current:-0.035 A -joint6ジョイントの現在角度が限界角度に到達しました、goal_currentを0.02917 Aに制限します. -joint5ジョイントの現在角度が限界角度に到達しました、goal_currentを0.02917 Aに制限します. -set current:0.04 A -... -スレッドを停止します. -wristグループにはcurrentのsync_writeが設定されています. -安全のため, stop_thread()関数内で目標電流 0 Aを書き込みます. -CRANE-X7との接続を解除します. -wristグループにはcurrentのsync_writeが設定されています. -安全のため, disconnect()関数内で目標速度 0 Aを書き込みます. -``` - -### 動画 - -[![](https://img.youtube.com/vi/Qc3TnaccRmc/sddefault.jpg)](https://youtu.be/Qc3TnaccRmc) - -### 解説 - -サーボモータに目標電流を書き込むため、 -コンフィグファイルのジョイントグループに`sync_write:current`と`sync_read:position`を追加します. -`sync_read:position`は可動範囲超過を検出するために必要です. - -ジョイントの`operating_mode`は`0`に設定します. -コンフィグファイル読み込み時に、サーボモータ内部のパラメータである`Operating Mode`に -`0 (電流制御モード)`が書き込まれます. - -ジョイントの`pos_limit_margin`は、 -サーボモータ内部に設定された可動範囲（`Max/Min Position Limit`）から、 -どれくらいの角度余裕(radian)を設けるかというパラメータです. - -**サーボモータが電流制御モードの時、サーボモータ内部の角度制限機能が動作しません。** -そのため、`Hardware`クラスのスレッド内部で、 -サーボモータの現在角度を観察し、可動範囲を超える場合は目標電流値を制限する処理（制限値は後述の`current_limit_margin`で調整可能）を実施しています. - -安全のためサーボモータ内部の可動範囲パラメータ（`Max/Min Position Limit`）が適切に設定されているかご確認ください。 - -ジョイントの`current_limit_margin`は、 -サーボモータ内部に設定された電流制限（`Current Limit`）から、 -どれくらいの電流余裕(A)を設けるかというパラメータです. - -サーボモータの現在角度が可動範囲を超える場合、 -目標電流値が`[Current Limit] - [current_limit_margin]`に制限されます。 - -```yaml -joint_groups: - ジョイントグループ名(1): - joints: - - ジョイント名(1) - - ジョイント名(2) - - ジョイント名(3) - sync_write: - - current - sync_read: - - position - -ジョイント名(1): { id : 0, dynamixel: "XM430", operating_mode: 1, pos_limit_margin: 0.5, current_limit_margin: 1.0} -ジョイント名(2): { id : 1, dynamixel: "XM430", operating_mode: 1, pos_limit_margin: 0.5, current_limit_margin: 1.0} -ジョイント名(3): { id : 2, dynamixel: "XM430", operating_mode: 1, pos_limit_margin: 0.5, current_limit_margin: 1.0} -``` - -サーボモータに目標電流を書き込む準備として、 -`Hardware.start_thread(group_names, update_cycle_ms)`を実行し、スレッドを起動します。 - -```cpp -std::vector group_names = {"arm", "hand"}; -hardware.start_thread(group_names, std::chrono::milliseconds(10)); -``` - -サーボモータの目標電流を設定するため、`Hardware.set_current(id, current)`を実行します。 -引数にはサーボモータのIDと、目標電流(A)を入力します。 - -```cpp -double current = 0.1; -hardware.set_current(2, current); -``` - -ジョイント名で指定することも可能です。 - -```cpp -double current = -0.1; -hardware.set_current("joint1", current); -``` - -ジョイントグループの目標電流を一括で設定する場合は、`Hardware.set_currents(group_name, currents)`を実行します。 -引数にはジョイントグループ名と、目標電流を入力します。 - -```cpp -std::vector currents(7, 0.0); -hardware.get_currents("arm", currents); -``` - -電流制御時は、安全のため`Hardware.stop_thread()`と`Hardware.disconnect()`の内部で -目標電流`0 A`が書き込まれます。 - -**目標電流値が`0 A`になると、ロボットが脱力します。** - -脱力によってロボットが人や物にぶつからないように支えてください。 - -```cpp -hardware.stop_thread(); -hardware.disconnect(); -``` diff --git a/samples/samples01/build_samples.bash b/samples/samples01/build_samples.bash deleted file mode 100755 index 53af683..0000000 --- a/samples/samples01/build_samples.bash +++ /dev/null @@ -1,18 +0,0 @@ -#!/usr/bin/env bash - -set -e - -BUILD_DIR=build - -echo "サンプルプログラムをビルドします" -cd $(dirname $0) - -if [ ! -d $BUILD_DIR ]; then - mkdir $BUILD_DIR -fi - -cd $BUILD_DIR -cmake .. -make - -echo "サンプルプログラムをビルドしました" \ No newline at end of file diff --git a/samples/samples01/config/crane-x7_current.yaml b/samples/samples01/config/crane-x7_current.yaml deleted file mode 100644 index 1068913..0000000 --- a/samples/samples01/config/crane-x7_current.yaml +++ /dev/null @@ -1,14 +0,0 @@ -joint_groups: - wrist: - joints: - - joint5 - - joint6 - - joint7 - sync_read: - - position - sync_write: - - current - -joint5: { id: 6, dynamixel: "XM430", operating_mode: 0, pos_limit_margin: 1.047, current_limit_margin: 3.18} -joint6: { id: 7, dynamixel: "XM430", operating_mode: 0, pos_limit_margin: 1.047, current_limit_margin: 3.18} -joint7: { id: 8, dynamixel: "XM430", operating_mode: 0, pos_limit_margin: 1.047, current_limit_margin: 3.18} diff --git a/samples/samples01/config/crane-x7_read.yaml b/samples/samples01/config/crane-x7_read.yaml deleted file mode 100644 index 15b3f2a..0000000 --- a/samples/samples01/config/crane-x7_read.yaml +++ /dev/null @@ -1,34 +0,0 @@ -joint_groups: - arm: - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 - sync_read: - - position - - velocity - - current - - voltage - - temperature - hand: - joints: - - joint_hand - sync_read: - - position - - velocity - - current - - voltage - - temperature - -joint1: { id: 2, dynamixel: "XM430", operating_mode: 3 } -joint2: { id: 3, dynamixel: "XM540", operating_mode: 3 } -joint3: { id: 4, dynamixel: "XM430", operating_mode: 3 } -joint4: { id: 5, dynamixel: "XM430", operating_mode: 3 } -joint5: { id: 6, dynamixel: "XM430", operating_mode: 3 } -joint6: { id: 7, dynamixel: "XM430", operating_mode: 3 } -joint7: { id: 8, dynamixel: "XM430", operating_mode: 3 } -joint_hand: { id: 9, dynamixel: "XM430", operating_mode: 3 } \ No newline at end of file diff --git a/samples/samples01/config/crane-x7_velocity.yaml b/samples/samples01/config/crane-x7_velocity.yaml deleted file mode 100644 index 53f987a..0000000 --- a/samples/samples01/config/crane-x7_velocity.yaml +++ /dev/null @@ -1,14 +0,0 @@ -joint_groups: - wrist: - joints: - - joint5 - - joint6 - - joint7 - sync_read: - - position - sync_write: - - velocity - -joint5: { id: 6, dynamixel: "XM430", operating_mode: 1, pos_limit_margin: 0.524} -joint6: { id: 7, dynamixel: "XM430", operating_mode: 1, pos_limit_margin: 0.524} -joint7: { id: 8, dynamixel: "XM430", operating_mode: 1, pos_limit_margin: 0.524} diff --git a/samples/samples01/config/sciurus17.yaml b/samples/samples01/config/sciurus17.yaml deleted file mode 100644 index 2b210fd..0000000 --- a/samples/samples01/config/sciurus17.yaml +++ /dev/null @@ -1,75 +0,0 @@ - -joint_groups: - right_arm: - joints: - - right_arm_joint1 - - right_arm_joint2 - - right_arm_joint3 - - right_arm_joint4 - - right_arm_joint5 - - right_arm_joint6 - - right_arm_joint7 - sync_read: - - position - sync_write: - - position - - right_hand: - joints: - - right_arm_joint_hand - sync_read: - - position - sync_write: - - position - - left_arm: - joints: - - left_arm_joint1 - - left_arm_joint2 - - left_arm_joint3 - - left_arm_joint4 - - left_arm_joint5 - - left_arm_joint6 - - left_arm_joint7 - sync_read: - - position - sync_write: - - position - - left_hand: - joints: - - left_arm_joint_hand - sync_read: - - position - sync_write: - - position - - torso: - joints: - - waist_joint - - neck_yaw_joint - - neck_pitch_joint - sync_read: - - position - sync_write: - - position - -right_arm_joint1: { id: 2, dynamixel: "XM540", operating_mode: 3 } -right_arm_joint2: { id: 3, dynamixel: "XM540", operating_mode: 3 } -right_arm_joint3: { id: 4, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint4: { id: 5, dynamixel: "XM540", operating_mode: 3 } -right_arm_joint5: { id: 6, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint6: { id: 7, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint7: { id: 8, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint_hand: { id: 9, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint1: { id: 10, dynamixel: "XM540", operating_mode: 3 } -left_arm_joint2: { id: 11, dynamixel: "XM540", operating_mode: 3 } -left_arm_joint3: { id: 12, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint4: { id: 13, dynamixel: "XM540", operating_mode: 3 } -left_arm_joint5: { id: 14, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint6: { id: 15, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint7: { id: 16, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint_hand: { id: 17, dynamixel: "XM430", operating_mode: 3 } -waist_joint: { id: 18, dynamixel: "XM430", operating_mode: 3 } -neck_yaw_joint: { id: 19, dynamixel: "XM430", operating_mode: 3 } -neck_pitch_joint: { id: 20, dynamixel: "XM430", operating_mode: 3 } diff --git a/samples/samples01/config/sciurus17_current.yaml b/samples/samples01/config/sciurus17_current.yaml deleted file mode 100644 index 6e53d83..0000000 --- a/samples/samples01/config/sciurus17_current.yaml +++ /dev/null @@ -1,23 +0,0 @@ -joint_groups: - right_wrist: - joints: - - right_arm_joint6 - - right_arm_joint7 - sync_read: - - position - sync_write: - - current - - left_wrist: - joints: - - left_arm_joint6 - - left_arm_joint7 - sync_read: - - position - sync_write: - - current - -right_arm_joint6: { id: 7, dynamixel: "XM430", operating_mode: 0, pos_limit_margin: 1.047, current_limit_margin: 3.18} -right_arm_joint7: { id: 8, dynamixel: "XM430", operating_mode: 0, pos_limit_margin: 1.047, current_limit_margin: 3.18} -left_arm_joint6: { id: 15, dynamixel: "XM430", operating_mode: 0, pos_limit_margin: 1.047, current_limit_margin: 3.18} -left_arm_joint7: { id: 16, dynamixel: "XM430", operating_mode: 0, pos_limit_margin: 1.047, current_limit_margin: 3.18} diff --git a/samples/samples01/config/sciurus17_read.yaml b/samples/samples01/config/sciurus17_read.yaml deleted file mode 100644 index ac94991..0000000 --- a/samples/samples01/config/sciurus17_read.yaml +++ /dev/null @@ -1,86 +0,0 @@ - -joint_groups: - right_arm: - joints: - - right_arm_joint1 - - right_arm_joint2 - - right_arm_joint3 - - right_arm_joint4 - - right_arm_joint5 - - right_arm_joint6 - - right_arm_joint7 - sync_read: - - position - - velocity - - current - - voltage - - temperature - - right_hand: - joints: - - right_arm_joint_hand - sync_read: - - position - - velocity - - current - - voltage - - temperature - - left_arm: - joints: - - left_arm_joint1 - - left_arm_joint2 - - left_arm_joint3 - - left_arm_joint4 - - left_arm_joint5 - - left_arm_joint6 - - left_arm_joint7 - sync_read: - - position - - velocity - - current - - voltage - - temperature - - left_hand: - joints: - - left_arm_joint_hand - sync_read: - - position - - velocity - - current - - voltage - - temperature - - torso: - joints: - - waist_joint - - neck_yaw_joint - - neck_pitch_joint - sync_read: - - position - - velocity - - current - - voltage - - temperature - -right_arm_joint1: { id: 2, dynamixel: "XM540", operating_mode: 3 } -right_arm_joint2: { id: 3, dynamixel: "XM540", operating_mode: 3 } -right_arm_joint3: { id: 4, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint4: { id: 5, dynamixel: "XM540", operating_mode: 3 } -right_arm_joint5: { id: 6, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint6: { id: 7, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint7: { id: 8, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint_hand: { id: 9, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint1: { id: 10, dynamixel: "XM540", operating_mode: 3 } -left_arm_joint2: { id: 11, dynamixel: "XM540", operating_mode: 3 } -left_arm_joint3: { id: 12, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint4: { id: 13, dynamixel: "XM540", operating_mode: 3 } -left_arm_joint5: { id: 14, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint6: { id: 15, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint7: { id: 16, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint_hand: { id: 17, dynamixel: "XM430", operating_mode: 3 } -waist_joint: { id: 18, dynamixel: "XM430", operating_mode: 3 } -neck_yaw_joint: { id: 19, dynamixel: "XM430", operating_mode: 3 } -neck_pitch_joint: { id: 20, dynamixel: "XM430", operating_mode: 3 } - diff --git a/samples/samples01/config/sciurus17_velocity.yaml b/samples/samples01/config/sciurus17_velocity.yaml deleted file mode 100644 index d6bce5f..0000000 --- a/samples/samples01/config/sciurus17_velocity.yaml +++ /dev/null @@ -1,23 +0,0 @@ -joint_groups: - right_wrist: - joints: - - right_arm_joint6 - - right_arm_joint7 - sync_read: - - position - sync_write: - - velocity - - left_wrist: - joints: - - left_arm_joint6 - - left_arm_joint7 - sync_read: - - position - sync_write: - - velocity - -right_arm_joint6: { id: 7, dynamixel: "XM430", operating_mode: 1, pos_limit_margin: 0.524} -right_arm_joint7: { id: 8, dynamixel: "XM430", operating_mode: 1, pos_limit_margin: 0.524} -left_arm_joint6: { id: 15, dynamixel: "XM430", operating_mode: 1, pos_limit_margin: 0.524} -left_arm_joint7: { id: 16, dynamixel: "XM430", operating_mode: 1, pos_limit_margin: 0.524} diff --git a/samples/samples01/src/s17_onoff.cpp b/samples/samples01/src/s17_onoff.cpp deleted file mode 100644 index b37c565..0000000 --- a/samples/samples01/src/s17_onoff.cpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -int main() { - std::cout << "Sciurus17のトルクをON/OFFするサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/sciurus17.yaml"; - - std::cout << "Sciurus17("; - std::cout << "ポート:" << port_name; - std::cout << " ボーレート:" << std::to_string(baudrate); - std::cout << ")に接続します." << std::endl; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - std::cout << "コンフィグファイル:" << config_file << "を読み込みます." << std::endl; - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - std::string group_name = "right_arm"; - std::cout << "ジョイントグループ:" << group_name << "のトルクをONにします." << std::endl; - if (!hardware.torque_on(group_name)) { - std::cerr << group_name << "グループのトルクをONできませんでした." << std::endl; - return -1; - } - - std::cout << "10秒間スリープします." << std::endl; - std::cout << "サーボモータの制御モードが位置制御モードのとき、" - << "ロボットに触れるとトルクがONになっていることがわかります." - << std::endl; - std::cout << "Sciurus17制御基板の通信タイムアウト機能が働くと、トルクON後に脱力します." - << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(10)); - - std::cout << "ジョイントグループ:" << group_name << "のトルクをOFFにします." << std::endl; - if (!hardware.torque_off(group_name)) { - std::cerr << group_name << "グループのトルクをOFFできませんでした." << std::endl; - } - - std::cout << "Sciurus17との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples01/src/s17_read_position.cpp b/samples/samples01/src/s17_read_position.cpp deleted file mode 100644 index 10e0c97..0000000 --- a/samples/samples01/src/s17_read_position.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -int main() { - std::cout << "Sciurus17のサーボモータ角度を読み取るサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/sciurus17.yaml"; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - for (int i = 0; i < 1000; i++) { - if (!hardware.sync_read("right_arm")) { - std::cerr << "right_armグループのsync readに失敗しました." << std::endl; - break; - } - - if (!hardware.sync_read("right_hand")) { - std::cerr << "right_handグループのsync readに失敗しました." << std::endl; - break; - } - - if (!hardware.sync_read("left_arm")) { - std::cerr << "left_armグループのsync readに失敗しました." << std::endl; - break; - } - - if (!hardware.sync_read("left_hand")) { - std::cerr << "left_handグループのsync readに失敗しました." << std::endl; - break; - } - - if (!hardware.sync_read("torso")) { - std::cerr << "torsoグループのsync readに失敗しました." << std::endl; - break; - } - - double position; - int dxl_id = 2; - if (hardware.get_position(dxl_id, position)) { - std::cout << "ID:" << std::to_string(dxl_id) << "のサーボ角度は" << std::to_string(position) - << "radです." << std::endl; - } - - std::string joint_name = "left_arm_joint_hand"; - if (hardware.get_position(joint_name, position)) { - std::cout << joint_name << "のサーボ角度は" << std::to_string(position) << "radです." - << std::endl; - } - - std::vector positions; - if (hardware.get_positions("torso", positions)) { - for (int i = 0; i < positions.size(); i++) { - std::cout << "torsoグループの" << std::to_string(i) << "番目のサーボ角度は" - << std::to_string(positions[i]) << "radです." << std::endl; - } - } - - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - std::cout << "Sciurus17との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples01/src/s17_read_present_values.cpp b/samples/samples01/src/s17_read_present_values.cpp deleted file mode 100644 index 25a2759..0000000 --- a/samples/samples01/src/s17_read_present_values.cpp +++ /dev/null @@ -1,132 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -void print_names(void) { - std::cout << "position[rad], "; - std::cout << "velocity[rad/s], "; - std::cout << "current[A], "; - std::cout << "voltage[V], "; - std::cout << "temperature[deg]"; - std::cout << std::endl; -} - -void print_values(const double position, const double velocity, const double current, - const double voltage, const int8_t temperature) { - std::cout << std::to_string(position) << ", "; - std::cout << std::to_string(velocity) << ", "; - std::cout << std::to_string(current) << ", "; - std::cout << std::to_string(voltage) << ", "; - std::cout << std::to_string(temperature); - std::cout << std::endl; -} - -int main() { - std::cout << "Sciurus17のサーボモータ"; - std::cout << "位置、速度、電流、入力電圧、温度を読み取るサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/sciurus17_read.yaml"; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - std::cout << "サーボモータの電流値を観察しやすくするため、" << std::endl; - std::cout << "5秒後にright_handグループのトルクがONします." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - if (!hardware.torque_on("right_hand")) { - std::cerr << "right_handグループのトルクをONできませんでした." << std::endl; - return -1; - } - - std::vector groups = {"right_arm", "right_hand"}; - for (int i = 0; i < 1000; i++) { - for (auto group_name : groups) { - if (!hardware.sync_read(group_name)) { - std::cerr << group_name << "グループのsync readに失敗しました." << std::endl; - break; - } - - std::vector positions; - std::vector velocities; - std::vector currents; - std::vector voltages; - std::vector temperatures; - if (hardware.get_positions(group_name, positions) && - hardware.get_velocities(group_name, velocities) && - hardware.get_currents(group_name, currents) && - hardware.get_voltages(group_name, voltages) && - hardware.get_temperatures(group_name, temperatures)) { - std::cout << group_name << ": index, "; - print_names(); - for (int i = 0; i < positions.size(); i++) { - std::cout << std::to_string(i) << ", "; - print_values(positions[i], velocities[i], currents[i], voltages[i], temperatures[i]); - } - } - } - - const int dxl_id = 9; - const std::string joint_name = "right_arm_joint_hand"; - double position; - double velocity; - double current; - double voltage; - int8_t temperature; - if (hardware.get_position(dxl_id, position) && - hardware.get_velocity(dxl_id, velocity) && - hardware.get_current(dxl_id, current) && - hardware.get_voltage(dxl_id, voltage) && - hardware.get_temperature(dxl_id, temperature)) { - std::cout << "ID:" << std::to_string(dxl_id) << ": "; - print_names(); - print_values(position, velocity, current, voltage, temperature); - } - - if (hardware.get_position(joint_name, position) && - hardware.get_velocity(joint_name, velocity) && - hardware.get_current(joint_name, current) && - hardware.get_voltage(joint_name, voltage) && - hardware.get_temperature(joint_name, temperature)) { - std::cout << joint_name << ": "; - print_names(); - print_values(position, velocity, current, voltage, temperature); - } - - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - if (!hardware.torque_off("right_hand")) { - std::cerr << "right_handグループのトルクをOFFできませんでした." << std::endl; - } - - std::cout << "Sciurus17との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples01/src/s17_thread.cpp b/samples/samples01/src/s17_thread.cpp deleted file mode 100644 index 9b8cd10..0000000 --- a/samples/samples01/src/s17_thread.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -int main() { - std::cout << "スレッドを起動し、Sciurus17のサーボモータ角度を読み書きするサンプルです." - << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/sciurus17.yaml"; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - std::cout << "right_handジョイントグループのトルクをONにします." << std::endl; - if (!hardware.torque_on("right_hand")) { - std::cerr << "right_handグループのトルクをONできませんでした." << std::endl; - return -1; - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - std::vector group_names = {"right_arm", "right_hand"}; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::cout << "5秒後にSciurus17のハンドが開くので、ハンドに触れないでください." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - for (int i = 0; i < 2000; i++) { - for (std::string group_name : group_names) { - std::vector positions; - if (hardware.get_positions(group_name, positions)) { - for (int i = 0; i < positions.size(); i++) { - std::cout << group_name << "グループの" << std::to_string(i) << "番目のサーボ角度は" - << std::to_string(positions[i]) << "radです." << std::endl; - } - } - } - // 肘の関節を開くと、ハンドが開くように目標角度を設定する - double position; - hardware.get_position("right_arm_joint4", position); - hardware.set_position("right_arm_joint_hand", 0.5 * (M_PI - position)); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - if (!hardware.torque_off("right_hand")) { - std::cerr << "handグループのトルクをOFFできませんでした." << std::endl; - } - - std::cout << "Sciurus17との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples01/src/s17_write_current.cpp b/samples/samples01/src/s17_write_current.cpp deleted file mode 100644 index f444cd4..0000000 --- a/samples/samples01/src/s17_write_current.cpp +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -int main() { - std::cout << "Sciurus17のサーボモータに目標電流を書き込むサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/sciurus17_current.yaml"; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - std::vector group_names = {"right_wrist", "left_wrist"}; - - for (const auto & group_name : group_names) { - if (!hardware.torque_on(group_name)) { - std::cerr << group_name << "グループのトルクをONできませんでした." << std::endl; - return -1; - } - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::cout << "5秒後に手先が動き出すため、手先の周りに物や人を近づけないで下さい." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - // 目標速度を段階的に早くする - const double MAX_CURRENT = 0.05; // Ampere - const double STEPS = 10; - for (int i=1; i <=STEPS; i++) { - double goal_current = MAX_CURRENT * i / static_cast(STEPS); - std::cout << "set current:" << goal_current << " A" << std::endl; - hardware.set_current(7, goal_current); - hardware.set_current(8, goal_current); - hardware.set_current(15, goal_current); - hardware.set_current(16, goal_current); - std::this_thread::sleep_for(std::chrono::seconds(1)); - std::cout << "set current:" << -goal_current << " A" << std::endl; - hardware.set_current("right_arm_joint6", -goal_current); - hardware.set_current("right_arm_joint7", -goal_current); - hardware.set_current("left_arm_joint6", -goal_current); - hardware.set_current("left_arm_joint7", -goal_current); - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - - std::cout << "set current: 0.0 A" << std::endl; - std::vector goal_currents = {0.0, 0.0}; - - for (const auto & group_name : group_names) { - hardware.set_currents(group_name, goal_currents); - } - std::this_thread::sleep_for(std::chrono::seconds(1)); - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - - for (const auto & group_name : group_names) { - if (!hardware.torque_off(group_name)) { - std::cerr << group_name << "グループのトルクをOFFできませんでした." << std::endl; - } - } - - std::cout << "Sciurus17との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples01/src/s17_write_position.cpp b/samples/samples01/src/s17_write_position.cpp deleted file mode 100644 index 9cbe40f..0000000 --- a/samples/samples01/src/s17_write_position.cpp +++ /dev/null @@ -1,162 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -int main() { - std::cout << "Sciurus17のサーボモータ目標角度を書き込むサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/sciurus17.yaml"; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - std::cout - << "right_handグループのサーボ最大加速度を0.5pi rad/s^2、最大速度を0.5pi rad/sに設定します." - << std::endl; - if (!hardware.write_max_acceleration_to_group("right_hand", 0.5 * M_PI)) { - std::cerr << "right_handグループの最大加速度を設定できませんでした." << std::endl; - return -1; - } - - if (!hardware.write_max_velocity_to_group("right_hand", 0.5 * M_PI)) { - std::cerr << "right_handグループの最大速度を設定できませんでした." << std::endl; - return -1; - } - - std::cout << "torsoグループのサーボ最大加速度を0.5pi rad/s^2、最大速度を0.5pi rad/sに設定します." - << std::endl; - if (!hardware.write_max_acceleration_to_group("torso", 0.5 * M_PI)) { - std::cerr << "torsoグループの最大加速度を設定できませんでした." << std::endl; - return -1; - } - - if (!hardware.write_max_velocity_to_group("torso", 0.5 * M_PI)) { - std::cerr << "torsoグループの最大速度を設定できませんでした." << std::endl; - return -1; - } - - std::cout << "right_hand、torsoグループのサーボ位置制御PIDゲインに(800, 0, 0)を書き込みます." - << std::endl; - if (!hardware.write_position_pid_gain_to_group("right_hand", 800, 0, 0)) { - std::cerr << "right_handグループにPIDゲインを書き込めませんでした." << std::endl; - return -1; - } - if (!hardware.write_position_pid_gain_to_group("torso", 800, 0, 0)) { - std::cerr << "torsoグループにPIDゲインを書き込めませんでした." << std::endl; - return -1; - } - // PIDゲインは指定したサーボモータにも設定できます. - if (!hardware.write_position_pid_gain(9, 800, 0, 0)) { - std::cerr << "ID:9ジョイントにPIDゲインを書き込めませんでした." << std::endl; - return -1; - } - if (!hardware.write_position_pid_gain("right_arm_joint_hand", 800, 0, 0)) { - std::cerr << "right_arm_joint_handジョイントにPIDゲインを書き込めませんでした." << std::endl; - return -1; - } - - if (!hardware.torque_on("right_hand")) { - std::cerr << "right_handグループのトルクをONできませんでした." << std::endl; - return -1; - } - - if (!hardware.torque_on("torso")) { - std::cerr << "torsoグループのトルクをONできませんでした." << std::endl; - return -1; - } - - std::vector target_positions(3, 0.0); - std::cout << "torsoグループのサーボ目標角度に0.0 radを書き込みます." << std::endl; - std::cout << "5秒後にSciurus17が正面を向く姿勢へ移行するため、Sciurus17の周りに物や人を近づけない" - "で下さい." - << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - hardware.set_positions("torso", target_positions); - if (!hardware.sync_write("torso")) { - std::cerr << "torsoグループのsync writeに失敗しました." << std::endl; - } - std::cout << "5秒間スリープして動作完了を待ちます." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - int dxl_id = 9; - double target_position = 30.0 * M_PI / 180.0; // radian - std::cout << "ID:" << std::to_string(dxl_id) << "のサーボ目標角度に" - << std::to_string(target_position) << " radを書き込みます." << std::endl; - hardware.set_position(dxl_id, target_position); - if (!hardware.sync_write("right_hand")) { - std::cerr << "right_handグループのsync writeに失敗しました." << std::endl; - } - std::cout << "5秒間スリープして動作完了を待ちます." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - std::string joint_name = "right_arm_joint_hand"; - target_position = 0.0; // radian - std::cout << joint_name << "ジョイントのサーボ目標角度に" << std::to_string(target_position) - << " radを書き込みます." << std::endl; - hardware.set_position(joint_name, target_position); - if (!hardware.sync_write("right_hand")) { - std::cerr << "right_handグループのsync writeに失敗しました." << std::endl; - } - std::cout << "5秒間スリープして動作完了を待ちます." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - std::cout - << "right_hand、torsoグループのサーボ位置制御PIDゲインに(5, 0, 0)を書き込み、脱力させます." - << std::endl; - if (!hardware.write_position_pid_gain_to_group("right_hand", 5, 0, 0)) { - std::cerr << "right_handグループにPIDゲインを書き込めませんでした." << std::endl; - } - if (!hardware.write_position_pid_gain_to_group("torso", 5, 0, 0)) { - std::cerr << "torsoグループにPIDゲインを書き込めませんでした." << std::endl; - } - std::cout << "5秒間スリープします." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - if (!hardware.torque_off("right_hand")) { - std::cerr << "right_handグループのトルクをOFFできませんでした." << std::endl; - } - - if (!hardware.torque_off("torso")) { - std::cerr << "torsoグループのトルクをOFFできませんでした." << std::endl; - } - - if (!hardware.write_position_pid_gain_to_group("right_hand", 800, 0, 0)) { - std::cerr << "right_handグループにPIDゲインを書き込めませんでした." << std::endl; - } - - if (!hardware.write_position_pid_gain_to_group("torso", 800, 0, 0)) { - std::cerr << "torsoグループにPIDゲインを書き込めませんでした." << std::endl; - } - std::this_thread::sleep_for(std::chrono::seconds(1)); - - std::cout << "Sciurus17との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples01/src/s17_write_velocity.cpp b/samples/samples01/src/s17_write_velocity.cpp deleted file mode 100644 index 335e7d7..0000000 --- a/samples/samples01/src/s17_write_velocity.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -int main() { - std::cout << "Sciurus17のサーボモータに目標速度を書き込むサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/sciurus17_velocity.yaml"; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - std::vector group_names = {"right_wrist", "left_wrist"}; - - for (const auto & group_name : group_names) { - std::cout << group_name << "グループのサーボ最大加速度を5pi rad/s^2に設定します." - << std::endl; - if (!hardware.write_max_acceleration_to_group(group_name, 5.0 * M_PI)) { - std::cerr << group_name << "グループの最大加速度を設定できませんでした." << std::endl; - return -1; - } - - std::cout << group_name << "グループのサーボ速度制御PIゲインに(100, 1920)を書き込みます." - << std::endl; - if (!hardware.write_velocity_pi_gain_to_group(group_name, 100, 1920)) { - std::cerr << group_name << "グループにPIゲインを書き込めませんでした." << std::endl; - return -1; - } - } - // PIゲインは指定したサーボモータにも設定できます. - if (!hardware.write_velocity_pi_gain(7, 100, 1920)) { - std::cerr << "ID:7ジョイントにPIゲインを書き込めませんでした." << std::endl; - return -1; - } - if (!hardware.write_velocity_pi_gain("right_arm_joint7", 100, 1920)) { - std::cerr << "right_arm_joint7ジョイントにPIゲインを書き込めませんでした." << std::endl; - return -1; - } - - for (const auto & group_name : group_names) { - if (!hardware.torque_on(group_name)) { - std::cerr << group_name << "グループのトルクをONできませんでした." << std::endl; - return -1; - } - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::cout << "5秒後に手先が動き出すため、手先の周りに物や人を近づけないで下さい." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - // 目標速度を段階的に早くする - const double MAX_VELOCITY = M_PI; - const double STEPS = 10; - for (int i=1; i <=STEPS; i++) { - double goal_velocity = MAX_VELOCITY * i / static_cast(STEPS); - std::cout << "set velocity:" << goal_velocity << " rad/s" << std::endl; - hardware.set_velocity(7, goal_velocity); - hardware.set_velocity(8, goal_velocity); - hardware.set_velocity(15, goal_velocity); - hardware.set_velocity(16, goal_velocity); - std::this_thread::sleep_for(std::chrono::seconds(1)); - std::cout << "set velocity:" << -goal_velocity << " rad/s" << std::endl; - hardware.set_velocity("right_arm_joint6", -goal_velocity); - hardware.set_velocity("right_arm_joint7", -goal_velocity); - hardware.set_velocity("left_arm_joint6", -goal_velocity); - hardware.set_velocity("left_arm_joint7", -goal_velocity); - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - - std::cout << "set velocity: 0.0 rad/s" << std::endl; - std::vector goal_velocities = {0.0, 0.0}; - - for (const auto & group_name : group_names) { - hardware.set_velocities(group_name, goal_velocities); - } - std::this_thread::sleep_for(std::chrono::seconds(1)); - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - - for (const auto & group_name : group_names) { - if (!hardware.torque_off(group_name)) { - std::cerr << group_name << "グループのトルクをOFFできませんでした." << std::endl; - } - } - - std::cout << "Sciurus17との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples01/src/x7_onoff.cpp b/samples/samples01/src/x7_onoff.cpp deleted file mode 100644 index 9ed7048..0000000 --- a/samples/samples01/src/x7_onoff.cpp +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -int main() { - std::cout << "CRANE-X7のトルクをON/OFFするサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/crane-x7.yaml"; - - std::cout << "CRANE-X7("; - std::cout << "ポート:" << port_name; - std::cout << " ボーレート:" << std::to_string(baudrate); - std::cout << ")に接続します." << std::endl; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - std::cout << "コンフィグファイル:" << config_file << "を読み込みます." << std::endl; - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - std::string group_name = "arm"; - std::cout << "ジョイントグループ:" << group_name << "のトルクをONにします." << std::endl; - if (!hardware.torque_on(group_name)) { - std::cerr << group_name << "グループのトルクをONできませんでした." << std::endl; - return -1; - } - - std::cout << "10秒間スリープします." << std::endl; - std::cout << "サーボモータの制御モードが位置制御モードのとき、" - << "ロボットに触れるとトルクがONになっていることがわかります." - << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(10)); - - std::cout << "ジョイントグループ:" << group_name << "のトルクをOFFにします." << std::endl; - if (!hardware.torque_off(group_name)) { - std::cerr << group_name << "グループのトルクをOFFできませんでした." << std::endl; - } - - std::cout << "CRANE-X7との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples01/src/x7_read_position.cpp b/samples/samples01/src/x7_read_position.cpp deleted file mode 100644 index 48e91fc..0000000 --- a/samples/samples01/src/x7_read_position.cpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -int main() { - std::cout << "CRANE-X7のサーボモータ角度を読み取るサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/crane-x7.yaml"; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - for (int i = 0; i < 1000; i++) { - if (!hardware.sync_read("arm")) { - std::cerr << "armグループのsync readに失敗しました." << std::endl; - break; - } - - if (!hardware.sync_read("hand")) { - std::cerr << "handグループのsync readに失敗しました." << std::endl; - break; - } - - double position; - int dxl_id = 2; - if (hardware.get_position(dxl_id, position)) { - std::cout << "ID:" << std::to_string(dxl_id) << "のサーボ角度は" << std::to_string(position) - << "radです." << std::endl; - } - - std::string joint_name = "joint_hand"; - if (hardware.get_position(joint_name, position)) { - std::cout << joint_name << "のサーボ角度は" << std::to_string(position) << "radです." - << std::endl; - } - - std::vector positions; - if (hardware.get_positions("arm", positions)) { - for (int i = 0; i < positions.size(); i++) { - std::cout << "armグループの" << std::to_string(i) << "番目のサーボ角度は" - << std::to_string(positions[i]) << "radです." << std::endl; - } - } - - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - std::cout << "CRANE-X7との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples01/src/x7_read_present_values.cpp b/samples/samples01/src/x7_read_present_values.cpp deleted file mode 100644 index dd9132b..0000000 --- a/samples/samples01/src/x7_read_present_values.cpp +++ /dev/null @@ -1,132 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -void print_names(void) { - std::cout << "position[rad], "; - std::cout << "velocity[rad/s], "; - std::cout << "current[A], "; - std::cout << "voltage[V], "; - std::cout << "temperature[deg]"; - std::cout << std::endl; -} - -void print_values(const double position, const double velocity, const double current, - const double voltage, const int8_t temperature) { - std::cout << std::to_string(position) << ", "; - std::cout << std::to_string(velocity) << ", "; - std::cout << std::to_string(current) << ", "; - std::cout << std::to_string(voltage) << ", "; - std::cout << std::to_string(temperature); - std::cout << std::endl; -} - -int main() { - std::cout << "CRANE-X7のサーボモータ"; - std::cout << "位置、速度、電流、入力電圧、温度を読み取るサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/crane-x7_read.yaml"; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - std::cout << "サーボモータの電流値を観察しやすくするため、" << std::endl; - std::cout << "5秒後にhandグループのトルクがONします." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - if (!hardware.torque_on("hand")) { - std::cerr << "handグループのトルクをONできませんでした." << std::endl; - return -1; - } - - std::vector groups = {"arm", "hand"}; - for (int i = 0; i < 1000; i++) { - for (auto group_name : groups) { - if (!hardware.sync_read(group_name)) { - std::cerr << group_name << "グループのsync readに失敗しました." << std::endl; - break; - } - - std::vector positions; - std::vector velocities; - std::vector currents; - std::vector voltages; - std::vector temperatures; - if (hardware.get_positions(group_name, positions) && - hardware.get_velocities(group_name, velocities) && - hardware.get_currents(group_name, currents) && - hardware.get_voltages(group_name, voltages) && - hardware.get_temperatures(group_name, temperatures)) { - std::cout << group_name << ": index, "; - print_names(); - for (int i = 0; i < positions.size(); i++) { - std::cout << std::to_string(i) << ", "; - print_values(positions[i], velocities[i], currents[i], voltages[i], temperatures[i]); - } - } - } - - const int dxl_id = 9; - const std::string joint_name = "joint_hand"; - double position; - double velocity; - double current; - double voltage; - int8_t temperature; - if (hardware.get_position(dxl_id, position) && - hardware.get_velocity(dxl_id, velocity) && - hardware.get_current(dxl_id, current) && - hardware.get_voltage(dxl_id, voltage) && - hardware.get_temperature(dxl_id, temperature)) { - std::cout << "ID:" << std::to_string(dxl_id) << ": "; - print_names(); - print_values(position, velocity, current, voltage, temperature); - } - - if (hardware.get_position(joint_name, position) && - hardware.get_velocity(joint_name, velocity) && - hardware.get_current(joint_name, current) && - hardware.get_voltage(joint_name, voltage) && - hardware.get_temperature(joint_name, temperature)) { - std::cout << joint_name << ": "; - print_names(); - print_values(position, velocity, current, voltage, temperature); - } - - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - if (!hardware.torque_off("hand")) { - std::cerr << "handグループのトルクをOFFできませんでした." << std::endl; - } - - std::cout << "CRANE-X7との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples01/src/x7_thread.cpp b/samples/samples01/src/x7_thread.cpp deleted file mode 100644 index 5cf5a9b..0000000 --- a/samples/samples01/src/x7_thread.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -int main() { - std::cout << "スレッドを起動し、CRANE-X7のサーボモータ角度を読み書きするサンプルです." - << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/crane-x7.yaml"; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - std::cout << "handジョイントグループのトルクをONにします." << std::endl; - if (!hardware.torque_on("hand")) { - std::cerr << "handグループのトルクをONできませんでした." << std::endl; - return -1; - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - std::vector group_names = {"arm", "hand"}; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::cout << "5秒後にCRANE-X7のハンドが開くので、ハンドに触れないでください." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - for (int i = 0; i < 2000; i++) { - for (std::string group_name : group_names) { - std::vector positions; - if (hardware.get_positions(group_name, positions)) { - for (int i = 0; i < positions.size(); i++) { - std::cout << group_name << "グループの" << std::to_string(i) << "番目のサーボ角度は" - << std::to_string(positions[i]) << "radです." << std::endl; - } - } - } - // 肘の関節を開くと、ハンドが開くように目標角度を設定する - double position; - hardware.get_position("joint4", position); - hardware.set_position("joint_hand", 0.5 * (M_PI + position)); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - if (!hardware.torque_off("hand")) { - std::cerr << "handグループのトルクをOFFできませんでした." << std::endl; - } - - std::cout << "CRANE-X7との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples01/src/x7_write_current.cpp b/samples/samples01/src/x7_write_current.cpp deleted file mode 100644 index 06b5cbd..0000000 --- a/samples/samples01/src/x7_write_current.cpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -int main() { - std::cout << "CRANE-X7のサーボモータに目標電流を書き込むサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/crane-x7_current.yaml"; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - if (!hardware.torque_on("wrist")) { - std::cerr << "wristグループのトルクをONできませんでした." << std::endl; - return -1; - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - std::vector group_names = {"wrist"}; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::cout << "5秒後に手先が動き出すため、手先の周りに物や人を近づけないで下さい." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - // 目標速度を段階的に早くする - const double MAX_CURRENT = 0.05; // Ampere - const double STEPS = 10; - for (int i=1; i <=STEPS; i++) { - double goal_current = MAX_CURRENT * i / static_cast(STEPS); - std::cout << "set current:" << goal_current << " A" << std::endl; - hardware.set_current(6, goal_current); - hardware.set_current(7, goal_current); - hardware.set_current(8, goal_current); - std::this_thread::sleep_for(std::chrono::seconds(1)); - std::cout << "set current:" << -goal_current << " A" << std::endl; - hardware.set_current("joint5", -goal_current); - hardware.set_current("joint6", -goal_current); - hardware.set_current("joint7", -goal_current); - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - - std::cout << "set current: 0.0 A" << std::endl; - std::vector goal_currents = {0.0, 0.0, 0.0}; - hardware.set_currents("wrist", goal_currents); - std::this_thread::sleep_for(std::chrono::seconds(1)); - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - if (!hardware.torque_off("wrist")) { - std::cerr << "wristグループのトルクをOFFできませんでした." << std::endl; - } - - std::cout << "CRANE-X7との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples01/src/x7_write_position.cpp b/samples/samples01/src/x7_write_position.cpp deleted file mode 100644 index fb13b0c..0000000 --- a/samples/samples01/src/x7_write_position.cpp +++ /dev/null @@ -1,169 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -int main() { - std::cout << "CRANE-X7のサーボモータ目標角度を書き込むサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/crane-x7.yaml"; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - std::cout << "armグループのサーボ最大加速度を0.5pi rad/s^2、最大速度を0.5pi rad/sに設定します." - << std::endl; - if (!hardware.write_max_acceleration_to_group("arm", 0.5 * M_PI)) { - std::cerr << "armグループの最大加速度を設定できませんでした." << std::endl; - return -1; - } - - if (!hardware.write_max_velocity_to_group("arm", 0.5 * M_PI)) { - std::cerr << "armグループの最大速度を設定できませんでした." << std::endl; - return -1; - } - - std::cout << "handグループのサーボ最大加速度を0.5pi rad/s^2、最大速度を0.5pi rad/sに設定します." - << std::endl; - if (!hardware.write_max_acceleration_to_group("hand", 0.5 * M_PI)) { - std::cerr << "handグループの最大加速度を設定できませんでした." << std::endl; - return -1; - } - - if (!hardware.write_max_velocity_to_group("hand", 0.5 * M_PI)) { - std::cerr << "handグループの最大速度を設定できませんでした." << std::endl; - return -1; - } - - std::cout << "arm、handグループのサーボ位置制御PIDゲインに(800, 0, 0)を書き込みます." - << std::endl; - if (!hardware.write_position_pid_gain_to_group("arm", 800, 0, 0)) { - std::cerr << "armグループにPIDゲインを書き込めませんでした." << std::endl; - return -1; - } - if (!hardware.write_position_pid_gain_to_group("hand", 800, 0, 0)) { - std::cerr << "handグループにPIDゲインを書き込めませんでした." << std::endl; - return -1; - } - // PIDゲインは指定したサーボモータにも設定できます. - if (!hardware.write_position_pid_gain(9, 800, 0, 0)) { - std::cerr << "ID:9ジョイントにPIDゲインを書き込めませんでした." << std::endl; - return -1; - } - if (!hardware.write_position_pid_gain("joint_hand", 800, 0, 0)) { - std::cerr << "joint_handジョイントにPIDゲインを書き込めませんでした." << std::endl; - return -1; - } - - if (!hardware.torque_on("arm")) { - std::cerr << "armグループのトルクをONできませんでした." << std::endl; - return -1; - } - - if (!hardware.torque_on("hand")) { - std::cerr << "handグループのトルクをONできませんでした." << std::endl; - return -1; - } - - std::vector target_positions(7, 0.0); - std::cout << "armグループのサーボ目標角度に0.0 radを書き込みます." << std::endl; - std::cout << "5秒後にX7が垂直姿勢へ移行するため、X7の周りに物や人を近づけないで下さい." - << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - hardware.set_positions("arm", target_positions); - if (!hardware.sync_write("arm")) { - std::cerr << "armグループのsync writeに失敗しました." << std::endl; - } - std::cout << "5秒間スリープして動作完了を待ちます." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - int dxl_id = 5; - double target_position = -120.0 * M_PI / 180.0; // radian - - std::cout << "ID:" << std::to_string(dxl_id) << "のサーボ目標角度に" - << std::to_string(target_position) << " radを書き込みます." << std::endl; - hardware.set_position(dxl_id, target_position); - if (!hardware.sync_write("arm")) { - std::cerr << "armグループのsync writeに失敗しました." << std::endl; - } - std::cout << "5秒間スリープして動作完了を待ちます." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - std::string joint_name = "joint_hand"; - target_position = 30.0 * M_PI / 180.0; // radian - std::cout << joint_name << "ジョイントのサーボ目標角度に" << std::to_string(target_position) - << " radを書き込みます." << std::endl; - hardware.set_position(joint_name, target_position); - if (!hardware.sync_write("hand")) { - std::cerr << "handグループのsync writeに失敗しました." << std::endl; - } - std::cout << "5秒間スリープして動作完了を待ちます." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - target_position = 0.0; // radian - std::cout << joint_name << "ジョイントのサーボ目標角度に" << std::to_string(target_position) - << " radを書き込みます." << std::endl; - hardware.set_position(joint_name, target_position); - if (!hardware.sync_write("hand")) { - std::cerr << "handグループのsync writeに失敗しました." << std::endl; - } - std::cout << "5秒間スリープして動作完了を待ちます." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - std::cout << "arm、handグループのサーボ位置制御PIDゲインに(5, 0, 0)を書き込み、脱力させます." - << std::endl; - if (!hardware.write_position_pid_gain_to_group("arm", 5, 0, 0)) { - std::cerr << "armグループにPIDゲインを書き込めませんでした." << std::endl; - } - if (!hardware.write_position_pid_gain_to_group("hand", 5, 0, 0)) { - std::cerr << "handグループにPIDゲインを書き込めませんでした." << std::endl; - } - std::cout << "10秒間スリープします." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(10)); - - if (!hardware.torque_off("arm")) { - std::cerr << "armグループのトルクをOFFできませんでした." << std::endl; - } - - if (!hardware.torque_off("hand")) { - std::cerr << "handグループのトルクをOFFできませんでした." << std::endl; - } - - if (!hardware.write_position_pid_gain_to_group("arm", 800, 0, 0)) { - std::cerr << "armグループにPIDゲインを書き込めませんでした." << std::endl; - } - if (!hardware.write_position_pid_gain_to_group("hand", 800, 0, 0)) { - std::cerr << "handグループにPIDゲインを書き込めませんでした." << std::endl; - } - std::this_thread::sleep_for(std::chrono::seconds(1)); - - std::cout << "CRANE-X7との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples01/src/x7_write_velocity.cpp b/samples/samples01/src/x7_write_velocity.cpp deleted file mode 100644 index 47566fd..0000000 --- a/samples/samples01/src/x7_write_velocity.cpp +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" - -int main() { - std::cout << "CRANE-X7のサーボモータに目標速度を書き込むサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string config_file = "../config/crane-x7_velocity.yaml"; - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - std::cout << "wristグループのサーボ最大加速度を5pi rad/s^2に設定します." - << std::endl; - if (!hardware.write_max_acceleration_to_group("wrist", 5.0 * M_PI)) { - std::cerr << "wristグループの最大加速度を設定できませんでした." << std::endl; - return -1; - } - - std::cout << "wristグループのサーボ速度制御PIゲインに(100, 1920)を書き込みます." - << std::endl; - if (!hardware.write_velocity_pi_gain_to_group("wrist", 100, 1920)) { - std::cerr << "wristグループにPIゲインを書き込めませんでした." << std::endl; - return -1; - } - // PIゲインは指定したサーボモータにも設定できます. - if (!hardware.write_velocity_pi_gain(7, 100, 1920)) { - std::cerr << "ID:7ジョイントにPIゲインを書き込めませんでした." << std::endl; - return -1; - } - if (!hardware.write_velocity_pi_gain("joint7", 100, 1920)) { - std::cerr << "joint7ジョイントにPIゲインを書き込めませんでした." << std::endl; - return -1; - } - - if (!hardware.torque_on("wrist")) { - std::cerr << "wristグループのトルクをONできませんでした." << std::endl; - return -1; - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - std::vector group_names = {"wrist"}; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::cout << "5秒後に手先が動き出すため、手先の周りに物や人を近づけないで下さい." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - // 目標速度を段階的に早くする - const double MAX_VELOCITY = M_PI; - const double STEPS = 10; - for (int i=1; i <=STEPS; i++) { - double goal_velocity = MAX_VELOCITY * i / static_cast(STEPS); - std::cout << "set velocity:" << goal_velocity << " rad/s" << std::endl; - hardware.set_velocity(6, goal_velocity); - hardware.set_velocity(7, goal_velocity); - hardware.set_velocity(8, goal_velocity); - std::this_thread::sleep_for(std::chrono::seconds(1)); - std::cout << "set velocity:" << -goal_velocity << " rad/s" << std::endl; - hardware.set_velocity("joint5", -goal_velocity); - hardware.set_velocity("joint6", -goal_velocity); - hardware.set_velocity("joint7", -goal_velocity); - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - - std::cout << "set velocity: 0.0 rad/s" << std::endl; - std::vector goal_velocities = {0.0, 0.0, 0.0}; - hardware.set_velocities("wrist", goal_velocities); - std::this_thread::sleep_for(std::chrono::seconds(1)); - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - if (!hardware.torque_off("wrist")) { - std::cerr << "wristグループのトルクをOFFできませんでした." << std::endl; - } - - std::cout << "CRANE-X7との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples02/.clang-format b/samples/samples02/.clang-format deleted file mode 100644 index 7ca5230..0000000 --- a/samples/samples02/.clang-format +++ /dev/null @@ -1,2 +0,0 @@ -BasedOnStyle: Google -ColumnLimit: 100 \ No newline at end of file diff --git a/samples/samples02/CMakeLists.txt b/samples/samples02/CMakeLists.txt deleted file mode 100644 index 691cee8..0000000 --- a/samples/samples02/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -cmake_minimum_required(VERSION 3.10) - -# set the project name and version -project(samples02 VERSION 1.0) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -# specify the C++ standard -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED True) - -# 生成された実行ファイルをソースのbinディレクトリに出力する -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) - -# サンプルのビルド -set(list_samples - x7_forward_kinematics - s17_forward_kinematics - x7_inverse_kinematics - s17_inverse_kinematics -) -foreach(sample IN LISTS list_samples) - message("${sample}") - add_executable(${sample} - src/${sample}.cpp - ) - target_link_libraries(${sample} PRIVATE rt_manipulators_cpp Eigen3::Eigen) -endforeach() diff --git a/samples/samples02/README.md b/samples/samples02/README.md deleted file mode 100644 index 590b183..0000000 --- a/samples/samples02/README.md +++ /dev/null @@ -1,283 +0,0 @@ -# サンプル集02 リンクの位置・姿勢を変更する - -- [サンプル集02 リンクの位置・姿勢を変更する](#サンプル集02-リンクの位置姿勢を変更する) - - [サンプルのビルド](#サンプルのビルド) - - [ロボットのサーボモータの動かし方について](#ロボットのサーボモータの動かし方について) - - [順運動学を解いてリンクの位置・姿勢を求める](#順運動学を解いてリンクの位置姿勢を求める) - - [動画](#動画) - - [解説](#解説) - - [逆運動学を解いて手先を任意の位置・姿勢に移動させる](#逆運動学を解いて手先を任意の位置姿勢に移動させる) - - [動画](#動画-1) - - [解説](#解説-1) - -## サンプルのビルド - -次のコマンドを実行して、サンプル集をビルドします。 - -```sh -$ ./build_samples.bash -``` - -ビルドに成功すると`samples02/bin/`ディレクトリに実行ファイルが生成されます。 - -## ロボットのサーボモータの動かし方について - -あらかじめ、["samples01のREADME"](../samples01/README.md)をよく読み、 -ロボットのサーボモータの動かし方を理解してください。 - -## 順運動学を解いてリンクの位置・姿勢を求める - -次のコマンドを実行します。 -ロボットの手先リンクの位置・姿勢（Z-Y-Xオイラー角）を表示します。 - -```sh -# CRANE-X7の場合 -$ cd bin/ -$ ./x7_forward_kinematics -# Sciurus17の場合 -$ ./s17_forward_kinematics -``` - -実行結果（CRANE-X7の場合） - -```sh -./x7_forward_kinematics -CRANE-X7のサーボモータ角度を読み取り、順運動学を解くサンプルです. -リンク情報ファイル:../config/crane-x7_links.csvを読み込みます -リンクID:1リンク名:CRANE-X7_Base -親:0, 子:2, 姉妹兄弟:0 -親リンクに対する関節軸ベクトル a: -0 -0 -0 -親リンクに対する相対位置 b: -... -リンク名: CRANE-X7_Link7 -位置 X: 0.0053478 [m] -位置 Y: 0.00468035 [m] -位置 Z: 0.185839 [m] -姿勢 Z:139.47 [deg] -姿勢 Y:80.9454 [deg] -姿勢 X:132.577 [deg] -リンク名: CRANE-X7_Link7 -位置 X: 0.0053478 [m] -位置 Y: 0.00468035 [m] -位置 Z: 0.185839 [m] -姿勢 Z:139.47 [deg] -姿勢 Y:80.9454 [deg] -姿勢 X:132.577 [deg] -... -``` - -### 動画 - -[![](https://img.youtube.com/vi/c76qW3ekAv4/sddefault.jpg)](https://youtu.be/c76qW3ekAv4) - -### 解説 - -RTマニピュレータc++ライブラリの運動学機能を使用する場合は`rt_manipulators_cpp/kinematics.hpp`、 -`rt_manipulators_cpp/kinematics_utils.hpp`、`rt_manipulators_cpp/link.hpp`をincludeします。 - -`kinematics.hpp`には運動学を計算する関数が定義されています。 -`kinematics_utils.hpp`には運動学計算を補助する関数が定義されています。 -`link.hpp`にはリンク情報を持つ`manipulators_link::Link`クラスが定義されています。 - -```cpp -#include "rt_manipulators_cpp/kinematics.hpp" -#include "rt_manipulators_cpp/kinematics_utils.hpp" -#include "rt_manipulators_cpp/link.hpp" -``` - -`manipulators_link::Link`クラスは次のようなパラメータを保持します。 -3次元空間のベクトルや行列は[Eigenライブラリ](https://eigen.tuxfamily.org/index.php?title=Main_Page) -を使用して表現しています。 - -```cpp -class Link{ - public: - std::string name; // リンク名 - int sibling; // 姉妹兄弟リンクID - int child; // 子リンクID - int parent; // 親リンクID - Eigen::Vector3d p; // ワールド座標系での位置 - Eigen::Matrix3d R; // ワールド座標系での姿勢 - Eigen::Vector3d v; // ワールド座標系での速度 - Eigen::Vector3d w; // ワールド座標系での角速度ベクトル - double q; // 関節位置 - double dq; // 関節速度 - double ddq; // 関節加速度 - Eigen::Vector3d a; // 親リンクに対する関節軸ベクトル - Eigen::Vector3d b; // 親リンクに対する相対位置 - double m; // 質量 - Eigen::Vector3d c; // 自リンクに対する重心位置 - Eigen::Matrix3d I; // 自リンクに対する慣性テンソル - int dxl_id; // 対応するDynamixelのID - double min_q; // 関節位置の下限値 - double max_q; // 関節位置の上限値 -}; -``` - -ロボットのリンク構成は`manipulators_link::Link`クラスを`std::vector`に格納して表現します。 - -```cpp -// ロボットのリンク構成を表現 -std::vector links; -``` - -リンク構成を表現したCSVファイルを`kinematics_utils::parse_link_config_file(file_path)`で読み込むことで、 -リンク構成を取得できます。 - -```cpp -// CSVファイルを解析してリンク構成を取得する -std::vector links = kinematics_utils::parse_link_config_file("../config/crane-x7_links.csv"); -``` - -`config`ディレクトリに用意されているCSVファイルは、 -公開されているCRANE-X7とSciurus17のリンク情報リファレンスをCSV形式でダウンロードしたものです。 - -- [CRANE-X7リンク情報リファレンス](https://docs.google.com/spreadsheets/d/1I268mnab4m-f6us0Au3AGd64-2iGkSwxaLrDplSjHY8/edit#gid=735472399) -- [Sciurus17リンク情報リファレンス](https://docs.google.com/spreadsheets/d/1Q4z3M3cS1pQOEn3iXKLiIQIOr6czvECxSXEPS2-PGvA/edit#gid=1687288769) - -各リンクのローカル座標系の姿勢は、ジョイント角度(q)が0 degのとき、 -ワールド座標系の姿勢(正面がX軸正方向、鉛直上向きがZ軸正方向となる[右手系](https://ja.wikipedia.org/wiki/%E5%8F%B3%E6%89%8B%E7%B3%BB))と一致します。 - -リンク番号とリンク構成(`std::vector`)のインデックスは一致するため、次のようにリンク情報へアクセスできます。 - -```cpp -// ダミーリンクの情報を取得 -auto dummy_link = links[0]; -// ベースリンクの情報を取得 -auto link_base_position = links[1]; -// リンクID4の位置を取得 -auto position = links[4].p; -// リンクID5を動かすジョイントの現在角度を書き込む -links[5].q = 0.0; -``` - -リンクの位置姿勢を更新するため、`kinematics::forward_kinematics(links, start_id)`を実行し、順運動学を解きます。 -引数にはリンク構成と、順運動学の計算開始リンク番号を入力します。 - -```cpp -// ベースリンクから逐次的に順運動学を解き、各リンクの位置・姿勢を更新する -kinematics::forward_kinematics(links, 1); -``` - -リンク構成の情報を一括で出力するため、`kinematics_utils::print_links(links, start_id)`を実行します。 -引数にはリンク構成と、リンク情報の出力開始リンク番号を入力します。 - -```cpp -// ベースリンクから終端リンクまでのリンク情報を出力する -kinematics_utils::print_links(links, 1); -``` - -## 逆運動学を解いて手先を任意の位置・姿勢に移動させる - -次のコマンドを実行します。 -CRANE-X7では、手先が0.2m前方の5点に向かって移動します。 -Sciurus17では、左右の手先が0.4m前方の5点に向かって移動します。 - -***安全のためロボットの周りに物や人を近づけないでください。*** - -```sh -# CRANE-X7の場合 -$ cd bin/ -$ ./x7_inverse_kinematics -# Sciurus17の場合 -$ ./s17_inverse_kinematics -``` - -実行結果（CRANE-X7の場合） - -```sh -./x7_inverse_kinematics -手先目標位置・姿勢をもとに逆運動学を解き、CRANE-X7を動かすサンプルです. -リンク情報ファイル:../config/crane-x7_links.csvを読み込みます -リンクID:1リンク名:CRANE-X7_Base -親:0, 子:2, 姉妹兄弟:0 -親リンクに対する関節軸ベクトル a: -0 -0 -0 -親リンクに対する相対位置 b: -... -5秒後にX7が垂直姿勢へ移行するため、X7の周りに物や人を近づけないで下さい. -正面へ移動 -目標位置: -0.2 - 0 -0.3 -目標姿勢: -2.22045e-16 0 1 - 0 1 0 - -1 0 2.22045e-16 ----------------------- -左上へ移動 -目標位置: -0.2 -0.2 -0.5 -目標姿勢: -1 0 0 -0 1 0 -0 0 1 ----------------------- -左下へ移動 -... -``` - -### 動画 - -[![](https://img.youtube.com/vi/jxaVbs9GzFY/sddefault.jpg)](https://youtu.be/jxaVbs9GzFY) - -### 解説 - -逆運動学を解く前に、リンク構成の各リンク関節位置の下限値`q_min`(radian)と上限値`q_max`(radian)を入力します。 - -```cpp -// リンクID2関節位置の下限値を設定する -links[2].q_min = -M_PI; -// リンクID2関節位置の上限値を設定する -links[2].q_max = -M_PI; -``` - -`Hardware.get_min_position_limit(id, min_position_limit)`、 -`Hardware.get_max_position_limit(id, max_position_limit)`、 -を使用することで、各サーボモータ関節位置の下限値と上限値を取得できます。 -引数にはサーボモータのIDを限界値の格納先を入力します。 - -```cpp -// リンクID2に対応するサーボモータの限界値を取得し、リンクパラメータに設定する -hardware.get_max_position_limit(links[2].dxl_id, links[2].max_q); -hardware.get_min_position_limit(links[2].dxl_id, links[2].min_q); -``` - -任意のリンクを任意の位置・姿勢へ移動させるため、 -`kinematics::inverse_kinematics_LM(links, target_id, target_p, target_R, q_list)`を実行し、逆運動学を解きます。 -引数にはリンク構成と、移動させたいリンク、目標位置(`Eigen::Vector3d`)、目標姿勢(`Eigen::Matrix3d`)、 -関節位置の格納先(`std::map` または `kinematics_utils::q_list_t`)を入力します。 - -`kinematics::inverse_kinematics_LM()`には、 -数値解法による逆運動学の計算アルゴリズムが実装されています。 -任意の位置姿勢に対してロボットを動かしたい場合は、 -**ロボットを動かす前に逆運動学の解が発散しないか十分に検証してください。** - -`kinematics_utils::rotation_from_euler_ZYX(z, y, x)`を使用すると、 -Z-Y-Xオイラー角から回転行列を生成できます。 - -```cpp -Eigen::Vector3d target_p; -Eigen::Matrix3d target_R; -kinematics_utils::q_list_t q_list; - -// 目標位置(X方向に0.2m、Y方向に0.0m、Z方向に0.3m) -target_p << 0.2, 0.0, 0.3; -// 目標姿勢(Y軸周りにpi/2回転) -target_R = kinematics_utils::rotation_from_euler_ZYX(0, M_PI_2, 0); - -// 逆運動学を解き、関節位置を取得する -kinematics::inverse_kinematics_LM(links, 8, target_p, target_R, q_list); -// 関節位置をサーボモータへ設定する -for (const auto & [target_id, q_value] : q_list) { - hardware.set_position(links[target_id].dxl_id, q_value); -} -``` diff --git a/samples/samples02/build_samples.bash b/samples/samples02/build_samples.bash deleted file mode 100755 index 53af683..0000000 --- a/samples/samples02/build_samples.bash +++ /dev/null @@ -1,18 +0,0 @@ -#!/usr/bin/env bash - -set -e - -BUILD_DIR=build - -echo "サンプルプログラムをビルドします" -cd $(dirname $0) - -if [ ! -d $BUILD_DIR ]; then - mkdir $BUILD_DIR -fi - -cd $BUILD_DIR -cmake .. -make - -echo "サンプルプログラムをビルドしました" \ No newline at end of file diff --git a/samples/samples02/config/crane-x7.yaml b/samples/samples02/config/crane-x7.yaml deleted file mode 100644 index 2a800b2..0000000 --- a/samples/samples02/config/crane-x7.yaml +++ /dev/null @@ -1,31 +0,0 @@ -joint_groups: - arm: - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 - sync_read: - - position - sync_write: - - position - hand: - joints: - - joint_hand - sync_read: - - position - sync_write: - - position - - -joint1: { id: 2, dynamixel: "XM430", operating_mode: 3 } -joint2: { id: 3, dynamixel: "XM540", operating_mode: 3 } -joint3: { id: 4, dynamixel: "XM430", operating_mode: 3 } -joint4: { id: 5, dynamixel: "XM430", operating_mode: 3 } -joint5: { id: 6, dynamixel: "XM430", operating_mode: 3 } -joint6: { id: 7, dynamixel: "XM430", operating_mode: 3 } -joint7: { id: 8, dynamixel: "XM430", operating_mode: 3 } -joint_hand: { id: 9, dynamixel: "XM430", operating_mode: 3 } \ No newline at end of file diff --git a/samples/samples02/config/sciurus17.yaml b/samples/samples02/config/sciurus17.yaml deleted file mode 100644 index 2b210fd..0000000 --- a/samples/samples02/config/sciurus17.yaml +++ /dev/null @@ -1,75 +0,0 @@ - -joint_groups: - right_arm: - joints: - - right_arm_joint1 - - right_arm_joint2 - - right_arm_joint3 - - right_arm_joint4 - - right_arm_joint5 - - right_arm_joint6 - - right_arm_joint7 - sync_read: - - position - sync_write: - - position - - right_hand: - joints: - - right_arm_joint_hand - sync_read: - - position - sync_write: - - position - - left_arm: - joints: - - left_arm_joint1 - - left_arm_joint2 - - left_arm_joint3 - - left_arm_joint4 - - left_arm_joint5 - - left_arm_joint6 - - left_arm_joint7 - sync_read: - - position - sync_write: - - position - - left_hand: - joints: - - left_arm_joint_hand - sync_read: - - position - sync_write: - - position - - torso: - joints: - - waist_joint - - neck_yaw_joint - - neck_pitch_joint - sync_read: - - position - sync_write: - - position - -right_arm_joint1: { id: 2, dynamixel: "XM540", operating_mode: 3 } -right_arm_joint2: { id: 3, dynamixel: "XM540", operating_mode: 3 } -right_arm_joint3: { id: 4, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint4: { id: 5, dynamixel: "XM540", operating_mode: 3 } -right_arm_joint5: { id: 6, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint6: { id: 7, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint7: { id: 8, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint_hand: { id: 9, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint1: { id: 10, dynamixel: "XM540", operating_mode: 3 } -left_arm_joint2: { id: 11, dynamixel: "XM540", operating_mode: 3 } -left_arm_joint3: { id: 12, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint4: { id: 13, dynamixel: "XM540", operating_mode: 3 } -left_arm_joint5: { id: 14, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint6: { id: 15, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint7: { id: 16, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint_hand: { id: 17, dynamixel: "XM430", operating_mode: 3 } -waist_joint: { id: 18, dynamixel: "XM430", operating_mode: 3 } -neck_yaw_joint: { id: 19, dynamixel: "XM430", operating_mode: 3 } -neck_pitch_joint: { id: 20, dynamixel: "XM430", operating_mode: 3 } diff --git a/samples/samples02/config/sciurus17_links.csv b/samples/samples02/config/sciurus17_links.csv deleted file mode 100644 index 38409de..0000000 --- a/samples/samples02/config/sciurus17_links.csv +++ /dev/null @@ -1,30 +0,0 @@ -リンク名称,ID,,,,ワールド座標系ホームポジション位置,,,ワールド座標系リンク間相対距離,,,ワールド座標系ホームポジション姿勢,,,質量[g],ローカル座標系質量位置,,,ローカル座標系慣性テンソル(重心),,,,,,ハードウェア可動範囲,,保護設定可動範囲,,ホームポジション角度,回転軸方向,サーボモータ種類,トルク定数,サーボモータID,サーボモータ可動範囲,,通信速度,Return Delay Time,Operation Mode -,自分リンク,妹リンク,子リンク,親リンク,x[mm],y[mm],z[mm],x[mm],y[mm],z[mm],X[deg],Y[deg],Z[deg],m[g],x[mm],y[mm],z[mm],Ixx[gmm^2],Ixy[gmm^2],Iyy[gmm^2],Ixz[gmm^2],Iyz[gmm^2],Izz[gmm^2],θ1[deg],θ2[deg],θ1[deg],θ2[deg],サーボ指令値[deg],±XYZ,型番,[Nm/A],ID,θ1[-],θ2[-],bps,-,- -Sciurus17_Base,1,-,2,-,0,0,0,0,0,0,0,0,0,1722.937,-24.012,0.139,59.566,6398092.663,-6808.969,7611664.357,-331509.04,2026.354,6331936.461,-,-,,,-,-,-,-,-,-,-,-,-,- -Sciurus17_Body,2,24,3,1,0,0,131.5,0,0,131.5,0,0,0,1482.929,57.11,0.372,238.229,15601031.58,-15890.903,13969734.5,-3805596.494,-20695.221,8327993.097,-110,110,-107,107,180,Z+,XM540-W270,2.409,18,831,3265,3Mbps,0,3 -Sciurus17_Neck1,3,5,4,2,80.83,0,471,80.83,0,339.5,0,0,0,168,-0.022,0.029,40.86,63143.268,-0.078,66522.419,-126.072,187.375,32744.236,-165,165,-162,162,180,Z+,XM430-W350,1.783,19,205,3891,3Mbps,0,3 -Sciurus17_Neck2,4,23,-,3,80.83,0,525,0,0,54,90,0,0,312.172,19.979,78.687,0.151,964350.46,-56112.026,1027677.585,-731.103,1684.659,1065891.198,-90,90,-90,60,180,Y-,XM430-W350,1.783,20,1024,2731,3Mbps,0,3 -Sciurus17_R_Link1,5,14,6,2,80.83,-96.5,420,80.83,-96.5,288.5,90,0,0,258,-0.656,-0.264,46.367,176217.476,-5.425,179207.793,586.979,-166.297,87950.071,-160,160,-157,157,180,Y-,XM540-W270,2.409,2,262,3834,3Mbps,0,3 -Sciurus17_R_Link2,6,-,7,5,80.83,-160.6,420,0,-64.1,0,0,90,0,134,0.033,-33.96,-0.185,172046.031,-119.176,98834.156,-535.109,580.743,136915.218,-90,90,-90,90,180,X-,XM540-W270,2.409,3,1024,3072,3Mbps,0,3 -Sciurus17_R_Link3,7,-,8,6,80.83,-225.6,420,0,-65,0,90,0,0,443,-13.791,0.245,113.391,2336253.636,-1190.372,2414711.907,-75998.279,-8024.912,236305.543,-160,160,-157,157,180,Y-,XM430-W350,1.783,4,262,3834,3Mbps,0,3 -Sciurus17_R_Link4,8,-,9,7,80.83,-410.6,420,0,-185,0,0,0,0,241.66,-8.881,-75.749,0.038,603280.74,-6670.94,143154.3,250.846,-2309.247,598152.333,0,156.2,0,156.2,180,Z+,XM540-W150,1.659,5,2048,3825,3Mbps,0,3 -Sciurus17_R_Link5,9,25,10,8,80.83,-531.6,420,0,-121,0,90,0,0,224,13.531,-0.304,70.369,294726.731,-4809.196,290568.087,-21387.985,13192.269,125378.656,-160,160,-157,157,180,Y-,XM430-W350,1.783,6,262,3834,3Mbps,0,3 -Sciurus17_R_Link6,10,-,11,9,80.83,-660.6,420,0,-129,0,0,0,0,140,-6.156,3.583,0.755,39499.661,-2921.88,60684.342,-612.042,357.397,73112.015,-120,60,-120,60,180,Z+,XM430-W350,1.783,7,683,2731,3Mbps,0,3 -Sciurus17_R_Link7,11,-,12,10,80.83,-703.6,420,0,-43,0,90,0,0,121,-0.118,1.006,20.65,32600.928,-69.84,41969.484,-254.224,-409.16,43040.018,-170,170,-167,167,180,Y-,XM430-W350,1.783,8,148,3948,3Mbps,0,3 -Sciurus17_R_HandA,12,13,-,11,68.83,-727.6,420,-12,-24,0,0,0,0,15.8,2.649,-26.849,3.757,10192.75,-197.895,5023.728,98.609,-554.407,6322.202,0,30,-5,90,180,Z+,XM430-W350,1.783,9,1991,3072,3Mbps,0,3 -Sciurus17_R_HandB,13,-,-,11,80.83,-751.6,420,12,-24,0,0,0,0,13.9,-2.707,-30.901,2.845,7483.7,361.283,4088.581,-63.385,-228.226,4372.991,0,30,-5,90,180,Z+,-,,-,-,-,-,-,- -Sciurus17_L_Link1,14,-,15,2,80.83,96.5,420,80.83,96.5,288.5,-90,0,0,258,-0.537,0.27,46.27,170007.992,13.313,175079.881,454.301,159.916,85282.27,-160,160,-157,157,180,Y+,XM540-W270,2.409,10,262,3834,3Mbps,0,3 -Sciurus17_L_Link2,15,-,16,14,80.83,160.6,420,0,64.1,0,0,90,0,134,-0.07,33.946,-0.199,172421.741,-98.964,99076.413,-266.684,-418.743,137151.205,-90,90,-90,90,180,X-,XM540-W270,2.409,11,1024,3072,3Mbps,0,3 -Sciurus17_L_Link3,16,-,17,15,80.83,225.6,420,0,65,0,-90,0,0,443,-13.943,-0.241,113.533,2343035.312,1178.732,2422253.94,-72414.462,8074.759,237122.927,-160,160,-157,157,180,Y+,XM430-W350,1.783,12,262,3834,3Mbps,0,3 -Sciurus17_L_Link4,17,-,18,16,80.83,410.6,420,0,185,0,0,0,0,241.66,-8.904,75.732,0.033,603838.474,6315.625,143107.114,116.002,2112.212,598666.56,-156.2,0,-156.2,0,180,Z+,XM540-W150,1.659,13,271,2048,3Mbps,0,3 -Sciurus17_L_Link5,18,26,19,17,80.83,531.6,420,0,121,0,-90,0,0,224,13.732,0.348,71.158,286973.35,4321.461,282370.572,-19101.45,-13052.23,125680.836,-160,160,-157,157,180,Y+,XM430-W350,1.783,14,262,3834,3Mbps,0,3 -Sciurus17_L_Link6,19,-,20,18,80.83,660.6,420,0,129,0,0,0,0,140,-6.173,-3.627,0.744,39902.954,3014.503,61099.851,-613.647,-361.668,73723.447,-60,120,-60,120,180,Z+,XM430-W350,1.783,15,1365,3413,3Mbps,0,3 -Sciurus17_L_Link7,20,-,21,19,80.83,703.6,420,0,43,0,90,0,0,121,-0.118,1.074,-20.65,32593.608,-72.866,41969.484,254.224,436.76,43032.698,-170,170,-167,167,180,Y+,XM430-W350,1.783,16,148,3948,3Mbps,0,3 -Sciurus17_L_HandA,21,22,-,20,68.83,727.6,420,-12,24,0,0,0,0,15.8,2.649,26.849,3.757,10192.695,197.91,5023.727,98.601,554.38,6322.142,-30,0,-90,5,180,Z+,XM430-W350,1.783,17,1024,2105,3Mbps,0,3 -Sciurus17_L_HandB,22,-,-,20,80.83,751.6,420,12,24,0,0,0,0,13.9,-3.008,30.851,2.845,7483.736,-361.268,4088.583,-63.393,228.245,4373.032,-30,0,-90,5,180,Z+,-,,-,-,-,-,-,- -cameraHead_link,23,-,-,3,162.615,20,616,81.785,20,91,0,0,0,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,- -cameraBreast_link,24,-,-,1,114.576,0,349.159,114.576,0,217.659,0,60,0,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,- -Sciurus17_R_Link5_ARmarker,25,-,-,8,127.53,-531.6,513,46.7,0,93,0,0,0,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,- -Sciurus17_L_Link5_ARmarker,26,-,-,17,127.53,531.6,513,46.7,0,93,0,0,0,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,- -右手系、正面はX方向,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, -,,,,,,,,,,,,,,756.36,,,,,,,,,,,,,,,,,,,,,,, \ No newline at end of file diff --git a/samples/samples02/src/s17_forward_kinematics.cpp b/samples/samples02/src/s17_forward_kinematics.cpp deleted file mode 100644 index b5f68e6..0000000 --- a/samples/samples02/src/s17_forward_kinematics.cpp +++ /dev/null @@ -1,138 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" -#include "rt_manipulators_cpp/kinematics.hpp" -#include "rt_manipulators_cpp/kinematics_utils.hpp" -#include "rt_manipulators_cpp/link.hpp" - -void set_right_arm_joint_positions(std::vector & links, - std::vector positions) { - // リンクにright_armジョイントの現在角度をセットする - if (positions.size() != 7) { - std::cerr << "引数positionsには7個のジョイント角度をセットしてください" << std::endl; - return; - } - - int start_id = 5; // R_Link1 - for (int i=0; i < positions.size(); i++) { - links[start_id + i].q = positions[i]; - } -} - -void set_left_arm_joint_positions(std::vector & links, - std::vector positions) { - // リンクにleft_armジョイントの現在角度をセットする - if (positions.size() != 7) { - std::cerr << "引数positionsには7個のジョイント角度をセットしてください" << std::endl; - return; - } - - int start_id = 14; // L_Link1 - for (int i=0; i < positions.size(); i++) { - links[start_id + i].q = positions[i]; - } -} - -void set_torso_joint_positions(std::vector & links, - std::vector positions) { - // リンクにtorsoジョイントの現在角度をセットする - if (positions.size() != 3) { - std::cerr << "引数positionsには3個のジョイント角度をセットしてください" << std::endl; - return; - } - - int start_id = 2; // Body - for (int i=0; i < positions.size(); i++) { - links[start_id + i].q = positions[i]; - } -} - -int main() { - std::cout << "Sciurus17のサーボモータ角度を読み取り、順運動学を解くサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string hardware_config_file = "../config/sciurus17.yaml"; - std::string link_config_file = "../config/sciurus17_links.csv"; - - auto links = kinematics_utils::parse_link_config_file(link_config_file); - kinematics::forward_kinematics(links, 1); - kinematics_utils::print_links(links, 1); - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(hardware_config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - std::vector group_names = {"right_arm", "right_hand", "left_arm", - "left_hand", "torso"}; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::this_thread::sleep_for(std::chrono::seconds(3)); - - for (int i = 0; i < 4000; i++) { - std::vector positions; - if (hardware.get_positions("right_arm", positions)) { - set_right_arm_joint_positions(links, positions); - kinematics::forward_kinematics(links, 1); - positions.clear(); - } - if (hardware.get_positions("left_arm", positions)) { - set_left_arm_joint_positions(links, positions); - kinematics::forward_kinematics(links, 1); - positions.clear(); - } - if (hardware.get_positions("torso", positions)) { - set_torso_joint_positions(links, positions); - kinematics::forward_kinematics(links, 1); - positions.clear(); - } - - std::vector target_links = {11, 20}; - for (auto target_link : target_links) { - std::cout << "リンク名: " << links[target_link].name << std::endl; - auto pos_xyz = links[target_link].p; - std::cout << "位置 X: " << pos_xyz[0] << "\t[m]" << std::endl; - std::cout << "位置 Y: " << pos_xyz[1] << "\t[m]" << std::endl; - std::cout << "位置 Z: " << pos_xyz[2] << "\t[m]" << std::endl; - auto euler_zyx = kinematics_utils::rotation_to_euler_ZYX(links[target_link].R); - std::cout << "姿勢 Z:" << euler_zyx[0] * 180.0 / M_PI << "\t[deg]" << std::endl; - std::cout << "姿勢 Y:" << euler_zyx[1] * 180.0 / M_PI << "\t[deg]" << std::endl; - std::cout << "姿勢 X:" << euler_zyx[2] * 180.0 / M_PI << "\t[deg]" << std::endl; - } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - std::cout << "Sciurus17との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples02/src/s17_inverse_kinematics.cpp b/samples/samples02/src/s17_inverse_kinematics.cpp deleted file mode 100644 index 5335a10..0000000 --- a/samples/samples02/src/s17_inverse_kinematics.cpp +++ /dev/null @@ -1,205 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" -#include "rt_manipulators_cpp/kinematics.hpp" -#include "rt_manipulators_cpp/kinematics_utils.hpp" -#include "rt_manipulators_cpp/link.hpp" - -void move_to(rt_manipulators_cpp::Hardware & hardware, - const kinematics_utils::links_t & links, - const kinematics_utils::link_id_t & target_id, - Eigen::Vector3d & target_p, const Eigen::Matrix3d & target_R) { - // 目標位置・姿勢をもとにIKを解き、関節角度をセットする - std::cout << "目標位置:" << std::endl << target_p << std::endl; - std::cout << "目標姿勢:" << std::endl << target_R << std::endl; - std::cout << "----------------------" << std::endl; - kinematics_utils::q_list_t q_list; - if (kinematics::inverse_kinematics_LM(links, target_id, target_p, target_R, q_list) == false) { - std::cout << "IKに失敗しました" << std::endl; - } else { - std::cout << "IKに成功しました" << std::endl; - } - for (const auto & [target_id, q_value] : q_list) { - hardware.set_position(links[target_id].dxl_id, q_value); - } -} - -int main() { - std::cout << "手先目標位置・姿勢をもとに逆運動学を解き、" - << "Sciurus17を動かすサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string hardware_config_file = "../config/sciurus17.yaml"; - std::string link_config_file = "../config/sciurus17_links.csv"; - - auto links = kinematics_utils::parse_link_config_file(link_config_file); - kinematics::forward_kinematics(links, 1); - // kinematics_utils::print_links(links, 1); - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(hardware_config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - // 手先リンクのIDを設定 - const int RIGHT = 0; - const int LEFT = 1; - const std::vector SIDES = {RIGHT, LEFT}; - std::map TARGET_LINK_ID{ - {RIGHT, 11}, - {LEFT, 20}, - }; - // 関節可動範囲の設定 - for (const auto & side : SIDES) { - for (auto link_id : kinematics_utils::find_route(links, TARGET_LINK_ID[side])) { - hardware.get_max_position_limit(links[link_id].dxl_id, links[link_id].max_q); - hardware.get_min_position_limit(links[link_id].dxl_id, links[link_id].min_q); - } - } - - std::vector group_names = {"right_arm", "left_arm", "torso"}; - for (auto group : group_names) { - std::cout << group << "グループのサーボ最大加速度をpi rad/s^2、最大速度をpi rad/sに設定します." - << std::endl; - if (!hardware.write_max_acceleration_to_group(group, 1.0 * M_PI)) { - std::cerr << group << "グループの最大加速度を設定できませんでした." << std::endl; - return -1; - } - - if (!hardware.write_max_velocity_to_group(group, 1.0 * M_PI)) { - std::cerr << group << "グループの最大速度を設定できませんでした." << std::endl; - return -1; - } - - std::cout << group << "グループのサーボ位置制御PIDゲインに(800, 0, 0)を書き込みます." - << std::endl; - if (!hardware.write_position_pid_gain_to_group(group, 800, 0, 0)) { - std::cerr << group << "グループにPIDゲインを書き込めませんでした." << std::endl; - return -1; - } - - if (!hardware.torque_on(group)) { - std::cerr << group << "グループのトルクをONできませんでした." << std::endl; - return -1; - } - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::cout << "5秒後にSciurus17が動作するため、X7の周りに物や人を近づけないで下さい." - << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - // 手先は正面に向ける - std::map target_R{ - {RIGHT, kinematics_utils::rotation_from_euler_ZYX(M_PI_2, -M_PI_2, 0)}, - {LEFT, kinematics_utils::rotation_from_euler_ZYX(-M_PI_2, -M_PI_2, 0)} - }; - // 上腕と前腕の磁石を付ける姿勢 - std::map> home_positions = { - {RIGHT, {-0.3574, -1.57, 0, 2.72, 0, -1.12, 0} }, - {LEFT, {0.3574, 1.57, 0, -2.72, 0, 1.12, 0} }, - }; - std::map arm_group_name = { - {RIGHT, "right_arm"}, - {LEFT, "left_arm"}, - }; - - for (const auto & side : SIDES) { - Eigen::Vector3d target_p; - kinematics_utils::q_list_t q_list; - - std::cout << "正面へ移動" << std::endl; - target_p << 0.4, 0.0, 0.6; - move_to(hardware, links, TARGET_LINK_ID[side], target_p, target_R[side]); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - std::cout << "左上へ移動" << std::endl; - target_p << 0.4, 0.2, 0.8; - move_to(hardware, links, TARGET_LINK_ID[side], target_p, target_R[side]); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - std::cout << "左下へ移動" << std::endl; - target_p << 0.4, 0.2, 0.4; - move_to(hardware, links, TARGET_LINK_ID[side], target_p, target_R[side]); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - std::cout << "右下へ移動" << std::endl; - target_p << 0.4, -0.2, 0.4; - move_to(hardware, links, TARGET_LINK_ID[side], target_p, target_R[side]); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - std::cout << "右上へ移動" << std::endl; - target_p << 0.4, -0.2, 0.8; - move_to(hardware, links, TARGET_LINK_ID[side], target_p, target_R[side]); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - std::cout << "正面へ移動" << std::endl; - target_p << 0.4, 0.0, 0.6; - move_to(hardware, links, TARGET_LINK_ID[side], target_p, target_R[side]); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - std::cout << "終了姿勢へ移動" << std::endl; - hardware.set_positions(arm_group_name[side], home_positions[side]); - std::this_thread::sleep_for(std::chrono::seconds(3)); - } - std::cout << "腰軸を終了姿勢へ移動" << std::endl; - hardware.set_position(18, 0.0); - std::this_thread::sleep_for(std::chrono::seconds(5)); - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - for (auto group : group_names) { - std::cout << group << "グループのサーボ位置制御PIDゲインに(5, 0, 0)を書き込み、脱力させます." - << std::endl; - if (!hardware.write_position_pid_gain_to_group(group, 5, 0, 0)) { - std::cerr << group << "グループにPIDゲインを書き込めませんでした." << std::endl; - } - } - - std::cout << "10秒間スリープします." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(10)); - - for (auto group : group_names) { - if (!hardware.torque_off(group)) { - std::cerr << group << "グループのトルクをOFFできませんでした." << std::endl; - } - - if (!hardware.write_position_pid_gain_to_group(group, 800, 0, 0)) { - std::cerr << group << "グループにPIDゲインを書き込めませんでした." << std::endl; - } - } - - std::cout << "Sciurus17との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples02/src/x7_forward_kinematics.cpp b/samples/samples02/src/x7_forward_kinematics.cpp deleted file mode 100644 index 0664971..0000000 --- a/samples/samples02/src/x7_forward_kinematics.cpp +++ /dev/null @@ -1,95 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" -#include "rt_manipulators_cpp/kinematics.hpp" -#include "rt_manipulators_cpp/kinematics_utils.hpp" -#include "rt_manipulators_cpp/link.hpp" - -void set_arm_joint_positions(std::vector & links, - std::vector positions) { - // リンクにarmジョイントの現在角度をセットする - if (positions.size() != 7) { - std::cerr << "引数positionsには7個のジョイント角度をセットしてください" << std::endl; - return; - } - - int start_id = 2; // Link1 - for (int i=0; i < positions.size(); i++) { - links[start_id + i].q = positions[i]; - } -} - -int main() { - std::cout << "CRANE-X7のサーボモータ角度を読み取り、順運動学を解くサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string hardware_config_file = "../config/crane-x7.yaml"; - std::string link_config_file = "../config/crane-x7_links.csv"; - - auto links = kinematics_utils::parse_link_config_file(link_config_file); - kinematics::forward_kinematics(links, 1); - kinematics_utils::print_links(links, 1); - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(hardware_config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - std::vector group_names = {"arm", "hand"}; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::this_thread::sleep_for(std::chrono::seconds(3)); - - for (int i = 0; i < 4000; i++) { - std::vector positions; - if (hardware.get_positions("arm", positions)) { - set_arm_joint_positions(links, positions); - kinematics::forward_kinematics(links, 1); - int target_link = 8; - std::cout << "リンク名: " << links[target_link].name << std::endl; - auto pos_xyz = links[target_link].p; - std::cout << "位置 X: " << pos_xyz[0] << "\t[m]" << std::endl; - std::cout << "位置 Y: " << pos_xyz[1] << "\t[m]" << std::endl; - std::cout << "位置 Z: " << pos_xyz[2] << "\t[m]" << std::endl; - auto euler_zyx = kinematics_utils::rotation_to_euler_ZYX(links[target_link].R); - std::cout << "姿勢 Z:" << euler_zyx[0] * 180.0 / M_PI << "\t[deg]" << std::endl; - std::cout << "姿勢 Y:" << euler_zyx[1] * 180.0 / M_PI << "\t[deg]" << std::endl; - std::cout << "姿勢 X:" << euler_zyx[2] * 180.0 / M_PI << "\t[deg]" << std::endl; - } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - std::cout << "CRANE-X7との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples02/src/x7_inverse_kinematics.cpp b/samples/samples02/src/x7_inverse_kinematics.cpp deleted file mode 100644 index 1f883c8..0000000 --- a/samples/samples02/src/x7_inverse_kinematics.cpp +++ /dev/null @@ -1,174 +0,0 @@ -// Copyright 2021 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" -#include "rt_manipulators_cpp/kinematics.hpp" -#include "rt_manipulators_cpp/kinematics_utils.hpp" -#include "rt_manipulators_cpp/link.hpp" - -void move_to(rt_manipulators_cpp::Hardware & hardware, - const kinematics_utils::links_t & links, - Eigen::Vector3d & target_p, const Eigen::Matrix3d & target_R) { - // 目標位置・姿勢をもとにIKを解き、関節角度をセットする - std::cout << "目標位置:" << std::endl << target_p << std::endl; - std::cout << "目標姿勢:" << std::endl << target_R << std::endl; - std::cout << "----------------------" << std::endl; - kinematics_utils::q_list_t q_list; - if (kinematics::inverse_kinematics_LM(links, 8, target_p, target_R, q_list) == false) { - std::cout << "IKに失敗しました" << std::endl; - } else { - std::cout << "IKに成功しました" << std::endl; - } - for (const auto & [target_id, q_value] : q_list) { - hardware.set_position(links[target_id].dxl_id, q_value); - } -} - -int main() { - std::cout << "手先目標位置・姿勢をもとに逆運動学を解き、" - << "CRANE-X7を動かすサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string hardware_config_file = "../config/crane-x7.yaml"; - std::string link_config_file = "../config/crane-x7_links.csv"; - - auto links = kinematics_utils::parse_link_config_file(link_config_file); - kinematics::forward_kinematics(links, 1); - kinematics_utils::print_links(links, 1); - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(hardware_config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - // 関節可動範囲の設定 - for (auto link_id : kinematics_utils::find_route(links, 8)) { - hardware.get_max_position_limit(links[link_id].dxl_id, links[link_id].max_q); - hardware.get_min_position_limit(links[link_id].dxl_id, links[link_id].min_q); - } - - std::cout << "armグループのサーボ最大加速度をpi rad/s^2、最大速度をpi rad/sに設定します." - << std::endl; - if (!hardware.write_max_acceleration_to_group("arm", 1.0 * M_PI)) { - std::cerr << "armグループの最大加速度を設定できませんでした." << std::endl; - return -1; - } - - if (!hardware.write_max_velocity_to_group("arm", 1.0 * M_PI)) { - std::cerr << "armグループの最大速度を設定できませんでした." << std::endl; - return -1; - } - - std::cout << "armグループのサーボ位置制御PIDゲインに(800, 0, 0)を書き込みます." - << std::endl; - if (!hardware.write_position_pid_gain_to_group("arm", 800, 0, 0)) { - std::cerr << "armグループにPIDゲインを書き込めませんでした." << std::endl; - return -1; - } - - if (!hardware.torque_on("arm")) { - std::cerr << "armグループのトルクをONできませんでした." << std::endl; - return -1; - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - std::vector group_names = {"arm"}; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::cout << "5秒後にX7が垂直姿勢へ移行するため、X7の周りに物や人を近づけないで下さい." - << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - // 垂直姿勢へ移動 - std::vector target_positions(7, 0.0); - hardware.set_positions("arm", target_positions); - std::this_thread::sleep_for(std::chrono::seconds(5)); - - Eigen::Vector3d target_p; - Eigen::Matrix3d target_R; - kinematics_utils::q_list_t q_list; - - std::cout << "正面へ移動" << std::endl; - target_p << 0.2, 0.0, 0.3; - target_R = kinematics_utils::rotation_from_euler_ZYX(0, M_PI_2, 0); - move_to(hardware, links, target_p, target_R); - std::this_thread::sleep_for(std::chrono::seconds(5)); - - std::cout << "左上へ移動" << std::endl; - target_p << 0.2, 0.2, 0.5; - target_R = kinematics_utils::rotation_from_euler_ZYX(0, 0, 0); - move_to(hardware, links, target_p, target_R); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - std::cout << "左下へ移動" << std::endl; - target_p << 0.2, 0.2, 0.2; - target_R = kinematics_utils::rotation_from_euler_ZYX(0, M_PI, 0); - move_to(hardware, links, target_p, target_R); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - std::cout << "右下へ移動" << std::endl; - target_p << 0.2, -0.2, 0.2; - target_R = kinematics_utils::rotation_from_euler_ZYX(M_PI_2, M_PI, 0); - move_to(hardware, links, target_p, target_R); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - std::cout << "右上へ移動" << std::endl; - target_p << 0.2, -0.2, 0.5; - target_R = kinematics_utils::rotation_from_euler_ZYX(-M_PI_2, 0, 0); - move_to(hardware, links, target_p, target_R); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - std::cout << "正面へ移動し、手先を真下へ向ける" << std::endl; - target_p << 0.2, 0.0, 0.1; - target_R = kinematics_utils::rotation_from_euler_ZYX(0, M_PI, 0); - move_to(hardware, links, target_p, target_R); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - std::cout << "armグループのサーボ位置制御PIDゲインに(5, 0, 0)を書き込み、脱力させます." - << std::endl; - if (!hardware.write_position_pid_gain_to_group("arm", 5, 0, 0)) { - std::cerr << "armグループにPIDゲインを書き込めませんでした." << std::endl; - } - std::cout << "10秒間スリープします." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(10)); - - if (!hardware.torque_off("arm")) { - std::cerr << "armグループのトルクをOFFできませんでした." << std::endl; - } - - if (!hardware.write_position_pid_gain_to_group("arm", 800, 0, 0)) { - std::cerr << "armグループにPIDゲインを書き込めませんでした." << std::endl; - } - - std::cout << "CRANE-X7との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples03/.clang-format b/samples/samples03/.clang-format deleted file mode 100644 index 7ca5230..0000000 --- a/samples/samples03/.clang-format +++ /dev/null @@ -1,2 +0,0 @@ -BasedOnStyle: Google -ColumnLimit: 100 \ No newline at end of file diff --git a/samples/samples03/CMakeLists.txt b/samples/samples03/CMakeLists.txt deleted file mode 100644 index f947f0b..0000000 --- a/samples/samples03/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 3.10) - -# set the project name and version -project(samples03 VERSION 1.0) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -# specify the C++ standard -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED True) - -# 生成された実行ファイルをソースのbinディレクトリに出力する -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) - -# サンプルのビルド -set(list_samples - x7_3dof_inverse_kinematics - s17_3dof_inverse_kinematics - x7_gravity_compensation - s17_gravity_compensation -) -foreach(sample IN LISTS list_samples) - message("${sample}") - add_executable(${sample} - src/${sample}.cpp - src/rt_manipulators_ik.cpp - src/rt_manipulators_dynamics.cpp - ) - target_include_directories(${sample} PUBLIC - include - ) - target_link_libraries(${sample} PRIVATE rt_manipulators_cpp Eigen3::Eigen) -endforeach() diff --git a/samples/samples03/README.md b/samples/samples03/README.md deleted file mode 100644 index 8c5e032..0000000 --- a/samples/samples03/README.md +++ /dev/null @@ -1,255 +0,0 @@ -# サンプル集03 解析的な逆運動学、トルク制御 - -- [サンプル集03 解析的な逆運動学、トルク制御](#サンプル集03-解析的な逆運動学トルク制御) - - [サンプルのビルド](#サンプルのビルド) - - [ロボットのサーボモータの動かし方について](#ロボットのサーボモータの動かし方について) - - [運動学計算で使用するライブラリについて](#運動学計算で使用するライブラリについて) - - [解析的に逆運動学を解いて手先を任意の位置・姿勢に移動させる](#解析的に逆運動学を解いて手先を任意の位置姿勢に移動させる) - - [動画](#動画) - - [解説](#解説) - - [重力補償トルクをサーボモータに入力する](#重力補償トルクをサーボモータに入力する) - - [動画](#動画-1) - - [解説](#解説-1) - -## サンプルのビルド - -次のコマンドを実行して、サンプル集をビルドします。 - -```sh -$ ./build_samples.bash -``` - -ビルドに成功すると`samples03/bin/`ディレクトリに実行ファイルが生成されます。 - -## ロボットのサーボモータの動かし方について - -あらかじめ、["samples01のREADME"](../samples01/README.md)をよく読み、 -ロボットのサーボモータの動かし方を理解してください。 - -## 運動学計算で使用するライブラリについて - -運動学計算で使用するライブラリ -(`kinematics.hpp`、`kinematics_utils.hpp`、`link.hpp`) -の使い方は["samples02のREADME"](../samples02/../README.md) -を参照してください。 - -## 解析的に逆運動学を解いて手先を任意の位置・姿勢に移動させる - -次のコマンドを実行します。 -CRANE-X7では、手先が0.3m前方の5点に向かって移動します。 -Sciurus17では、右手先が0.4m前方の4点に向かって移動します。 - -***安全のためロボットの周りに物や人を近づけないでください。*** - -```sh -# CRANE-X7の場合 -$ cd bin/ -$ ./x7_3dof_inverse_kinematics -# Sciurus17の場合 -$ ./s17_3dof_inverse_kinematics -``` - -実行結果（CRANE-X7の場合） - -```sh -$ ./x7_3dof_inverse_kinematics -手先目標位置をもとに解析的に逆運動学を解き、CRANE-X7を動かすサンプルです. -リンク情報ファイル:../config/crane-x7_links.csvを読み込みます -... - -5秒後にX7が垂直姿勢へ移行するため、X7の周りに物や人を近づけないで下さい. ----------------------- -目標位置: -0.2 - 0 -0.2 -IKに成功しました -左奥へ移動 -... - -ピッキング姿勢のIKを解きます -左奥へ移動 ----------------------- -目標位置: - 0.3 - 0.1 -0.05 -IKに成功しました -左手前へ移動 -... -``` - -### 動画 - -[![](https://img.youtube.com/vi/amp-ql9TS6w/sddefault.jpg)](https://youtu.be/amp-ql9TS6w) - -### 解説 - -解析的に逆運動学を解く関数を使用する場合、 -`samples03/include/rt_manipulators_ik.hpp`をincludeします。 - -```cpp -#include "rt_manipulators_ik.hpp" -``` - -CRANE-X7向けの逆運動学計算関数として、 -`samples03::x7_3dof_inverse_kinematics(links, target_p, q_list)`と -`samples03::x7_3dof_picking_inverse_kinematics(links, target_p, q_list)` -があります。 -引数にはリンク構成と、目標位置(`Eigen::Vector3d`)、 -関節角度の格納先(`std::map` または `kinematics_utils::q_list_t`)を入力します。 - -`samples03::x7_3dof_inverse_kinematics(links, target_p, q_list)` -は、第6リンクの原点を目標位置(`target_p`)として受け取り、 -第1、2、4関節の目標角度を出力する関数です。 - -`samples03::x7_3dof_picking_inverse_kinematics(links, target_p, q_list)` -は、ハンドの先端を目標位置(`target_p`)として受け取り、 -第1、2、4関節と、第6、7関節の目標角度を出力する関数です。 -手先が地面(-Z)方向を向くように逆運動学を解きます。 - -```cpp -Eigen::Vector3d target_p; -kinematics_utils::q_list_t q_list; - -// 目標位置(X方向に0.2m、Y方向に0.0m、Z方向に0.3m) -target_p << 0.2, 0.0, 0.3; - -// 逆運動学を解き、関節角度を取得する -samples03::x7_3dof_inverse_kinematics(links, target_p, q_list); -// 関節角度をサーボモータへ設定する -for (const auto & [target_id, q_value] : q_list) { - hardware.set_position(links[target_id].dxl_id, q_value); -} -``` - -```cpp -Eigen::Vector3d target_p; -kinematics_utils::q_list_t q_list; -// ピッキング姿勢の逆運動学 -// 手先の目標位置(X方向に0.2m、Y方向に0.0m、Z方向に0.0m) -target_p << 0.2, 0.0, 0.0; - -// 逆運動学を解き、関節角度を取得する -samples03::x7_3dof_picking_inverse_kinematics(links, target_p, q_list); -// 関節角度をサーボモータへ設定する -for (const auto & [target_id, q_value] : q_list) { - hardware.set_position(links[target_id].dxl_id, q_value); -} -``` - -Sciurus17向けの逆運動学計算関数には、 -`samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list)`と -`samples03::s17_3dof_right_arm_picking_inverse_kinematics(links, target_p, q_list)` -を用意しています。 -引数にはリンク構成と、目標位置(`Eigen::Vector3d`)、 -関節角度の格納先(`std::map` または `kinematics_utils::q_list_t`)を入力します。 - -`samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list)` -は、右腕の第6リンクの原点を目標位置(`target_p`)として受け取り、 -右腕の第1、2、4関節の目標角度を出力する関数です。 - -`samples03::s17_3dof_right_arm_picking_inverse_kinematics(links, target_p, q_list)` -は、右ハンドの先端を目標位置(`target_p`)として受け取り、 -右腕の第1、2、4関節と、第5、6、7関節の目標角度を出力する関数です。 -手先が地面(-Z)方向を向くように逆運動学を解きます。 - -```cpp -Eigen::Vector3d target_p; -kinematics_utils::q_list_t q_list; - -// 右手目標位置(X方向に0.2m、Y方向に-0.3m、Z方向に0.2m) -target_p << 0.2, -0.3, 0.2; - -// 逆運動学を解き、関節角度を取得する -samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list); -// 関節角度をサーボモータへ設定する -for (const auto & [target_id, q_value] : q_list) { - hardware.set_position(links[target_id].dxl_id, q_value); -} -``` - -```cpp -Eigen::Vector3d target_p; -kinematics_utils::q_list_t q_list; -// ピッキング姿勢の逆運動学 -// 手先の目標位置(X方向に0.2m、Y方向に-0.3m、Z方向に0.0m) -target_p << 0.2, -0.3, 0.0; - -// 逆運動学を解き、関節角度を取得する -samples03::s17_3dof_right_arm_picking_inverse_kinematics(links, target_p, q_list); -// 関節角度をサーボモータへ設定する -for (const auto & [target_id, q_value] : q_list) { - hardware.set_position(links[target_id].dxl_id, q_value); -} -``` - -## 重力補償トルクをサーボモータに入力する - -次のコマンドを実行します。 -サンプルを終了する場合はキーボードのEscキーを入力してください。 - -***安全のためいつでもブレーキモードボタンを押せるように準備してください*** - -```sh -# CRANE-X7の場合 -$ cd bin/ -$ ./x7_gravity_compensation -# Sciurus17の場合 -$ ./s17_gravity_compensation -``` - -実行結果（CRANE-X7の場合） - -```sh -$ ./x7_gravity_compensation -現在姿勢をもとに重力補償トルクを計算し、CRANE-X7のサーボモータに入力するサンプルです -リンク情報ファイル:../config/crane-x7_links.csvを読み込みます -... - -5秒後に重力補償トルクをサーボモータへ入力します -終了する場合はEscキーを押してください -... -``` - -### 動画 - -[![](https://img.youtube.com/vi/H_gSnJDFhqs/sddefault.jpg)](https://youtu.be/H_gSnJDFhqs) - -### 解説 - -重力補償トルクを計算する関数を使用する場合、 -`samples03/include/rt_manipulators_dynamics.hpp`をincludeします。 - -```cpp -#include "rt_manipulators_dynamics.hpp" -``` - -重力補償トルク計算関数として、 -`samples03_dynamics::gravity_compensation(links, target_id, torque_to_current, q_list)` -があります。 -引数にはリンク構成と、手先リンク番号、 -電流トルク比(`std::map` または `samples03_dynamics::torque_to_current_t`)、 -関節電流の格納先(`std::map` または `kinematics_utils::q_list_t`)を入力します。 - -```cpp -kinematics_utils::link_id_t target_id = 8; -samples03_dynamics::torque_to_current_t torque_to_current = { - {2, 1.0 / 2.20}, - {3, 1.0 / 3.60}, - {4, 1.0 / 2.20}, - {5, 1.0 / 2.20}, - {6, 1.0 / 2.20}, - {7, 1.0 / 2.20}, - {8, 1.0 / 2.20} -}; - -kinematics_utils::q_list_t q_list; - -// 重力補償トルクを計算する -samples03_dynamics::gravity_compensation(links, target_id, torque_to_current, q_list); -// 関節電流をサーボモータへ設定する -for (const auto & [target_id, q_value] : q_list) { - hardware.set_current(links[target_id].dxl_id, q_value); -} -``` diff --git a/samples/samples03/build_samples.bash b/samples/samples03/build_samples.bash deleted file mode 100755 index 53af683..0000000 --- a/samples/samples03/build_samples.bash +++ /dev/null @@ -1,18 +0,0 @@ -#!/usr/bin/env bash - -set -e - -BUILD_DIR=build - -echo "サンプルプログラムをビルドします" -cd $(dirname $0) - -if [ ! -d $BUILD_DIR ]; then - mkdir $BUILD_DIR -fi - -cd $BUILD_DIR -cmake .. -make - -echo "サンプルプログラムをビルドしました" \ No newline at end of file diff --git a/samples/samples03/config/crane-x7.yaml b/samples/samples03/config/crane-x7.yaml deleted file mode 100644 index 2a800b2..0000000 --- a/samples/samples03/config/crane-x7.yaml +++ /dev/null @@ -1,31 +0,0 @@ -joint_groups: - arm: - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 - sync_read: - - position - sync_write: - - position - hand: - joints: - - joint_hand - sync_read: - - position - sync_write: - - position - - -joint1: { id: 2, dynamixel: "XM430", operating_mode: 3 } -joint2: { id: 3, dynamixel: "XM540", operating_mode: 3 } -joint3: { id: 4, dynamixel: "XM430", operating_mode: 3 } -joint4: { id: 5, dynamixel: "XM430", operating_mode: 3 } -joint5: { id: 6, dynamixel: "XM430", operating_mode: 3 } -joint6: { id: 7, dynamixel: "XM430", operating_mode: 3 } -joint7: { id: 8, dynamixel: "XM430", operating_mode: 3 } -joint_hand: { id: 9, dynamixel: "XM430", operating_mode: 3 } \ No newline at end of file diff --git a/samples/samples03/config/crane-x7_current.yaml b/samples/samples03/config/crane-x7_current.yaml deleted file mode 100644 index 8bc6dba..0000000 --- a/samples/samples03/config/crane-x7_current.yaml +++ /dev/null @@ -1,31 +0,0 @@ -joint_groups: - arm: - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 - sync_read: - - position - sync_write: - - current - hand: - joints: - - joint_hand - sync_read: - - position - sync_write: - - position - - -joint1: { id: 2, dynamixel: "XM430", operating_mode: 0 } -joint2: { id: 3, dynamixel: "XM540", operating_mode: 0 } -joint3: { id: 4, dynamixel: "XM430", operating_mode: 0 } -joint4: { id: 5, dynamixel: "XM430", operating_mode: 0 } -joint5: { id: 6, dynamixel: "XM430", operating_mode: 0 } -joint6: { id: 7, dynamixel: "XM430", operating_mode: 0 } -joint7: { id: 8, dynamixel: "XM430", operating_mode: 0 } -joint_hand: { id: 9, dynamixel: "XM430", operating_mode: 3 } \ No newline at end of file diff --git a/samples/samples03/config/crane-x7_links.csv b/samples/samples03/config/crane-x7_links.csv deleted file mode 100644 index e20e7a6..0000000 --- a/samples/samples03/config/crane-x7_links.csv +++ /dev/null @@ -1,13 +0,0 @@ -リンク名称,ID,,,,ワールド座標系ホームポジション位置,,,ワールド座標系リンク間相対距離,,,ワールド座標系ホームポジション姿勢,,,質量[g],ローカル座標系質量位置,,,ローカル座標系慣性テンソル(重心),,,,,,ハードウェア可動範囲,,角度制限設定済可動範囲,,ホームポジション角度,回転軸方向,サーボモータ種類,トルク定数,サーボモータID,サーボモータ可動範囲,,通信速度,Return Delay Time,Operation Mode,Pゲイン,Iゲイン,Dゲイン -,自分リンク,妹リンク,子リンク,親リンク,x[mm],y[mm],z[mm],x[mm],y[mm],z[mm],X[deg],Y[deg],Z[deg],m[g],x[mm],y[mm],z[mm],Ixx[gmm^2],Ixy[gmm^2],Iyy[gmm^2],Ixz[gmm^2],Iyz[gmm^2],Izz[gmm^2],θ1[deg],θ2[deg],θ1[deg],θ2[deg],サーボ指令値[deg],±XYZ,型番,[Nm/A],ID,θ1[-],θ2[-],bps,-,,-,-,- -CRANE-X7_Base,1,-,2,-,0,0,0,0,0,0,0,0,0,388,-6.734,-0.001,17.921,324812.981,1.195,482993.163,-47122.898,19.475,585569.734,-,-,-,-,-,-,-,,-,-,-,-,-,,-,-,- -CRANE-X7_Link1,2,-,3,1,0,0,41,0,0,41,0,0,0,253,0.243,-0.105,46.95,178706.106,-73.234,176299.332,888.676,623.013,89586.08,-160,160,-157,157,180,Z+,XM430-W350,1.783,2,262,3834,3Mbps,0,3,1000,300,200 -CRANE-X7_Link2,3,-,4,2,0,0,105,0,0,64,90,0,0,136,0.031,34.172,0.019,168092.477,20.51,96197.568,-400.417,62.417,134475.028,-90,90,-90,90,180,Y-,XM540-W270,2.409,3,1024,3072,3Mbps,0,3,2200,500,500 -CRANE-X7_Link3,4,-,5,3,0,0,170,0,0,65,0,0,0,321,-13.722,-0.081,95.202,1819759.341,753.977,1883280.126,-37299.429,4147.003,158644.809,-160,160,-157,157,180,Z+,XM430-W350,1.783,4,262,3834,3Mbps,0,3,3000,800,1000 -CRANE-X7_Link4,5,-,6,4,0,0,355,0,0,185,90,0,0,222,-9.388,80.39,0.064,488905.787,-3904.567,114431.658,82.949,2823.368,502575.787,-160,0,-160,0,180,Y-,XM430-W350,1.783,5,228,2048,3Mbps,0,3,1600,400,400 -CRANE-X7_Link5,6,-,7,5,0,0,476,0,0,121,0,0,0,207,-0.084,0.462,63.718,290227.705,100.249,257036.585,-476.631,-16861.834,87512.332,-160,160,-157,157,180,Z+,XM430-W350,1.783,6,262,3834,3Mbps,0,3,800,200,200 -CRANE-X7_Link6,7,-,8,6,0,0,605,0,0,129,90,0,0,140,6.1,-3.549,0.826,40139.362,-2908.265,61345.454,639.429,-489.787,73485.846,-90,90,-90,90,180,Y-,XM430-W350,1.783,7,1024,3072,3Mbps,0,3,800,100,200 -CRANE-X7_Link7,8,-,9,7,0,0,624,0,0,19,0,0,0,121,0.108,-1.034,20.848,32035.813,-94.503,41433.837,261.225,388.481,42899.559,-170,170,-167,167,180,Z+,XM430-W350,1.783,8,148,3948,3Mbps,0,3,800,100,200 -CRANE-X7_HandA,9,10,-,8,-12,0,648,-12,0,24,90,0,0,15.8,-2.647,26.904,3.541,10056.667,-197.178,4954.751,-98.854,536.803,6239.775,0,90,-5,90,180,Y-,XM430-W350,1.783,9,1991,3072,3Mbps,0,3,-,-,- -CRANE-X7_HandB,10,-,-,8,12,0,648,12,0,24,90,0,0,13.9,3.008,30.851,2.845,7483.7,361.284,4088.581,63.385,228.226,4372.991,0,90,-5,90,180,Y+,-,,-,-,-,-,-,,-,-,- -右手系、正面はX方向,,,,,,,,,, ,,,,,,,,,,,,,,,,, c,,,,,,,,,,,,, \ No newline at end of file diff --git a/samples/samples03/config/sciurus17.yaml b/samples/samples03/config/sciurus17.yaml deleted file mode 100644 index 2b210fd..0000000 --- a/samples/samples03/config/sciurus17.yaml +++ /dev/null @@ -1,75 +0,0 @@ - -joint_groups: - right_arm: - joints: - - right_arm_joint1 - - right_arm_joint2 - - right_arm_joint3 - - right_arm_joint4 - - right_arm_joint5 - - right_arm_joint6 - - right_arm_joint7 - sync_read: - - position - sync_write: - - position - - right_hand: - joints: - - right_arm_joint_hand - sync_read: - - position - sync_write: - - position - - left_arm: - joints: - - left_arm_joint1 - - left_arm_joint2 - - left_arm_joint3 - - left_arm_joint4 - - left_arm_joint5 - - left_arm_joint6 - - left_arm_joint7 - sync_read: - - position - sync_write: - - position - - left_hand: - joints: - - left_arm_joint_hand - sync_read: - - position - sync_write: - - position - - torso: - joints: - - waist_joint - - neck_yaw_joint - - neck_pitch_joint - sync_read: - - position - sync_write: - - position - -right_arm_joint1: { id: 2, dynamixel: "XM540", operating_mode: 3 } -right_arm_joint2: { id: 3, dynamixel: "XM540", operating_mode: 3 } -right_arm_joint3: { id: 4, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint4: { id: 5, dynamixel: "XM540", operating_mode: 3 } -right_arm_joint5: { id: 6, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint6: { id: 7, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint7: { id: 8, dynamixel: "XM430", operating_mode: 3 } -right_arm_joint_hand: { id: 9, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint1: { id: 10, dynamixel: "XM540", operating_mode: 3 } -left_arm_joint2: { id: 11, dynamixel: "XM540", operating_mode: 3 } -left_arm_joint3: { id: 12, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint4: { id: 13, dynamixel: "XM540", operating_mode: 3 } -left_arm_joint5: { id: 14, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint6: { id: 15, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint7: { id: 16, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint_hand: { id: 17, dynamixel: "XM430", operating_mode: 3 } -waist_joint: { id: 18, dynamixel: "XM430", operating_mode: 3 } -neck_yaw_joint: { id: 19, dynamixel: "XM430", operating_mode: 3 } -neck_pitch_joint: { id: 20, dynamixel: "XM430", operating_mode: 3 } diff --git a/samples/samples03/config/sciurus17_current.yaml b/samples/samples03/config/sciurus17_current.yaml deleted file mode 100644 index cd49216..0000000 --- a/samples/samples03/config/sciurus17_current.yaml +++ /dev/null @@ -1,75 +0,0 @@ - -joint_groups: - right_arm: - joints: - - right_arm_joint1 - - right_arm_joint2 - - right_arm_joint3 - - right_arm_joint4 - - right_arm_joint5 - - right_arm_joint6 - - right_arm_joint7 - sync_read: - - position - sync_write: - - current - - right_hand: - joints: - - right_arm_joint_hand - sync_read: - - position - sync_write: - - position - - left_arm: - joints: - - left_arm_joint1 - - left_arm_joint2 - - left_arm_joint3 - - left_arm_joint4 - - left_arm_joint5 - - left_arm_joint6 - - left_arm_joint7 - sync_read: - - position - sync_write: - - current - - left_hand: - joints: - - left_arm_joint_hand - sync_read: - - position - sync_write: - - position - - torso: - joints: - - waist_joint - - neck_yaw_joint - - neck_pitch_joint - sync_read: - - position - sync_write: - - current - -right_arm_joint1: { id: 2, dynamixel: "XM540", operating_mode: 0 } -right_arm_joint2: { id: 3, dynamixel: "XM540", operating_mode: 0 } -right_arm_joint3: { id: 4, dynamixel: "XM430", operating_mode: 0 } -right_arm_joint4: { id: 5, dynamixel: "XM540", operating_mode: 0 } -right_arm_joint5: { id: 6, dynamixel: "XM430", operating_mode: 0 } -right_arm_joint6: { id: 7, dynamixel: "XM430", operating_mode: 0 } -right_arm_joint7: { id: 8, dynamixel: "XM430", operating_mode: 0 } -right_arm_joint_hand: { id: 9, dynamixel: "XM430", operating_mode: 3 } -left_arm_joint1: { id: 10, dynamixel: "XM540", operating_mode: 0 } -left_arm_joint2: { id: 11, dynamixel: "XM540", operating_mode: 0 } -left_arm_joint3: { id: 12, dynamixel: "XM430", operating_mode: 0 } -left_arm_joint4: { id: 13, dynamixel: "XM540", operating_mode: 0 } -left_arm_joint5: { id: 14, dynamixel: "XM430", operating_mode: 0 } -left_arm_joint6: { id: 15, dynamixel: "XM430", operating_mode: 0 } -left_arm_joint7: { id: 16, dynamixel: "XM430", operating_mode: 0 } -left_arm_joint_hand: { id: 17, dynamixel: "XM430", operating_mode: 3 } -waist_joint: { id: 18, dynamixel: "XM540", operating_mode: 0 } -neck_yaw_joint: { id: 19, dynamixel: "XM430", operating_mode: 0 } -neck_pitch_joint: { id: 20, dynamixel: "XM430", operating_mode: 0 } diff --git a/samples/samples03/config/sciurus17_links.csv b/samples/samples03/config/sciurus17_links.csv deleted file mode 100644 index 38409de..0000000 --- a/samples/samples03/config/sciurus17_links.csv +++ /dev/null @@ -1,30 +0,0 @@ -リンク名称,ID,,,,ワールド座標系ホームポジション位置,,,ワールド座標系リンク間相対距離,,,ワールド座標系ホームポジション姿勢,,,質量[g],ローカル座標系質量位置,,,ローカル座標系慣性テンソル(重心),,,,,,ハードウェア可動範囲,,保護設定可動範囲,,ホームポジション角度,回転軸方向,サーボモータ種類,トルク定数,サーボモータID,サーボモータ可動範囲,,通信速度,Return Delay Time,Operation Mode -,自分リンク,妹リンク,子リンク,親リンク,x[mm],y[mm],z[mm],x[mm],y[mm],z[mm],X[deg],Y[deg],Z[deg],m[g],x[mm],y[mm],z[mm],Ixx[gmm^2],Ixy[gmm^2],Iyy[gmm^2],Ixz[gmm^2],Iyz[gmm^2],Izz[gmm^2],θ1[deg],θ2[deg],θ1[deg],θ2[deg],サーボ指令値[deg],±XYZ,型番,[Nm/A],ID,θ1[-],θ2[-],bps,-,- -Sciurus17_Base,1,-,2,-,0,0,0,0,0,0,0,0,0,1722.937,-24.012,0.139,59.566,6398092.663,-6808.969,7611664.357,-331509.04,2026.354,6331936.461,-,-,,,-,-,-,-,-,-,-,-,-,- -Sciurus17_Body,2,24,3,1,0,0,131.5,0,0,131.5,0,0,0,1482.929,57.11,0.372,238.229,15601031.58,-15890.903,13969734.5,-3805596.494,-20695.221,8327993.097,-110,110,-107,107,180,Z+,XM540-W270,2.409,18,831,3265,3Mbps,0,3 -Sciurus17_Neck1,3,5,4,2,80.83,0,471,80.83,0,339.5,0,0,0,168,-0.022,0.029,40.86,63143.268,-0.078,66522.419,-126.072,187.375,32744.236,-165,165,-162,162,180,Z+,XM430-W350,1.783,19,205,3891,3Mbps,0,3 -Sciurus17_Neck2,4,23,-,3,80.83,0,525,0,0,54,90,0,0,312.172,19.979,78.687,0.151,964350.46,-56112.026,1027677.585,-731.103,1684.659,1065891.198,-90,90,-90,60,180,Y-,XM430-W350,1.783,20,1024,2731,3Mbps,0,3 -Sciurus17_R_Link1,5,14,6,2,80.83,-96.5,420,80.83,-96.5,288.5,90,0,0,258,-0.656,-0.264,46.367,176217.476,-5.425,179207.793,586.979,-166.297,87950.071,-160,160,-157,157,180,Y-,XM540-W270,2.409,2,262,3834,3Mbps,0,3 -Sciurus17_R_Link2,6,-,7,5,80.83,-160.6,420,0,-64.1,0,0,90,0,134,0.033,-33.96,-0.185,172046.031,-119.176,98834.156,-535.109,580.743,136915.218,-90,90,-90,90,180,X-,XM540-W270,2.409,3,1024,3072,3Mbps,0,3 -Sciurus17_R_Link3,7,-,8,6,80.83,-225.6,420,0,-65,0,90,0,0,443,-13.791,0.245,113.391,2336253.636,-1190.372,2414711.907,-75998.279,-8024.912,236305.543,-160,160,-157,157,180,Y-,XM430-W350,1.783,4,262,3834,3Mbps,0,3 -Sciurus17_R_Link4,8,-,9,7,80.83,-410.6,420,0,-185,0,0,0,0,241.66,-8.881,-75.749,0.038,603280.74,-6670.94,143154.3,250.846,-2309.247,598152.333,0,156.2,0,156.2,180,Z+,XM540-W150,1.659,5,2048,3825,3Mbps,0,3 -Sciurus17_R_Link5,9,25,10,8,80.83,-531.6,420,0,-121,0,90,0,0,224,13.531,-0.304,70.369,294726.731,-4809.196,290568.087,-21387.985,13192.269,125378.656,-160,160,-157,157,180,Y-,XM430-W350,1.783,6,262,3834,3Mbps,0,3 -Sciurus17_R_Link6,10,-,11,9,80.83,-660.6,420,0,-129,0,0,0,0,140,-6.156,3.583,0.755,39499.661,-2921.88,60684.342,-612.042,357.397,73112.015,-120,60,-120,60,180,Z+,XM430-W350,1.783,7,683,2731,3Mbps,0,3 -Sciurus17_R_Link7,11,-,12,10,80.83,-703.6,420,0,-43,0,90,0,0,121,-0.118,1.006,20.65,32600.928,-69.84,41969.484,-254.224,-409.16,43040.018,-170,170,-167,167,180,Y-,XM430-W350,1.783,8,148,3948,3Mbps,0,3 -Sciurus17_R_HandA,12,13,-,11,68.83,-727.6,420,-12,-24,0,0,0,0,15.8,2.649,-26.849,3.757,10192.75,-197.895,5023.728,98.609,-554.407,6322.202,0,30,-5,90,180,Z+,XM430-W350,1.783,9,1991,3072,3Mbps,0,3 -Sciurus17_R_HandB,13,-,-,11,80.83,-751.6,420,12,-24,0,0,0,0,13.9,-2.707,-30.901,2.845,7483.7,361.283,4088.581,-63.385,-228.226,4372.991,0,30,-5,90,180,Z+,-,,-,-,-,-,-,- -Sciurus17_L_Link1,14,-,15,2,80.83,96.5,420,80.83,96.5,288.5,-90,0,0,258,-0.537,0.27,46.27,170007.992,13.313,175079.881,454.301,159.916,85282.27,-160,160,-157,157,180,Y+,XM540-W270,2.409,10,262,3834,3Mbps,0,3 -Sciurus17_L_Link2,15,-,16,14,80.83,160.6,420,0,64.1,0,0,90,0,134,-0.07,33.946,-0.199,172421.741,-98.964,99076.413,-266.684,-418.743,137151.205,-90,90,-90,90,180,X-,XM540-W270,2.409,11,1024,3072,3Mbps,0,3 -Sciurus17_L_Link3,16,-,17,15,80.83,225.6,420,0,65,0,-90,0,0,443,-13.943,-0.241,113.533,2343035.312,1178.732,2422253.94,-72414.462,8074.759,237122.927,-160,160,-157,157,180,Y+,XM430-W350,1.783,12,262,3834,3Mbps,0,3 -Sciurus17_L_Link4,17,-,18,16,80.83,410.6,420,0,185,0,0,0,0,241.66,-8.904,75.732,0.033,603838.474,6315.625,143107.114,116.002,2112.212,598666.56,-156.2,0,-156.2,0,180,Z+,XM540-W150,1.659,13,271,2048,3Mbps,0,3 -Sciurus17_L_Link5,18,26,19,17,80.83,531.6,420,0,121,0,-90,0,0,224,13.732,0.348,71.158,286973.35,4321.461,282370.572,-19101.45,-13052.23,125680.836,-160,160,-157,157,180,Y+,XM430-W350,1.783,14,262,3834,3Mbps,0,3 -Sciurus17_L_Link6,19,-,20,18,80.83,660.6,420,0,129,0,0,0,0,140,-6.173,-3.627,0.744,39902.954,3014.503,61099.851,-613.647,-361.668,73723.447,-60,120,-60,120,180,Z+,XM430-W350,1.783,15,1365,3413,3Mbps,0,3 -Sciurus17_L_Link7,20,-,21,19,80.83,703.6,420,0,43,0,90,0,0,121,-0.118,1.074,-20.65,32593.608,-72.866,41969.484,254.224,436.76,43032.698,-170,170,-167,167,180,Y+,XM430-W350,1.783,16,148,3948,3Mbps,0,3 -Sciurus17_L_HandA,21,22,-,20,68.83,727.6,420,-12,24,0,0,0,0,15.8,2.649,26.849,3.757,10192.695,197.91,5023.727,98.601,554.38,6322.142,-30,0,-90,5,180,Z+,XM430-W350,1.783,17,1024,2105,3Mbps,0,3 -Sciurus17_L_HandB,22,-,-,20,80.83,751.6,420,12,24,0,0,0,0,13.9,-3.008,30.851,2.845,7483.736,-361.268,4088.583,-63.393,228.245,4373.032,-30,0,-90,5,180,Z+,-,,-,-,-,-,-,- -cameraHead_link,23,-,-,3,162.615,20,616,81.785,20,91,0,0,0,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,- -cameraBreast_link,24,-,-,1,114.576,0,349.159,114.576,0,217.659,0,60,0,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,- -Sciurus17_R_Link5_ARmarker,25,-,-,8,127.53,-531.6,513,46.7,0,93,0,0,0,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,- -Sciurus17_L_Link5_ARmarker,26,-,-,17,127.53,531.6,513,46.7,0,93,0,0,0,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,- -右手系、正面はX方向,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, -,,,,,,,,,,,,,,756.36,,,,,,,,,,,,,,,,,,,,,,, \ No newline at end of file diff --git a/samples/samples03/include/rt_manipulators_dynamics.hpp b/samples/samples03/include/rt_manipulators_dynamics.hpp deleted file mode 100644 index 42d6ca3..0000000 --- a/samples/samples03/include/rt_manipulators_dynamics.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2022 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef SAMPLES_SAMPLES03_INCLUDE_RT_MANIPULATORS_DYNAMICS_HPP_ -#define SAMPLES_SAMPLES03_INCLUDE_RT_MANIPULATORS_DYNAMICS_HPP_ - -#include - -#include "rt_manipulators_cpp/kinematics_utils.hpp" -#include "rt_manipulators_cpp/link.hpp" - -namespace samples03_dynamics { - -using torque_to_current_t = std::map; - -bool gravity_compensation(const kinematics_utils::links_t & links, - const kinematics_utils::link_id_t & target_id, - const torque_to_current_t & torque_to_current, - kinematics_utils::q_list_t & q_list); - -} // namespace samples03_dynamics - -#endif // SAMPLES_SAMPLES03_INCLUDE_RT_MANIPULATORS_DYNAMICS_HPP_ diff --git a/samples/samples03/include/rt_manipulators_ik.hpp b/samples/samples03/include/rt_manipulators_ik.hpp deleted file mode 100644 index 26a1a21..0000000 --- a/samples/samples03/include/rt_manipulators_ik.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2022 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef SAMPLES_SAMPLES03_INCLUDE_RT_MANIPULATORS_IK_HPP_ -#define SAMPLES_SAMPLES03_INCLUDE_RT_MANIPULATORS_IK_HPP_ - -#include "rt_manipulators_cpp/kinematics_utils.hpp" - -namespace samples03 { - -bool x7_3dof_inverse_kinematics(const kinematics_utils::links_t & links, - const Eigen::Vector3d & target_p, - kinematics_utils::q_list_t & q_list); -bool x7_3dof_picking_inverse_kinematics(const kinematics_utils::links_t & links, - const Eigen::Vector3d & target_p, - kinematics_utils::q_list_t & q_list); -bool s17_3dof_right_arm_inverse_kinematics(const kinematics_utils::links_t & links, - const Eigen::Vector3d & target_p, - kinematics_utils::q_list_t & q_list); -bool s17_3dof_right_arm_picking_inverse_kinematics(const kinematics_utils::links_t & links, - const Eigen::Vector3d & target_p, - kinematics_utils::q_list_t & q_list); - -} // namespace samples03 - -#endif // SAMPLES_SAMPLES03_INCLUDE_RT_MANIPULATORS_IK_HPP_ diff --git a/samples/samples03/run_test.bash b/samples/samples03/run_test.bash deleted file mode 100755 index 65ee735..0000000 --- a/samples/samples03/run_test.bash +++ /dev/null @@ -1,19 +0,0 @@ -#!/usr/bin/env bash - -set -e - -BUILD_DIR=build - -echo "samples03で使用するライブラリをテストします" -cd $(dirname $0)/test - -if [ ! -d $BUILD_DIR ]; then - mkdir $BUILD_DIR -fi - -cd $BUILD_DIR -cmake .. -make -CTEST_OUTPUT_ON_FAILURE=1 ctest - -echo "ライブラリをテストしました" \ No newline at end of file diff --git a/samples/samples03/src/rt_manipulators_dynamics.cpp b/samples/samples03/src/rt_manipulators_dynamics.cpp deleted file mode 100644 index fbda4b7..0000000 --- a/samples/samples03/src/rt_manipulators_dynamics.cpp +++ /dev/null @@ -1,115 +0,0 @@ -// Copyright 2022 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include "rt_manipulators_dynamics.hpp" - -namespace samples03_dynamics { - -using pos_t = Eigen::Vector3d; -using center_of_mass_t = Eigen::Vector3d; -using force_t = Eigen::Vector3d; -using moment_t = Eigen::Vector3d; -using pos_map_t = std::map; -using center_of_mass_map_t = std::map; -using force_map_t = std::map; -using moment_map_t = std::map; - -bool gravity_compensation( - const kinematics_utils::links_t & links, - const kinematics_utils::link_id_t & target_id, - const torque_to_current_t & torque_to_current, - kinematics_utils::q_list_t & q_list) { - // 運動方程式から重力補償トルク項のみを計算し、電流値に変換する - // 参考：細田耕. 「実践ロボット制御 -基礎から動力学まで-」. オーム社, p137, 2019 - - // 根本から手先までのリンク経路を取得 - auto route = kinematics_utils::find_route(links, target_id); - - // トルク電流比がセットされているか検証 - for (const auto & link_i : route) { - if (torque_to_current.count(link_i) == 0) { - std::cout << "リンクID:" << link_i << "のトルク電流比がセットされていません" << std::endl; - return false; - } - } - - // 計算に使用する変数 - pos_map_t dd_p; // 加速度ベクトル - center_of_mass_map_t dd_s; // 重心の加速度ベクトル - force_map_t f_hat; // 慣性による外力ベクトル - force_map_t f; // リンク間力ベクトル - moment_map_t n; // リンク間モーメントベクトル - - // 変数の初期化 - for (const auto & link_i : route) { - dd_p[link_i] << 0, 0, 0; - dd_s[link_i] << 0, 0, 0; - f_hat[link_i] << 0, 0, 0; - f[link_i] << 0, 0, 0; - n[link_i] << 0, 0, 0; - } - // 根本リンクを追加する - auto base_i = links[route[0]].parent; - auto base_to_route = route; - base_to_route.insert(base_to_route.begin(), base_i); - dd_p[base_i] << 0, 0, 0; - // 子リンクを追加する - auto end_i = links[route.back()].child; - auto route_to_end = route; - route_to_end.push_back(end_i); - f[end_i] << 0, 0, 0; - n[end_i] << 0, 0, 0; - - // 重力加速度を根本リンクの加速度に設定 - Eigen::Vector3d g; - g << 0, 0, -9.8; - dd_p[base_i] = -g; - - // 根本から手先に向かってリンクの加速度と外力を求める - for (auto itr = std::next(base_to_route.begin()); itr != base_to_route.end(); ++itr) { - const auto link_i = *itr; - const auto parent_i = *std::prev(itr); - const auto link = links[link_i]; - const Eigen::Matrix3d RfromParent = links[parent_i].R.transpose() * link.R; - - // 加速度を求める - dd_p[link_i] = RfromParent.transpose() * dd_p[parent_i]; - - // 外力を求める - dd_s[link_i] = dd_p[link_i]; - f_hat[link_i] = link.m * dd_s[link_i]; - } - - // 手先から根本に向かってリンクの力とモーメントを求める - for (auto itr = std::next(route_to_end.rbegin()); itr != route_to_end.rend(); ++itr) { - const auto link_i = *itr; - const auto child_i = *std::prev(itr); - const Eigen::Matrix3d RtoChild = links[link_i].R.transpose() * links[child_i].R; - - f[link_i] = RtoChild * f[child_i] + f_hat[link_i]; - n[link_i] = RtoChild * n[child_i] - + links[child_i].b.cross(RtoChild * f[child_i]) - + links[link_i].c.cross(f_hat[link_i]); - - // モーメントを回転トルクに分解する - q_list[link_i] = links[link_i].a.transpose() * n[link_i]; - // トルクを電流値に変換する - q_list[link_i] *= torque_to_current.at(link_i); - } - - return true; -} - -} // namespace samples03_dynamics diff --git a/samples/samples03/src/rt_manipulators_ik.cpp b/samples/samples03/src/rt_manipulators_ik.cpp deleted file mode 100644 index a428bd1..0000000 --- a/samples/samples03/src/rt_manipulators_ik.cpp +++ /dev/null @@ -1,256 +0,0 @@ -// Copyright 2022 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include "rt_manipulators_ik.hpp" -#include "rt_manipulators_cpp/kinematics.hpp" - -namespace samples03 { - -bool q_list_are_in_range( - const kinematics_utils::links_t & links, const kinematics_utils::q_list_t & q_list) { - // 目標角度が可動範囲内にあるかチェックする - for (const auto & [target_id, q_value] : q_list) { - if (std::isnan(q_value)) { - std::cerr << "リンクID:" << target_id << " の目標角度が異常値です" << std::endl; - return false; - } - - if (q_value < links[target_id].min_q || q_value > links[target_id].max_q) { - std::cerr << "リンクID:" << target_id << " の目標角度:" << q_value; - std::cerr << " が可動範囲外です" << std::endl; - return false; - } - } - return true; -} - -bool x7_3dof_inverse_kinematics(const kinematics_utils::links_t & links, - const Eigen::Vector3d & target_p, - kinematics_utils::q_list_t & q_list) { - // 目標位置をもとに、CRANE-X7の第1, 2, 4関節の目標角度を計算する - - // リンク相対位置から、関節間のリンク長を求める - const double L1 = links[2].b.z() + links[3].b.z(); - const double L2 = links[4].b.z() + links[5].b.z(); - const double L3 = links[6].b.z() + links[7].b.z(); - const double L2_2 = std::pow(L2, 2); - const double L3_2 = std::pow(L3, 2); - - // 計算式 - // ※CRANE-X7の軸の正回転方向を考慮して、θ2' = -θ2, θ3' = -θ3としている - // ※θ2,θ3が求まり次第、符号を反転する - // PX = (L2 * S2 + L3 * S23) * C1 - // PY = (L2 * S2 + L3 * S23) * S1 - // PZ = L1 + L2 * C2 + L3 * C23 - // ※以下、PZ' = PZ - L1 として実装している - - const double PX = target_p.x(); - const double PY = target_p.y(); - const double PZ = target_p.z() - L1; - const double PX_2 = std::pow(PX, 2); - const double PY_2 = std::pow(PY, 2); - const double PZ_2 = std::pow(PZ, 2); - const double PXYZ_2 = PX_2 + PY_2 + PZ_2; - - // 目標位置が可動範囲内にあるかチェック - if (std::pow(L2 - L3, 2) > PXYZ_2 || std::pow(L2 + L3, 2) < PXYZ_2) { - std::cerr << "目標位置が可動範囲外です" << std::endl; - return false; - } - double theta3 = std::acos((PXYZ_2 - L2_2 - L3_2) / (2 * L2 * L3)); - double theta1 = std::atan2(PY, PX); - - const double C3 = std::cos(theta3); - const double S3 = std::sin(theta3); - const double L2_L3C3 = L2 + L3 * C3; - const double L3S3 = L3 * S3; - const double SQRT_PXY = std::sqrt(PX_2 + PY_2); - - double theta2 = std::atan2( - +L2_L3C3 * SQRT_PXY - L3S3 * PZ, - +L3S3 * SQRT_PXY + L2_L3C3 * PZ); - - // θ2とθ3の符号を反転する - theta2 *= -1.0; - theta3 *= -1.0; - - q_list[2] = theta1; - q_list[3] = theta2; - q_list[5] = theta3; - - // 関節角度が可動範囲内にあるかチェック - if (q_list_are_in_range(links, q_list) == false) { - return false; - } - - return true; -} - -bool x7_3dof_picking_inverse_kinematics(const kinematics_utils::links_t & links, - const Eigen::Vector3d & target_p, - kinematics_utils::q_list_t & q_list) { - // 手先先端の目標位置をもとに、CRANE-X7の第1, 2, 4, 6, 7関節の目標角度を計算する - // 手先は常に地面（-Z）方向を向く - - // 目標位置Zに手先リンク長を加え、それを手首の目標位置とする - const double HAND_LENGTH = 0.06; // Ref: https://rt-net.jp/products/crane-x7/ - Eigen::Vector3d wrist_target_p = target_p; - wrist_target_p[2] += links[8].b.z() + links[9].b.z() + HAND_LENGTH; - - if (x7_3dof_inverse_kinematics(links, wrist_target_p, q_list) == false) { - return false; - } - - // 第2, 4関節の大きさから、手先を下に向けるための第6関節の目標角度を求める - q_list[7] = -M_PI - q_list[3] - q_list[5]; - - // 手先姿勢を維持するため、第1関節と第7関節の目標角度を同じにする - q_list[8] = q_list[2]; - - // 関節角度が可動範囲内にあるかチェック - if (q_list_are_in_range(links, q_list) == false) { - return false; - } - - return true; -} - -bool s17_3dof_right_arm_inverse_kinematics(const kinematics_utils::links_t & links, - const Eigen::Vector3d & target_p, - kinematics_utils::q_list_t & q_list) { - // 目標位置をもとに、Sciurus17の右腕の第1, 2, 4関節の目標角度を計算する - - // リンク相対位置から、関節間のリンク長を求める - // 右腕の原点：P1 = Body + R_Link1 + R_Link2 - const Eigen::Vector3d P1 = links[2].b + links[5].b + links[6].b; - const double L2 = std::fabs(links[7].b.y() + links[8].b.y()); - const double L3 = std::fabs(links[9].b.y() + links[10].b.y()); - const double L2_2 = std::pow(L2, 2); - const double L3_2 = std::pow(L3, 2); - - // 順運動学の計算式 - // PX = P1 + -L3*(-C1*S3 + S1*S2*C3) - S1*S2*L2 - // PY = P1 + -C2*C3*L3 -C2*L2 - // PZ = P1 + -L3*(-S1*S3 + C1*S2*C3) - C1*S2*L2 - // 以下、PX'=PX-L1, PY'=PY-L1, PZ'=PZ-L1 として実装している - const double PX = target_p.x() - P1.x(); - const double PY = target_p.y() - P1.y(); - const double PZ = target_p.z() - P1.z(); - const double PX_2 = std::pow(PX, 2); - const double PY_2 = std::pow(PY, 2); - const double PZ_2 = std::pow(PZ, 2); - const double PXYZ_2 = PX_2 + PY_2 + PZ_2; - - // 目標位置が可動範囲内にあるかチェック - if (std::pow(L2 - L3, 2) > PXYZ_2 || std::pow(L2 + L3, 2) < PXYZ_2) { - std::cerr << "目標位置が可動範囲外です" << std::endl; - return false; - } - const double theta3 = std::acos((PXYZ_2 - L2_2 - L3_2) / (2 * L2 * L3)); - - const double C3 = std::cos(theta3); - const double theta2 = -std::acos(-PY / (C3 * L3 + L2)); - - const double S2 = std::sin(theta2); - const double S3 = std::sin(theta3); - const double A = S2 * C3 * L3 + S2 * L2; - const double B = S3 * L3; - const double theta1 = std::atan2(-A * PX + B * PZ, - B * PX + A * PZ); - - q_list[5] = theta1; - q_list[6] = theta2; - q_list[8] = theta3; - - // 関節角度が可動範囲内にあるかチェック - if (q_list_are_in_range(links, q_list) == false) { - return false; - } - return true; -} - -bool s17_3dof_right_arm_picking_inverse_kinematics(const kinematics_utils::links_t & links, - const Eigen::Vector3d & target_p, - kinematics_utils::q_list_t & q_list) { - // 手先先端の目標位置をもとに、Sciurus17の右腕の第1, 2, 4, 5, 6, 7関節の目標角度を計算する - // 手先は常に地面（-Z）方向を向く - - // 目標位置Zに手先リンク長を加え、それを手首の目標位置とする - Eigen::Vector3d wrist_target_p = target_p; - wrist_target_p[2] += 0.103; // Ref: https://rt-net.jp/products/sciurus17/ - - if (s17_3dof_right_arm_inverse_kinematics(links, wrist_target_p, q_list) == false) { - return false; - } - - // 手先の目標姿勢を定める - Eigen::Matrix3d wrist_target_R = kinematics_utils::rotation_from_euler_ZYX(0.0, 0.0, M_PI_2); - - // 手先姿勢の回転行列を求める - auto calc_links = links; - kinematics_utils::set_q_list(calc_links, q_list); - kinematics::forward_kinematics(calc_links, 1); - Eigen::Matrix3d wrist_R = calc_links[8].R; - - // 現在の手先姿勢から目標姿勢に遷移するための回転行列を求める - const Eigen::Matrix3d diff_R = wrist_R.transpose() * wrist_target_R; - const double R00 = diff_R(0, 0); - const double R01 = diff_R(0, 1); - const double R02 = diff_R(0, 2); - const double R10 = diff_R(1, 0); - const double R11 = diff_R(1, 1); - const double R12 = diff_R(1, 2); - const double R21 = diff_R(2, 1); - const double R01_2 = std::pow(R01, 2); - const double R21_2 = std::pow(R21, 2); - - // diff_R = R(-Y軸でtheta4回転) * R(+Z軸でtheta5回転) * R(-Y軸でtheta6回転)とし、 - // 行列要素から回転角度を求める - - double theta4 = 0; - double theta5 = 0; - double theta6 = 0; - if (std::sqrt(R01_2 + R21_2) == 0) { - // sin(theta5)が0のとき、 - // R11 (cos(theta5)) は±1になるため、次の式でtheta5, theta6を求める - theta5 = M_PI / 2 - R11 * M_PI / 2; - theta6 = atan2(-R02, R00) - R11 * theta4; - } else { - theta5 = std::atan2(-std::sqrt(R01_2 + R21_2), R11); - theta4 = atan2(+R21, +R01); - theta6 = atan2(+R12, -R10); - q_list[9] = theta4; - q_list[10] = theta5; - q_list[11] = theta6; - // 解が可動範囲外の場合、theta5の符号を反転した値をもとに解を求める - if (q_list_are_in_range(links, q_list) == false) { - theta5 = std::atan2(std::sqrt(R01_2 + R21_2), R11); - theta4 = atan2(-R21, -R01); - theta6 = atan2(-R12, +R10); - } - } - q_list[9] = theta4; - q_list[10] = theta5; - q_list[11] = theta6; - - // 関節角度が可動範囲内にあるかチェック - if (q_list_are_in_range(links, q_list) == false) { - return false; - } - - return true; -} - -} // namespace samples03 diff --git a/samples/samples03/src/s17_3dof_inverse_kinematics.cpp b/samples/samples03/src/s17_3dof_inverse_kinematics.cpp deleted file mode 100644 index ccd566d..0000000 --- a/samples/samples03/src/s17_3dof_inverse_kinematics.cpp +++ /dev/null @@ -1,203 +0,0 @@ -// Copyright 2022 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" -#include "rt_manipulators_cpp/kinematics.hpp" -#include "rt_manipulators_cpp/kinematics_utils.hpp" -#include "rt_manipulators_cpp/link.hpp" -#include "rt_manipulators_ik.hpp" - -void move_to(rt_manipulators_cpp::Hardware & hardware, - const kinematics_utils::links_t & links, - Eigen::Vector3d & target_p, - bool move_to_picking_pose = false) { - // 目標位置をもとにIKを解き、関節角度をセットする - std::cout << "----------------------" << std::endl; - std::cout << "目標位置:" << std::endl << target_p << std::endl; - kinematics_utils::q_list_t q_list; - - if (move_to_picking_pose == true && - samples03::s17_3dof_right_arm_picking_inverse_kinematics(links, target_p, q_list) == false) { - std::cout << "ピッキング姿勢のIKに失敗しました" << std::endl; - return; - } else if (move_to_picking_pose == false && - samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list) == false) { - std::cout << "IKに失敗しました" << std::endl; - return; - } - - std::cout << "IKに成功しました" << std::endl; - for (const auto & [target_id, q_value] : q_list) { - hardware.set_position(links[target_id].dxl_id, q_value); - } -} - -void move_demo(rt_manipulators_cpp::Hardware & hardware, - const kinematics_utils::links_t & links, - bool move_to_picking_pose = false) { - Eigen::Vector3d target_p; - double target_z = 0.2; - if (move_to_picking_pose) { - // ピッキング姿勢のIKを解く場合は、目標位置が手先となる - target_z = 0.1; - } - - std::cout << "正面奥へ移動" << std::endl; - target_p << 0.4, -0.2, target_z; - move_to(hardware, links, target_p, move_to_picking_pose); - std::this_thread::sleep_for(std::chrono::seconds(2)); - - std::cout << "右奥へ移動" << std::endl; - target_p << 0.4, -0.3, target_z; - move_to(hardware, links, target_p, move_to_picking_pose); - std::this_thread::sleep_for(std::chrono::seconds(2)); - - std::cout << "右手前へ移動" << std::endl; - target_p << 0.2, -0.3, target_z; - move_to(hardware, links, target_p, move_to_picking_pose); - std::this_thread::sleep_for(std::chrono::seconds(2)); - - std::cout << "正面手前へ移動" << std::endl; - target_p << 0.2, -0.2, target_z; - move_to(hardware, links, target_p, move_to_picking_pose); - std::this_thread::sleep_for(std::chrono::seconds(2)); -} - -int main() { - std::cout << "手先目標位置をもとに解析的に逆運動学を解き、" - << "Sciurus17を動かすサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string hardware_config_file = "../config/sciurus17.yaml"; - std::string link_config_file = "../config/sciurus17_links.csv"; - - auto links = kinematics_utils::parse_link_config_file(link_config_file); - kinematics::forward_kinematics(links, 1); - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(hardware_config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - // 手先リンクのIDを設定 - const int RIGHT = 0; - const int LEFT = 1; - const std::vector SIDES = {RIGHT, LEFT}; - std::map TARGET_LINK_ID{ - {RIGHT, 12}, - {LEFT, 21}, - }; - // 関節可動範囲の設定 - for (const auto & side : SIDES) { - for (auto link_id : kinematics_utils::find_route(links, TARGET_LINK_ID[side])) { - hardware.get_max_position_limit(links[link_id].dxl_id, links[link_id].max_q); - hardware.get_min_position_limit(links[link_id].dxl_id, links[link_id].min_q); - } - } - - std::vector group_names = {"right_arm", "left_arm", "torso"}; - for (auto group : group_names) { - std::cout << group << "グループのサーボ最大加速度をpi rad/s^2、最大速度をpi rad/sに設定します." - << std::endl; - if (!hardware.write_max_acceleration_to_group(group, 1.0 * M_PI)) { - std::cerr << group << "グループの最大加速度を設定できませんでした." << std::endl; - return -1; - } - - if (!hardware.write_max_velocity_to_group(group, 1.0 * M_PI)) { - std::cerr << group << "グループの最大速度を設定できませんでした." << std::endl; - return -1; - } - - std::cout << group << "グループのサーボ位置制御PIDゲインに(800, 0, 0)を書き込みます." - << std::endl; - if (!hardware.write_position_pid_gain_to_group(group, 800, 0, 0)) { - std::cerr << group << "グループにPIDゲインを書き込めませんでした." << std::endl; - return -1; - } - - if (!hardware.torque_on(group)) { - std::cerr << group << "グループのトルクをONできませんでした." << std::endl; - return -1; - } - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::cout << "5秒後にSciurus17が動作するため、Sciurus17の周りに物や人を近づけないで下さい." - << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - std::cout << "腰軸を0度へ移動" << std::endl; - hardware.set_position(18, 0.0); - std::this_thread::sleep_for(std::chrono::seconds(1)); - - // ホームポジション：上腕と前腕の磁石を付ける姿勢 - std::vector home_positions = {-0.3574, -1.57, 0, 2.72, 0, -1.12, 0}; - std::cout << "右腕をホームポジションへ移動" << std::endl; - hardware.set_positions("right_arm", home_positions); - std::this_thread::sleep_for(std::chrono::seconds(1)); - - move_demo(hardware, links, false); - std::cout << "ピッキング姿勢のIKを解きます" << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(2)); - move_demo(hardware, links, true); - - std::cout << "右腕をホームポジションへ移動" << std::endl; - hardware.set_positions("right_arm", home_positions); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - for (auto group : group_names) { - std::cout << group << "グループのサーボ位置制御PIDゲインに(5, 0, 0)を書き込み、脱力させます." - << std::endl; - if (!hardware.write_position_pid_gain_to_group(group, 5, 0, 0)) { - std::cerr << group << "グループにPIDゲインを書き込めませんでした." << std::endl; - } - } - - std::cout << "10秒間スリープします." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(10)); - - for (auto group : group_names) { - if (!hardware.torque_off(group)) { - std::cerr << group << "グループのトルクをOFFできませんでした." << std::endl; - } - - if (!hardware.write_position_pid_gain_to_group(group, 800, 0, 0)) { - std::cerr << group << "グループにPIDゲインを書き込めませんでした." << std::endl; - } - } - - std::cout << "Sciurus17との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples03/src/s17_gravity_compensation.cpp b/samples/samples03/src/s17_gravity_compensation.cpp deleted file mode 100644 index d52eb08..0000000 --- a/samples/samples03/src/s17_gravity_compensation.cpp +++ /dev/null @@ -1,240 +0,0 @@ -// Copyright 2022 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// For getch(), kbhit() -// Ref: https://github.com/ROBOTIS-GIT/DynamixelSDK/blob/master/c%2B%2B/example/protocol2.0/read_write/read_write.cpp -#include -#include -#define STDIN_FILENO 0 -#define ESC_ASCII_VALUE 0x1b - -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" -#include "rt_manipulators_cpp/kinematics.hpp" -#include "rt_manipulators_cpp/kinematics_utils.hpp" -#include "rt_manipulators_cpp/link.hpp" -#include "rt_manipulators_dynamics.hpp" - -int getch() { - // Ref: https://github.com/ROBOTIS-GIT/DynamixelSDK/blob/c7e1eb71c911b87f7bdeda3c2c9e92276c2b4627/c%2B%2B/example/protocol2.0/read_write/read_write.cpp#L100-L114 - struct termios oldt, newt; - int ch; - tcgetattr(STDIN_FILENO, &oldt); - newt = oldt; - newt.c_lflag &= ~(ICANON | ECHO); - tcsetattr(STDIN_FILENO, TCSANOW, &newt); - ch = getchar(); - tcsetattr(STDIN_FILENO, TCSANOW, &oldt); - return ch; -} - -int kbhit(void) { - // Ref: https://github.com/ROBOTIS-GIT/DynamixelSDK/blob/c7e1eb71c911b87f7bdeda3c2c9e92276c2b4627/c%2B%2B/example/protocol2.0/read_write/read_write.cpp#L116-L143 - struct termios oldt, newt; - int ch; - int oldf; - - tcgetattr(STDIN_FILENO, &oldt); - newt = oldt; - newt.c_lflag &= ~(ICANON | ECHO); - tcsetattr(STDIN_FILENO, TCSANOW, &newt); - oldf = fcntl(STDIN_FILENO, F_GETFL, 0); - fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); - - ch = getchar(); - - tcsetattr(STDIN_FILENO, TCSANOW, &oldt); - fcntl(STDIN_FILENO, F_SETFL, oldf); - - if (ch != EOF) { - ungetc(ch, stdin); - return 1; - } - - return 0; -} - -void set_right_arm_joint_positions(std::vector & links, - std::vector positions) { - // リンクにright_armジョイントの現在角度をセットする - if (positions.size() != 7) { - std::cerr << "引数positionsには7個のジョイント角度をセットしてください" << std::endl; - return; - } - - int start_id = 5; // R_Link1 - for (int i=0; i < positions.size(); i++) { - links[start_id + i].q = positions[i]; - } -} - -void set_left_arm_joint_positions(std::vector & links, - std::vector positions) { - // リンクにleft_armジョイントの現在角度をセットする - if (positions.size() != 7) { - std::cerr << "引数positionsには7個のジョイント角度をセットしてください" << std::endl; - return; - } - - int start_id = 14; // L_Link1 - for (int i=0; i < positions.size(); i++) { - links[start_id + i].q = positions[i]; - } -} - -void set_torso_joint_positions(std::vector & links, - std::vector positions) { - // リンクにtorsoジョイントの現在角度をセットする - if (positions.size() != 3) { - std::cerr << "引数positionsには3個のジョイント角度をセットしてください" << std::endl; - return; - } - - int start_id = 2; // Body - for (int i=0; i < positions.size(); i++) { - links[start_id + i].q = positions[i]; - } -} - -int main() { - std::cout << "現在姿勢をもとに重力補償トルクを計算し、" - << "Sciurus17のサーボモータに入力するサンプルです" << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string hardware_config_file = "../config/sciurus17_current.yaml"; - std::string link_config_file = "../config/sciurus17_links.csv"; - - auto links = kinematics_utils::parse_link_config_file(link_config_file); - kinematics::forward_kinematics(links, 1); - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(hardware_config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - // 手先リンクのIDを設定 - kinematics_utils::link_id_t target_id_right = 11; - kinematics_utils::link_id_t target_id_left = 20; - std::vector target_id_list{target_id_right, target_id_left}; - // 関節可動範囲の設定 - for (const auto & target_id : target_id_list) { - for (auto link_id : kinematics_utils::find_route(links, target_id)) { - hardware.get_max_position_limit(links[link_id].dxl_id, links[link_id].max_q); - hardware.get_min_position_limit(links[link_id].dxl_id, links[link_id].min_q); - } - } - - std::vector group_names = {"right_arm", "left_arm", "torso"}; - for (const auto & group_name : group_names) { - if (!hardware.torque_on(group_name)) { - std::cerr << group_name << "グループのトルクをONできませんでした." << std::endl; - return -1; - } - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::cout << "5秒後に重力補償トルクをサーボモータへ入力します" << std::endl; - std::cout << "終了する場合はEscキーを押してください" << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - kinematics_utils::q_list_t q_list; - // トルク・電流比 A/Nm - // Dynamixelのe-manualに記載されたパラメータをもとに微調整しています - samples03_dynamics::torque_to_current_t torque_to_current_right = { - {2, 1.0 / 3.60}, - {5, 1.0 / 3.60}, - {6, 1.0 / 3.60}, - {7, 1.0 / 2.20}, - {8, 1.0 / 2.00}, - {9, 1.0 / 2.20}, - {10, 1.0 / 2.20}, - {11, 1.0 / 2.20} - }; - - samples03_dynamics::torque_to_current_t torque_to_current_left = { - {2, 1.0 / 3.60}, - {14, 1.0 / 3.60}, - {15, 1.0 / 3.60}, - {16, 1.0 / 2.20}, - {17, 1.0 / 2.00}, - {18, 1.0 / 2.20}, - {19, 1.0 / 2.20}, - {20, 1.0 / 2.20} - }; - - while (1) { - if (kbhit()) { - if (getch() == ESC_ASCII_VALUE) { - std::cout << "Escが入力されました" << std::endl; - break; - } - } - - // 姿勢を更新 - std::vector positions; - if (hardware.get_positions("right_arm", positions)) { - set_right_arm_joint_positions(links, positions); - positions.clear(); - } - if (hardware.get_positions("left_arm", positions)) { - set_left_arm_joint_positions(links, positions); - positions.clear(); - } - if (hardware.get_positions("torso", positions)) { - set_torso_joint_positions(links, positions); - positions.clear(); - } - kinematics::forward_kinematics(links, 1); - - // ここで重力補償分の電流値を計算 - samples03_dynamics::gravity_compensation( - links, target_id_right, torque_to_current_right, q_list); - samples03_dynamics::gravity_compensation( - links, target_id_left, torque_to_current_left, q_list); - for (const auto & [target_id, q_value] : q_list) { - hardware.set_current(links[target_id].dxl_id, q_value); - } - } - - - std::cout << "終了します" << std::endl; - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - for (const auto & group_name : group_names) { - if (!hardware.torque_off(group_name)) { - std::cerr << group_name << "グループのトルクをOFFできませんでした." << std::endl; - } - } - - std::cout << "Sciurus17との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples03/src/x7_3dof_inverse_kinematics.cpp b/samples/samples03/src/x7_3dof_inverse_kinematics.cpp deleted file mode 100644 index 1bafe91..0000000 --- a/samples/samples03/src/x7_3dof_inverse_kinematics.cpp +++ /dev/null @@ -1,189 +0,0 @@ -// Copyright 2022 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" -#include "rt_manipulators_cpp/kinematics.hpp" -#include "rt_manipulators_cpp/kinematics_utils.hpp" -#include "rt_manipulators_cpp/link.hpp" -#include "rt_manipulators_ik.hpp" - -void move_to(rt_manipulators_cpp::Hardware & hardware, - const kinematics_utils::links_t & links, - Eigen::Vector3d & target_p, - bool move_to_picking_pose = false) { - // 目標位置をもとにIKを解き、関節角度をセットする - std::cout << "----------------------" << std::endl; - std::cout << "目標位置:" << std::endl << target_p << std::endl; - kinematics_utils::q_list_t q_list; - - if (move_to_picking_pose == true && - samples03::x7_3dof_picking_inverse_kinematics(links, target_p, q_list) == false) { - std::cout << "ピッキング姿勢のIKに失敗しました" << std::endl; - return; - } else if (move_to_picking_pose == false && - samples03::x7_3dof_inverse_kinematics(links, target_p, q_list) == false) { - std::cout << "IKに失敗しました" << std::endl; - return; - } - - std::cout << "IKに成功しました" << std::endl; - for (const auto & [target_id, q_value] : q_list) { - hardware.set_position(links[target_id].dxl_id, q_value); - } -} - -void move_demo(rt_manipulators_cpp::Hardware & hardware, - const kinematics_utils::links_t & links, - bool move_to_picking_pose = false) { - Eigen::Vector3d target_p; - double target_z = 0.2; - if (move_to_picking_pose) { - // ピッキング姿勢のIKを解く場合は、目標位置が手先となる - target_z = 0.05; - } - - std::cout << "左奥へ移動" << std::endl; - target_p << 0.3, 0.1, target_z; - move_to(hardware, links, target_p, move_to_picking_pose); - std::this_thread::sleep_for(std::chrono::seconds(2)); - - std::cout << "左手前へ移動" << std::endl; - target_p << 0.1, 0.1, target_z; - move_to(hardware, links, target_p, move_to_picking_pose); - std::this_thread::sleep_for(std::chrono::seconds(2)); - - std::cout << "右手前へ移動" << std::endl; - target_p << 0.1, -0.1, target_z; - move_to(hardware, links, target_p, move_to_picking_pose); - std::this_thread::sleep_for(std::chrono::seconds(2)); - - std::cout << "右奥へ移動" << std::endl; - target_p << 0.3, -0.1, target_z; - move_to(hardware, links, target_p, move_to_picking_pose); - std::this_thread::sleep_for(std::chrono::seconds(2)); -} - -int main() { - std::cout << "手先目標位置をもとに解析的に逆運動学を解き、" - << "CRANE-X7を動かすサンプルです." << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string hardware_config_file = "../config/crane-x7.yaml"; - std::string link_config_file = "../config/crane-x7_links.csv"; - - auto links = kinematics_utils::parse_link_config_file(link_config_file); - kinematics::forward_kinematics(links, 1); - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(hardware_config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - // 関節可動範囲の設定 - for (auto link_id : kinematics_utils::find_route(links, 8)) { - hardware.get_max_position_limit(links[link_id].dxl_id, links[link_id].max_q); - hardware.get_min_position_limit(links[link_id].dxl_id, links[link_id].min_q); - } - - std::cout << "armグループのサーボ最大加速度をpi rad/s^2、最大速度をpi rad/sに設定します." - << std::endl; - if (!hardware.write_max_acceleration_to_group("arm", 1.0 * M_PI)) { - std::cerr << "armグループの最大加速度を設定できませんでした." << std::endl; - return -1; - } - - if (!hardware.write_max_velocity_to_group("arm", 1.0 * M_PI)) { - std::cerr << "armグループの最大速度を設定できませんでした." << std::endl; - return -1; - } - - std::cout << "armグループのサーボ位置制御PIDゲインに(800, 0, 0)を書き込みます." - << std::endl; - if (!hardware.write_position_pid_gain_to_group("arm", 800, 0, 0)) { - std::cerr << "armグループにPIDゲインを書き込めませんでした." << std::endl; - return -1; - } - - if (!hardware.torque_on("arm")) { - std::cerr << "armグループのトルクをONできませんでした." << std::endl; - return -1; - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - std::vector group_names = {"arm"}; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::cout << "5秒後にX7が垂直姿勢へ移行するため、X7の周りに物や人を近づけないで下さい." - << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - // 垂直姿勢へ移動 - std::vector target_positions(7, 0.0); - hardware.set_positions("arm", target_positions); - std::this_thread::sleep_for(std::chrono::seconds(5)); - - Eigen::Vector3d target_p; - - std::cout << "正面へ移動" << std::endl; - target_p << 0.2, 0.0, 0.2; - move_to(hardware, links, target_p); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - move_demo(hardware, links, false); - std::cout << "ピッキング姿勢のIKを解きます" << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(2)); - move_demo(hardware, links, true); - - std::cout << "正面へ移動し、手先を下げる" << std::endl; - target_p << 0.2, 0.0, 0.01; - move_to(hardware, links, target_p, true); - std::this_thread::sleep_for(std::chrono::seconds(3)); - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - std::cout << "armグループのサーボ位置制御PIDゲインに(5, 0, 0)を書き込み、脱力させます." - << std::endl; - if (!hardware.write_position_pid_gain_to_group("arm", 5, 0, 0)) { - std::cerr << "armグループにPIDゲインを書き込めませんでした." << std::endl; - } - std::cout << "10秒間スリープします." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(10)); - - if (!hardware.torque_off("arm")) { - std::cerr << "armグループのトルクをOFFできませんでした." << std::endl; - } - - if (!hardware.write_position_pid_gain_to_group("arm", 800, 0, 0)) { - std::cerr << "armグループにPIDゲインを書き込めませんでした." << std::endl; - } - - std::cout << "CRANE-X7との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples03/src/x7_gravity_compensation.cpp b/samples/samples03/src/x7_gravity_compensation.cpp deleted file mode 100644 index 04d0539..0000000 --- a/samples/samples03/src/x7_gravity_compensation.cpp +++ /dev/null @@ -1,180 +0,0 @@ -// Copyright 2022 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// For getch(), kbhit() -// Ref: https://github.com/ROBOTIS-GIT/DynamixelSDK/blob/master/c%2B%2B/example/protocol2.0/read_write/read_write.cpp -#include -#include -#define STDIN_FILENO 0 -#define ESC_ASCII_VALUE 0x1b - -#include -#include -#include -#include -#include "rt_manipulators_cpp/hardware.hpp" -#include "rt_manipulators_cpp/kinematics.hpp" -#include "rt_manipulators_cpp/kinematics_utils.hpp" -#include "rt_manipulators_cpp/link.hpp" -#include "rt_manipulators_dynamics.hpp" - -int getch() { - // Ref: https://github.com/ROBOTIS-GIT/DynamixelSDK/blob/c7e1eb71c911b87f7bdeda3c2c9e92276c2b4627/c%2B%2B/example/protocol2.0/read_write/read_write.cpp#L100-L114 - struct termios oldt, newt; - int ch; - tcgetattr(STDIN_FILENO, &oldt); - newt = oldt; - newt.c_lflag &= ~(ICANON | ECHO); - tcsetattr(STDIN_FILENO, TCSANOW, &newt); - ch = getchar(); - tcsetattr(STDIN_FILENO, TCSANOW, &oldt); - return ch; -} - -int kbhit(void) { - // Ref: https://github.com/ROBOTIS-GIT/DynamixelSDK/blob/c7e1eb71c911b87f7bdeda3c2c9e92276c2b4627/c%2B%2B/example/protocol2.0/read_write/read_write.cpp#L116-L143 - struct termios oldt, newt; - int ch; - int oldf; - - tcgetattr(STDIN_FILENO, &oldt); - newt = oldt; - newt.c_lflag &= ~(ICANON | ECHO); - tcsetattr(STDIN_FILENO, TCSANOW, &newt); - oldf = fcntl(STDIN_FILENO, F_GETFL, 0); - fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); - - ch = getchar(); - - tcsetattr(STDIN_FILENO, TCSANOW, &oldt); - fcntl(STDIN_FILENO, F_SETFL, oldf); - - if (ch != EOF) { - ungetc(ch, stdin); - return 1; - } - - return 0; -} - -void set_arm_joint_positions(std::vector & links, - std::vector positions) { - // リンクにarmジョイントの現在角度をセットする - if (positions.size() != 7) { - std::cerr << "引数positionsには7個のジョイント角度をセットしてください" << std::endl; - return; - } - - int start_id = 2; // Link1 - for (int i=0; i < positions.size(); i++) { - links[start_id + i].q = positions[i]; - } -} - -int main() { - std::cout << "現在姿勢をもとに重力補償トルクを計算し、" - << "CRANE-X7のサーボモータに入力するサンプルです" << std::endl; - - std::string port_name = "/dev/ttyUSB0"; - int baudrate = 3000000; // 3Mbps - std::string hardware_config_file = "../config/crane-x7_current.yaml"; - std::string link_config_file = "../config/crane-x7_links.csv"; - - auto links = kinematics_utils::parse_link_config_file(link_config_file); - kinematics::forward_kinematics(links, 1); - - rt_manipulators_cpp::Hardware hardware(port_name); - if (!hardware.connect(baudrate)) { - std::cerr << "ロボットとの接続に失敗しました." << std::endl; - return -1; - } - - if (!hardware.load_config_file(hardware_config_file)) { - std::cerr << "コンフィグファイルの読み込みに失敗しました." << std::endl; - return -1; - } - - // 関節可動範囲の設定 - for (auto link_id : kinematics_utils::find_route(links, 8)) { - hardware.get_max_position_limit(links[link_id].dxl_id, links[link_id].max_q); - hardware.get_min_position_limit(links[link_id].dxl_id, links[link_id].min_q); - } - - if (!hardware.torque_on("arm")) { - std::cerr << "armグループのトルクをONできませんでした." << std::endl; - return -1; - } - - std::cout << "read/writeスレッドを起動します." << std::endl; - std::vector group_names = {"arm"}; - if (!hardware.start_thread(group_names, std::chrono::milliseconds(10))) { - std::cerr << "スレッドの起動に失敗しました." << std::endl; - return -1; - } - - std::cout << "5秒後に重力補償トルクをサーボモータへ入力します" << std::endl; - std::cout << "終了する場合はEscキーを押してください" << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - kinematics_utils::q_list_t q_list; - kinematics_utils::link_id_t target_id = 8; - // トルク・電流比 A/Nm - // Dynamixelのe-manualに記載されたパラメータをもとに微調整しています - samples03_dynamics::torque_to_current_t torque_to_current = { - {2, 1.0 / 2.20}, - {3, 1.0 / 3.60}, - {4, 1.0 / 2.20}, - {5, 1.0 / 2.20}, - {6, 1.0 / 2.20}, - {7, 1.0 / 2.20}, - {8, 1.0 / 2.20} - }; - - while (1) { - if (kbhit()) { - if (getch() == ESC_ASCII_VALUE) { - std::cout << "Escが入力されました" << std::endl; - break; - } - } - - // 姿勢を更新 - std::vector positions; - if (hardware.get_positions("arm", positions)) { - set_arm_joint_positions(links, positions); - kinematics::forward_kinematics(links, 1); - } - - // ここで重力補償分の電流値を計算 - samples03_dynamics::gravity_compensation( - links, target_id, torque_to_current, q_list); - for (const auto & [target_id, q_value] : q_list) { - hardware.set_current(links[target_id].dxl_id, q_value); - } - } - - - std::cout << "終了します" << std::endl; - - std::cout << "スレッドを停止します." << std::endl; - hardware.stop_thread(); - - if (!hardware.torque_off("arm")) { - std::cerr << "armグループのトルクをOFFできませんでした." << std::endl; - } - - std::cout << "CRANE-X7との接続を解除します." << std::endl; - hardware.disconnect(); - return 0; -} diff --git a/samples/samples03/test/CMakeLists.txt b/samples/samples03/test/CMakeLists.txt deleted file mode 100644 index ddfa2e8..0000000 --- a/samples/samples03/test/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ -cmake_minimum_required(VERSION 3.10) - -# set the project name and version -project(samples03_test VERSION 1.0) - -# specify the C++ standard -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED True) - -enable_testing() - -find_package(GTest REQUIRED) - -include(GoogleTest) - -set(list_tests - test_rt_manipulators_ik - test_rt_manipulators_dynamics -) -foreach(test_executable IN LISTS list_tests) - message("${test_executable}") - add_executable(${test_executable} - ${test_executable}.cpp - ../src/rt_manipulators_ik.cpp - ../src/rt_manipulators_dynamics.cpp - ) - target_include_directories(${test_executable} PUBLIC - ../include - ) - target_link_libraries(${test_executable} - GTest::GTest - GTest::Main - rt_manipulators_cpp - ) - gtest_discover_tests(${test_executable}) -endforeach() diff --git a/samples/samples03/test/test_rt_manipulators_dynamics.cpp b/samples/samples03/test/test_rt_manipulators_dynamics.cpp deleted file mode 100644 index a3a0e34..0000000 --- a/samples/samples03/test/test_rt_manipulators_dynamics.cpp +++ /dev/null @@ -1,196 +0,0 @@ -// Copyright 2022 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include "gtest/gtest.h" -#include "rt_manipulators_cpp/kinematics.hpp" -#include "rt_manipulators_cpp/kinematics_utils.hpp" -#include "rt_manipulators_cpp/link.hpp" -#include "rt_manipulators_dynamics.hpp" - - -class X7KinematicsFixture: public ::testing::Test { - protected: - virtual void SetUp() { - links = kinematics_utils::parse_link_config_file("../../config/crane-x7_links.csv"); - kinematics::forward_kinematics(links, 1); - - // 目標値と判定値との許容誤差を設定 - TOLERANCE_Q = 0.001; - } - - virtual void TearDown() { - } - - std::vector links; - double TOLERANCE_Q; -}; - -class S17KinematicsFixture: public ::testing::Test { - protected: - virtual void SetUp() { - links = kinematics_utils::parse_link_config_file("../../config/sciurus17_links.csv"); - kinematics::forward_kinematics(links, 1); - - // 目標値と判定値との許容誤差を設定 - TOLERANCE_Q = 0.001; - } - - virtual void TearDown() { - } - - std::vector links; - double TOLERANCE_Q; -}; - -TEST_F(X7KinematicsFixture, gravity_compensation) { - kinematics_utils::q_list_t q_list; - kinematics_utils::link_id_t target_id = 8; - - // トルク電流比に抜けがあればfalseを返す - samples03_dynamics::torque_to_current_t invalid_values = { - {3, 1.0}, - {4, 1.0}, - {5, 1.0}, - {6, 1.0}, - {7, 1.0}, - {8, 1.0} - }; - EXPECT_FALSE(samples03_dynamics::gravity_compensation( - links, target_id, invalid_values, q_list)); - - samples03_dynamics::torque_to_current_t torque_to_current = { - {2, 1.0}, - {3, 1.0}, - {4, 1.0}, - {5, 1.0}, - {6, 1.0}, - {7, 1.0}, - {8, 1.0} - }; - - // 原点姿勢（腕を垂直に延ばした姿勢） - kinematics::forward_kinematics(links, 1); - EXPECT_TRUE(samples03_dynamics::gravity_compensation( - links, target_id, torque_to_current, q_list)); - EXPECT_EQ(q_list.size(), 7); - EXPECT_NEAR(q_list[2], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[3], -0.055, TOLERANCE_Q); - EXPECT_NEAR(q_list[4], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[5], -0.012, TOLERANCE_Q); - EXPECT_NEAR(q_list[6], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[7], 0.008, TOLERANCE_Q); - EXPECT_NEAR(q_list[8], 0.0, TOLERANCE_Q); - - // 適当に関節を曲げ、意図した関節にトルクが発生していることを期待 - links[3].q = -M_PI_2; - links[4].q = -M_PI_2; - links[5].q = -M_PI_2; - links[7].q = -M_PI_2; - kinematics::forward_kinematics(links, 1); - EXPECT_TRUE(samples03_dynamics::gravity_compensation( - links, target_id, torque_to_current, q_list)); - EXPECT_NEAR(q_list[2], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[3], 2.218, TOLERANCE_Q); - EXPECT_NEAR(q_list[4], -1.137, TOLERANCE_Q); - EXPECT_NEAR(q_list[5], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[6], -0.042, TOLERANCE_Q); - EXPECT_NEAR(q_list[7], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[8], 0.0, TOLERANCE_Q); -} - -TEST_F(S17KinematicsFixture, gravity_compensation) { - kinematics_utils::q_list_t q_list; - kinematics_utils::link_id_t target_id_right = 11; - kinematics_utils::link_id_t target_id_left = 20; - - samples03_dynamics::torque_to_current_t torque_to_current_right = { - {2, 1.0}, - {5, 1.0}, - {6, 1.0}, - {7, 1.0}, - {8, 1.0}, - {9, 1.0}, - {10, 1.0}, - {11, 1.0} - }; - - samples03_dynamics::torque_to_current_t torque_to_current_left = { - {2, 1.0}, - {14, 1.0}, - {15, 1.0}, - {16, 1.0}, - {17, 1.0}, - {18, 1.0}, - {19, 1.0}, - {20, 1.0} - }; - - // 原点姿勢（腕を左右に延ばした姿勢） - kinematics::forward_kinematics(links, 1); - EXPECT_TRUE(samples03_dynamics::gravity_compensation( - links, target_id_right, torque_to_current_right, q_list)); - EXPECT_EQ(q_list.size(), 8); - EXPECT_NEAR(q_list[2], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[5], -0.061, TOLERANCE_Q); - EXPECT_NEAR(q_list[6], 3.909, TOLERANCE_Q); - EXPECT_NEAR(q_list[7], -0.060, TOLERANCE_Q); - EXPECT_NEAR(q_list[8], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[9], 0.021, TOLERANCE_Q); - EXPECT_NEAR(q_list[10], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[11], 0.0, TOLERANCE_Q); - q_list.clear(); - - EXPECT_TRUE(samples03_dynamics::gravity_compensation( - links, target_id_left, torque_to_current_left, q_list)); - EXPECT_EQ(q_list.size(), 8); - EXPECT_NEAR(q_list[2], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[14], 0.061, TOLERANCE_Q); - EXPECT_NEAR(q_list[15], -3.862, TOLERANCE_Q); - EXPECT_NEAR(q_list[16], 0.060, TOLERANCE_Q); - EXPECT_NEAR(q_list[17], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[18], -0.021, TOLERANCE_Q); - EXPECT_NEAR(q_list[19], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[20], 0.0, TOLERANCE_Q); - q_list.clear(); - - // 適当に関節を曲げ、意図した関節にトルクが発生していることを期待 - links[5].q = -M_PI_2; - kinematics::forward_kinematics(links, 1); - EXPECT_TRUE(samples03_dynamics::gravity_compensation( - links, target_id_right, torque_to_current_right, q_list)); - EXPECT_NEAR(q_list[2], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[5], 0.002, TOLERANCE_Q); - EXPECT_NEAR(q_list[6], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[7], 0.003, TOLERANCE_Q); - EXPECT_NEAR(q_list[8], -1.309, TOLERANCE_Q); - EXPECT_NEAR(q_list[9], 0.002, TOLERANCE_Q); - EXPECT_NEAR(q_list[10], -0.071, TOLERANCE_Q); - EXPECT_NEAR(q_list[11], 0.001, TOLERANCE_Q); - - links[14].q = M_PI_2; - kinematics::forward_kinematics(links, 1); - EXPECT_TRUE(samples03_dynamics::gravity_compensation( - links, target_id_left, torque_to_current_left, q_list)); - EXPECT_NEAR(q_list[2], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[14], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[15], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[16], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[17], 1.262, TOLERANCE_Q); - EXPECT_NEAR(q_list[18], 0.001, TOLERANCE_Q); - EXPECT_NEAR(q_list[19], 0.022, TOLERANCE_Q); - EXPECT_NEAR(q_list[20], 0.001, TOLERANCE_Q); -} diff --git a/samples/samples03/test/test_rt_manipulators_ik.cpp b/samples/samples03/test/test_rt_manipulators_ik.cpp deleted file mode 100644 index 49ffc41..0000000 --- a/samples/samples03/test/test_rt_manipulators_ik.cpp +++ /dev/null @@ -1,271 +0,0 @@ -// Copyright 2022 RT Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include "gtest/gtest.h" -#include "rt_manipulators_cpp/kinematics.hpp" -#include "rt_manipulators_cpp/kinematics_utils.hpp" -#include "rt_manipulators_cpp/link.hpp" -#include "rt_manipulators_ik.hpp" - - -class X7KinematicsFixture: public ::testing::Test { - protected: - virtual void SetUp() { - links = kinematics_utils::parse_link_config_file("../../config/crane-x7_links.csv"); - kinematics::forward_kinematics(links, 1); - // 関節の可動範囲を制限 - links[2].min_q = -150 * M_PI / 180.0; - links[2].max_q = 150 * M_PI / 180.0; - links[3].min_q = -90 * M_PI / 180.0; - links[3].max_q = 90 * M_PI / 180.0; - links[4].min_q = -150 * M_PI / 180.0; - links[4].max_q = 150 * M_PI / 180.0; - links[5].min_q = -160 * M_PI / 180.0; - links[5].max_q = 0 * M_PI / 180.0; - links[6].min_q = -150 * M_PI / 180.0; - links[6].max_q = 150 * M_PI / 180.0; - links[7].min_q = -90 * M_PI / 180.0; - links[7].max_q = 90 * M_PI / 180.0; - links[8].min_q = -160 * M_PI / 180.0; - links[8].max_q = 160 * M_PI / 180.0; - - // 目標角度と判定値との許容誤差を設定 - TOLERANCE_Q = 0.1 * M_PI / 180.0; - } - - virtual void TearDown() { - } - - std::vector links; - double TOLERANCE_Q; -}; - -class S17KinematicsFixture: public ::testing::Test { - protected: - virtual void SetUp() { - links = kinematics_utils::parse_link_config_file("../../config/sciurus17_links.csv"); - kinematics::forward_kinematics(links, 1); - // 関節の可動範囲を制限 - // 腰 - links[2].min_q = -150 * M_PI / 180.0; - links[2].max_q = 150 * M_PI / 180.0; - // 首 - links[3].min_q = -90 * M_PI / 180.0; - links[3].max_q = 90 * M_PI / 180.0; - links[4].min_q = -90 * M_PI / 180.0; - links[4].max_q = 90 * M_PI / 180.0; - // 右腕 - links[5].min_q = -150 * M_PI / 180.0; - links[5].max_q = 150 * M_PI / 180.0; - links[6].min_q = -90 * M_PI / 180.0; - links[6].max_q = 90 * M_PI / 180.0; - links[7].min_q = -150 * M_PI / 180.0; - links[7].max_q = 150 * M_PI / 180.0; - links[8].min_q = 0 * M_PI / 180.0; - links[8].max_q = 150 * M_PI / 180.0; - links[9].min_q = -150 * M_PI / 180.0; - links[9].max_q = 150 * M_PI / 180.0; - links[10].min_q = -120 * M_PI / 180.0; - links[10].max_q = 60 * M_PI / 180.0; - links[11].min_q = -160 * M_PI / 180.0; - links[11].max_q = 160 * M_PI / 180.0; - // 左腕 - links[14].min_q = -150 * M_PI / 180.0; - links[14].max_q = 150 * M_PI / 180.0; - links[15].min_q = -90 * M_PI / 180.0; - links[15].max_q = 90 * M_PI / 180.0; - links[16].min_q = -150 * M_PI / 180.0; - links[16].max_q = 150 * M_PI / 180.0; - links[17].min_q = -150 * M_PI / 180.0; - links[17].max_q = 0 * M_PI / 180.0; - links[18].min_q = -150 * M_PI / 180.0; - links[18].max_q = 150 * M_PI / 180.0; - links[19].min_q = -60 * M_PI / 180.0; - links[19].max_q = 120 * M_PI / 180.0; - links[20].min_q = -160 * M_PI / 180.0; - links[20].max_q = 160 * M_PI / 180.0; - - // 目標角度と判定値との許容誤差を設定 - TOLERANCE_Q = 0.1 * M_PI / 180.0; - } - - virtual void TearDown() { - } - - std::vector links; - double TOLERANCE_Q; -}; - -TEST_F(X7KinematicsFixture, 3dof_inverse_kinematics) { - Eigen::Vector3d target_p; - Eigen::Matrix3d target_R; - kinematics_utils::q_list_t q_list; - - // 目標角度が可動範囲外になることを期待 - target_p << 0.0, 0.0, 0.0; - EXPECT_FALSE(samples03::x7_3dof_inverse_kinematics(links, target_p, q_list)); - - target_p << -0.2, 0.0, 0.3; - EXPECT_FALSE(samples03::x7_3dof_inverse_kinematics(links, target_p, q_list)); - - // 解が求まらないことを期待 - target_p << 1.0, 0.0, 1.0; - EXPECT_FALSE(samples03::x7_3dof_inverse_kinematics(links, target_p, q_list)); - - // 解が求まることを期待 - target_p << 0.2, 0.0, 0.3; - EXPECT_TRUE(samples03::x7_3dof_inverse_kinematics(links, target_p, q_list)); - EXPECT_NEAR(q_list[2], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[3], 0.180, TOLERANCE_Q); - EXPECT_NEAR(q_list[5], -1.956, TOLERANCE_Q); - - target_p << -0.2, 0.2, 0.3; - EXPECT_TRUE(samples03::x7_3dof_inverse_kinematics(links, target_p, q_list)); - EXPECT_NEAR(q_list[2], 2.356, TOLERANCE_Q); - EXPECT_NEAR(q_list[3], -0.154, TOLERANCE_Q); - EXPECT_NEAR(q_list[5], -1.627, TOLERANCE_Q); - - target_p << -0.2, -0.2, 0.3; - EXPECT_TRUE(samples03::x7_3dof_inverse_kinematics(links, target_p, q_list)); - EXPECT_NEAR(q_list[2], -2.356, TOLERANCE_Q); - EXPECT_NEAR(q_list[3], -0.154, TOLERANCE_Q); - EXPECT_NEAR(q_list[5], -1.627, TOLERANCE_Q); -} - -TEST_F(X7KinematicsFixture, 3dof_picking_inverse_kinematics) { - Eigen::Vector3d target_p; - Eigen::Matrix3d target_R; - kinematics_utils::q_list_t q_list; - - // 目標角度が可動範囲外になることを期待 - target_p << 0.0, 0.0, 0.0; - EXPECT_FALSE(samples03::x7_3dof_picking_inverse_kinematics(links, target_p, q_list)); - - target_p << -0.2, 0.0, 0.3; - EXPECT_FALSE(samples03::x7_3dof_picking_inverse_kinematics(links, target_p, q_list)); - - // 解が求まらないことを期待 - target_p << 1.0, 0.0, 1.0; - EXPECT_FALSE(samples03::x7_3dof_picking_inverse_kinematics(links, target_p, q_list)); - - // 解が求まることを期待 - target_p << 0.2, 0.0, 0.0; - EXPECT_TRUE(samples03::x7_3dof_picking_inverse_kinematics(links, target_p, q_list)); - EXPECT_NEAR(q_list[2], 0.0, TOLERANCE_Q); - EXPECT_NEAR(q_list[3], -0.422, TOLERANCE_Q); - EXPECT_NEAR(q_list[5], -2.319, TOLERANCE_Q); - EXPECT_NEAR(q_list[7], -0.402, TOLERANCE_Q); - EXPECT_NEAR(q_list[8], 0.0, TOLERANCE_Q); - - target_p << -0.2, 0.2, 0.0; - EXPECT_TRUE(samples03::x7_3dof_picking_inverse_kinematics(links, target_p, q_list)); - EXPECT_NEAR(q_list[2], 2.356, TOLERANCE_Q); - EXPECT_NEAR(q_list[3], -0.608, TOLERANCE_Q); - EXPECT_NEAR(q_list[5], -1.939, TOLERANCE_Q); - EXPECT_NEAR(q_list[7], -0.594, TOLERANCE_Q); - EXPECT_NEAR(q_list[8], 2.356, TOLERANCE_Q); - - target_p << -0.2, -0.2, 0.0; - EXPECT_TRUE(samples03::x7_3dof_picking_inverse_kinematics(links, target_p, q_list)); - EXPECT_NEAR(q_list[2], -2.356, TOLERANCE_Q); - EXPECT_NEAR(q_list[3], -0.608, TOLERANCE_Q); - EXPECT_NEAR(q_list[5], -1.939, TOLERANCE_Q); - EXPECT_NEAR(q_list[7], -0.594, TOLERANCE_Q); - EXPECT_NEAR(q_list[8], -2.356, TOLERANCE_Q); -} - -TEST_F(S17KinematicsFixture, 3dof_right_arm_inverse_kinematics) { - Eigen::Vector3d target_p; - Eigen::Matrix3d target_R; - kinematics_utils::q_list_t q_list; - - // 目標角度が可動範囲外になることを期待 - target_p << 0.0, 0.0, 0.0; - EXPECT_FALSE(samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list)); - - target_p << 0.2, 0.0, 0.2; - EXPECT_FALSE(samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list)); - - // 解が求まらないことを期待 - target_p << 1.0, 0.0, 1.0; - EXPECT_FALSE(samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list)); - - // 解が求まらないことを期待 - target_p << 0.2, -0.4, 0.3; - EXPECT_FALSE(samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list)); - - // 解が求まることを期待 - target_p << 0.2, -0.2, 0.2; - EXPECT_TRUE(samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list)); - EXPECT_NEAR(q_list[5], -0.565, TOLERANCE_Q); - EXPECT_NEAR(q_list[6], -1.258, TOLERANCE_Q); - EXPECT_NEAR(q_list[8], 2.079, TOLERANCE_Q); - - target_p << 0.0, -0.3, 0.2; - EXPECT_TRUE(samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list)); - EXPECT_NEAR(q_list[5], -1.700, TOLERANCE_Q); - EXPECT_NEAR(q_list[6], -0.356, TOLERANCE_Q); - EXPECT_NEAR(q_list[8], 1.988, TOLERANCE_Q); - - target_p << -0.2, -0.2, 0.2; - EXPECT_TRUE(samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list)); - EXPECT_NEAR(q_list[5], -1.682, TOLERANCE_Q); - EXPECT_NEAR(q_list[6], -1.417, TOLERANCE_Q); - EXPECT_NEAR(q_list[8], 1.540, TOLERANCE_Q); -} - -TEST_F(S17KinematicsFixture, 3dof_right_arm_picking_inverse_kinematics) { - Eigen::Vector3d target_p; - Eigen::Matrix3d target_R; - kinematics_utils::q_list_t q_list; - - // 目標角度が可動範囲外になることを期待 - target_p << 0.0, 0.0, 0.0; - EXPECT_FALSE(samples03::s17_3dof_right_arm_picking_inverse_kinematics(links, target_p, q_list)); - - // 解が求まらないことを期待 - target_p << 1.0, 0.0, 1.0; - EXPECT_FALSE(samples03::s17_3dof_right_arm_picking_inverse_kinematics(links, target_p, q_list)); - - // 解が求まることを期待 - target_p << 0.2, -0.2, 0.0; - EXPECT_TRUE(samples03::s17_3dof_right_arm_picking_inverse_kinematics(links, target_p, q_list)); - EXPECT_NEAR(q_list[5], -0.468, TOLERANCE_Q); - EXPECT_NEAR(q_list[6], -1.401, TOLERANCE_Q); - EXPECT_NEAR(q_list[8], 1.641, TOLERANCE_Q); - EXPECT_NEAR(q_list[9], 0.165, TOLERANCE_Q); - EXPECT_NEAR(q_list[10], -1.171, TOLERANCE_Q); - EXPECT_NEAR(q_list[11], 0.013, TOLERANCE_Q); - - target_p << 0.0, -0.3, 0.0; - EXPECT_TRUE(samples03::s17_3dof_right_arm_picking_inverse_kinematics(links, target_p, q_list)); - EXPECT_NEAR(q_list[5], -1.119, TOLERANCE_Q); - EXPECT_NEAR(q_list[6], -0.987, TOLERANCE_Q); - EXPECT_NEAR(q_list[8], 1.559, TOLERANCE_Q); - EXPECT_NEAR(q_list[9], 0.597, TOLERANCE_Q); - EXPECT_NEAR(q_list[10], -0.442, TOLERANCE_Q); - EXPECT_NEAR(q_list[11], -0.015, TOLERANCE_Q); - - target_p << -0.1, -0.2, 0.0; - EXPECT_TRUE(samples03::s17_3dof_right_arm_picking_inverse_kinematics(links, target_p, q_list)); - EXPECT_NEAR(q_list[5], -1.270, TOLERANCE_Q); - EXPECT_NEAR(q_list[6], -1.424, TOLERANCE_Q); - EXPECT_NEAR(q_list[8], 1.493, TOLERANCE_Q); - EXPECT_NEAR(q_list[9], 0.196, TOLERANCE_Q); - EXPECT_NEAR(q_list[10], -0.224, TOLERANCE_Q); - EXPECT_NEAR(q_list[11], -0.051, TOLERANCE_Q); -}