├── open_manipulator_applications ├── CMakeLists.txt └── package.xml ├── .travis.yml ├── open_manipulator_master_slave ├── launch │ └── open_manipulator_master.launch ├── package.xml ├── CMakeLists.txt ├── include │ └── open_manipulator_master_slave │ │ └── open_manipulator_master.h └── src │ └── open_manipulator_master.cpp ├── README.md └── LICENSE /open_manipulator_applications/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(open_manipulator_applications) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This config file for Travis CI utilizes ros-industrial/industrial_ci package. 2 | # For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst 3 | 4 | sudo: required 5 | dist: trusty 6 | services: 7 | - docker 8 | language: generic 9 | python: 10 | - "2.7" 11 | compiler: 12 | - gcc 13 | notifications: 14 | email: 15 | on_success: change 16 | on_failure: always 17 | recipients: 18 | - jhshim@robotis.com 19 | env: 20 | matrix: 21 | - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian 22 | # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie 23 | branches: 24 | only: 25 | - master 26 | - develop 27 | install: 28 | - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config 29 | script: 30 | - source .ci_config/travis.sh 31 | -------------------------------------------------------------------------------- /open_manipulator_master_slave/launch/open_manipulator_master.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /open_manipulator_master_slave/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | open_manipulator_master_slave 4 | 1.0.0 5 | 6 | Example of OpenManipulator 7 | 8 | Apache 2.0 9 | Darby Lim 10 | Hye-Jong KIM 11 | Ryan Shim 12 | Yong-Ho Na 13 | Pyo 14 | http://wiki.ros.org/open_manipulator_description 15 | http://emanual.robotis.com/docs/en/platform/openmanipulator 16 | https://github.com/ROBOTIS-GIT/open_manipulator 17 | https://github.com/ROBOTIS-GIT/open_manipulator/issues 18 | catkin 19 | roscpp 20 | robotis_manipulator 21 | open_manipulator_libs 22 | open_manipulator_msgs 23 | 24 | -------------------------------------------------------------------------------- /open_manipulator_applications/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | open_manipulator_applications 4 | 1.0.0 5 | 6 | This package is applications of ROS-enabled OpenManipulator. 7 | 8 | 9 | Apache 2.0 10 | Darby Lim 11 | Hye-Jong KIM 12 | Ryan Shim 13 | Yong-Ho Na 14 | Pyo 15 | Hye-Jong KIM 16 | http://wiki.ros.org/open_manipulator 17 | http://emanual.robotis.com/docs/en/platform/openmanipulator 18 | https://github.com/ROBOTIS-GIT/open_manipulator 19 | https://github.com/ROBOTIS-GIT/open_manipulator/issues 20 | 21 | catkin 22 | open_manipulator_master_slave 23 | 24 | 25 | -------------------------------------------------------------------------------- /open_manipulator_master_slave/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(open_manipulator_master_slave) 6 | 7 | add_compile_options(-std=c++11) 8 | 9 | ################################################################################ 10 | # Find catkin packages and libraries for catkin and system dependencies 11 | ################################################################################ 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | robotis_manipulator 15 | open_manipulator_libs 16 | open_manipulator_msgs 17 | ) 18 | 19 | ################################################################################ 20 | # Setup for python modules and scripts 21 | ################################################################################ 22 | 23 | ################################################################################ 24 | # Declare ROS messages, services and actions 25 | ################################################################################ 26 | 27 | ################################################################################ 28 | ## Declare ROS dynamic reconfigure parameters 29 | ################################################################################ 30 | 31 | ################################################################################ 32 | # Declare catkin specific configuration to be passed to dependent projects 33 | ################################################################################ 34 | catkin_package( 35 | INCLUDE_DIRS include 36 | CATKIN_DEPENDS roscpp robotis_manipulator open_manipulator_libs open_manipulator_msgs 37 | 38 | ) 39 | 40 | ################################################################################ 41 | # Build 42 | ################################################################################ 43 | include_directories( 44 | include 45 | ${catkin_INCLUDE_DIRS} 46 | ) 47 | 48 | add_executable(open_manipulator_master src/open_manipulator_master.cpp) 49 | add_dependencies(open_manipulator_master ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 50 | target_link_libraries(open_manipulator_master ${catkin_LIBRARIES} ) 51 | 52 | ################################################################################ 53 | # Install 54 | ################################################################################ 55 | install(TARGETS open_manipulator_master 56 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 57 | ) 58 | 59 | install(DIRECTORY include/${PROJECT_NAME}/ 60 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 61 | ) 62 | 63 | install(DIRECTORY launch 64 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 65 | ) 66 | 67 | ################################################################################ 68 | # Test 69 | ################################################################################ 70 | -------------------------------------------------------------------------------- /open_manipulator_master_slave/include/open_manipulator_master_slave/open_manipulator_master.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ 18 | 19 | #ifndef OPEN_MANIPULATOR_MASTER_H 20 | #define OPEN_MANIPULATOR_MASTER_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "open_manipulator_libs/dynamixel.h" 27 | #include "open_manipulator_libs/kinematics.h" 28 | #include "open_manipulator_msgs/SetJointPosition.h" 29 | 30 | #define JOINT_DYNAMIXEL "joint_dxl" 31 | #define TOOL_DYNAMIXEL "tool_dxl" 32 | #define NUM_OF_JOINT 4 33 | 34 | #define X_AXIS robotis_manipulator::math::vector3(1.0, 0.0, 0.0) 35 | #define Y_AXIS robotis_manipulator::math::vector3(0.0, 1.0, 0.0) 36 | #define Z_AXIS robotis_manipulator::math::vector3(0.0, 0.0, 1.0) 37 | 38 | #define MASTER_SLAVE_MODE 0 39 | #define START_RECORDING_TRAJECTORY_MODE 1 40 | #define STOP_RECORDING_TRAJECTORY_MODE 2 41 | #define PLAY_RECORDED_TRAJECTORY_MODE 3 42 | 43 | using namespace robotis_manipulator; 44 | 45 | typedef struct _WaypointBuffer 46 | { 47 | std::vector joint_angle; 48 | double tool_position; 49 | } WaypointBuffer; 50 | 51 | 52 | class OpenManipulatorMaster : public RobotisManipulator 53 | { 54 | private: 55 | // ROS NodeHandle 56 | ros::NodeHandle node_handle_; 57 | ros::NodeHandle priv_node_handle_; 58 | ros::ServiceClient goal_joint_space_path_client_; 59 | ros::ServiceClient goal_tool_control_client_; 60 | 61 | uint8_t mode_state_; 62 | double service_call_period_; 63 | std::vector dxl_id_; 64 | std::vector goal_joint_position_; 65 | double goal_tool_position_; 66 | 67 | std::vector record_buffer_; 68 | int buffer_index_; 69 | 70 | JointActuator *actuator_; 71 | ToolActuator *tool_; 72 | 73 | public: 74 | OpenManipulatorMaster(std::string usb_port, std::string baud_rate); 75 | ~OpenManipulatorMaster(); 76 | 77 | void initOpenManipulator(STRING usb_port = "/dev/ttyUSB0", STRING baud_rate = "1000000", double service_call_period = 0.010); 78 | void initServiceClient(); 79 | void syncOpenManipulator(bool recorded_state); 80 | double getServiceCallPeriod() {return service_call_period_;} 81 | 82 | void publishCallback(const ros::TimerEvent&); 83 | bool setJointSpacePath(double path_time, std::vector set_goal_joint_position = {}); 84 | bool setToolPath(double set_goal_tool_position = -1.0); 85 | void setModeState(char ch); 86 | 87 | void printText(); 88 | bool kbhit(); 89 | 90 | }; 91 | 92 | #endif //OPEN_MANIPULATOR_MASTER_H 93 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # OpenManipulator 2 | 3 | 4 | 5 | ## ROS Packages for OpenManipulator 6 | |Version|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| 7 | |:---:|:---:|:---:| 8 | |[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2Fopen_manipulator_applications.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2Fopen_manipulator_applications)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/open_manipulator_applications.svg?branch=kinetic-devel)](https://travis-ci.org/ROBOTIS-GIT/open_manipulator_applications)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/open_manipulator_applications.svg?branch=melodic-devel)](https://travis-ci.org/ROBOTIS-GIT/open_manipulator_applications)| 9 | 10 | ## ROBOTIS e-Manual for OpenManipulator 11 | - [ROBOTIS e-Manual for OpenManipulator](http://emanual.robotis.com/docs/en/platform/openmanipulator/) 12 | 13 | ## Wiki for open_manipulator_applications Package 14 | - 15 | 16 | ## Open Source related to OpenManipulator 17 | - [robotis_manipulator](https://github.com/ROBOTIS-GIT/robotis_manipulator) 18 | - [open_manipulator](https://github.com/ROBOTIS-GIT/open_manipulator) 19 | - [open_manipulator_msgs](https://github.com/ROBOTIS-GIT/open_manipulator_msgs) 20 | - [open_manipulator_simulations](https://github.com/ROBOTIS-GIT/open_manipulator_simulations) 21 | - [open_manipulator_perceptions](https://github.com/ROBOTIS-GIT/open_manipulator_perceptions) 22 | - [open_manipulator_processing](https://github.com/ROBOTIS-GIT/open_manipulator_processing) 23 | - [open_manipulator_friends](https://github.com/ROBOTIS-GIT/open_manipulator_friends) 24 | - [open_manipulator_with_tb3](https://github.com/ROBOTIS-GIT/open_manipulator_with_tb3) 25 | - [open_manipulator_with_tb3_msgs](https://github.com/ROBOTIS-GIT/open_manipulator_with_tb3_msgs) 26 | - [open_manipulator_with_tb3_simulations](https://github.com/ROBOTIS-GIT/open_manipulator_with_tb3_simulations) 27 | - [turtlebot3](https://github.com/ROBOTIS-GIT/turtlebot3) 28 | - [turtlebot3_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_msgs) 29 | - [turtlebot3_simulations](https://github.com/ROBOTIS-GIT/turtlebot3_simulations) 30 | - [turtlebot3_applications](https://github.com/ROBOTIS-GIT/turtlebot3_applications) 31 | - [turtlebot3_applications_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_applications_msgs) 32 | - [turtlebot3_autorace](https://github.com/ROBOTIS-GIT/turtlebot3_autorace) 33 | - [turtlebot3_deliver](https://github.com/ROBOTIS-GIT/turtlebot3_deliver) 34 | - [hls_lfcd_lds_driver](https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver) 35 | - [manipulator_h](https://github.com/ROBOTIS-GIT/ROBOTIS-MANIPULATOR-H) 36 | - [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) 37 | - [dynamixel_workbench](https://github.com/ROBOTIS-GIT/dynamixel-workbench) 38 | - [OpenCR-Hardware](https://github.com/ROBOTIS-GIT/OpenCR-Hardware) 39 | - [OpenCR](https://github.com/ROBOTIS-GIT/OpenCR) 40 | 41 | ## Documents and Videos related to OpenManipulator 42 | - [ROBOTIS e-Manual for OpenManipulator](http://emanual.robotis.com/docs/en/platform/openmanipulator/) 43 | - [ROBOTIS e-Manual for TurtleBot3](http://turtlebot3.robotis.com/) 44 | - [ROBOTIS e-Manual for ROBOTIS MANIPULATOR-H](http://emanual.robotis.com/docs/en/platform/manipulator_h/introduction/) 45 | - [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) 46 | - [ROBOTIS e-Manual for Dynamixel Workbench](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/) 47 | - [e-Book for TurtleBot3 and OpenManipulator](https://community.robotsource.org/t/download-the-ros-robot-programming-book-for-free/51/) 48 | - [Videos for OpenManipulator](https://www.youtube.com/playlist?list=PLRG6WP3c31_WpEsB6_Rdt3KhiopXQlUkb) 49 | - [Videos for TurtleBot3 and OpenManipulator](https://www.youtube.com/playlist?list=PLRG6WP3c31_XI3wlvHlx2Mp8BYqgqDURU) 50 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /open_manipulator_master_slave/src/open_manipulator_master.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ 18 | 19 | #include "open_manipulator_master_slave/open_manipulator_master.h" 20 | 21 | OpenManipulatorMaster::OpenManipulatorMaster(std::string usb_port, std::string baud_rate) 22 | :node_handle_(""), 23 | priv_node_handle_("~"), 24 | service_call_period_(0.01), 25 | mode_state_(MASTER_SLAVE_MODE), 26 | buffer_index_(0) 27 | { 28 | service_call_period_ = priv_node_handle_.param("service_call_period", 0.010f); 29 | dxl_id_.push_back(priv_node_handle_.param("joint1_id", 1)); 30 | dxl_id_.push_back(priv_node_handle_.param("joint2_id", 2)); 31 | dxl_id_.push_back(priv_node_handle_.param("joint3_id", 3)); 32 | dxl_id_.push_back(priv_node_handle_.param("joint4_id", 4)); 33 | dxl_id_.push_back(priv_node_handle_.param("gripper_id", 5)); 34 | 35 | goal_joint_position_.resize(NUM_OF_JOINT); 36 | goal_tool_position_ = 0.0; 37 | 38 | initOpenManipulator(usb_port, baud_rate, service_call_period_); 39 | initServiceClient(); 40 | 41 | syncOpenManipulator(false); 42 | } 43 | 44 | OpenManipulatorMaster::~OpenManipulatorMaster() 45 | { 46 | delete actuator_; 47 | delete tool_; 48 | 49 | if(ros::isStarted()) { 50 | ros::shutdown(); 51 | ros::waitForShutdown(); 52 | } 53 | } 54 | 55 | void OpenManipulatorMaster::initServiceClient() 56 | { 57 | goal_joint_space_path_client_ = node_handle_.serviceClient("goal_joint_space_path"); 58 | goal_tool_control_client_ = node_handle_.serviceClient("goal_tool_control"); 59 | } 60 | 61 | void OpenManipulatorMaster::initOpenManipulator(STRING usb_port, STRING baud_rate, double service_call_period) 62 | { 63 | /***************************************************************************** 64 | ** Initialize Manipulator Parameter 65 | *****************************************************************************/ 66 | addWorld("world", // world name 67 | "joint1"); // child name 68 | 69 | addJoint("joint1", // my name 70 | "world", // parent name 71 | "joint2", // child name 72 | math::vector3(0.012, 0.0, 0.017), // relative position 73 | math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation 74 | Z_AXIS, // axis of rotation 75 | dxl_id_.at(0), // actuator id 76 | M_PI, // max joint limit (3.14 rad) 77 | -M_PI); // min joint limit (-3.14 rad) 78 | 79 | addJoint("joint2", // my name 80 | "joint1", // parent name 81 | "joint3", // child name 82 | math::vector3(0.0, 0.0, 0.0595), // relative position 83 | math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation 84 | Y_AXIS, // axis of rotation 85 | dxl_id_.at(1), // actuator id 86 | M_PI_2, // max joint limit (1.67 rad) 87 | -2.05); // min joint limit (-2.05 rad) 88 | 89 | addJoint("joint3", // my name 90 | "joint2", // parent name 91 | "joint4", // child name 92 | math::vector3(0.024, 0.0, 0.128), // relative position 93 | math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation 94 | Y_AXIS, // axis of rotation 95 | dxl_id_.at(2), // actuator id 96 | 1.53, // max joint limit (1.53 rad) 97 | -M_PI_2); // min joint limit (-1.67 rad) 98 | 99 | addJoint("joint4", // my name 100 | "joint3", // parent name 101 | "gripper", // child name 102 | math::vector3(0.124, 0.0, 0.0), // relative position 103 | math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation 104 | Y_AXIS, // axis of rotation 105 | dxl_id_.at(3), // actuator id 106 | 2.0, // max joint limit (2.0 rad) 107 | -1.8); // min joint limit (-1.8 rad) 108 | 109 | addTool("gripper", // my name 110 | "joint4", // parent name 111 | math::vector3(0.126, 0.0, 0.0), // relative position 112 | math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation 113 | dxl_id_.at(4), // actuator id 114 | 0.010, // max gripper limit (0.01 m) 115 | -0.010, // min gripper limit (-0.01 m) 116 | -0.015); // Change unit from `meter` to `radian` 117 | 118 | /***************************************************************************** 119 | ** Initialize Joint Actuator 120 | *****************************************************************************/ 121 | actuator_ = new dynamixel::JointDynamixelProfileControl(service_call_period); 122 | 123 | // Set communication arguments 124 | STRING dxl_comm_arg[2] = {usb_port, baud_rate}; 125 | void *p_dxl_comm_arg = &dxl_comm_arg; 126 | 127 | // Set joint actuator id 128 | std::vector jointDxlId; 129 | jointDxlId.push_back(dxl_id_.at(0)); 130 | jointDxlId.push_back(dxl_id_.at(1)); 131 | jointDxlId.push_back(dxl_id_.at(2)); 132 | jointDxlId.push_back(dxl_id_.at(3)); 133 | addJointActuator(JOINT_DYNAMIXEL, actuator_, jointDxlId, p_dxl_comm_arg); 134 | 135 | // Set joint actuator control mode 136 | STRING joint_dxl_mode_arg = "position_mode"; 137 | void *p_joint_dxl_mode_arg = &joint_dxl_mode_arg; 138 | setJointActuatorMode(JOINT_DYNAMIXEL, jointDxlId, p_joint_dxl_mode_arg); 139 | 140 | /***************************************************************************** 141 | ** Initialize Tool Actuator 142 | *****************************************************************************/ 143 | tool_ = new dynamixel::GripperDynamixel(); 144 | 145 | uint8_t gripperDxlId = dxl_id_.at(4); 146 | addToolActuator(TOOL_DYNAMIXEL, tool_, gripperDxlId, p_dxl_comm_arg); 147 | 148 | // Set gripper actuator control mode 149 | STRING gripper_dxl_mode_arg = "current_based_position_mode"; 150 | void *p_gripper_dxl_mode_arg = &gripper_dxl_mode_arg; 151 | setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_mode_arg); 152 | 153 | // Disable All Actuators 154 | disableAllActuator(); 155 | } 156 | 157 | void OpenManipulatorMaster::syncOpenManipulator(bool recorded_state) 158 | { 159 | log::println("Synchronizing Open Manipulators.", "GREEN"); 160 | double sync_path_time = 1.0; 161 | 162 | receiveAllJointActuatorValue(); 163 | receiveAllToolActuatorValue(); 164 | 165 | if(recorded_state) // move to first pose of recorded buffer 166 | { 167 | setJointSpacePath(sync_path_time, record_buffer_.at(buffer_index_).joint_angle); 168 | setToolPath(record_buffer_.at(buffer_index_).tool_position); 169 | } 170 | else // move to present master pose 171 | { 172 | setJointSpacePath(sync_path_time); 173 | setToolPath(); 174 | } 175 | 176 | ros::WallDuration sleep_time(sync_path_time); 177 | sleep_time.sleep(); 178 | 179 | return; 180 | } 181 | 182 | bool OpenManipulatorMaster::setJointSpacePath(double path_time, std::vector set_goal_joint_position) 183 | { 184 | auto joint_name = getManipulator()->getAllActiveJointComponentName(); 185 | std::vector joint_value; 186 | if(set_goal_joint_position.size()) joint_value = set_goal_joint_position; 187 | else 188 | { 189 | joint_value.push_back(getJointValue(joint_name.at(0)).position); 190 | joint_value.push_back(getJointValue(joint_name.at(1)).position); 191 | joint_value.push_back(getJointValue(joint_name.at(2)).position); 192 | joint_value.push_back(getJointValue(joint_name.at(3)).position); 193 | } 194 | 195 | open_manipulator_msgs::SetJointPosition srv; 196 | srv.request.joint_position.joint_name = joint_name; 197 | 198 | for(int i = 0; i < NUM_OF_JOINT; i ++) 199 | { 200 | if(getManipulator()->checkJointLimit(joint_name.at(i), joint_value.at(i))) 201 | srv.request.joint_position.position.push_back(joint_value.at(i)); 202 | else 203 | srv.request.joint_position.position.push_back(goal_joint_position_.at(i)); 204 | } 205 | 206 | goal_joint_position_ = srv.request.joint_position.position; 207 | srv.request.path_time = path_time; 208 | 209 | if(goal_joint_space_path_client_.call(srv)) 210 | { 211 | return srv.response.is_planned; 212 | } 213 | return false; 214 | } 215 | 216 | bool OpenManipulatorMaster::setToolPath(double set_goal_tool_position) 217 | { 218 | double tool_value; 219 | if(set_goal_tool_position < -0.1) 220 | tool_value = getAllToolValue().at(0).position; 221 | else 222 | tool_value = set_goal_tool_position; 223 | 224 | open_manipulator_msgs::SetJointPosition srv; 225 | srv.request.joint_position.joint_name.push_back("gripper"); 226 | 227 | if(getManipulator()->checkJointLimit("gripper", tool_value)) 228 | srv.request.joint_position.position.push_back(tool_value); 229 | else 230 | srv.request.joint_position.position.push_back(goal_tool_position_); 231 | 232 | goal_tool_position_ = srv.request.joint_position.position.at(0); 233 | 234 | if(goal_tool_control_client_.call(srv)) 235 | { 236 | return srv.response.is_planned; 237 | } 238 | 239 | return false; 240 | } 241 | 242 | 243 | void OpenManipulatorMaster::publishCallback(const ros::TimerEvent&) 244 | { 245 | static int count = 0; 246 | if(!(count % 5)) 247 | { 248 | printText(); 249 | if(kbhit()) 250 | setModeState(std::getchar()); 251 | } 252 | count ++; 253 | 254 | receiveAllJointActuatorValue(); 255 | receiveAllToolActuatorValue(); 256 | 257 | if(mode_state_ == MASTER_SLAVE_MODE) 258 | { 259 | setJointSpacePath(service_call_period_); 260 | setToolPath(); 261 | } 262 | else if(mode_state_ == START_RECORDING_TRAJECTORY_MODE) 263 | { 264 | setJointSpacePath(service_call_period_); 265 | setToolPath(); 266 | 267 | WaypointBuffer temp; 268 | temp.joint_angle = goal_joint_position_; 269 | temp.tool_position = goal_tool_position_; 270 | record_buffer_.push_back(temp); 271 | } 272 | else if(mode_state_ == STOP_RECORDING_TRAJECTORY_MODE) 273 | {} 274 | else if(mode_state_ == PLAY_RECORDED_TRAJECTORY_MODE) 275 | { 276 | if(record_buffer_.size() > buffer_index_) 277 | { 278 | setJointSpacePath(service_call_period_, record_buffer_.at(buffer_index_).joint_angle); 279 | setToolPath(record_buffer_.at(buffer_index_).tool_position); 280 | buffer_index_ ++; 281 | } 282 | } 283 | } 284 | void OpenManipulatorMaster::setModeState(char ch) 285 | { 286 | if(ch == '1') 287 | { 288 | syncOpenManipulator(false); 289 | mode_state_ = MASTER_SLAVE_MODE; 290 | } 291 | else if(ch == '2') 292 | { 293 | syncOpenManipulator(false); 294 | record_buffer_.clear(); 295 | mode_state_ = START_RECORDING_TRAJECTORY_MODE; 296 | } 297 | else if(ch == '3') 298 | { 299 | mode_state_ = STOP_RECORDING_TRAJECTORY_MODE; 300 | } 301 | else if(ch == '4') 302 | { 303 | buffer_index_ = 0; 304 | if(record_buffer_.size()) syncOpenManipulator(true); 305 | mode_state_ = PLAY_RECORDED_TRAJECTORY_MODE; 306 | } 307 | } 308 | 309 | void OpenManipulatorMaster::printText() 310 | { 311 | system("clear"); 312 | 313 | printf("\n"); 314 | printf("-----------------------------\n"); 315 | printf("Control Your OpenManipulator!\n"); 316 | 317 | printf("-----------------------------\n"); 318 | printf("Present Control Mode\n"); 319 | if(mode_state_ == MASTER_SLAVE_MODE) 320 | { 321 | printf("Master - Slave Mode\n"); 322 | } 323 | else if(mode_state_ == START_RECORDING_TRAJECTORY_MODE) 324 | { 325 | printf("Start Recording Trajectory\n"); 326 | printf("Buffer Size : %d\n", (int)(record_buffer_.size())); 327 | } 328 | else if(mode_state_ == STOP_RECORDING_TRAJECTORY_MODE) 329 | { 330 | printf("Stop Recording Trajectory\n"); 331 | printf("Buffer Size : %d\n", (int)(record_buffer_.size())); 332 | } 333 | else if(mode_state_ == PLAY_RECORDED_TRAJECTORY_MODE) 334 | { 335 | printf("Play Recorded Trajectory Mode\n"); 336 | printf("Buffer Size : %d\n", (int)(record_buffer_.size())); 337 | printf("Buffer index : %d\n", buffer_index_); 338 | } 339 | 340 | printf("-----------------------------\n"); 341 | printf("1 : Master - Slave Mode\n"); 342 | printf("2 : Start Recording Trajectory\n"); 343 | printf("3 : Stop Recording Trajectory\n"); 344 | printf("4 : Play Recorded Trajectory\n"); 345 | 346 | printf("-----------------------------\n"); 347 | printf("Present Joint Angle J1: %.3lf J2: %.3lf J3: %.3lf J4: %.3lf\n", 348 | goal_joint_position_.at(0), 349 | goal_joint_position_.at(1), 350 | goal_joint_position_.at(2), 351 | goal_joint_position_.at(3)); 352 | printf("Present Tool Position: %.3lf\n", goal_tool_position_); 353 | printf("-----------------------------\n"); 354 | } 355 | 356 | bool OpenManipulatorMaster::kbhit() 357 | { 358 | termios term; 359 | tcgetattr(0, &term); 360 | 361 | termios term2 = term; 362 | term2.c_lflag &= ~ICANON; 363 | tcsetattr(0, TCSANOW, &term2); 364 | 365 | int byteswaiting; 366 | ioctl(0, FIONREAD, &byteswaiting); 367 | 368 | tcsetattr(0, TCSANOW, &term); 369 | 370 | return byteswaiting > 0; 371 | } 372 | 373 | int main(int argc, char **argv) 374 | { 375 | // Init ROS node 376 | ros::init(argc, argv, "open_manipulator_master_slave"); 377 | ros::NodeHandle node_handle(""); 378 | 379 | std::string usb_port = "/dev/ttyUSB0"; 380 | std::string baud_rate = "1000000"; 381 | 382 | if (argc < 3) 383 | { 384 | log::error("Please set '-port_name' and '-baud_rate' arguments for connected Dynamixels"); 385 | return 0; 386 | } 387 | else 388 | { 389 | usb_port = argv[1]; 390 | baud_rate = argv[2]; 391 | } 392 | 393 | OpenManipulatorMaster open_manipulator_master(usb_port, baud_rate); 394 | 395 | ros::Timer publish_timer = node_handle.createTimer(ros::Duration(open_manipulator_master.getServiceCallPeriod()), &OpenManipulatorMaster::publishCallback, &open_manipulator_master); 396 | 397 | while (ros::ok()) 398 | { 399 | ros::spinOnce(); 400 | } 401 | return 0; 402 | } 403 | --------------------------------------------------------------------------------