├── CMakeLists.txt ├── Clearning.py ├── LICENSE ├── README.md ├── control.launch ├── display.launch ├── msg ├── Angles.msg ├── Coords.msg ├── CoordsWithTS4.msg └── CoordsWithTime.msg ├── package.xml ├── python └── uarm_python.py ├── scripts ├── move_to_node.py ├── pump_node.py ├── report_angles_node.py ├── report_coords_node.py ├── report_stopper_node.py ├── uarm_core.py ├── uarm_status_node.py └── write_angles_node.py ├── src └── uarm_status.cpp ├── urdf ├── daes │ ├── add_1.dae │ ├── add_2.dae │ ├── add_3.dae │ ├── add_4.dae │ ├── add_5.dae │ ├── base.STL │ ├── base_rot.dae │ ├── link_1.dae │ ├── link_2.dae │ ├── link_3.dae │ ├── link_3_2.dae │ ├── link_end.dae │ └── link_end_2.dae └── model.urdf └── visualization ├── visual_control.py └── visual_display.py /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(uarm) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | message_generation 12 | visualization_msgs 13 | interactive_markers 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | add_message_files( 51 | FILES 52 | Angles.msg 53 | Coords.msg 54 | CoordsWithTime.msg 55 | CoordsWithTS4.msg 56 | # Message1.msg 57 | # Message2.msg 58 | ) 59 | 60 | ## Generate services in the 'srv' folder 61 | # add_service_files( 62 | # FILES 63 | # Service1.srv 64 | # Service2.srv 65 | # ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | generate_messages( 76 | DEPENDENCIES 77 | std_msgs 78 | ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if you package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | # INCLUDE_DIRS include 111 | # LIBRARIES uarm 112 | CATKIN_DEPENDS roscpp rospy std_msgs message_runtime 113 | 114 | # DEPENDS system_lib 115 | ) 116 | 117 | ########### 118 | ## Build ## 119 | ########### 120 | 121 | ## Specify additional locations of header files 122 | ## Your package locations should be listed before other locations 123 | # include_directories(include) 124 | include_directories( 125 | ${catkin_INCLUDE_DIRS} 126 | ) 127 | 128 | ## Declare a C++ library 129 | # add_library(uarm 130 | # src/${PROJECT_NAME}/uarm.cpp 131 | # ) 132 | 133 | ## Add cmake target dependencies of the library 134 | ## as an example, code may need to be generated before libraries 135 | ## either from message generation or dynamic reconfigure 136 | # add_dependencies(uarm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 137 | 138 | ## Declare a C++ executable 139 | # add_executable(uarm_node src/uarm_node.cpp) 140 | 141 | ## Add cmake target dependencies of the executable 142 | ## same as for the library above 143 | # add_dependencies(uarm_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 144 | 145 | ## Specify libraries to link a library or executable target against 146 | # target_link_libraries(uarm_node 147 | # ${catkin_LIBRARIES} 148 | # ) 149 | 150 | ############# 151 | ## Install ## 152 | ############# 153 | 154 | # all install targets should use catkin DESTINATION variables 155 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 156 | 157 | ## Mark executable scripts (Python etc.) for installation 158 | ## in contrast to setup.py, you can choose the destination 159 | # install(PROGRAMS 160 | # scripts/my_python_script 161 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 162 | # ) 163 | 164 | ## Mark executables and/or libraries for installation 165 | # install(TARGETS uarm uarm_node 166 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 167 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 168 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark cpp header files for installation 172 | # install(DIRECTORY include/${PROJECT_NAME}/ 173 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 174 | # FILES_MATCHING PATTERN "*.h" 175 | # PATTERN ".svn" EXCLUDE 176 | # ) 177 | 178 | ## Mark other files for installation (e.g. launch and bag files, etc.) 179 | # install(FILES 180 | # # myfile1 181 | # # myfile2 182 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 183 | # ) 184 | 185 | ############# 186 | ## Testing ## 187 | ############# 188 | 189 | ## Add gtest based cpp test target and link libraries 190 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_uarm.cpp) 191 | # if(TARGET ${PROJECT_NAME}-test) 192 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 193 | # endif() 194 | 195 | ## Add folders to be run by python nosetests 196 | # catkin_add_nosetests(test) 197 | 198 | add_executable(uarm_status src/uarm_status.cpp) 199 | target_link_libraries(uarm_status ${catkin_LIBRARIES}) 200 | 201 | 202 | 203 | 204 | -------------------------------------------------------------------------------- /Clearning.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | 4 | counts = 0 5 | check_mode = 0 6 | 7 | def check_docs(dirs,suffix_input): 8 | global counts 9 | for doc in os.listdir(dirs): 10 | doc_dir = dirs + '/' + doc 11 | 12 | if os.path.isfile(doc_dir): 13 | if doc[-1] == suffix_input: 14 | if check_mode == 0: 15 | os.remove(doc_dir) 16 | counts += 1 17 | print 'delete this file: ' + doc_dir + '/'+ doc 18 | elif check_mode == 1: 19 | counts += 1 20 | print 'detect the file: ' + doc_dir + '/'+ doc 21 | else: 22 | pass 23 | pass 24 | if os.path.isdir(doc_dir): 25 | check_docs(doc_dir,suffix_input) 26 | pass 27 | 28 | 29 | def deleteFileSuffix(suffix_input): 30 | 31 | current_dir = os.getcwd() 32 | check_docs(current_dir,suffix_input) 33 | pass 34 | 35 | if __name__ == '__main__': 36 | 37 | delete_str = '~' 38 | 39 | print 'Process suffix: ' + delete_str 40 | 41 | if len(sys.argv) == 1: 42 | print 'Input clearn or check' 43 | 44 | elif sys.argv[1].lower() == 'clean' or sys.argv[1].lower() == 'delete': 45 | check_mode = 0 46 | deleteFileSuffix(delete_str) 47 | print 'Cleaning done' 48 | print 'Cleaned ' + str(counts) + ' docs' 49 | 50 | elif sys.argv[1].lower() == 'check' or sys.argv[1].lower() == 'detect': 51 | check_mode = 1 52 | deleteFileSuffix(delete_str) 53 | print 'Check done' 54 | print 'There are ' + str(counts) + ' docs' 55 | 56 | else: 57 | pass 58 | 59 | #if len(sys.argv[1]) == 1 and sys.argv[1] != 'y' 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, UFACTORY 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 18 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 20 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 21 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 22 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 23 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # UArmForROS 2 | This is the uarm ROS package designed by Joey Song ( joey@ufactory.cc / astainsong@gmail.com) 3 | 4 | ## 0. Quickstart and Execution 5 | --- 6 | Three ways to control uArm movements 7 | ![](http://obmqyor62.bkt.clouddn.com/ROS_QuickStart.jpg) 8 | 9 | ## 1. Installation 10 | --- 11 | ### 1.1 Pre-Requirements 12 | Connect uArm and get USB permission to access uArm 13 | ```bash 14 | $ cd /etc/udev/rules.d 15 | ``` 16 | Creat a file `ttyUSB.rules` and put the following line: `KERNEL=="ttyUSB*", MODE="0666"`. Save the file and **reconnect** uArm USB to make it effective. (if you already have the permission to access USB, you can skip this step) 17 | > If your uArm is connected to ttyACM instead of ttyUSB (you can check using dmesg -c command after connecting your uArm), 18 | Creat a file `ttyACM.rules` and put the following line: `KERNEL=="ttyACM*", MODE="0666"`. Save the file and **reconnect** uArm USB to make it effective. (if you already have the permission to access USB, you can skip this step) 19 | 20 | 21 | For using this package, the [pyUarm](https://github.com/uArm-Developer/pyuarm) library **SHOULD** be installed at first. 22 | 23 | ```bash 24 | $ pip install pyuarm 25 | ``` 26 | 27 | And check your uArms's VID:PID using lsusb after connecting uArm. 28 | ```bash 29 | $ lsusb 30 | Bus 001 Device 020: ID 1234:1234 Arduino SA Mega 2560 R3 (CDC ACM) 31 | ``` 32 | 33 | And you need to specify your device number by editing this file 34 | e.g. if you prefer nano as an editor) 35 | ```bash 36 | $ sudo nano /usr/local/lib/python2.7/dist-packages/pyuarm/tools/list_uarms.py 37 | ``` 38 | In this file, you need to change this line, VID:PID with your own device. 39 | 40 | ```python 41 | UARM_HWID_KEYWORD = "USB VID:PID=1234:1234" 42 | ``` 43 | 44 | 45 | Connect uArm to computer and upgrade your uArmProtocol Firmware 46 | ```bash 47 | $ uarm-firmware -u 48 | ``` 49 | or 50 | ```bash 51 | $ python -m pyuarm.tools.firmware -d 52 | ``` 53 | 54 | ### 1.2 Package Download and Install 55 | Install ros package in your src folder of your [Catkin workspace](http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment). 56 | ```bash 57 | $ cd ~/catkin_ws/src 58 | $ git clone https://github.com/uArm-Developer/UArmForROS.git 59 | $ cd ~/catkin_ws 60 | $ catkin_make 61 | ``` 62 | ## 2. Source FIles 63 | --- 64 | Before you use any packages in uarmForROS, source all setup.bash files which allow you to access uarm package 65 | ```bash 66 | 67 | # System configure ROS environment variables automatically every time you open a shall 68 | echo "source /opt/ros/[ROS_version]/setup.bash" >> ~/.bashrc 69 | # For example, if you are using kinetic version of ROS 70 | echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc 71 | 72 | # Source setup.bash 73 | echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc 74 | source ~/.bashrc 75 | ``` 76 | 77 | ## 3. Package Modules 78 | --- 79 | ### 3.1 Nodes 80 | - `uarm_core.py` is the main node. Run this node before anything else. This node has two main modes: **Control-Mode** and **Monitor-Mode**. **Control-Mode** is used to control uarm directly in this node. **Monitor-mode** is to subscrib/listen to all topics which can be used to control uarm through these nodes. This node will automatically load **Control-Mode** first. 81 | 82 | **Step 1**: Connect uArm 83 | 84 | Set up ROS enviroment at first 85 | ```bash 86 | roscore 87 | ``` 88 | Open another shall to connect uArm before use. 89 | ```bash 90 | rosrun uarm uarm_core.py connect // this will find uarm automatically 91 | ``` 92 | 93 | **Step 2**: Control-Mode 94 | 95 | Once connect uArm, you can use commands to control. Input `h` to see all the commands 96 | ```bash 97 | # For example: attach uarm (use at in short for attach) 98 | Input Commands (Input h to see all commands): attach # or at 99 | # For example: read current x,y,z (use cc in short for currentCoords) 100 | Input Commands (Input h to see all commands): currentCoords # or cc 101 | # For example: move uarm x,y,z (use mt in short for moveTo) 102 | Input Commands (Input h to see all commands): moveTo 12 -12 12 103 | ``` 104 | Input `l` to exit control-mode and get into Monitor mode 105 | 106 | **Step 3**: Monitor-Mode 107 | 108 | If you get the information as below, you can use both Topics-Pub and other ROS Nodes to control uarm through ROS. 109 | ```bash 110 | Begin monitor mode - listening to all fucntional topics 111 | ======================================================= 112 | Use rqt_graph to check the connection 113 | ======================================================= 114 | ``` 115 | 116 | - `uarm_status_node.py` is the node which can control the attach-status or detach-status of uArm. 117 | 118 | Open another shall, and use this node in the **monitor-mode** of `uarm_core.py` node 119 | ```bash 120 | # attach uarm 121 | rosrun uarm uarm_status_node.py attach 122 | # detach uarm 123 | rosrun uarm uarm_status_node.py detach 124 | ``` 125 | 126 | - `pump_node.py` is the node which can control the pump on or off. 127 | 128 | Use this node in the **monitor-mode** of `uarm_core.py` node 129 | ```bash 130 | # pump on 131 | rosrun uarm pump_node.py on 132 | # pump off 133 | rosrun uarm pump_node.py off 134 | ``` 135 | 136 | - `report_angles_node.py` is the node which will report current angles. 137 | 138 | Use this node in the **monitor-mode** of `uarm_core.py` node 139 | ```bash 140 | # report once 141 | rosrun uarm report_angles_node.py 142 | # report 10 times 143 | rosrun uarm report_angles_node.py 10 144 | # report 10 times per 2 time_sec 145 | rosrun uarm report_angles_node.py 10 2 146 | ``` 147 | 148 | - `report_coords_node.py` is the node which will report current coords. 149 | 150 | Use this node in the **monitor-mode** of `uarm_core.py` node 151 | ```bash 152 | # report once 153 | rosrun uarm report_coords_node.py 154 | # report 10 times 155 | rosrun uarm report_coords_node.py 10 156 | # report 10 times per 2 time_sec 157 | rosrun uarm report_coords_node.py 10 2 158 | ``` 159 | 160 | - `report_stopper_node.py` is the node which will report stoppper status. 161 | 162 | Use this node in the **monitor-mode** of `uarm_core.py` node 163 | ```bash 164 | # report once 165 | rosrun uarm report_stopper_node.py 166 | # report 10 times 167 | rosrun uarm report_stopper_node.py 10 168 | # report 10 times per 2 time_sec 169 | rosrun uarm report_stopper_node.py 10 2 170 | ``` 171 | 172 | - `write_angles_node.py` is the node which will control 4 servo angles. 173 | 174 | Use this node in the **monitor-mode** of `uarm_core.py` node 175 | ```bash 176 | # write 4 servo angles 177 | rosrun uarm write_angles_node.py 90 50 50 10 178 | ``` 179 | 180 | - `move_to_node.py` is the node which will move to x,y,z position. 181 | 182 | Use this node in the **monitor-mode** of `uarm_core.py` node 183 | ```bash 184 | # move to x,y,z 185 | rosrun uarm move_to_node.py 12 -12 12 186 | # move to x,y,z,time (point 12 -12 12 in 2 seconds) 187 | rosrun uarm move_to_node.py 12 -12 12 2 188 | # move to x,y,z,time,servo_4 angle (servo_4 angle is 54) 189 | rosrun uarm move_to_node.py 12 -12 12 2 54 190 | ``` 191 | ### 3.2 Topics 192 | 193 | - `uarm_status` - control uarm status. 194 | ``` 195 | Message_type: `std_msgs/String`. 196 | Data: attach / detach 197 | ``` 198 | - `pump_control` - control pump on or off. 199 | ``` 200 | Message_type: `std_msgs/UInt8`. 201 | Data: 1 / 0 202 | ``` 203 | - `pump_str_control` - control pump on or off. 204 | ``` 205 | Message_type: `std_msgs/String`. 206 | Data: high / low 207 | ``` 208 | - `read_coords` - report coords. Coords will also be written in parameters 209 | ``` 210 | Message_type: `std_msgs/Int32`. 211 | Data: int (read times). 212 | Set-param: 'current_x' 'current_y' 'current_z' ('write in ros-parameter and can be invoked') 213 | ``` 214 | - `read_angles` - report angles. Angles will also be written in parameters 215 | ``` 216 | Message_type: `std_msgs/Int32`. 217 | Data: int (read times). 218 | Set-param: 's1' 's2' 's3' 's4' ('will be written in ros_parameter for 4 `servo_angles') 219 | ``` 220 | - `stopper_status` - report stopper status. Status will also be written in parameters 221 | ``` 222 | Message_type: `std_msgs/Int32`. 223 | Data: int (read times). 224 | Set-param: 'stopper_status' 225 | ``` 226 | - `write_angles` - write 4 servo angles. 227 | ``` 228 | Message_type: uarm/Angles 229 | Data: int int int int 230 | ``` 231 | - `move_to` - move to x,y,z position. 232 | ``` 233 | Message_type: uarm/Coords 234 | Data: float float float 235 | ``` 236 | - `move_to_time` - move to with time. 237 | ``` 238 | Message_type: uarm/CoordsWithTime 239 | Data: float float float int 240 | ``` 241 | - `move_to_time_s4` - move to with time and servo_4 angle. 242 | ``` 243 | Message_type: uarm/CoordsWithTS4 244 | Data: float float float int int 245 | ``` 246 | 247 | 248 | ### 3.3 Messages 249 | - `uarm/Angles` 250 | ``` 251 | uint8: servo_1 252 | uint8: servo_2 253 | uint8: servo_3 254 | uint8: servo_4 255 | ``` 256 | - `uarm/Angles` 257 | ``` 258 | float32: x 259 | float32: y 260 | float32: z 261 | ``` 262 | - `uarm/Angles` 263 | ``` 264 | float32: x 265 | float32: y 266 | float32: z 267 | uint8: time 268 | ``` 269 | - `uarm/Angles` 270 | ``` 271 | float32: x 272 | float32: y 273 | float32: z 274 | uint8: time 275 | uint8: servo_4 276 | ``` 277 | 278 | 279 | ## 4. Visualization in RViz 280 | --- 281 | ### 4.1 Functions 282 | 283 | - Display -- display.launch: This function will display robot movement in realtime when you manually move uArm 284 | - Control -- control.launch: This function will allow you control the end-effector movement in 3 DOF along x,y,z axis. 285 | 286 | ### 4.2 Launch and Run 287 | -**Step 1**: Set up ROS enviroment in **one** shall 288 | ``` 289 | roscore 290 | ``` 291 | In the **second** shall, connect uArm and set the listen mode as shown above 292 | ``` 293 | rosrun uarm uarm_core.py connect // connect uArm 294 | l // transfer to monitor mode 295 | ``` 296 | -**Step 2**: Launch 297 | 298 | a) For visualization function, in the **third** shall, run 299 | 300 | roslaunch uarm display.launch 301 | 302 | b) Or for control function, in the **third** shall, run 303 | 304 | roslaunch uarm control.launch 305 | 306 | -**Step 3**: Display and control: 307 | Open rviz to view robot in the **fourth** shall 308 | ``` 309 | rosrun rviz rviz 310 | ``` 311 | For both functions, import robot model in "Displays" panel on the left: 312 | 313 | ``` 314 | Add -> RobotModel // click "add" and choose "RobotModel" 315 | set Cell Size -> 0.1 // change "Cell Size" to 0.1 in "Grid" 316 | set Fixed Frame -> base // change "Fixed Frame" to base in "Global Options" 317 | ``` 318 | a) For Display function, right now a robot will display in the main window 319 | 320 | b) For Control function, stop Display function and set 321 | 322 | ``` 323 | Add -> InteractiveMarker // click "add" and choose "InteractiveMarkers" 324 | Update Topic -> /uarm_controller/update // change "Update topic" in "InteractiveMarkers" 325 | ``` 326 | Drag 3 pairs of arrows to control uArm along x, y, z axis. 327 | -------------------------------------------------------------------------------- /control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /display.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /msg/Angles.msg: -------------------------------------------------------------------------------- 1 | uint8 servo_1 2 | uint8 servo_2 3 | uint8 servo_3 4 | uint8 servo_4 5 | -------------------------------------------------------------------------------- /msg/Coords.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 z 4 | 5 | -------------------------------------------------------------------------------- /msg/CoordsWithTS4.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 z 4 | uint8 time 5 | uint8 servo_4 6 | -------------------------------------------------------------------------------- /msg/CoordsWithTime.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 z 4 | uint8 time 5 | 6 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uarm 4 | 0.0.0 5 | The uarm package 6 | 7 | 8 | 9 | 10 | joey 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | message_generation 36 | 37 | 38 | 39 | message_runtime 40 | 41 | 42 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /python/uarm_python.py: -------------------------------------------------------------------------------- 1 | 2 | from pyfirmata import Arduino, util 3 | import math 4 | import time 5 | 6 | 7 | # PUMP INFORMATION 8 | PUMP_EN = 6 9 | VALVE_EN =5 10 | 11 | # UARM SPECIFICATIONS 12 | MATH_PI = 3.141592653589793238463 13 | MATH_TRANS = 57.2958 14 | MATH_L1 = (10.645+0.6) 15 | MATH_L2 = 2.117 16 | MATH_L3 = 14.825 17 | MATH_L4 = 16.02 18 | MATH_L43 = MATH_L4/MATH_L3 19 | 20 | # UARM OFFSETS 21 | TopOffset = -1.5 22 | BottomOffset = 1.5 23 | 24 | 25 | class Uarm(object): 26 | 27 | 28 | uarm = None 29 | kAddrOffset = 90 30 | kAddrAandB = 60 31 | 32 | uarm_status = 0 33 | pin2_status = 0 34 | coord = {} 35 | g_interpol_val_arr = {} 36 | angle = {} 37 | 38 | attachStaus = 0 39 | 40 | def __init__(self,port): 41 | 42 | self.uarm = Arduino(port) 43 | self.uarmDetach() 44 | 45 | 46 | def servoAttach(self,servo_number): 47 | if servo_number == 1: 48 | self.servo_base = self.uarm.get_pin('d:11:s') 49 | elif servo_number == 2: 50 | self.servo_left = self.uarm.get_pin('d:13:s') 51 | elif servo_number == 3: 52 | self.servo_right = self.uarm.get_pin('d:12:s') 53 | elif servo_number == 4: 54 | self.servo_end = self.uarm.get_pin('d:10:s') 55 | else: 56 | return 57 | 58 | 59 | def servoDetach(self,servo_number): 60 | if servo_number == 1: 61 | self.uarm.servoDetach(11) 62 | elif servo_number == 2: 63 | self.uarm.servoDetach(13) 64 | elif servo_number == 3: 65 | self.uarm.servoDetach(12) 66 | elif servo_number == 4: 67 | self.uarm.servoDetach(10) 68 | else: 69 | return 70 | 71 | 72 | def uarmDisconnect(self): 73 | self.uarm.exit() 74 | 75 | 76 | def uarmAttach(self): 77 | 78 | curAngles = {} 79 | 80 | if self.uarm_status == 0: 81 | for n in range(1,5): 82 | curAngles[n] = self.readAngle(n) 83 | time.sleep(0.1) 84 | n = 1 85 | while n<5: 86 | self.servoAttach(n) 87 | n += 1 88 | time.sleep(0.1) 89 | self.writeAngle(curAngles[1],curAngles[2],curAngles[3],curAngles[4]) 90 | self.uarm_status = 1 91 | 92 | 93 | def uarmDetach(self): 94 | n = 1 95 | while n<5: 96 | self.servoDetach(n) 97 | n += 1 98 | self.uarm_status = 0 99 | 100 | 101 | def angleConstrain(self,Angle): 102 | if Angle <0: 103 | return 0 104 | elif Angle >180: 105 | return 180 106 | else: 107 | return Angle 108 | 109 | 110 | def writeServoAngleRaw(self,servo_number,Angle): 111 | 112 | if servo_number == 1: 113 | self.servo_base.write(self.angleConstrain(round(Angle))) 114 | elif servo_number == 2: 115 | self.servo_left.write(self.angleConstrain(round(Angle))) 116 | elif servo_number == 3: 117 | 118 | self.servo_right.write(self.angleConstrain(round(Angle))) 119 | elif servo_number == 4: 120 | 121 | self.servo_end.write(self.angleConstrain(round(Angle))) 122 | else: 123 | return 124 | 125 | 126 | def writeServoAngle(self,servo_number,Angle): 127 | 128 | if servo_number == 1: 129 | self.servo_base.write(self.angleConstrain(Angle+ self.readServoOffset(servo_number))) 130 | elif servo_number == 2: 131 | self.servo_left.write(self.angleConstrain(Angle+ self.readServoOffset(servo_number))) 132 | elif servo_number == 3: 133 | self.servo_right.write(self.angleConstrain(Angle+ self.readServoOffset(servo_number))) 134 | elif servo_number == 4: 135 | self.servo_end.write(self.angleConstrain(Angle+ self.readServoOffset(servo_number))) 136 | else: 137 | return 138 | 139 | 140 | def writeAngle(self,servo_1,servo_2,servo_3,servo_4): 141 | 142 | servoAngles = {} 143 | servoAngles[1] = servo_1 + self.readServoOffset(1) 144 | servoAngles[2] = servo_2 + self.readServoOffset(2) 145 | servoAngles[3] = servo_3 + self.readServoOffset(3) 146 | servoAngles[4] = servo_4 + self.readServoOffset(4) 147 | 148 | self.servo_base.write(self.angleConstrain(servoAngles[1])) 149 | self.servo_left.write(self.angleConstrain(servoAngles[2])) 150 | self.servo_right.write(self.angleConstrain(servoAngles[3])) 151 | self.servo_end.write(self.angleConstrain(servoAngles[4])) 152 | 153 | 154 | def writeAngleRaw(self,servo_1,servo_2,servo_3,servo_4): 155 | 156 | self.servo_base.write(self.angleConstrain(servo_1)) 157 | self.servo_left.write(self.angleConstrain(servo_2)) 158 | self.servo_right.write(self.angleConstrain(servo_3)) 159 | self.servo_end.write(self.angleConstrain(servo_4)) 160 | 161 | 162 | def readAnalog(self,servo_number): 163 | 164 | if servo_number == 1: 165 | for i in range(1,4): 166 | data = self.uarm.readAnalogPin(2) 167 | return data 168 | elif servo_number == 2: 169 | for i in range(1,4): 170 | data = self.uarm.readAnalogPin(0) 171 | return data 172 | elif servo_number == 3: 173 | for i in range(1,4): 174 | data = self.uarm.readAnalogPin(1) 175 | return data 176 | elif servo_number == 4: 177 | for i in range(1,4): 178 | data = self.uarm.readAnalogPin(3) 179 | return data 180 | else: 181 | return 182 | 183 | 184 | def readServoOffset(self,servo_number): 185 | if servo_number ==1 or servo_number ==2 or servo_number ==3: 186 | addr = self.kAddrOffset + (servo_number - 1)*2 187 | servo_offset = (self.uarm.readEEPROM(addr+1))/10.00 188 | if(self.uarm.readEEPROM(addr) == 0): 189 | servo_offset *= -1 190 | return servo_offset 191 | elif servo_number == 4: 192 | return 0 193 | else: 194 | pass 195 | 196 | 197 | def readToAngle(self,input_analog,servo_number,tirgger): 198 | addr = self.kAddrAandB + (servo_number-1)*6 199 | 200 | data_a = (self.uarm.readEEPROM(addr+1)+ (self.uarm.readEEPROM(addr+2)*256))/10.0 201 | 202 | if (self.uarm.readEEPROM(addr) == 0): 203 | data_a = -data_a 204 | data_b = (self.uarm.readEEPROM(addr+4)+ (self.uarm.readEEPROM(addr+5)*256))/100.0 205 | 206 | if (self.uarm.readEEPROM(addr+3) == 0): 207 | data_b = -data_b 208 | 209 | if tirgger == 0 : 210 | return (data_a + data_b *input_analog) - self.readServoOffset(servo_number) 211 | elif tirgger == 1: 212 | return (data_a + data_b *input_analog) 213 | else: 214 | pass 215 | 216 | 217 | def fwdKine(self,theta_1,theta_2,theta_3): 218 | g_l3_1 = MATH_L3 * math.cos(theta_2/MATH_TRANS) 219 | g_l4_1 = MATH_L4 * math.cos(theta_3 / MATH_TRANS); 220 | g_l5 = (MATH_L2 + MATH_L3*math.cos(theta_2 / MATH_TRANS) + MATH_L4*math.cos(theta_3 / MATH_TRANS)); 221 | 222 | self.coord[1] = -math.cos(abs(theta_1 / MATH_TRANS))*g_l5; 223 | self.coord[2] = -math.sin(abs(theta_1 / MATH_TRANS))*g_l5; 224 | self.coord[3] = MATH_L1 + MATH_L3*math.sin(abs(theta_2 / MATH_TRANS)) - MATH_L4*math.sin(abs(theta_3 / MATH_TRANS)); 225 | return self.coord 226 | 227 | 228 | def currentCoord(self): 229 | return self.fwdKine(self.readAngle(1),self.readAngle(2),self.readAngle(3)) 230 | 231 | 232 | def currentX(self): 233 | self.currentCoord() 234 | return self.coord[1] 235 | 236 | 237 | def currentY(self): 238 | self.currentCoord() 239 | return self.coord[2] 240 | 241 | 242 | def currentZ(self): 243 | self.currentCoord() 244 | return self.coord[3] 245 | 246 | 247 | def readAngle(self,servo_number): 248 | if servo_number == 1: 249 | return self.readToAngle(self.readAnalog(1),1,0) 250 | elif servo_number == 2: 251 | return self.readToAngle(self.readAnalog(2),2,0) 252 | elif servo_number == 3: 253 | return self.readToAngle(self.readAnalog(3),3,0) 254 | elif servo_number == 4: 255 | return self.readToAngle(self.readAnalog(4),4,0) 256 | else: 257 | pass 258 | 259 | 260 | def readAngleRaw(self,servo_number): 261 | if servo_number == 1: 262 | return self.readToAngle(self.readAnalog(1),1,1) 263 | elif servo_number == 2: 264 | return self.readToAngle(self.readAnalog(2),2,1) 265 | elif servo_number == 3: 266 | return self.readToAngle(self.readAnalog(3),3,1) 267 | elif servo_number == 4: 268 | return self.readToAngle(self.readAnalog(4),4,1) 269 | else: 270 | pass 271 | 272 | 273 | def interpolation(self,init_val,final_val): 274 | #by using the formula theta_t = l_a_0 + l_a_1 * t + l_a_2 * t^2 + l_a_3 * t^3 275 | #theta(0) = init_val; theta(t_f) = final_val 276 | #theta_dot(0) = 0; theta_dot(t_f) = 0 277 | 278 | l_time_total = 1 279 | 280 | l_a_0 = init_val; 281 | l_a_1 = 0; 282 | l_a_2 = (3 * (final_val - init_val)) / (l_time_total*l_time_total); 283 | l_a_3 = (-2 * (final_val - init_val)) / (l_time_total*l_time_total*l_time_total); 284 | 285 | i = 0 286 | while i<51: 287 | 288 | l_t_step = (l_time_total / 50.0) *i 289 | self.g_interpol_val_arr[i] = l_a_0 + l_a_1 * (l_t_step) + l_a_2 * (l_t_step *l_t_step ) + l_a_3 * (l_t_step *l_t_step *l_t_step); 290 | i+=1 291 | return self.g_interpol_val_arr 292 | 293 | 294 | def pumpOn(self): 295 | self.uarm.digital[PUMP_EN].write(1) 296 | self.uarm.digital[VALVE_EN].write(0) 297 | 298 | 299 | def pumpOff(self): 300 | self.uarm.digital[PUMP_EN].write(0) 301 | self.uarm.digital[VALVE_EN].write(1) 302 | time.sleep(0.02) 303 | self.uarm.digital[VALVE_EN].write(0) 304 | 305 | 306 | def ivsKine(self, x, y, z): 307 | if z > (MATH_L1 + MATH_L3 + TopOffset): 308 | z = MATH_L1 + MATH_L3 + TopOffset 309 | if z < (MATH_L1 - MATH_L4 + BottomOffset): 310 | z = MATH_L1 - MATH_L4 + BottomOffset 311 | 312 | g_y_in = (-y-MATH_L2)/MATH_L3 313 | g_z_in = (z - MATH_L1) / MATH_L3 314 | g_right_all = (1 - g_y_in*g_y_in - g_z_in*g_z_in - MATH_L43*MATH_L43) / (2 * MATH_L43) 315 | g_sqrt_z_y = math.sqrt(g_z_in*g_z_in + g_y_in*g_y_in) 316 | 317 | if x == 0: 318 | # Calculate value of theta 1 319 | g_theta_1 = 90; 320 | # Calculate value of theta 3 321 | if g_z_in == 0: 322 | g_phi = 90 323 | else: 324 | g_phi = math.atan(-g_y_in / g_z_in)*MATH_TRANS 325 | if g_phi > 0: 326 | g_phi = g_phi - 180 327 | g_theta_3 = math.asin(g_right_all / g_sqrt_z_y)*MATH_TRANS - g_phi 328 | 329 | if g_theta_3<0: 330 | g_theta_3 = 0 331 | # Calculate value of theta 2 332 | g_theta_2 = math.asin((z + MATH_L4*math.sin(g_theta_3 / MATH_TRANS) - MATH_L1) / MATH_L3)*MATH_TRANS 333 | else: 334 | # Calculate value of theta 1 335 | g_theta_1 = math.atan(y / x)*MATH_TRANS 336 | if (y/x) > 0: 337 | g_theta_1 = g_theta_1 338 | if (y/x) < 0: 339 | g_theta_1 = g_theta_1 + 180 340 | if y == 0: 341 | if x > 0: 342 | g_theta_1 = 180 343 | else: 344 | g_theta_1 = 0 345 | # Calculate value of theta 3 346 | g_x_in = (-x / math.cos(g_theta_1 / MATH_TRANS) - MATH_L2) / MATH_L3; 347 | if g_z_in == 0: 348 | g_phi = 90 349 | else: 350 | g_phi = math.atan(-g_x_in / g_z_in)*MATH_TRANS 351 | if g_phi > 0: 352 | g_phi = g_phi - 180 353 | 354 | g_sqrt_z_x = math.sqrt(g_z_in*g_z_in + g_x_in*g_x_in) 355 | 356 | g_right_all_2 = -1 * (g_z_in*g_z_in + g_x_in*g_x_in + MATH_L43*MATH_L43 - 1) / (2 * MATH_L43) 357 | g_theta_3 = math.asin(g_right_all_2 / g_sqrt_z_x)*MATH_TRANS 358 | g_theta_3 = g_theta_3 - g_phi 359 | 360 | if g_theta_3 <0 : 361 | g_theta_3 = 0 362 | # Calculate value of theta 2 363 | g_theta_2 = math.asin(g_z_in + MATH_L43*math.sin(abs(g_theta_3 / MATH_TRANS)))*MATH_TRANS 364 | 365 | g_theta_1 = abs(g_theta_1); 366 | g_theta_2 = abs(g_theta_2); 367 | 368 | if g_theta_3 < 0 : 369 | pass 370 | else: 371 | self.fwdKine(g_theta_1,g_theta_2, g_theta_3) 372 | if (self.coord[2]>y+0.1) or (self.coord[2]0: 403 | self.interpolation(curXYZ[1],x) 404 | for n in range(0,50): 405 | x_arr[n] = self.g_interpol_val_arr[n] 406 | 407 | self.interpolation(curXYZ[2],y) 408 | for n in range(0,50): 409 | y_arr[n] = self.g_interpol_val_arr[n] 410 | 411 | self.interpolation(curXYZ[3],z) 412 | for n in range(0,50): 413 | z_arr[n] = self.g_interpol_val_arr[n] 414 | 415 | for n in range(0,50): 416 | self.ivsKine(x_arr[n],y_arr[n],z_arr[n]) 417 | self.writeAngle(self.angle[1],self.angle[2],self.angle[3],self.angle[1]*servo_4_relative+servo_4) 418 | time.sleep(timeSpend/50.0) 419 | 420 | elif time == 0: 421 | self.ivsKine(x,y,z) 422 | self.writeAngle(self.angle[1],self.angle[2],self.angle[3],self.angle[1]*servo_4_relative+servo_4) 423 | 424 | else: 425 | pass 426 | 427 | 428 | def moveTo(self,x,y,z): 429 | 430 | if self.uarm_status == 0: 431 | self.uarmAttach() 432 | self.uarm_status = 1 433 | 434 | 435 | curXYZ = self.currentCoord() 436 | 437 | x_arr = {} 438 | y_arr = {} 439 | z_arr = {} 440 | 441 | self.interpolation(curXYZ[1],x) 442 | for n in range(0,50): 443 | x_arr[n] = self.g_interpol_val_arr[n] 444 | 445 | self.interpolation(curXYZ[2],y) 446 | for n in range(0,50): 447 | y_arr[n] = self.g_interpol_val_arr[n] 448 | 449 | self.interpolation(curXYZ[3],z) 450 | for n in range(0,50): 451 | z_arr[n] = self.g_interpol_val_arr[n] 452 | 453 | for n in range(0,50): 454 | self.ivsKine(x_arr[n],y_arr[n],z_arr[n]) 455 | self.writeAngle(self.angle[1],self.angle[2],self.angle[3],0) 456 | 457 | time.sleep(0.04) 458 | 459 | 460 | def moveToWithTime(self,x,y,z,timeSpend): 461 | 462 | if self.uarm_status == 0: 463 | self.uarmAttach() 464 | self.uarm_status = 1 465 | 466 | curXYZ = self.currentCoord() 467 | x_arr = {} 468 | y_arr = {} 469 | z_arr = {} 470 | 471 | if time >0: 472 | self.interpolation(curXYZ[1],x) 473 | for n in range(0,50): 474 | x_arr[n] = self.g_interpol_val_arr[n] 475 | 476 | self.interpolation(curXYZ[2],y) 477 | for n in range(0,50): 478 | y_arr[n] = self.g_interpol_val_arr[n] 479 | 480 | self.interpolation(curXYZ[3],z) 481 | for n in range(0,50): 482 | z_arr[n] = self.g_interpol_val_arr[n] 483 | 484 | for n in range(0,50): 485 | self.ivsKine(x_arr[n],y_arr[n],z_arr[n]) 486 | self.writeAngle(self.angle[1],self.angle[2],self.angle[3],0) 487 | time.sleep(timeSpend/50.0) 488 | 489 | elif time == 0: 490 | 491 | self.ivsKine(x,y,z) 492 | self.writeAngle(self.angle[1],self.angle[2],self.angle[3],0) 493 | 494 | else: 495 | pass 496 | 497 | def moveToAtOnce(self,x,y,z): 498 | 499 | if self.uarm_status == 0: 500 | self.uarmAttach() 501 | self.uarm_status = 1 502 | 503 | self.ivsKine(x,y,z) 504 | self.writeAngle(self.angle[1],self.angle[2],self.angle[3],0) 505 | 506 | 507 | def moveRelative(self,x,y,z,time,servo_4_relative,servo_4): 508 | pass 509 | 510 | 511 | def stopperStatus(self): 512 | val = self.uarm.pumpStatus(0) 513 | 514 | if val ==2 or val ==1: 515 | return val-1 516 | else: 517 | print 'ERROR: Stopper is not deteceted' 518 | return -1 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | 527 | 528 | 529 | 530 | 531 | 532 | -------------------------------------------------------------------------------- /scripts/move_to_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | 5 | # File Name : move_to_node.py 6 | # Author : Joey Song 7 | # Version : V1.0 8 | # Date : 6 Jan, 2016 9 | # Modified Date : 6 Jan, 2016 10 | # Description : This documents is for uarm ROS Library and ROS package 11 | # Copyright(C) 2016 uArm Team. All right reserved. 12 | 13 | ''' 14 | 15 | # All libraries needed to import 16 | # Import system library 17 | 18 | import rospy 19 | import sys 20 | from uarm.msg import Coords 21 | from uarm.msg import CoordsWithTime 22 | from uarm.msg import CoordsWithTS4 23 | 24 | # raise error 25 | def raiseError(): 26 | print 'ERROR: Input 3 parameter represents x,y,z and time and servo_4 angle' 27 | 28 | # main exection function 29 | def execute(): 30 | 31 | # define publisher and its topic 32 | pub_1 = rospy.Publisher('move_to',Coords,queue_size = 10) 33 | pub_2 = rospy.Publisher('move_to_time',CoordsWithTime,queue_size = 10) 34 | pub_3 = rospy.Publisher('move_to_time_s4',CoordsWithTS4,queue_size = 10) 35 | rospy.init_node('move_to_node',anonymous = True) 36 | rate = rospy.Rate(10) 37 | 38 | # input x,y,z 39 | if len(sys.argv)>3: 40 | x = float(sys.argv[1]) 41 | y = float(sys.argv[2]) 42 | if y> 0: 43 | y = -y 44 | z = float(sys.argv[3]) 45 | 46 | if len(sys.argv) == 4: 47 | pub_1.publish(x,y,z) 48 | 49 | elif len(sys.argv) == 5: 50 | Time = int(sys.argv[4]) 51 | pub_2.publish(x,y,z,Time) 52 | 53 | elif len(sys.argv) == 6: 54 | Time = int(sys.argv[4]) 55 | S4 = int(sys.argv[5]) 56 | pub_3.publish(x,y,z,Time,S4) 57 | 58 | else: 59 | raiseError() 60 | 61 | rate.sleep() 62 | 63 | # main function 64 | if __name__ == '__main__': 65 | try: 66 | execute() 67 | except: 68 | print '==========================================' 69 | print 'ERROR: exectuion error' 70 | raiseError() 71 | pass 72 | 73 | 74 | -------------------------------------------------------------------------------- /scripts/pump_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | 5 | # File Name : pump_node.py 6 | # Author : Joey Song 7 | # Version : V1.0 8 | # Date : 6 Jan, 2016 9 | # Modified Date : 6 Jan, 2016 10 | # Description : This documents is for uarm ROS Library and ROS package 11 | # Copyright(C) 2016 uArm Team. All right reserved. 12 | 13 | ''' 14 | 15 | # All libraries needed to import 16 | # Import system library 17 | 18 | import rospy 19 | import sys 20 | from std_msgs.msg import String 21 | 22 | # raise error 23 | def raiseError(): 24 | print 'ERROR: Input Incorrect' 25 | print 'ERROR: Input off / low / 0 or on / high / 1' 26 | 27 | # main exection function 28 | def execute(): 29 | 30 | # define publisher and its topic 31 | pub = rospy.Publisher('pump_str_control',String,queue_size = 10) 32 | rospy.init_node('pump_node',anonymous = True) 33 | rate = rospy.Rate(10) 34 | 35 | # process different inputs 36 | if len(sys.argv) == 2: 37 | data_input = sys.argv[1] 38 | 39 | # control pump on 40 | if data_input.lower() == 'on' or data_input == '1' or data_input.lower() == 'high': 41 | pub.publish('on') 42 | 43 | # control pump off 44 | elif data_input.lower() == 'off' or data_input == '0' or data_input.lower() == 'low': 45 | pub.publish('off') 46 | else: 47 | raiseError() 48 | else: 49 | raiseError() 50 | 51 | rate.sleep() 52 | 53 | 54 | # main function 55 | if __name__ == '__main__': 56 | try: 57 | execute() 58 | except: 59 | print '==========================================' 60 | print 'ERROR: exectuion error' 61 | raiseError() 62 | pass 63 | -------------------------------------------------------------------------------- /scripts/report_angles_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | 5 | # File Name : report_anlges_node.py 6 | # Author : Joey Song 7 | # Version : V1.0 8 | # Date : 6 Jan, 2016 9 | # Modified Date : 6 Jan, 2016 10 | # Description : This documents is for uarm ROS Library and ROS package 11 | # Copyright(C) 2016 uArm Team. All right reserved. 12 | 13 | ''' 14 | 15 | # All libraries needed to import 16 | # Import system library 17 | 18 | import rospy 19 | import sys 20 | import time 21 | from std_msgs.msg import Int32 22 | 23 | # raise error 24 | def raiseError(): 25 | print 'ERROR: Input Incorrect' 26 | print 'ERROR: 3 options for print angles' 27 | print 'ERROR: 1: Input nothing after report_anlges_node.py (example: rosrun uarm report_angles_node.py)' 28 | print 'ERROR: 2: Input how many times to print (example: rosrun uarm report_angles_node.py 12)' 29 | print 'ERROR: 3: Input times and time_sec between each times (example: rosrun uarm report_angles_node.py 12 2)' 30 | 31 | # main exection function 32 | def execute(): 33 | 34 | # define publisher and its topic 35 | pub = rospy.Publisher('read_angles',Int32,queue_size = 10) 36 | rospy.init_node('report_angles_node',anonymous = True) 37 | rate = rospy.Rate(10) 38 | 39 | # report once 40 | if len(sys.argv) == 1: 41 | pub.publish(1) 42 | time.sleep(0.15) 43 | print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(rospy.get_param('servo_1'),rospy.get_param('servo_2'),rospy.get_param('servo_3'),rospy.get_param('servo_4')) 44 | 45 | # report multiple time 46 | elif len(sys.argv) == 2: 47 | for i in range(0,int(sys.argv[1])): 48 | pub.publish(1) 49 | time.sleep(0.15) 50 | print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(rospy.get_param('servo_1'),rospy.get_param('servo_2'),rospy.get_param('servo_3'),rospy.get_param('servo_4')) 51 | 52 | elif len(sys.argv) == 3: 53 | for i in range(0,int(sys.argv[1])): 54 | pub.publish(1) 55 | time.sleep(0.15) 56 | print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(rospy.get_param('servo_1'),rospy.get_param('servo_2'),rospy.get_param('servo_3'),rospy.get_param('servo_4')) 57 | time.sleep(float(sys.argv[2])-0.15) 58 | 59 | else: 60 | raiseError() 61 | 62 | rate.sleep() 63 | 64 | # main function 65 | if __name__ == '__main__': 66 | try: 67 | execute() 68 | pass 69 | except: 70 | print '==========================================' 71 | print 'ERROR: Exectuion error. Input numbers only' 72 | raiseError() 73 | pass 74 | -------------------------------------------------------------------------------- /scripts/report_coords_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | 5 | # File Name : report_coords_node.py 6 | # Author : Joey Song 7 | # Version : V1.0 8 | # Date : 6 Jan, 2016 9 | # Modified Date : 6 Jan, 2016 10 | # Description : This documents is for uarm ROS Library and ROS package 11 | # Copyright(C) 2016 uArm Team. All right reserved. 12 | 13 | ''' 14 | 15 | # All libraries needed to import 16 | # Import system library 17 | 18 | import rospy 19 | import sys 20 | import time 21 | from std_msgs.msg import Int32 22 | 23 | # raise error 24 | def raiseError(): 25 | print 'ERROR: Input Incorrect' 26 | print 'ERROR: 3 options for print coords' 27 | print 'ERROR: 1: Input nothing after report_coords_node.py (example: rosrun uarm report_coords_node.py)' 28 | print 'ERROR: 2: Input how many times to print (example: rosrun uarm report_coords_node.py 12)' 29 | print 'ERROR: 3: Input times and time_sec between each times (example: rosrun uarm report_coords_node.py 12 2)' 30 | 31 | # main exection function 32 | def execute(): 33 | 34 | # define publisher and its topic 35 | pub = rospy.Publisher('read_coords',Int32,queue_size = 10) 36 | rospy.init_node('report_coords_node',anonymous = True) 37 | rate = rospy.Rate(10) 38 | 39 | # report once 40 | if len(sys.argv) == 1: 41 | pub.publish(1) 42 | time.sleep(0.1) 43 | print 'Current location is x: %2.2fcm, y: %2.2fcm, z: %2.2fcm.' %(rospy.get_param('current_x'),rospy.get_param('current_y'),rospy.get_param('current_z')) 44 | 45 | # report multiple time 46 | elif len(sys.argv) == 2: 47 | for i in range(0,int(sys.argv[1])): 48 | pub.publish(1) 49 | time.sleep(0.1) 50 | print 'Current location is x: %2.2fcm, y: %2.2fcm, z: %2.2fcm.' %(rospy.get_param('current_x'),rospy.get_param('current_y'),rospy.get_param('current_z')) 51 | 52 | elif len(sys.argv) == 3: 53 | for i in range(0,int(sys.argv[1])): 54 | pub.publish(1) 55 | time.sleep(0.1) 56 | print 'Current location is x: %2.2fcm, y: %2.2fcm, z: %2.2fcm.' %(rospy.get_param('current_x'),rospy.get_param('current_y'),rospy.get_param('current_z')) 57 | time.sleep(float(sys.argv[2])-0.1) 58 | else: 59 | raiseError() 60 | 61 | rate.sleep() 62 | 63 | # main function 64 | if __name__ == '__main__': 65 | try: 66 | execute() 67 | pass 68 | except: 69 | print '==========================================' 70 | print 'ERROR: Exectuion error. Input numbers only' 71 | raiseError() 72 | pass 73 | -------------------------------------------------------------------------------- /scripts/report_stopper_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | 5 | # File Name : report_stopper_node.py 6 | # Author : Joey Song 7 | # Version : V1.0 8 | # Date : 6 Jan, 2016 9 | # Modified Date : 6 Jan, 2016 10 | # Description : This documents is for uarm ROS Library and ROS package 11 | # Copyright(C) 2016 uArm Team. All right reserved. 12 | 13 | ''' 14 | 15 | # All libraries needed to import 16 | # Import system library 17 | 18 | import rospy 19 | import sys 20 | import time 21 | from std_msgs.msg import Int32 22 | 23 | # raise error 24 | def raiseError(): 25 | print 'ERROR: Input Incorrect' 26 | print 'ERROR: 3 options for print status of stopper' 27 | print 'ERROR: 1: Input nothing after report_stopper_node.py (example: rosrun uarm report_stopper_node.py)' 28 | print 'ERROR: 2: Input how many times to print (example: rosrun uarm report_stopper_node.py 12)' 29 | print 'ERROR: 3: Input times and time_sec between each times (example: rosrun uarm report_stopper_node.py 12 2)' 30 | 31 | # main exection function 32 | def execute(): 33 | 34 | # define publisher and its topic 35 | pub = rospy.Publisher('stopper_status',Int32,queue_size = 10) 36 | rospy.init_node('report_stopper_node',anonymous = True) 37 | rate = rospy.Rate(10) 38 | 39 | # report once 40 | if len(sys.argv) == 1: 41 | 42 | pub.publish(1) 43 | time.sleep(0.1) 44 | status = rospy.get_param('stopper_status') 45 | 46 | if status == 'HIGH': 47 | print 'Stopper is actived' 48 | elif status == 'LOW': 49 | print 'Stopper is not actived' 50 | 51 | # report multiple time 52 | elif len(sys.argv) == 2: 53 | for i in range(0,int(sys.argv[1])): 54 | pub.publish(1) 55 | time.sleep(0.1) 56 | 57 | status = rospy.get_param('stopper_status') 58 | if status == 'HIGH': 59 | print 'Stopper is actived' 60 | elif status == 'LOW': 61 | print 'Stopper is not actived' 62 | 63 | elif len(sys.argv) == 3: 64 | for i in range(0,int(sys.argv[1])): 65 | 66 | pub.publish(1) 67 | time.sleep(0.1) 68 | 69 | status = rospy.get_param('stopper_status') 70 | if status == 'HIGH': 71 | print 'Stopper is actived' 72 | elif status == 'LOW': 73 | print 'Stopper is not actived' 74 | time.sleep(float(sys.argv[2])-0.1) 75 | else: 76 | raiseError() 77 | 78 | rate.sleep() 79 | 80 | # main function 81 | if __name__ == '__main__': 82 | try: 83 | execute() 84 | pass 85 | except: 86 | print '==========================================' 87 | print 'ERROR: Exectuion error. Input numbers only' 88 | raiseError() 89 | pass 90 | -------------------------------------------------------------------------------- /scripts/uarm_core.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | 5 | # File Name : uarm_core.py 6 | # Author : Joey Song 7 | # Version : V1.0 8 | # Date : 6 Jan, 2016 9 | # Modified Date : 6 Jan, 2016 10 | # Description : This documents is for uarm ROS Library and ROS package 11 | # Copyright(C) 2016 uArm Team. All right reserved. 12 | 13 | ''' 14 | 15 | 16 | # All libraries needed to import 17 | # Import system library 18 | import sys 19 | import time 20 | import rospy 21 | 22 | # Import uarm for python library 23 | #from UArmForPython.uarm_python import Uarm 24 | import pyuarm 25 | 26 | # Import messages type 27 | from std_msgs.msg import String 28 | from std_msgs.msg import Float32 29 | from std_msgs.msg import UInt8 30 | from std_msgs.msg import Int32 31 | from uarm.msg import Angles 32 | from uarm.msg import Coords 33 | from uarm.msg import CoordsWithTime 34 | from uarm.msg import CoordsWithTS4 35 | 36 | # Read current Coords function 37 | def readCurrentCoords(): 38 | cc = uarm.get_position() 39 | print 'Current location is x: %2.2fcm, y: %2.2fcm, z: %2.2fcm.' %(float(cc[0]), float(cc[1]), float(cc[2])) 40 | 41 | rospy.set_param('current_x', cc[0]) 42 | rospy.set_param('current_y', float(cc[1])) 43 | rospy.set_param('current_z', float(cc[2])) 44 | 45 | # Read current Angles function 46 | def readCurrentAngles(): 47 | ra = {} 48 | ra['s1'] = uarm.get_servo_angle(0) 49 | ra['s2'] = uarm.get_servo_angle(1) 50 | ra['s3'] = uarm.get_servo_angle(2) 51 | ra['s4'] = uarm.get_servo_angle(3) 52 | 53 | print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(ra['s1'], ra['s2'],ra['s3'],ra['s4']) 54 | 55 | rospy.set_param('servo_1',ra['s1']) 56 | rospy.set_param('servo_2',ra['s2']) 57 | rospy.set_param('servo_3',ra['s3']) 58 | rospy.set_param('servo_4',ra['s4']) 59 | 60 | # Read stopper function 61 | def readStopperStatus(): 62 | val = uarm.get_tip_sensor() 63 | if val == True: 64 | print 'Stopper is actived' 65 | rospy.set_param('stopper_status','HIGH') 66 | else: 67 | print 'Stopper is not actived' 68 | rospy.set_param('stopper_status','LOW') 69 | 70 | 71 | # Write single angle 72 | def writeSingleAngle(num, angle): 73 | # limits of each servo 74 | s3_max = 170 - uarm.get_servo_angle(1) 75 | if num == 1: 76 | if angle > 160: 77 | angle = 160 78 | elif angle < 20: 79 | angle = 20 80 | elif num == 2: 81 | if angle > 140: 82 | angle = 140 83 | elif angle < 0: 84 | angle = 0 85 | elif num == 3: 86 | if angle > s3_max: 87 | angle = s3_max 88 | elif angle < 0: 89 | angle = 0 90 | elif num == 4: 91 | if angle > 180: 92 | angle = 180 93 | elif angle < 0: 94 | angle = 0 95 | else: 96 | print "Input incorrect: wrong servo number" 97 | 98 | uarm.set_servo_angle(num-1, angle) 99 | 100 | 101 | # Write coordinate: x y z and speed 102 | def writeCoordinate(coordinate): 103 | x = float(coordinate[1]) 104 | y = float(coordinate[2]) 105 | z = float(coordinate[3]) 106 | if len(coordinate) == 4: 107 | speed = 5 108 | 109 | elif len(coordinate) == 5: 110 | speed = float(coordinate[4]) 111 | 112 | else: 113 | print "Input incorrect: wrong number of arguments" 114 | 115 | if y > 0: 116 | y = -y 117 | 118 | uarm.set_position(x, y, z, speed) 119 | 120 | 121 | # Main connect function 122 | def connectFcn(): 123 | 124 | global failed_number 125 | global connectionStatus 126 | connectionStatus = 0 127 | global uarm 128 | global controlFcnLoop 129 | global listenerFcn 130 | controlFcnLoop = True 131 | listenerFcn = True 132 | 133 | if len(sys.argv)<2: 134 | # 1 means no input argument 135 | failed_number = 20 136 | controlFcnLoop = False 137 | listenerFcn = False 138 | print 'Input Incorrect' 139 | sys.exit(0) 140 | return 1 141 | 142 | if sys.argv[1] == 'connect': 143 | print '=======================================================' 144 | print 'Connecting ...' 145 | print '=======================================================' 146 | 147 | if len(sys.argv) == 2: 148 | failed_number = 21 149 | uarm = pyuarm.get_uarm() 150 | uarm.connect() 151 | connectionStatus = 1 152 | 153 | print 'Connected' 154 | return 21 155 | 156 | elif len(sys.argv) == 3: 157 | 158 | failed_number = 22 159 | 160 | if sys.argv[2] == 'l': 161 | failed_number = 23 162 | uarm = pyuarm.get_uarm() 163 | uarm.connect() 164 | controlFcnLoop = False 165 | print 'Connected' 166 | else: 167 | connectionStatus = 1 168 | print 'Connected' 169 | return 22 170 | 171 | # followed by l - directly listen to nodes 172 | elif len(sys.argv) == 4: 173 | failed_number = 23 174 | uarm = pyuarm.get_uarm() 175 | uarm.connect() 176 | 177 | connectionStatus = 1 178 | print 'Connected' 179 | 180 | if sys.argv[3] == 'l': 181 | print 'Directly to listen mode' 182 | controlFcnLoop = False 183 | return 25 184 | return 23 185 | 186 | else: 187 | # 2 means input argument is wrong 188 | return 24 189 | #if sys.argv[1] == 'd' 190 | 191 | 192 | # Main control function 193 | def controlFcn(): 194 | 195 | while controlFcnLoop: 196 | commands = raw_input("Input Commands (Input h to see all commands): ") 197 | 198 | if commands == 'h': 199 | print ' ' 200 | print 'Control mode' 201 | print '==================================================================' 202 | print 'h - show help and list all commands' 203 | print 'cc - current coordinates' 204 | print 'ra - read angles' 205 | print 'wa a1 a2 a3 a4 - write angles' 206 | print ' for example: wa 120 80 40 40' 207 | print 'wsa servo a - write single angle' 208 | print ' for example: wsa 1 50' 209 | print 'mt x y z - move to a point with speed(defaut: 5mm/s)' 210 | print ' for example: mt 0 12 12' 211 | print 'mt x y z speed - move to a point with speed(mm/s)' 212 | print ' for example: mt 0 12 12 10' 213 | print 'pp 1/0 - pump on/off' 214 | print 'ss - status of stopper' 215 | print 'at - attach uarm' 216 | print 'de - detach uarm' 217 | print 'l - begin listener mode' 218 | print 'e - exit' 219 | print '==================================================================' 220 | print ' ' 221 | 222 | elif commands == 'l': 223 | print 'Exit: Break the control fuction loop' 224 | break; 225 | 226 | elif commands == 'e': 227 | print 'Detach all servos and exit the program' 228 | uarm.set_servo_detach() 229 | sys.exit(0) 230 | 231 | elif len(commands) == 0: 232 | print 'len is 0' 233 | 234 | else: 235 | commands_split = commands.split() 236 | 237 | # Detach 238 | if commands_split[0] == 'detach'or commands_split[0] == 'de': 239 | if len(commands_split) == 1: 240 | uarm.set_servo_detach() 241 | else: 242 | print 'no other commands should be input' 243 | pass 244 | 245 | # Attach 246 | if commands_split[0] == 'attach'or commands_split[0] == 'at': 247 | if len(commands_split) == 1: 248 | uarm.set_servo_attach() 249 | else: 250 | print 'no other commands should be input' 251 | pass 252 | 253 | # Stopper Status 254 | elif commands_split[0] == 'stopperStatus'or commands_split[0] == 'ss': 255 | if len(commands_split) == 1: 256 | readStopperStatus() 257 | else: 258 | print 'no other commands should be input' 259 | pass 260 | 261 | # Pump Control 262 | elif commands_split[0] == 'pump' or commands_split[0] == 'pp': 263 | if len(commands_split) == 1: 264 | print 'Status should be input' 265 | elif len(commands_split) >2: 266 | print 'Too many inputs' 267 | else: 268 | if commands_split[1] == '1' or commands_split[1].lower() == 'high' or commands_split[1].lower() == 'on': 269 | uarm.set_pump(1) 270 | elif commands_split[1] == '0' or commands_split[1].lower() == 'low'or commands_split[1].lower() == 'off': 271 | uarm.set_pump(0) 272 | else: 273 | print 'Incorrect inputs, should input 1 / 0 / HIGH / LOW / ON / OFF' 274 | pass 275 | 276 | # Current Coordinates 277 | elif commands_split[0] == 'currentCoords' or commands_split[0] == 'cc': 278 | if len(commands_split) == 1: 279 | readCurrentCoords() 280 | else: 281 | print 'no other commands should be input' 282 | pass 283 | 284 | # Write Single Angles 285 | elif commands_split[0] == 'writeSingleAngles' or commands_split[0] == 'wsa': 286 | if len(commands_split) == 3: 287 | writeSingleAngle(int(commands_split[1]), float(commands_split[2])) 288 | else: 289 | print 'Input incorrect: Wrong number of arguments' 290 | pass 291 | 292 | # Write Angles 293 | elif commands_split[0] == 'wa': 294 | if len(commands_split) == 5: 295 | writeSingleAngle(1, float(commands_split[1])) 296 | writeSingleAngle(2, float(commands_split[2])) 297 | writeSingleAngle(3, float(commands_split[3])) 298 | writeSingleAngle(4, float(commands_split[4])) 299 | else: 300 | print 'Input incorrect: Wrong number of arguments' 301 | pass 302 | 303 | # Current Angles 304 | elif commands_split[0] == 'readAngles' or commands_split[0] == 'ra': 305 | if len(commands_split) == 1: 306 | readCurrentAngles() 307 | else: 308 | print 'no other commands should be input' 309 | pass 310 | 311 | # Move Tos 312 | elif commands_split[0] == 'moveTo' or commands_split[0] == 'mt': 313 | if len(commands_split) == 4 or len(commands_split) == 5: 314 | writeCoordinate(commands_split) 315 | else: 316 | print 'Input incorrect: Wrong number of arguments' 317 | else: 318 | pass 319 | 320 | 321 | # pump control function once received data from topic 322 | def pumpCallack(data): 323 | 324 | data_input = data.data 325 | 326 | if data_input == 0: 327 | uarm.pump_control(0) 328 | print 'Pump: Off' 329 | elif data_input == 1: 330 | uarm.pump_control(1) 331 | print 'Pump: On' 332 | else: 333 | pass 334 | 335 | 336 | # pump str control function once received data from topic 337 | def pumpStrCallack(data): 338 | 339 | data_input = data.data 340 | print data_input 341 | 342 | if data_input.lower() == 'low' or data_input.lower() == 'off': 343 | uarm.pump_control(0) 344 | print 'Pump: Off' 345 | elif data_input.lower() == 'on' or data_input.lower() == 'high': 346 | uarm.pump_control(1) 347 | print 'Pump: On' 348 | else: 349 | pass 350 | 351 | 352 | # angles control function once received data from topic 353 | def writeAnglesCallback(servos): 354 | 355 | servo = {} 356 | servo['s1'] = servos.servo_1 357 | servo['s2'] = servos.servo_2 358 | servo['s3'] = servos.servo_3 359 | servo['s4'] = servos.servo_4 360 | for i in servo: 361 | if servo[i]>180: servo[i] = 180 362 | if servo[i]<0: servo[i] = 0 363 | 364 | uarm.set_servo_angle(0, servo['s1']) 365 | uarm.set_servo_angle(1, servo['s2']) 366 | uarm.set_servo_angle(2, servo['s3']) 367 | uarm.set_servo_angle(3, servo['s4']) 368 | 369 | print 'Movement: Moved Once' 370 | 371 | 372 | # attach or detach uarm function once received data from topic 373 | def attchCallback(attachStatus): 374 | data_input = attachStatus.data 375 | 376 | if data_input.lower() == 'attach' : 377 | uarm.attach_all_servos() 378 | print 'uArm: Attach' 379 | elif data_input.lower() == 'detach': 380 | uarm.detach_all_servos() 381 | print 'uArm: Detach' 382 | else: 383 | pass 384 | 385 | 386 | # move to function once received data from topic 387 | def moveToCallback(coords): 388 | x = coords.x 389 | y = coords.y 390 | if y>0: 391 | y = -y 392 | z = coords.z 393 | uarm.set_position(x, y, z, 2) 394 | print 'Movement: Moved Once' 395 | 396 | 397 | # moveto functions once received data from topic 398 | def moveToTimeCallback(coordsAndT): 399 | x = coordsAndT.x 400 | y = coordsAndT.y 401 | if y>0: 402 | y = -y 403 | z = coordsAndT.z 404 | time = coordsAndT.time 405 | if time == 0: 406 | uarm.set_position(x, y, z, 2) 407 | else: 408 | uarm.set_position(x, y, z, 2) 409 | 410 | print 'Movement: Moved Once' 411 | pass 412 | 413 | 414 | # moveto functions once received data from topic 415 | def moveToTimeAndS4Callback(coordsAndTS4): 416 | 417 | x = coordsAndTS4.x 418 | y = coordsAndTS4.y 419 | if y>0: 420 | y = -y 421 | z = coordsAndTS4.z 422 | time = coordsAndTS4.time 423 | s4 = coordsAndTS4.servo_4 424 | if s4 > 180: s4 = 180 425 | if s4 <0 : s4 =0 426 | uarm.set_position(x, y, z, s4, 0, time, 0, 0) 427 | print 'Movement: Moved Once' 428 | pass 429 | 430 | 431 | # print current coords once received data from topic 432 | def currentCoordsCallback(times): 433 | 434 | Times = times.data 435 | if Times == 1: 436 | readCurrentCoords() 437 | elif Times < 0: 438 | pass 439 | else: 440 | for i in range(0,Times): 441 | readCurrentCoords() 442 | time.sleep(1) 443 | 444 | 445 | # print current angles once received data from topic 446 | def readAnglesCallback(times): 447 | 448 | Times = times.data 449 | if Times == 1: 450 | readCurrentAngles() 451 | elif Times < 0: 452 | pass 453 | else: 454 | for i in range(0,Times): 455 | readCurrentAngles() 456 | time.sleep(1) 457 | 458 | 459 | # print stoppers status once received data from topic 460 | def stopperStatusCallback(times): 461 | Times = times.data 462 | if Times == 1: 463 | readStopperStatus() 464 | elif Times < 0: 465 | pass 466 | else: 467 | for i in range(0,Times): 468 | readStopperStatus() 469 | time.sleep(1) 470 | 471 | 472 | # monitor mode for listening to all topics 473 | def listener(): 474 | print ' ' 475 | print 'Begin monitor mode - listening to all fucntional topics' 476 | print '=======================================================' 477 | print ' Use rqt_graph to check the connection ' 478 | print '=======================================================' 479 | 480 | rospy.init_node('uarm_core',anonymous=True) 481 | 482 | rospy.Subscriber("uarm_status",String, attchCallback) 483 | rospy.Subscriber("pump_control",UInt8, pumpCallack) 484 | rospy.Subscriber("pump_str_control",String, pumpStrCallack) 485 | 486 | rospy.Subscriber("read_coords",Int32, currentCoordsCallback) 487 | rospy.Subscriber("read_angles",Int32, readAnglesCallback) 488 | rospy.Subscriber("stopper_status",Int32, stopperStatusCallback) 489 | 490 | rospy.Subscriber("write_angles",Angles, writeAnglesCallback) 491 | rospy.Subscriber("move_to",Coords, moveToCallback) 492 | rospy.Subscriber("move_to_time",CoordsWithTime, moveToTimeCallback) 493 | rospy.Subscriber("move_to_time_s4",CoordsWithTS4, moveToTimeAndS4Callback) 494 | 495 | rospy.spin() 496 | pass 497 | 498 | 499 | # show eroors 500 | def processFailedNum(failed_number): 501 | 502 | if failed_number > 20 and failed_number < 26: 503 | print 'ERROR: Input Connection Address Is Incorrect' 504 | if failed_number == 20: 505 | print 'uArm: Please Connect uArm first ' 506 | 507 | 508 | if __name__ == '__main__': 509 | 510 | try: 511 | # Connect uarm first 512 | return_value = connectFcn() 513 | 514 | # Control uarm through commands 515 | controlFcn() 516 | 517 | # Monitor mode 518 | if listenerFcn == True: 519 | listener() 520 | 521 | except: 522 | processFailedNum(failed_number) 523 | print 'ERROR: Execution Failed' 524 | pass 525 | 526 | finally: 527 | print 'DONE: Program Stopped' 528 | pass 529 | -------------------------------------------------------------------------------- /scripts/uarm_status_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | 5 | # File Name : uarm_stauts_node.py 6 | # Author : Joey Song 7 | # Version : V1.0 8 | # Date : 6 Jan, 2016 9 | # Modified Date : 6 Jan, 2016 10 | # Description : This documents is for uarm ROS Library and ROS package 11 | # Copyright(C) 2016 uArm Team. All right reserved. 12 | 13 | ''' 14 | 15 | # All libraries needed to import 16 | # Import system library 17 | 18 | import rospy 19 | import sys 20 | from std_msgs.msg import String 21 | 22 | # raise error 23 | def raiseError(): 24 | print 'ERROR: Input Incorrect' 25 | print 'ERROR: Input detach / 0 or attach / 1' 26 | 27 | # main exection function 28 | def execute(): 29 | 30 | # define publisher and its topic 31 | pub = rospy.Publisher('uarm_status',String,queue_size = 10) 32 | rospy.init_node('uarm_status_node',anonymous = True) 33 | rate = rospy.Rate(10) 34 | 35 | if len(sys.argv) == 2: 36 | data_input = sys.argv[1] 37 | 38 | # attach uarm 39 | if data_input.lower() == 'attach' or data_input == '1' : 40 | pub.publish('attach') 41 | 42 | # detach uarm 43 | elif data_input.lower() == 'detach' or data_input == '0' : 44 | pub.publish('detach') 45 | 46 | else: 47 | raiseError() 48 | else: 49 | raiseError() 50 | 51 | rate.sleep() 52 | 53 | # main function 54 | if __name__ == '__main__': 55 | try: 56 | execute() 57 | except: 58 | print '==========================================' 59 | print 'ERROR: exectuion error' 60 | raiseError() 61 | pass 62 | -------------------------------------------------------------------------------- /scripts/write_angles_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | 5 | # File Name : write_angles_node.py 6 | # Author : Joey Song 7 | # Version : V1.0 8 | # Date : 6 Jan, 2016 9 | # Modified Date : 6 Jan, 2016 10 | # Description : This documents is for uarm ROS Library and ROS package 11 | # Copyright(C) 2016 uArm Team. All right reserved. 12 | 13 | ''' 14 | 15 | # All libraries needed to import 16 | # Import system library 17 | 18 | import rospy 19 | import sys 20 | from uarm.msg import Angles 21 | 22 | # raise error 23 | def raiseError(): 24 | print 'ERROR: Input 4 servo angles between 0 ~ 180 degree' 25 | 26 | # main exection function 27 | def execute(): 28 | 29 | # define publisher and its topic 30 | pub = rospy.Publisher('write_angles',Angles,queue_size = 10) 31 | rospy.init_node('write_angles_node',anonymous = True) 32 | rate = rospy.Rate(10) 33 | 34 | # write 4 angles 35 | if len(sys.argv) == 5: 36 | s1 = int(sys.argv[1]) 37 | s2 = int(sys.argv[2]) 38 | s3 = int(sys.argv[3]) 39 | s4 = int(sys.argv[4]) 40 | pub.publish(s1,s2,s3,s4) 41 | 42 | else: 43 | raiseError() 44 | 45 | rate.sleep() 46 | 47 | # main function 48 | if __name__ == '__main__': 49 | try: 50 | execute() 51 | except: 52 | print '==========================================' 53 | print 'ERROR: exectuion error' 54 | raiseError() 55 | pass 56 | 57 | -------------------------------------------------------------------------------- /src/uarm_status.cpp: -------------------------------------------------------------------------------- 1 | 2 | /****************************************************************************************** 3 | # File Name : uarm_core.py 4 | # Author : Joey Song 5 | # Version : V1.0 6 | # Date : 6 Jan, 2016 7 | # Modified Date : 6 Jan, 2016 8 | # Description : This documents is for uarm ROS Library and ROS package 9 | # Copyright(C) 2016 uArm Team. All right reserved. 10 | *******************************************************************************************/ 11 | 12 | #include "ros/ros.h" 13 | #include "std_msgs/String.h" 14 | #include 15 | #include 16 | 17 | // Main function 18 | int main(int argc, char **argv) 19 | { 20 | // initial node 21 | ros::init(argc, argv, "uarm_status"); 22 | ros::NodeHandle n; 23 | ros::Publisher execute = n.advertise("uarm_status", 10); 24 | 25 | ros::Rate loop_rate(10); 26 | 27 | std_msgs::String msg; 28 | int executeTrigger =0; 29 | 30 | // create a loop which will allow publisher publishs after set-up 31 | while(ros::ok()){ 32 | // check connection - if there is a subscriber 33 | if (execute.getNumSubscribers()!=0) 34 | { 35 | if(executeTrigger == 0) 36 | if (argc == 2) 37 | { 38 | std::string string_1 = argv[1]; 39 | 40 | // attach uarm 41 | if ((string_1== "attach") || (string_1== "1") || (string_1== "ATTACH")) 42 | { 43 | msg.data = "attach"; 44 | execute.publish(msg); 45 | std::cout << "uArm: Attached uarm" << std::endl; 46 | executeTrigger= 1; 47 | } 48 | // detach uarm 49 | else if ((string_1 == "detach")|| (string_1== "0") || (string_1== "DETACH")) 50 | { 51 | msg.data = "detach"; 52 | execute.publish(msg); 53 | std::cout << "uArm: Detached uarm" << std::endl; 54 | executeTrigger= 1; 55 | } 56 | // print errors 57 | else 58 | { 59 | std::cout << "ERROR: Input Incorrect" << std::endl; 60 | } 61 | } 62 | else 63 | { 64 | std::cout << "ERROR: Input detach/0 or attach/1" << std::endl; 65 | } 66 | sleep(0.1); 67 | } 68 | } 69 | return 0; 70 | } 71 | -------------------------------------------------------------------------------- /urdf/daes/add_1.dae: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | 6 | Rhinoceros 7 | Output from Rhinoceros COLLADA Exporter 8 | 9 | 10 | 11 | 2016-01-18T10:21:05Z 12 | 2016-01-18T10:21:05Z 13 | 14 | Z_UP 15 | 16 | 17 | 18 | 19 | 20 | 21 | 47.837389 22 | 1.848039 23 | 311.304332 24 | 366.769558 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 1.0 34 | 1.850490 35 | 9602.588597 36 | 9678.632863 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 1.0 46 | 1.855037 47 | 80175.136504 48 | 80196.730804 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 1.0 58 | 1.852580 59 | 50710.028880 60 | 50713.049251 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 1 1 1 1 78 | 79 | 80 | 1 1 1 1 81 | 82 | 83 | 1 1 1 1 84 | 85 | 86 | 1.000000 87 | 88 | 89 | 1 1 1 1 90 | 91 | 92 | 1.000000 93 | 94 | 95 | 0.000000 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 1 -0.0189210809767246 55.5999946594238 1 -0.631170511245728 55.4779968261719 1 -1.15012955665588 55.1309814453125 1 -1.49679100513458 54.6117897033691 1 -1.61837208271027 53.9994506835938 1 -1.496373295784 53.3871994018555 1 -1.1493626832962 52.8682403564453 1 -0.630165576934814 52.5215835571289 1 -0.0178319849073887 52.3999977111816 1 0.594425559043884 52.5219955444336 1 1.11338353157043 52.8690071105957 1 1.46003901958466 53.3882064819336 1 1.58162605762482 54.0005378723145 1 1.45962822437286 54.6127967834473 1 1.11261022090912 55.1317558288574 1 0.593413710594177 55.4784164428711 1.00001430511475 0.00115829310379922 -3.40005254745483 1.00001525878906 -0.80222761631012 -3.52757120132446 1.00001430511475 -1.52688944339752 -3.89712142944336 1.00001525878906 -2.10188174247742 -4.47250556945801 1.00001525878906 -2.47093033790588 -5.1974139213562 1.00001430511475 -2.5979061126709 -6.00089073181152 1.00001525878906 -2.47038173675537 -6.80426597595215 1.00001430511475 -2.10083818435669 -7.52891874313354 1.00001430511475 -1.52545201778412 -8.10392284393311 1.00001430511475 -0.800546765327454 -8.47296714782715 1.00001430511475 0.0029286106582731 -8.59994983673096 1.00001430511475 0.806307077407837 -8.47243022918701 1.00001430511475 1.53096890449524 -8.10288047790527 1.00001525878906 2.1059684753418 -7.5274977684021 1.00001525878906 2.47501707077026 -6.80258941650391 1.00001430511475 2.60199284553528 -5.99911260604858 1.00001525878906 2.47446918487549 -5.19573354721069 1.00001525878906 2.10492777824402 -4.47106552124023 1.00001430511475 1.52953958511353 -3.89607644081116 1.00001525878906 0.804628193378448 -3.52702140808105 1.00001430511475 -0.00292677083052695 8.59994506835938 1.00001430511475 -0.806312799453735 8.47242641448975 1.00001430511475 -1.5309671163559 8.10287475585938 1.00001525878906 -2.10596680641174 7.52749156951904 1.00001525878906 -2.47501540184021 6.80258321762085 1.00001430511475 -2.60198903083801 5.99912166595459 1.00001430511475 -2.47446727752686 5.19572973251343 1.00001430511475 -2.10492348670959 4.47107648849487 1.00001430511475 -1.5295375585556 3.89607262611389 1.00001525878906 -0.80463182926178 3.52703022956848 1.00001430511475 -0.00115675129927695 3.40004515647888 1.00001430511475 0.80223149061203 3.52757906913757 1.00001430511475 1.52688348293304 3.89711499214172 1.00001430511475 2.10188341140747 4.47249937057495 1.00001430511475 2.47093200683594 5.1974081993103 1.00001430511475 2.59790778160095 6.00088453292847 1.00001525878906 2.47038602828979 6.80427694320679 1.00001430511475 2.10084247589111 7.52892971038818 1.00001430511475 1.52545392513275 8.10391712188721 1.00001525878906 0.800543069839478 8.472975730896 1 -5.0011248588562 54.4149627685547 1 -4.77711343765259 55.5344047546387 1 -4.30501079559326 56.5738525390625 1 -3.60943269729614 57.479118347168 1 -2.72664713859558 58.2029991149902 1 -1.70266759395599 58.7077713012695 1 -0.590879678726196 58.967113494873 1 0.550749897956848 58.9675064086914 1 1.66271591186523 58.7089195251465 1 2.68703508377075 58.2048416137695 1 3.570317029953 57.4815635681152 1 4.26651239395142 56.5767669677734 1 4.73931694030762 55.5376472473145 1 4.96409511566162 54.4183578491211 1 -9.46722030639648 0.788445770740509 1 9.46668720245361 0.794884860515594 1 9.47415924072266 -0.7002312541008 1 9.24696445465088 -2.17800498008728 1 8.79073047637939 -3.60182166099548 1.00000095367432 8.11675262451172 -4.93642091751099 1 7.24173069000244 -6.14876747131348 1 6.18733739852905 -7.20880699157715 1 4.97968578338623 -8.09028625488281 1 3.64869427680969 -8.77136898040771 0.999999046325684 2.22732424736023 -9.23521137237549 1 0.750786423683167 -9.4702844619751 1 -0.744346857070923 -9.47079849243164 1 -2.22103977203369 -9.23671817779541 1 -3.64272499084473 -8.77385807037354 1 -4.97418165206909 -8.09366703033447 1 -6.18243026733398 -7.21300745010376 1 -7.23754644393921 -6.15369606018066 1 -8.1133918762207 -4.9419584274292 1 -8.78827476501465 -3.60780835151672 1 -9.24547576904297 -2.18428778648376 1 -9.47367668151855 -0.706675827503204 1 0.00105148158036172 -3.09999823570251 1.00000095367432 0.874383151531219 -2.97412943840027 1 1.67687439918518 -2.60731506347656 1 2.34351515769959 -2.02926874160767 1 2.82029938697815 -1.28682708740234 1 3.068598985672 -0.440130680799484 1 3.06829690933228 0.442212462425232 1 2.81942296028137 1.2887407541275 1 2.34213447570801 2.03086543083191 1 1.67510008811951 2.60845756530762 1 0.872356116771698 2.97472381591797 0.999999046325684 -0.00105744996108115 3.09999179840088 1 -0.874381542205811 2.97412204742432 1 -1.67687284946442 2.60730767250061 1 -2.34351348876953 2.02926301956177 0.999999046325684 -2.8202977180481 1.28682160377502 1 -3.06859755516052 0.44012314081192 1 -3.06829524040222 -0.442218124866486 1 -2.8194215297699 -1.28874814510345 1 -2.34213280677795 -2.03087115287781 1 -1.67509853839874 -2.60846519470215 1 -0.87236213684082 -2.97473001480103 -1 -0.0189204849302769 55.5999984741211 1 -0.0189210809767246 55.5999946594238 1 -0.631170511245728 55.4779968261719 -1 -1.61837089061737 53.9994583129883 1 -1.61837208271027 53.9994506835938 -1 -1.496373295784 53.3871994018555 1 -1.496373295784 53.3871994018555 -1 -0.630166172981262 52.5215797424316 1 -0.630165576934814 52.5215835571289 -1 -0.0178319849073887 52.3999977111816 1 -0.0178319849073887 52.3999977111816 1 -1.1493626832962 52.8682403564453 1 -1.49679100513458 54.6117897033691 1 -1.15012955665588 55.1309814453125 0.333333969116211 -0.0189204849302769 55.5999984741211 -0.333333969116211 -0.0189204849302769 55.5999984741211 -1 -0.631170511245728 55.4779968261719 -1 -1.15012955665588 55.1309814453125 -1 -1.49679148197174 54.6117858886719 -1 -1.1493626832962 52.8682403564453 -0.333333969116211 -0.0178319849073887 52.3999977111816 0.333333015441895 -0.0178319849073887 52.3999977111816 0 -0.631170511245728 55.4779968261719 0 -1.49679100513458 54.6117897033691 0 -1.61837208271027 53.9994506835938 0 -1.496373295784 53.3871994018555 0 -1.1493626832962 52.8682403564453 0 -0.630165576934814 52.5215835571289 -1 -0.0178319849073887 52.3999977111816 1 -0.0178319849073887 52.3999977111816 1 0.594425559043884 52.5219955444336 -1 1.58162605762482 54.0005378723145 1 1.58162605762482 54.0005378723145 -1 1.45962822437286 54.6127967834473 1 1.45962822437286 54.6127967834473 -1 0.593413114547729 55.4784126281738 1 0.593413710594177 55.4784164428711 -1 -0.0189204849302769 55.5999984741211 1 -0.0189210809767246 55.5999946594238 -0.333333969116211 -0.0189204849302769 55.5999984741211 0.333333969116211 -0.0189204849302769 55.5999984741211 0.333333015441895 -0.0178319849073887 52.3999977111816 -0.333333969116211 -0.0178319849073887 52.3999977111816 1 1.11261022090912 55.1317558288574 1 1.46003901958466 53.3882064819336 1 1.11338353157043 52.8690071105957 -1 0.594425559043884 52.5219955444336 -1 1.11338460445404 52.8690147399902 -1 1.46003842353821 53.3882026672363 -1 1.11261022090912 55.1317558288574 0 0.594425559043884 52.5219955444336 0 1.46003901958466 53.3882064819336 0 1.58162605762482 54.0005378723145 0 1.45962822437286 54.6127967834473 0 1.11261022090912 55.1317558288574 0 0.593413710594177 55.4784164428711 -1 -0.0189204849302769 55.5999984741211 -1 -0.631170511245728 55.4779968261719 -1 -1.15012955665588 55.1309814453125 -1 -1.49679148197174 54.6117858886719 -1 -1.61837089061737 53.9994583129883 -1 -1.496373295784 53.3871994018555 -1 -1.1493626832962 52.8682403564453 -1 -0.630166172981262 52.5215797424316 -1 -0.0178319849073887 52.3999977111816 -1 0.594425559043884 52.5219955444336 -1 1.11338460445404 52.8690147399902 -1 1.46003842353821 53.3882026672363 -1 1.58162605762482 54.0005378723145 -1 1.45962822437286 54.6127967834473 -1 1.11261022090912 55.1317558288574 -1 0.593413114547729 55.4784126281738 -1 0.00149547657929361 -4.40000200271606 -1 -0.449250400066376 -4.4649600982666 -1 -0.863437652587891 -4.65428781509399 -1 -1.20751333236694 -4.95262956619263 -1 -1.45359814167023 -5.33582448959351 -1 -1.58175337314606 -5.77283620834351 -1 -1.58159196376801 -6.22823619842529 -1 -1.45314145088196 -6.66515254974365 -1 -1.20679974555969 -7.04818964004517 -1 -0.862526774406433 -7.3462929725647 -1 -0.448208212852478 -7.53534746170044 -1 0.00258397660218179 -7.59999990463257 -1 0.453339636325836 -7.5350284576416 -1 0.86752462387085 -7.34571504592896 -1 1.21159505844116 -7.0473575592041 -1 1.45767974853516 -6.66416215896606 -1 1.58583211898804 -6.22716999053955 -1 1.58568120002747 -5.77175188064575 -1 1.45723056793213 -5.33483600616455 -1 1.21088671684265 -4.95181369781494 -1 0.866608440876007 -4.65369415283203 -0.999999046325684 0.452295154333115 -4.46465587615967 -1 -0.00259024300612509 7.59999132156372 -1 -0.453335762023926 7.53503561019897 -1 -0.867523074150085 7.34570741653442 -1 -1.21159875392914 7.04736566543579 -1 -1.45767593383789 6.66416931152344 -1 -1.58583080768585 6.22715997695923 -1 -1.58567702770233 5.77176094055176 -1 -1.45722675323486 5.33484315872192 -1 -1.21088480949402 4.95180749893188 -1 -0.866604566574097 4.6537013053894 -1 -0.452291309833527 4.46466302871704 -1 -0.00150114693678916 4.39999723434448 -1 0.449254244565964 4.46496725082397 -1 0.863439500331879 4.6542820930481 -1 1.20751750469208 4.95263862609863 -1 1.45359468460083 5.33583498001099 -1 1.58174705505371 5.7728271484375 -1 1.58159554004669 6.22824144363403 -1 1.4531455039978 6.66516160964966 -1 1.20680105686188 7.048180103302 -1 0.862523376941681 7.34630298614502 -1 0.448209762573242 7.53533935546875 -1 -5.00112390518188 54.4149703979492 -1 -4.77711343765259 55.5344047546387 -1 -4.30501127243042 56.5738487243652 -1 -3.60943269729614 57.479118347168 -1 -2.72664713859558 58.2029991149902 -1 -1.70266759395599 58.7077713012695 -1 -0.590879201889038 58.9671173095703 -1 0.550749897956848 58.9675064086914 -1 1.66271591186523 58.7089195251465 -1 2.68703508377075 58.2048416137695 -1 3.570317029953 57.4815635681152 -1 4.26651239395142 56.5767669677734 -1 4.73931694030762 55.5376472473145 -1.00000095367432 4.96409463882446 54.4183540344238 -1 -9.4672212600708 0.788443863391876 -1 9.46668720245361 0.794882953166962 -1 9.47415924072266 -0.7002312541008 -1 9.24696636199951 -2.17799186706543 -1 8.79073047637939 -3.60182166099548 -1 8.11675262451172 -4.93642473220825 -1 7.24173021316528 -6.14877128601074 -1 6.18733739852905 -7.20880699157715 -1 4.97968578338623 -8.09028625488281 -1 3.64869427680969 -8.77136898040771 -1 2.22732663154602 -9.23519611358643 -1 0.750785827636719 -9.47028827667236 -1 -0.744346857070923 -9.47079849243164 -1 -2.22103977203369 -9.23671817779541 -1 -3.64272499084473 -8.77385807037354 -1 -4.97418165206909 -8.09366703033447 -1 -6.1824312210083 -7.21301126480103 -1 -7.23754644393921 -6.15369606018066 -1 -8.1133918762207 -4.9419584274292 -0.999999046325684 -8.7882719039917 -3.60779309272766 -1 -9.24547576904297 -2.18428778648376 -1 -9.47367763519287 -0.706677734851837 -1 0.00105118355713785 -3.10000014305115 -1 0.874383151531219 -2.97412943840027 -1 1.6768741607666 -2.6073169708252 -1 2.34351491928101 -2.0292706489563 -1 2.82029914855957 -1.28682899475098 -1 3.06859874725342 -0.440132558345795 -1 3.06829881668091 0.442225694656372 -1.00000095367432 2.81942296028137 1.2887407541275 -1 2.34213423728943 2.03086352348328 -1 1.67509973049164 2.60845589637756 -1 0.872355878353119 2.97472190856934 -1 -0.00105518498457968 3.10000705718994 -1 -0.874379277229309 2.97413730621338 -1 -1.67687284946442 2.60730767250061 -0.999999046325684 -2.3435115814209 2.0292763710022 -1 -2.82029795646667 1.28681969642639 -1 -3.06859540939331 0.440138220787048 -1 -3.0682954788208 -0.442220002412796 -1 -2.81942176818848 -1.28875005245209 -1 -2.34213304519653 -2.03087282180786 -1 -1.67509615421295 -2.60844993591309 -1.00000095367432 -0.87236213684082 -2.97473001480103 -1 0.00149547657929361 -4.40000200271606 2.47955322265625E-05 0.00149547657929361 -4.40000200271606 -1 0.00258397660218179 -7.59999990463257 2.38418579101563E-05 0.00258397660218179 -7.59999990463257 2.38418579101563E-05 -0.448205888271332 -7.53533220291138 2.47955322265625E-05 -0.862519264221191 -7.34629392623901 2.38418579101563E-05 -1.20679974555969 -7.04818964004517 2.38418579101563E-05 -1.45313382148743 -6.66515350341797 2.47955322265625E-05 -1.58158433437347 -6.22823715209961 2.38418579101563E-05 -1.58174574375153 -5.77283716201782 2.38418579101563E-05 -1.45359063148499 -5.33582592010498 2.47955322265625E-05 -1.2075058221817 -4.95263051986694 2.38418579101563E-05 -0.863437652587891 -4.65428781509399 2.47955322265625E-05 -0.449252665042877 -4.46497488021851 -0.333317756652832 0.00149601302109659 -4.3999981880188 -0.666658401489258 0.00149660906754434 -4.39999437332153 -1 -0.449250400066376 -4.4649600982666 -1 -0.863437652587891 -4.65428781509399 -1 -1.20751333236694 -4.95262956619263 -1 -1.45359814167023 -5.33582448959351 -1 -1.58175337314606 -5.77283620834351 -1 -1.58159196376801 -6.22823619842529 -1 -1.45314145088196 -6.66515254974365 -1 -1.20679974555969 -7.04818964004517 -1 -0.862526774406433 -7.3462929725647 -1 -0.448208212852478 -7.53534746170044 -0.666657447814941 0.0025856455322355 -7.59998893737793 -0.333316802978516 0.00258510909043252 -7.5999927520752 -0.499990463256836 -0.610754013061523 -4.52199697494507 -0.499991416931152 -1.47637498378754 -5.38820791244507 -0.499991416931152 -1.59795439243317 -6.00053834915161 -0.499991416931152 -1.47595775127411 -6.61280345916748 -0.499991416931152 -1.128946185112 -7.13175392150879 -0.499991416931152 -0.609750270843506 -7.47842025756836 2.47955322265625E-05 0.00149547657929361 -4.40000200271606 1.00001430511475 0.00115829310379922 -3.40005254745483 1.00001430511475 -2.5979061126709 -6.00089073181152 2.38418579101563E-05 0.00258397660218179 -7.59999990463257 1.00001430511475 0.0029286106582731 -8.59994983673096 2.47955322265625E-05 -0.449252665042877 -4.46497488021851 2.38418579101563E-05 -0.863437652587891 -4.65428781509399 2.47955322265625E-05 -1.2075058221817 -4.95263051986694 2.38418579101563E-05 -1.45359063148499 -5.33582592010498 2.38418579101563E-05 -1.58174574375153 -5.77283716201782 2.47955322265625E-05 -1.58158433437347 -6.22823715209961 2.38418579101563E-05 -1.45313382148743 -6.66515350341797 2.38418579101563E-05 -1.20679974555969 -7.04818964004517 2.47955322265625E-05 -0.862519264221191 -7.34629392623901 2.38418579101563E-05 -0.448205888271332 -7.53533220291138 1.00001430511475 -0.800546765327454 -8.47296714782715 1.00001430511475 -1.52545201778412 -8.10392284393311 1.00001430511475 -2.10083818435669 -7.52891874313354 1.00001525878906 -2.47038173675537 -6.80426597595215 1.00001525878906 -2.47093033790588 -5.1974139213562 1.00001525878906 -2.10188174247742 -4.47250556945801 1.00001430511475 -1.52688944339752 -3.89712142944336 1.00001525878906 -0.80222761631012 -3.52757120132446 0.500019073486328 0.00133066973648965 -3.90002775192261 0.500019073486328 0.00275629363022745 -8.09997463226318 2.38418579101563E-05 0.00258397660218179 -7.59999990463257 1.00001430511475 0.0029286106582731 -8.59994983673096 2.47955322265625E-05 0.00149547657929361 -4.40000200271606 0.500019073486328 0.00133066973648965 -3.90002775192261 0.500019073486328 0.00275629363022745 -8.09997463226318 1.00001430511475 0.00115829310379922 -3.40005254745483 1.00001525878906 0.804628193378448 -3.52702140808105 1.00001430511475 1.52953958511353 -3.89607644081116 1.00001525878906 2.10492777824402 -4.47106552124023 1.00001525878906 2.47446918487549 -5.19573354721069 1.00001430511475 2.60199284553528 -5.99911260604858 1.00001525878906 2.47501707077026 -6.80258941650391 1.00001525878906 2.1059684753418 -7.5274977684021 1.00001430511475 1.53096890449524 -8.10288047790527 1.00001430511475 0.806307077407837 -8.47243022918701 2.47955322265625E-05 0.453332096338272 -7.53502702713013 2.38418579101563E-05 0.867519319057465 -7.34569883346558 2.38418579101563E-05 1.21159505844116 -7.0473575592041 2.47955322265625E-05 1.45767223834991 -6.66416120529175 2.38418579101563E-05 1.5858246088028 -6.22716856002808 2.38418579101563E-05 1.58567309379578 -5.7717547416687 2.38418579101563E-05 1.45722079277039 -5.33484983444214 2.38418579101563E-05 1.21087920665741 -4.95181226730347 2.47955322265625E-05 0.866606175899506 -4.65370941162109 2.38418579101563E-05 0.452287614345551 -4.46465492248535 -1 0.00258397660218179 -7.59999990463257 2.38418579101563E-05 0.00258397660218179 -7.59999990463257 -1 0.00149547657929361 -4.40000200271606 2.47955322265625E-05 0.00149547657929361 -4.40000200271606 -0.666658401489258 0.00149660906754434 -4.39999437332153 -0.333317756652832 0.00149601302109659 -4.3999981880188 -0.333316802978516 0.00258510909043252 -7.5999927520752 -0.666657447814941 0.0025856455322355 -7.59998893737793 2.38418579101563E-05 0.452287614345551 -4.46465492248535 2.47955322265625E-05 0.866606175899506 -4.65370941162109 2.38418579101563E-05 1.21087920665741 -4.95181226730347 2.38418579101563E-05 1.45722079277039 -5.33484983444214 2.38418579101563E-05 1.58567309379578 -5.7717547416687 2.38418579101563E-05 1.5858246088028 -6.22716856002808 2.47955322265625E-05 1.45767223834991 -6.66416120529175 2.38418579101563E-05 1.21159505844116 -7.0473575592041 2.38418579101563E-05 0.867519319057465 -7.34569883346558 2.47955322265625E-05 0.453332096338272 -7.53502702713013 -1 0.453339636325836 -7.5350284576416 -1 0.86752462387085 -7.34571504592896 -1 1.21159505844116 -7.0473575592041 -1 1.45767974853516 -6.66416215896606 -1 1.58583211898804 -6.22716999053955 -1 1.58568120002747 -5.77175188064575 -1 1.45723056793213 -5.33483600616455 -1 1.21088671684265 -4.95181369781494 -1 0.866608440876007 -4.65369415283203 -0.999999046325684 0.452295154333115 -4.46465587615967 -0.499991416931152 0.614833414554596 -7.47800493240356 -0.499991416931152 1.48045444488525 -6.61179399490356 -0.499990463256836 1.60204362869263 -5.99944972991943 -0.499990463256836 1.48004472255707 -5.38719987869263 -0.499991416931152 1.13302564620972 -4.868248462677 -0.499990463256836 0.613829612731934 -4.52158212661743 -1 -0.00259024300612509 7.59999132156372 2.38418579101563E-05 -0.00258267321623862 7.5999903678894 -1 -0.00150114693678916 4.39999723434448 2.38418579101563E-05 -0.00149888196028769 4.40001201629639 2.38418579101563E-05 -0.452291309833527 4.46466302871704 2.47955322265625E-05 -0.866604566574097 4.6537013053894 2.47955322265625E-05 -1.21087503433228 4.95182132720947 2.47955322265625E-05 -1.45721924304962 5.33484172821045 2.47955322265625E-05 -1.5856693983078 5.77175998687744 2.38418579101563E-05 -1.5858234167099 6.22715902328491 2.38418579101563E-05 -1.45766842365265 6.66416835784912 2.38418579101563E-05 -1.21159112453461 7.04736471176147 2.38418579101563E-05 -0.867523074150085 7.34570741653442 2.38418579101563E-05 -0.453338086605072 7.53502035140991 -0.333317756652832 -0.00258183875121176 7.5999960899353 -0.666658401489258 -0.00258851447142661 7.60000276565552 -1 -0.453335762023926 7.53503561019897 -1 -0.867523074150085 7.34570741653442 -1 -1.21159875392914 7.04736566543579 -1 -1.45767593383789 6.66416931152344 -1 -1.58583080768585 6.22715997695923 -1 -1.58567702770233 5.77176094055176 -1 -1.45722675323486 5.33484315872192 -1 -1.21088480949402 4.95180749893188 -1 -0.866604566574097 4.6537013053894 -1 -0.452291309833527 4.46466302871704 -0.666658401489258 -0.00149971642531455 4.40000677108765 -0.333316802978516 -0.00150025286711752 4.40000295639038 -0.499991416931152 -0.614839673042297 7.47799634933472 -0.499991416931152 -1.48046004772186 6.61178922653198 -0.499991416931152 -1.60203969478607 5.99945688247681 -0.499991416931152 -1.48004066944122 5.38720893859863 -0.499991416931152 -1.13302409648895 4.86824035644531 -0.499991416931152 -0.61383330821991 4.52159023284912 2.38418579101563E-05 -0.00258267321623862 7.5999903678894 1.00001430511475 -0.00292677083052695 8.59994506835938 2.38418579101563E-05 -0.00149888196028769 4.40001201629639 2.38418579101563E-05 -0.453338086605072 7.53502035140991 2.38418579101563E-05 -0.867523074150085 7.34570741653442 2.38418579101563E-05 -1.21159112453461 7.04736471176147 2.38418579101563E-05 -1.45766842365265 6.66416835784912 2.38418579101563E-05 -1.5858234167099 6.22715902328491 2.47955322265625E-05 -1.5856693983078 5.77175998687744 2.47955322265625E-05 -1.45721924304962 5.33484172821045 2.47955322265625E-05 -1.21087503433228 4.95182132720947 2.47955322265625E-05 -0.866604566574097 4.6537013053894 2.38418579101563E-05 -0.452291309833527 4.46466302871704 1.00001430511475 -0.00115675129927695 3.40004515647888 1.00001525878906 -0.80463182926178 3.52703022956848 1.00001430511475 -1.5295375585556 3.89607262611389 1.00001430511475 -2.10492348670959 4.47107648849487 1.00001430511475 -2.47446727752686 5.19572973251343 1.00001430511475 -2.60198903083801 5.99912166595459 1.00001525878906 -2.47501540184021 6.80258321762085 1.00001525878906 -2.10596680641174 7.52749156951904 1.00001430511475 -1.5309671163559 8.10287475585938 1.00001430511475 -0.806312799453735 8.47242641448975 0.500019073486328 -0.00275475182570517 8.09996795654297 0.500019073486328 -0.00132877030409873 3.90002226829529 2.38418579101563E-05 -0.00149888196028769 4.40001201629639 1.00001430511475 -0.00115675129927695 3.40004515647888 1.00001430511475 2.59790778160095 6.00088453292847 2.38418579101563E-05 -0.00258267321623862 7.5999903678894 1.00001430511475 -0.00292677083052695 8.59994506835938 0.500019073486328 -0.00275475182570517 8.09996795654297 0.500019073486328 -0.00132877030409873 3.90002226829529 1.00001525878906 0.800543069839478 8.472975730896 1.00001430511475 1.52545392513275 8.10391712188721 1.00001430511475 2.10084247589111 7.52892971038818 1.00001525878906 2.47038602828979 6.80427694320679 1.00001430511475 2.47093200683594 5.1974081993103 1.00001430511475 2.10188341140747 4.47249937057495 1.00001430511475 1.52688348293304 3.89711499214172 1.00001430511475 0.80223149061203 3.52757906913757 2.47955322265625E-05 0.449254244565964 4.46496725082397 2.38418579101563E-05 0.86344176530838 4.65429735183716 2.38418579101563E-05 1.20750987529755 4.95264005661011 2.47955322265625E-05 1.45358717441559 5.3358359336853 2.38418579101563E-05 1.58173954486847 5.77282857894897 2.47955322265625E-05 1.58158791065216 6.22824239730835 2.47955322265625E-05 1.45313799381256 6.66516256332397 2.38418579101563E-05 1.20679342746735 7.04818105697632 2.38418579101563E-05 0.862521052360535 7.34628820419312 2.38418579101563E-05 0.448209762573242 7.53533935546875 -1 -0.00150114693678916 4.39999723434448 2.38418579101563E-05 -0.00149888196028769 4.40001201629639 -1 -0.00259024300612509 7.59999132156372 2.38418579101563E-05 -0.00258267321623862 7.5999903678894 -0.666658401489258 -0.00258851447142661 7.60000276565552 -0.333317756652832 -0.00258183875121176 7.5999960899353 -0.333316802978516 -0.00150025286711752 4.40000295639038 -0.666658401489258 -0.00149971642531455 4.40000677108765 2.38418579101563E-05 0.448209762573242 7.53533935546875 2.38418579101563E-05 0.862521052360535 7.34628820419312 2.38418579101563E-05 1.20679342746735 7.04818105697632 2.47955322265625E-05 1.45313799381256 6.66516256332397 2.47955322265625E-05 1.58158791065216 6.22824239730835 2.38418579101563E-05 1.58173954486847 5.77282857894897 2.47955322265625E-05 1.45358717441559 5.3358359336853 2.38418579101563E-05 1.20750987529755 4.95264005661011 2.38418579101563E-05 0.86344176530838 4.65429735183716 2.47955322265625E-05 0.449254244565964 4.46496725082397 -1 0.449254244565964 4.46496725082397 -1 0.863439500331879 4.6542820930481 -1 1.20751750469208 4.95263862609863 -1 1.45359468460083 5.33583498001099 -1 1.58174705505371 5.7728271484375 -1 1.58159554004669 6.22824144363403 -1 1.4531455039978 6.66516160964966 -1 1.20680105686188 7.048180103302 -1 0.862523376941681 7.34630298614502 -1 0.448209762573242 7.53533935546875 -0.499991416931152 0.610758125782013 4.52200603485107 -0.499991416931152 1.4763787984848 5.38821506500244 -0.499990463256836 1.5979585647583 6.00054740905762 -0.499990463256836 1.47595906257629 6.61279344558716 -0.499991416931152 1.12894248962402 7.13176202774048 -0.499990463256836 0.609752058982849 7.4784140586853 -1.00000095367432 4.96409463882446 54.4183540344238 1 -5.0011248588562 54.4149627685547 1 -4.77711343765259 55.5344047546387 1 -4.30501079559326 56.5738525390625 1 -3.60943269729614 57.479118347168 1 -2.72664713859558 58.2029991149902 1 -1.70266759395599 58.7077713012695 1 -0.590879678726196 58.967113494873 1 0.550749897956848 58.9675064086914 1 1.66271591186523 58.7089195251465 1 2.68703508377075 58.2048416137695 1 3.570317029953 57.4815635681152 1 4.26651239395142 56.5767669677734 1 4.73931694030762 55.5376472473145 -1 -5.00112390518188 54.4149703979492 0 -5.00112390518188 54.4149703979492 -1 4.73931694030762 55.5376472473145 -1 4.26651239395142 56.5767669677734 -1 3.570317029953 57.4815635681152 -1 2.68703508377075 58.2048416137695 -1 1.66271591186523 58.7089195251465 -1 0.550749897956848 58.9675064086914 -1 -0.590879201889038 58.9671173095703 -1 -1.70266759395599 58.7077713012695 -1 -2.72664713859558 58.2029991149902 -1 -3.60943269729614 57.479118347168 -1 -4.30501127243042 56.5738487243652 -1 -4.77711343765259 55.5344047546387 1 4.96409511566162 54.4183578491211 0 4.96409511566162 54.4183578491211 -1 -9.4672212600708 0.788443863391876 1 -9.46722030639648 0.788445770740509 -1 -5.00112390518188 54.4149703979492 1 -5.0011248588562 54.4149627685547 0 -5.00112390518188 54.4149703979492 0 -9.4672212600708 0.788443863391876 1 9.46668720245361 0.794884860515594 -1 9.46668720245361 0.794882953166962 1 -9.46722030639648 0.788445770740509 -1 -9.4672212600708 0.788443863391876 1 9.47415924072266 -0.7002312541008 1 9.24696445465088 -2.17800498008728 1 8.79073047637939 -3.60182166099548 1.00000095367432 8.11675262451172 -4.93642091751099 1 7.24173069000244 -6.14876747131348 1 6.18733739852905 -7.20880699157715 1 4.97968578338623 -8.09028625488281 1 3.64869427680969 -8.77136898040771 0.999999046325684 2.22732424736023 -9.23521137237549 1 0.750786423683167 -9.4702844619751 1 -0.744346857070923 -9.47079849243164 1 -2.22103977203369 -9.23671817779541 1 -3.64272499084473 -8.77385807037354 1 -4.97418165206909 -8.09366703033447 1 -6.18243026733398 -7.21300745010376 1 -7.23754644393921 -6.15369606018066 1 -8.1133918762207 -4.9419584274292 1 -8.78827476501465 -3.60780835151672 1 -9.24547576904297 -2.18428778648376 1 -9.47367668151855 -0.706675827503204 0 9.46668720245361 0.794884860515594 -1 -9.47367763519287 -0.706677734851837 -1 -9.24547576904297 -2.18428778648376 -0.999999046325684 -8.7882719039917 -3.60779309272766 -1 -8.1133918762207 -4.9419584274292 -1 -7.23754644393921 -6.15369606018066 -1 -6.1824312210083 -7.21301126480103 -1 -4.97418165206909 -8.09366703033447 -1 -3.64272499084473 -8.77385807037354 -1 -2.22103977203369 -9.23671817779541 -1 -0.744346857070923 -9.47079849243164 -1 0.750785827636719 -9.47028827667236 -1 2.22732663154602 -9.23519611358643 -1 3.64869427680969 -8.77136898040771 -1 4.97968578338623 -8.09028625488281 -1 6.18733739852905 -7.20880699157715 -1 7.24173021316528 -6.14877128601074 -1 8.11675262451172 -4.93642473220825 -1 8.79073047637939 -3.60182166099548 -1 9.24696636199951 -2.17799186706543 -1 9.47415924072266 -0.7002312541008 0 -9.4672212600708 0.788443863391876 -1.00000095367432 4.96409463882446 54.4183540344238 1 4.96409511566162 54.4183578491211 -1 9.46668720245361 0.794882953166962 1 9.46668720245361 0.794884860515594 0 4.96409511566162 54.4183578491211 0 9.46668720245361 0.794884860515594 -1 0.00105118355713785 -3.10000014305115 1 0.00105148158036172 -3.09999823570251 -1 -0.00105518498457968 3.10000705718994 0.999999046325684 -0.00105744996108115 3.09999179840088 1 0.872356116771698 2.97472381591797 1 1.67510008811951 2.60845756530762 1 2.34213447570801 2.03086543083191 1 2.81942296028137 1.2887407541275 1 3.06829690933228 0.442212462425232 1 3.068598985672 -0.440130680799484 1 2.82029938697815 -1.28682708740234 1 2.34351515769959 -2.02926874160767 1 1.67687439918518 -2.60731506347656 1.00000095367432 0.874383151531219 -2.97412943840027 0 0.00105148158036172 -3.09999823570251 -1 0.874383151531219 -2.97412943840027 -1 1.6768741607666 -2.6073169708252 -1 2.34351491928101 -2.0292706489563 -1 2.82029914855957 -1.28682899475098 -1 3.06859874725342 -0.440132558345795 -1 3.06829881668091 0.442225694656372 -1.00000095367432 2.81942296028137 1.2887407541275 -1 2.34213423728943 2.03086352348328 -1 1.67509973049164 2.60845589637756 -1 0.872355878353119 2.97472190856934 -9.5367431640625E-07 -0.00105744996108115 3.09999179840088 -1 -0.00105518498457968 3.10000705718994 0.999999046325684 -0.00105744996108115 3.09999179840088 -1 0.00105118355713785 -3.10000014305115 1 0.00105148158036172 -3.09999823570251 0 0.00105148158036172 -3.09999823570251 -9.5367431640625E-07 -0.00105744996108115 3.09999179840088 1 -0.87236213684082 -2.97473001480103 1 -1.67509853839874 -2.60846519470215 1 -2.34213280677795 -2.03087115287781 1 -2.8194215297699 -1.28874814510345 1 -3.06829524040222 -0.442218124866486 1 -3.06859755516052 0.44012314081192 0.999999046325684 -2.8202977180481 1.28682160377502 1 -2.34351348876953 2.02926301956177 1 -1.67687284946442 2.60730767250061 1 -0.874381542205811 2.97412204742432 -1 -0.874379277229309 2.97413730621338 -1 -1.67687284946442 2.60730767250061 -0.999999046325684 -2.3435115814209 2.0292763710022 -1 -2.82029795646667 1.28681969642639 -1 -3.06859540939331 0.440138220787048 -1 -3.0682954788208 -0.442220002412796 -1 -2.81942176818848 -1.28875005245209 -1 -2.34213304519653 -2.03087282180786 -1 -1.67509615421295 -2.60844993591309 -1.00000095367432 -0.87236213684082 -2.97473001480103 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 0.382998 -0.923749 0.000000 1.000000 0.000340 0.000000 1.000000 0.000340 0.000000 0.923749 0.382998 0.000000 0.923749 0.382998 0.000000 0.382369 0.924010 0.000000 0.382369 0.924010 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 0.706866 0.707347 0.000000 0.924010 -0.382369 0.000000 0.707347 -0.706866 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 0.382998 -0.923749 0.000000 0.707347 -0.706866 0.000000 0.924010 -0.382369 0.000000 0.706866 0.707347 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 0.382998 -0.923749 0.000000 0.924010 -0.382369 0.000000 1.000000 0.000340 0.000000 0.923749 0.382998 0.000000 0.706866 0.707347 0.000000 0.382369 0.924010 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 -0.382998 0.923749 0.000000 -1.000000 -0.000340 0.000000 -1.000000 -0.000340 0.000000 -0.923749 -0.382998 0.000000 -0.923749 -0.382998 0.000000 -0.382369 -0.924010 0.000000 -0.382369 -0.924010 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 -0.706866 -0.707347 0.000000 -0.924010 0.382369 0.000000 -0.707347 0.706866 0.000000 -0.382998 0.923749 0.000000 -0.707347 0.706866 0.000000 -0.924010 0.382369 0.000000 -0.706866 -0.707347 0.000000 -0.382998 0.923749 0.000000 -0.924010 0.382369 0.000000 -1.000000 -0.000340 0.000000 -0.923749 -0.382998 0.000000 -0.706866 -0.707347 0.000000 -0.382369 -0.924010 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 0.281406 0.959589 0.000000 0.540355 0.841437 0.000000 0.755527 0.655118 0.000000 0.909491 0.415725 0.000000 0.989773 0.142652 0.000000 0.989870 -0.141978 0.000000 0.909773 -0.415105 0.000000 0.755972 -0.654604 0.000000 0.540927 -0.841070 0.000000 0.282059 -0.959397 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 0.282059 -0.959397 0.000000 0.540927 -0.841070 0.000000 0.755972 -0.654604 0.000000 0.909773 -0.415105 0.000000 0.989870 -0.141978 0.000000 0.989773 0.142652 0.000000 0.909491 0.415725 0.000000 0.755527 0.655118 0.000000 0.540355 0.841437 0.000000 0.281406 0.959589 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 0.382998 -0.923749 0.000000 0.924010 -0.382369 0.000000 1.000000 0.000340 0.000000 0.923749 0.382998 0.000000 0.706866 0.707347 0.000000 0.382369 0.924010 0.707097 0.000241 -0.707117 0.707097 0.000241 -0.707117 0.707097 0.707117 0.000241 0.707097 -0.000241 0.707117 0.707097 -0.000241 0.707117 0.707097 0.199449 -0.678406 0.707097 0.382499 -0.594734 0.707097 0.534561 -0.462881 0.707097 0.643316 -0.293528 0.707097 0.699953 -0.100395 0.707097 0.699885 0.100871 0.707097 0.643116 0.293966 0.707097 0.534246 0.463245 0.707097 0.382094 0.594994 0.707097 0.198987 0.678541 0.707097 0.218282 0.672582 0.707097 0.415438 0.572211 0.707097 0.571928 0.415827 0.707097 0.672433 0.218740 0.707097 0.672582 -0.218282 0.707097 0.572211 -0.415438 0.707097 0.415827 -0.571928 0.707097 0.218740 -0.672433 0.707097 0.000241 -0.707117 0.707097 -0.000241 0.707117 0.707097 -0.000241 0.707117 0.707097 -0.000241 0.707117 0.707097 0.000241 -0.707117 0.707097 0.000241 -0.707117 0.707097 -0.000241 0.707117 0.707097 0.000241 -0.707117 0.707097 -0.218282 -0.672582 0.707097 -0.415438 -0.572211 0.707097 -0.571928 -0.415827 0.707097 -0.672433 -0.218740 0.707097 -0.707117 -0.000241 0.707097 -0.672582 0.218282 0.707097 -0.572211 0.415438 0.707097 -0.415827 0.571928 0.707097 -0.218740 0.672433 0.707097 -0.199449 0.678406 0.707097 -0.382499 0.594734 0.707097 -0.534561 0.462881 0.707097 -0.643316 0.293528 0.707097 -0.699953 0.100395 0.707097 -0.699885 -0.100871 0.707097 -0.643116 -0.293966 0.707097 -0.534245 -0.463245 0.707097 -0.382094 -0.594994 0.707097 -0.198987 -0.678541 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 -0.281406 -0.959589 0.000000 -0.540355 -0.841437 0.000000 -0.755527 -0.655118 0.000000 -0.909491 -0.415725 0.000000 -0.989773 -0.142652 0.000000 -0.989870 0.141978 0.000000 -0.909773 0.415105 0.000000 -0.755972 0.654604 0.000000 -0.540927 0.841070 0.000000 -0.282059 0.959397 0.000000 -0.282059 0.959397 0.000000 -0.540927 0.841070 0.000000 -0.755972 0.654604 0.000000 -0.909773 0.415105 0.000000 -0.989870 0.141978 0.000000 -0.989773 -0.142652 0.000000 -0.909491 -0.415725 0.000000 -0.755527 -0.655118 0.000000 -0.540355 -0.841437 0.000000 -0.281406 -0.959589 0.000000 -0.382998 0.923749 0.000000 -0.924010 0.382369 0.000000 -1.000000 -0.000340 0.000000 -0.923749 -0.382998 0.000000 -0.706866 -0.707347 0.000000 -0.382369 -0.924010 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 0.281406 0.959589 0.000000 0.540355 0.841437 0.000000 0.755527 0.655118 0.000000 0.909491 0.415725 0.000000 0.989773 0.142652 0.000000 0.989870 -0.141978 0.000000 0.909773 -0.415105 0.000000 0.755972 -0.654604 0.000000 0.540927 -0.841070 0.000000 0.282059 -0.959397 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 0.282059 -0.959397 0.000000 0.540927 -0.841070 0.000000 0.755972 -0.654604 0.000000 0.909773 -0.415105 0.000000 0.989870 -0.141978 0.000000 0.989773 0.142652 0.000000 0.909491 0.415725 0.000000 0.755527 0.655118 0.000000 0.540355 0.841437 0.000000 0.281406 0.959589 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 0.382998 -0.923749 0.000000 0.924010 -0.382369 0.000000 1.000000 0.000340 0.000000 0.923749 0.382998 0.000000 0.706866 0.707347 0.000000 0.382369 0.924010 0.707097 0.000241 -0.707117 0.707097 0.000241 -0.707117 0.707097 -0.000241 0.707117 0.707097 0.199449 -0.678406 0.707097 0.382499 -0.594734 0.707097 0.534561 -0.462881 0.707097 0.643316 -0.293528 0.707097 0.699953 -0.100395 0.707097 0.699885 0.100871 0.707097 0.643116 0.293966 0.707097 0.534246 0.463245 0.707097 0.382094 0.594994 0.707097 0.198987 0.678541 0.707097 -0.000241 0.707117 0.707097 0.218282 0.672582 0.707097 0.415438 0.572211 0.707097 0.571928 0.415827 0.707097 0.672433 0.218740 0.707097 0.707117 0.000241 0.707097 0.672582 -0.218282 0.707097 0.572211 -0.415438 0.707097 0.415827 -0.571928 0.707097 0.218740 -0.672433 0.707097 0.000241 -0.707117 0.707097 -0.000241 0.707117 0.707097 -0.000241 0.707117 0.707097 -0.000241 0.707117 0.707097 -0.707117 -0.000241 0.707097 0.000241 -0.707117 0.707097 0.000241 -0.707117 0.707097 0.000241 -0.707117 0.707097 -0.000241 0.707117 0.707097 -0.218282 -0.672582 0.707097 -0.415438 -0.572211 0.707097 -0.571928 -0.415827 0.707097 -0.672433 -0.218740 0.707097 -0.672582 0.218282 0.707097 -0.572211 0.415438 0.707097 -0.415827 0.571928 0.707097 -0.218740 0.672433 0.707097 -0.199449 0.678406 0.707097 -0.382499 0.594734 0.707097 -0.534561 0.462881 0.707097 -0.643316 0.293528 0.707097 -0.699953 0.100395 0.707097 -0.699885 -0.100871 0.707097 -0.643116 -0.293966 0.707097 -0.534245 -0.463245 0.707097 -0.382094 -0.594994 0.707097 -0.198987 -0.678541 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 -0.281406 -0.959589 0.000000 -0.540355 -0.841437 0.000000 -0.755527 -0.655118 0.000000 -0.909491 -0.415725 0.000000 -0.989773 -0.142652 0.000000 -0.989870 0.141978 0.000000 -0.909773 0.415105 0.000000 -0.755972 0.654604 0.000000 -0.540927 0.841070 0.000000 -0.282059 0.959397 0.000000 -0.282059 0.959397 0.000000 -0.540927 0.841070 0.000000 -0.755972 0.654604 0.000000 -0.909773 0.415105 0.000000 -0.989870 0.141978 0.000000 -0.989773 -0.142652 0.000000 -0.909491 -0.415725 0.000000 -0.755527 -0.655118 0.000000 -0.540355 -0.841437 0.000000 -0.281406 -0.959589 0.000000 -0.382998 0.923749 0.000000 -0.924010 0.382369 0.000000 -1.000000 -0.000340 0.000000 -0.923749 -0.382998 0.000000 -0.706866 -0.707347 0.000000 -0.382369 -0.924010 0.000000 0.996493 0.083672 0.000000 -0.996550 0.082994 0.000000 -0.951748 0.306882 0.000000 -0.857327 0.514771 0.000000 -0.718212 0.695824 0.000000 -0.541655 0.840601 0.000000 -0.336859 0.941555 0.000000 -0.114501 0.993423 0.000000 0.113825 0.993501 0.000000 0.336218 0.941784 0.000000 0.541082 0.840970 0.000000 0.717739 0.696313 0.000000 0.856977 0.515355 0.000000 0.951538 0.307530 0.000000 -0.996550 0.082994 0.000000 -0.996550 0.082994 0.000000 0.951538 0.307530 0.000000 0.856977 0.515355 0.000000 0.717739 0.696313 0.000000 0.541082 0.840970 0.000000 0.336218 0.941784 0.000000 0.113825 0.993501 0.000000 -0.114501 0.993423 0.000000 -0.336859 0.941555 0.000000 -0.541655 0.840601 0.000000 -0.718212 0.695824 0.000000 -0.857327 0.514771 0.000000 -0.951748 0.306882 0.000000 0.996493 0.083672 0.000000 0.996493 0.083672 0.000000 -0.996550 0.082994 0.000000 -0.996550 0.082994 0.000000 -0.996550 0.082994 0.000000 -0.996550 0.082994 0.000000 -0.996550 0.082994 0.000000 -0.996550 0.082994 0.000000 0.996493 0.083672 0.000000 0.996493 0.083672 0.000000 -0.996550 0.082994 0.000000 -0.996550 0.082994 0.000000 0.997280 -0.073708 0.000000 0.973365 -0.229263 0.000000 0.925340 -0.379139 0.000000 0.854395 -0.519624 0.000000 0.762287 -0.647239 0.000000 0.651299 -0.758822 0.000000 0.524178 -0.851609 0.000000 0.384073 -0.923303 0.000000 0.234455 -0.972127 0.000000 0.079030 -0.996872 0.000000 -0.078352 -0.996926 0.000000 -0.233794 -0.972286 0.000000 -0.383445 -0.923564 0.000000 -0.523598 -0.851965 0.000000 -0.650782 -0.759265 0.000000 -0.761847 -0.647757 0.000000 -0.854041 -0.520206 0.000000 -0.925081 -0.379769 0.000000 -0.973208 -0.229925 0.000000 -0.997229 -0.074387 0.000000 0.996493 0.083672 0.000000 -0.997229 -0.074387 0.000000 -0.973208 -0.229925 0.000000 -0.925081 -0.379769 0.000000 -0.854041 -0.520206 0.000000 -0.761847 -0.647757 0.000000 -0.650782 -0.759265 0.000000 -0.523598 -0.851965 0.000000 -0.383445 -0.923564 0.000000 -0.233794 -0.972286 0.000000 -0.078352 -0.996926 0.000000 0.079030 -0.996872 0.000000 0.234455 -0.972127 0.000000 0.384073 -0.923303 0.000000 0.524178 -0.851609 0.000000 0.651299 -0.758822 0.000000 0.762287 -0.647239 0.000000 0.854395 -0.519624 0.000000 0.925340 -0.379139 0.000000 0.973365 -0.229263 0.000000 0.997280 -0.073708 0.000000 -0.996550 0.082994 0.000000 0.996493 0.083672 0.000000 0.996493 0.083672 0.000000 0.996493 0.083672 0.000000 0.996493 0.083672 0.000000 0.996493 0.083672 0.000000 0.996493 0.083672 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 -0.281406 -0.959589 0.000000 -0.540355 -0.841437 0.000000 -0.755527 -0.655118 0.000000 -0.909491 -0.415725 0.000000 -0.989773 -0.142652 0.000000 -0.989870 0.141978 0.000000 -0.909773 0.415105 0.000000 -0.755972 0.654604 0.000000 -0.540927 0.841070 0.000000 -0.282059 0.959397 0.000000 -0.000340 1.000000 0.000000 -0.282059 0.959397 0.000000 -0.540927 0.841070 0.000000 -0.755972 0.654604 0.000000 -0.909773 0.415105 0.000000 -0.989870 0.141978 0.000000 -0.989773 -0.142652 0.000000 -0.909491 -0.415725 0.000000 -0.755527 -0.655118 0.000000 -0.540355 -0.841437 0.000000 -0.281406 -0.959589 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 0.000340 -1.000000 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 -0.000340 1.000000 0.000000 0.000340 -1.000000 0.000000 0.281406 0.959589 0.000000 0.540355 0.841437 0.000000 0.755527 0.655118 0.000000 0.909491 0.415725 0.000000 0.989773 0.142652 0.000000 0.989870 -0.141978 0.000000 0.909773 -0.415105 0.000000 0.755972 -0.654604 0.000000 0.540927 -0.841070 0.000000 0.282059 -0.959397 0.000000 0.282059 -0.959397 0.000000 0.540927 -0.841070 0.000000 0.755972 -0.654604 0.000000 0.909773 -0.415105 0.000000 0.989870 -0.141978 0.000000 0.989773 0.142652 0.000000 0.909491 0.415725 0.000000 0.755527 0.655118 0.000000 0.540355 0.841437 0.000000 0.281406 0.959589 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 0.826922 0.873174 0.834065 0.871698 0.840120 0.867494 0.844166 0.861203 0.845587 0.853781 0.844166 0.846360 0.840120 0.840069 0.834065 0.835865 0.826922 0.834389 0.819778 0.835865 0.813723 0.840069 0.809677 0.846360 0.808256 0.853781 0.809677 0.861203 0.813723 0.867494 0.819778 0.871698 0.826922 0.158085 0.836294 0.156543 0.844750 0.152067 0.851460 0.145096 0.855768 0.136311 0.857253 0.126574 0.855768 0.116836 0.851460 0.108051 0.844750 0.101080 0.836294 0.096604 0.826922 0.095062 0.817549 0.096604 0.809093 0.101080 0.802383 0.108051 0.798075 0.116836 0.796590 0.126574 0.798075 0.136311 0.802383 0.145096 0.809093 0.152067 0.817549 0.156543 0.826922 0.303527 0.836294 0.301985 0.844750 0.297509 0.851460 0.290537 0.855768 0.281753 0.857253 0.272015 0.855768 0.262277 0.851460 0.253493 0.844750 0.246522 0.836294 0.242046 0.826922 0.240503 0.817549 0.242046 0.809093 0.246522 0.802383 0.253493 0.798075 0.262277 0.796590 0.272015 0.798075 0.281753 0.802383 0.290537 0.809093 0.297509 0.817549 0.301985 0.885049 0.858831 0.882431 0.872398 0.876920 0.884995 0.868801 0.895964 0.858500 0.904734 0.846552 0.910847 0.833581 0.913986 0.820262 0.913986 0.807291 0.910847 0.795343 0.904734 0.785042 0.895964 0.776923 0.884995 0.771412 0.872398 0.768794 0.858831 0.937364 0.208889 0.716479 0.208889 0.716398 0.190768 0.719054 0.172859 0.724382 0.155604 0.732250 0.139431 0.742463 0.124741 0.754768 0.111897 0.768860 0.101218 0.784390 0.092969 0.800974 0.087353 0.818200 0.084510 0.835643 0.084510 0.852869 0.087353 0.869453 0.092969 0.884983 0.101218 0.899075 0.111897 0.911380 0.124741 0.921593 0.139431 0.929461 0.155604 0.934789 0.172859 0.937445 0.190768 0.826922 0.161722 0.816733 0.163244 0.807369 0.167686 0.799590 0.174690 0.794025 0.183686 0.791125 0.193947 0.791125 0.204641 0.794025 0.214902 0.799590 0.223899 0.807369 0.230902 0.816733 0.235345 0.826922 0.236867 0.837110 0.235345 0.846474 0.230902 0.854253 0.223899 0.859818 0.214902 0.862718 0.204641 0.862718 0.193947 0.859818 0.183686 0.854253 0.174690 0.846474 0.167686 0.837110 0.163244 0.033064 0.560108 0.033064 0.601063 0.041330 0.601063 0.066129 0.560108 0.066129 0.601063 0.074395 0.560108 0.074395 0.601063 0.090927 0.560108 0.090927 0.601063 0.099193 0.560108 0.099193 0.601063 0.082661 0.601063 0.057863 0.601063 0.049596 0.601063 0.033064 0.587411 0.033064 0.573760 0.041330 0.560108 0.049596 0.560108 0.057863 0.560108 0.082661 0.560108 0.099193 0.573760 0.099193 0.587411 0.041330 0.580586 0.057863 0.580586 0.066129 0.580586 0.074395 0.580586 0.082661 0.580586 0.090927 0.580586 0.033064 0.375409 0.033064 0.416364 0.041330 0.416364 0.066129 0.375409 0.066129 0.416364 0.074395 0.375409 0.074395 0.416364 0.090927 0.375409 0.090927 0.416364 0.099193 0.375409 0.099193 0.416364 0.099193 0.389061 0.099193 0.402713 0.033064 0.402713 0.033064 0.389061 0.082661 0.416364 0.057863 0.416364 0.049596 0.416364 0.041330 0.375409 0.049596 0.375409 0.057863 0.375409 0.082661 0.375409 0.041330 0.395887 0.057863 0.395887 0.066129 0.395887 0.074395 0.395887 0.082661 0.395887 0.090927 0.395887 0.515360 0.873174 0.522503 0.871698 0.528559 0.867494 0.532605 0.861203 0.534026 0.853781 0.532605 0.846360 0.528559 0.840069 0.522503 0.835865 0.515360 0.834389 0.508217 0.835865 0.502161 0.840069 0.498115 0.846360 0.496694 0.853781 0.498115 0.861203 0.502161 0.867494 0.508217 0.871698 0.515360 0.145966 0.520619 0.145180 0.525452 0.142887 0.529467 0.139273 0.532339 0.134629 0.533836 0.129333 0.533836 0.123814 0.532339 0.118518 0.529467 0.113874 0.525452 0.110260 0.520619 0.107967 0.515360 0.107181 0.510101 0.107967 0.505269 0.110260 0.501253 0.113874 0.498381 0.118518 0.496884 0.123814 0.496884 0.129333 0.498381 0.134629 0.501253 0.139273 0.505269 0.142887 0.510101 0.145180 0.515360 0.291407 0.520619 0.290622 0.525452 0.288329 0.529467 0.284714 0.532339 0.280071 0.533836 0.274775 0.533836 0.269255 0.532339 0.263959 0.529467 0.259316 0.525452 0.255701 0.520619 0.253408 0.515360 0.252623 0.510101 0.253408 0.505269 0.255701 0.501253 0.259316 0.498381 0.263959 0.496884 0.269255 0.496884 0.274775 0.498381 0.280071 0.501253 0.284714 0.505269 0.288329 0.510101 0.290622 0.573488 0.858831 0.570870 0.872398 0.565358 0.884995 0.557240 0.895964 0.546938 0.904734 0.534991 0.910847 0.522019 0.913986 0.508701 0.913986 0.495730 0.910847 0.483782 0.904734 0.473480 0.895964 0.465362 0.884995 0.459850 0.872398 0.457232 0.858831 0.625803 0.208889 0.404917 0.208889 0.404836 0.190768 0.407492 0.172859 0.412821 0.155604 0.420689 0.139431 0.430901 0.124741 0.443206 0.111897 0.457298 0.101218 0.472829 0.092969 0.489412 0.087353 0.506639 0.084510 0.524081 0.084510 0.541308 0.087353 0.557891 0.092969 0.573422 0.101218 0.587514 0.111897 0.599819 0.124741 0.610031 0.139431 0.617899 0.155604 0.623228 0.172859 0.625884 0.190768 0.515360 0.161722 0.505171 0.163244 0.495808 0.167686 0.488028 0.174690 0.482463 0.183686 0.479563 0.193947 0.479563 0.204641 0.482463 0.214902 0.488028 0.223899 0.495808 0.230902 0.505171 0.235345 0.515360 0.236867 0.525549 0.235345 0.534912 0.230902 0.542692 0.223899 0.548257 0.214902 0.551157 0.204641 0.551157 0.193947 0.548257 0.183686 0.542692 0.174690 0.534912 0.167686 0.525549 0.163244 0.033064 0.096042 0.033064 0.115143 0.099193 0.096042 0.099193 0.115143 0.093181 0.115143 0.087169 0.115143 0.081158 0.115143 0.075146 0.115143 0.069134 0.115143 0.063123 0.115143 0.057111 0.115143 0.051099 0.115143 0.045088 0.115143 0.039076 0.115143 0.033064 0.108776 0.033064 0.102409 0.039076 0.096042 0.045088 0.096042 0.051099 0.096042 0.057111 0.096042 0.063123 0.096042 0.069134 0.096042 0.075146 0.096042 0.081158 0.096042 0.087169 0.096042 0.093181 0.096042 0.099193 0.102409 0.099193 0.108776 0.041330 0.105593 0.057863 0.105593 0.066129 0.105593 0.074395 0.105593 0.082661 0.105593 0.090927 0.105593 0.144348 0.033430 0.136496 0.033430 0.136496 0.066861 0.144348 0.100291 0.136496 0.100291 0.144348 0.039509 0.144348 0.045587 0.144348 0.051665 0.144348 0.057744 0.144348 0.063822 0.144348 0.069900 0.144348 0.075978 0.144348 0.082057 0.144348 0.088135 0.144348 0.094213 0.136496 0.093605 0.136496 0.086919 0.136496 0.080233 0.136496 0.073547 0.136496 0.060175 0.136496 0.053489 0.136496 0.046803 0.136496 0.040117 0.140422 0.033430 0.140422 0.100291 0.167538 0.033430 0.159685 0.033430 0.167538 0.100291 0.163612 0.100291 0.163612 0.033430 0.159685 0.100291 0.159685 0.093605 0.159685 0.086919 0.159685 0.080233 0.159685 0.073547 0.159685 0.066861 0.159685 0.060175 0.159685 0.053489 0.159685 0.046803 0.159685 0.040117 0.167538 0.039509 0.167538 0.045587 0.167538 0.051665 0.167538 0.057744 0.167538 0.063822 0.167538 0.069900 0.167538 0.075978 0.167538 0.082057 0.167538 0.088135 0.167538 0.094213 0.033064 0.003691 0.033064 0.022792 0.099193 0.003691 0.099193 0.022792 0.099193 0.010058 0.099193 0.016425 0.033064 0.016425 0.033064 0.010058 0.093181 0.022792 0.087169 0.022792 0.081158 0.022792 0.075146 0.022792 0.069134 0.022792 0.063123 0.022792 0.057111 0.022792 0.051099 0.022792 0.045088 0.022792 0.039076 0.022792 0.039076 0.003691 0.045088 0.003691 0.051099 0.003691 0.057111 0.003691 0.063123 0.003691 0.069134 0.003691 0.075146 0.003691 0.081158 0.003691 0.087169 0.003691 0.093181 0.003691 0.041330 0.013241 0.057863 0.013241 0.066129 0.013241 0.074395 0.013241 0.082661 0.013241 0.090927 0.013241 0.033064 0.280744 0.033064 0.299845 0.099193 0.280744 0.099193 0.299845 0.093181 0.299845 0.087169 0.299845 0.081158 0.299845 0.075146 0.299845 0.069134 0.299845 0.063123 0.299845 0.057111 0.299845 0.051099 0.299845 0.045088 0.299845 0.039076 0.299845 0.033064 0.293478 0.033064 0.287111 0.039076 0.280744 0.045088 0.280744 0.051099 0.280744 0.057111 0.280744 0.063123 0.280744 0.069134 0.280744 0.075146 0.280744 0.081158 0.280744 0.087169 0.280744 0.093181 0.280744 0.099193 0.287111 0.099193 0.293478 0.041330 0.290295 0.057863 0.290295 0.066129 0.290295 0.074395 0.290295 0.082661 0.290295 0.090927 0.290295 0.033064 0.751228 0.033064 0.758995 0.099193 0.751228 0.039076 0.751228 0.045088 0.751228 0.051099 0.751228 0.057111 0.751228 0.063123 0.751228 0.069134 0.751228 0.075146 0.751228 0.081158 0.751228 0.087169 0.751228 0.093181 0.751228 0.099193 0.758995 0.092580 0.758995 0.085967 0.758995 0.079354 0.758995 0.072741 0.758995 0.066129 0.758995 0.059516 0.758995 0.052903 0.758995 0.046290 0.758995 0.039677 0.758995 0.033064 0.755111 0.099193 0.755111 0.033064 0.881827 0.033064 0.889593 0.066129 0.889593 0.099193 0.881827 0.099193 0.889593 0.099193 0.885710 0.033064 0.885710 0.092580 0.889593 0.085967 0.889593 0.079354 0.889593 0.072741 0.889593 0.059516 0.889593 0.052903 0.889593 0.046290 0.889593 0.039677 0.889593 0.039076 0.881827 0.045088 0.881827 0.051099 0.881827 0.057111 0.881827 0.063123 0.881827 0.069134 0.881827 0.075146 0.881827 0.081158 0.881827 0.087169 0.881827 0.093181 0.881827 0.033064 0.188393 0.033064 0.207494 0.099193 0.188393 0.099193 0.207494 0.099193 0.194760 0.099193 0.201127 0.033064 0.201127 0.033064 0.194760 0.093181 0.207494 0.087169 0.207494 0.081158 0.207494 0.075146 0.207494 0.069134 0.207494 0.063123 0.207494 0.057111 0.207494 0.051099 0.207494 0.045088 0.207494 0.039076 0.207494 0.039076 0.188393 0.045088 0.188393 0.051099 0.188393 0.057111 0.188393 0.063123 0.188393 0.069134 0.188393 0.075146 0.188393 0.081158 0.188393 0.087169 0.188393 0.093181 0.188393 0.041330 0.197944 0.057863 0.197944 0.066129 0.197944 0.074395 0.197944 0.082661 0.197944 0.090927 0.197944 0.183820 0.122137 0.209179 0.003332 0.209179 0.012471 0.209179 0.021610 0.209179 0.030749 0.209179 0.039888 0.209179 0.049026 0.209179 0.058165 0.209179 0.067304 0.209179 0.076443 0.209179 0.085582 0.209179 0.094721 0.209179 0.103860 0.209179 0.112998 0.183820 0.003332 0.196499 0.003332 0.183820 0.112998 0.183820 0.103860 0.183820 0.094721 0.183820 0.085582 0.183820 0.076443 0.183820 0.067304 0.183820 0.058165 0.183820 0.049026 0.183820 0.039888 0.183820 0.030749 0.183820 0.021610 0.183820 0.012471 0.209179 0.122137 0.196499 0.122137 0.373159 0.074677 0.347799 0.074677 0.373159 0.807733 0.347799 0.807733 0.360479 0.807733 0.360479 0.074677 0.307567 0.121428 0.282207 0.121428 0.307567 0.391529 0.282207 0.391529 0.307567 0.134290 0.307567 0.147152 0.307567 0.160014 0.307567 0.172876 0.307567 0.185738 0.307567 0.198600 0.307567 0.211462 0.307567 0.224324 0.307567 0.237185 0.307567 0.250047 0.307567 0.262909 0.307567 0.275771 0.307567 0.288633 0.307567 0.301495 0.307567 0.314357 0.307567 0.327219 0.307567 0.340081 0.307567 0.352943 0.307567 0.365805 0.307567 0.378667 0.294887 0.121428 0.282207 0.378667 0.282207 0.365805 0.282207 0.352943 0.282207 0.340081 0.282207 0.327219 0.282207 0.314357 0.282207 0.301495 0.282207 0.288633 0.282207 0.275771 0.282207 0.262909 0.282207 0.250047 0.282207 0.237185 0.282207 0.224324 0.282207 0.211462 0.282207 0.198600 0.282207 0.185738 0.282207 0.172876 0.282207 0.160014 0.282207 0.147152 0.282207 0.134290 0.294887 0.391529 0.340363 0.074677 0.315003 0.074677 0.340363 0.807733 0.315003 0.807733 0.327683 0.074677 0.327683 0.807733 0.274771 0.039860 0.249412 0.039860 0.274771 0.119581 0.249412 0.119581 0.249412 0.112333 0.249412 0.105086 0.249412 0.097839 0.249412 0.090591 0.249412 0.083344 0.249412 0.076097 0.249412 0.068849 0.249412 0.061602 0.249412 0.054355 0.249412 0.047108 0.262091 0.039860 0.274771 0.047108 0.274771 0.054355 0.274771 0.061602 0.274771 0.068849 0.274771 0.076097 0.274771 0.083344 0.274771 0.090591 0.274771 0.097839 0.274771 0.105086 0.274771 0.112333 0.262091 0.119581 0.241975 0.039860 0.216616 0.039860 0.241975 0.119581 0.216616 0.119581 0.229295 0.119581 0.229295 0.039860 0.216616 0.112333 0.216616 0.105086 0.216616 0.097839 0.216616 0.090591 0.216616 0.083344 0.216616 0.076097 0.216616 0.068849 0.216616 0.061602 0.216616 0.054355 0.216616 0.047108 0.241975 0.047108 0.241975 0.054355 0.241975 0.061602 0.241975 0.068849 0.241975 0.076097 0.241975 0.083344 0.241975 0.090591 0.241975 0.097839 0.241975 0.105086 0.241975 0.112333 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 |

84 84 21 21 110 110

142 |

21 21 20 20 110 110

143 |

85 85 109 109 86 86

144 |

21 21 84 84 22 22

145 |

87 87 86 86 108 108

146 |

85 85 84 84 109 109

147 |

83 83 22 22 84 84

148 |

25 25 82 82 26 26

149 |

83 83 82 82 24 24

150 |

81 81 26 26 82 82

151 |

22 22 83 83 23 23

152 |

82 82 25 25 24 24

153 |

83 83 24 24 23 23

154 |

70 70 88 88 87 87

155 |

90 90 89 89 70 70

156 |

91 91 90 90 70 70

157 |

86 86 109 109 108 108

158 |

108 108 70 70 87 87

159 |

108 108 107 107 41 41

160 |

41 41 70 70 108 108

161 |

84 84 110 110 109 109

162 |

112 112 19 19 18 18

163 |

113 113 18 18 17 17

164 |

20 20 19 19 111 111

165 |

18 18 113 113 112 112

166 |

19 19 112 112 111 111

167 |

20 20 111 111 110 110

168 |

89 89 88 88 70 70

169 |

113 113 17 17 92 92

170 |

28 28 81 81 80 80

171 |

27 27 26 26 81 81

172 |

27 27 81 81 28 28

173 |

80 80 29 29 28 28

174 |

80 80 30 30 29 29

175 |

79 79 30 30 80 80

176 |

97 97 78 78 77 77

177 |

78 78 97 97 79 79

178 |

76 76 98 98 77 77

179 |

32 32 31 31 96 96

180 |

75 75 71 71 76 76

181 |

96 96 31 31 79 79

182 |

31 31 30 30 79 79

183 |

33 33 32 32 95 95

184 |

92 92 16 16 35 35

185 |

93 93 35 35 34 34

186 |

35 35 93 93 92 92

187 |

94 94 33 33 95 95

188 |

34 34 94 94 93 93

189 |

92 92 17 17 16 16

190 |

32 32 96 96 95 95

191 |

74 74 73 73 71 71

192 |

97 97 96 96 79 79

193 |

73 73 72 72 71 71

194 |

77 77 98 98 97 97

195 |

71 71 75 75 74 74

196 |

98 98 71 71 51 51

197 |

51 51 99 99 98 98

198 |

71 71 98 98 76 76

199 |

33 33 94 94 34 34

200 |

105 105 43 43 106 106

201 |

42 42 106 106 43 43

202 |

43 43 105 105 44 44

203 |

107 107 106 106 42 42

204 |

56 56 40 40 39 39

205 |

41 41 40 40 70 70

206 |

42 42 41 41 107 107

207 |

46 46 45 45 103 103

208 |

104 104 103 103 45 45

209 |

47 47 46 46 103 103

210 |

37 37 6 6 38 38

211 |

104 104 45 45 44 44

212 |

44 44 105 105 104 104

213 |

38 38 6 6 39 39

214 |

56 56 4 4 58 58

215 |

5 5 4 4 56 56

216 |

57 57 56 56 58 58

217 |

4 4 60 60 59 59

218 |

4 4 3 3 60 60

219 |

59 59 58 58 4 4

220 |

61 61 60 60 3 3

221 |

6 6 37 37 7 7

222 |

56 56 6 6 5 5

223 |

2 2 61 61 3 3

224 |

2 2 1 1 62 62

225 |

2 2 62 62 61 61

226 |

7 7 37 37 36 36

227 |

56 56 39 39 6 6

228 |

40 40 56 56 70 70

229 |

1 1 0 0 62 62

230 |

48 48 47 47 102 102

231 |

47 47 103 103 102 102

232 |

101 101 48 48 102 102

233 |

9 9 36 36 55 55

234 |

7 7 36 36 8 8

235 |

55 55 54 54 10 10

236 |

53 53 10 10 54 54

237 |

49 49 101 101 100 100

238 |

49 49 48 48 101 101

239 |

50 50 49 49 100 100

240 |

69 69 52 52 71 71

241 |

69 69 53 53 52 52

242 |

50 50 99 99 51 51

243 |

100 100 99 99 50 50

244 |

8 8 36 36 9 9

245 |

69 69 11 11 10 10

246 |

53 53 69 69 10 10

247 |

15 15 63 63 0 0

248 |

62 62 0 0 63 63

249 |

63 63 15 15 14 14

250 |

63 63 14 14 64 64

251 |

12 12 69 69 67 67

252 |

69 69 68 68 67 67

253 |

65 65 13 13 12 12

254 |

11 11 69 69 12 12

255 |

65 65 12 12 66 66

256 |

66 66 12 12 67 67

257 |

64 64 13 13 65 65

258 |

64 64 14 14 13 13

259 |

9 9 55 55 10 10

260 |

51 51 71 71 52 52

261 |

136 136 130 130 129 129

262 |

114 114 129 129 130 130

263 |

128 128 136 136 129 129

264 |

136 136 131 131 130 130

265 |

132 132 131 131 137 137

266 |

136 136 137 137 131 131

267 |

132 132 137 137 138 138 117 117

268 |

115 115 116 116 136 136 128 128

269 |

137 137 126 126 118 118 138 138

270 |

137 137 127 127 126 126

271 |

136 136 116 116 127 127

272 |

137 137 136 136 127 127

273 |

138 138 139 139 119 119 117 117

274 |

119 119 139 139 140 140 133 133

275 |

118 118 120 120 139 139 138 138

276 |

134 134 123 123 121 121

277 |

134 134 121 121 141 141

278 |

141 141 121 121 133 133 140 140

279 |

139 139 120 120 125 125 140 140

280 |

122 122 124 124 135 135 141 141

281 |

134 134 141 141 135 135

282 |

122 122 141 141 140 140 125 125

283 |

164 164 160 160 156 156

284 |

142 142 156 156 160 160

285 |

155 155 164 164 156 156

286 |

164 164 161 161 160 160

287 |

162 162 161 161 165 165

288 |

164 164 165 165 161 161

289 |

162 162 165 165 166 166 145 145

290 |

143 143 144 144 164 164 155 155

291 |

165 165 158 158 146 146 166 166

292 |

165 165 159 159 158 158

293 |

164 164 144 144 159 159

294 |

165 165 164 164 159 159

295 |

166 166 167 167 147 147 145 145

296 |

147 147 167 167 168 168 163 163

297 |

146 146 148 148 167 167 166 166

298 |

151 151 149 149 169 169 153 153

299 |

169 169 149 149 163 163 168 168

300 |

167 167 148 148 157 157 168 168

301 |

150 150 152 152 154 154 169 169

302 |

153 153 169 169 154 154

303 |

150 150 169 169 168 168 157 157

304 |

257 257 285 285 191 191

305 |

191 191 286 286 190 190

306 |

258 258 259 259 283 283

307 |

191 191 192 192 257 257

308 |

260 260 283 283 259 259

309 |

258 258 285 285 257 257

310 |

193 193 257 257 192 192

311 |

197 197 256 256 196 196

312 |

195 195 196 196 256 256

313 |

255 255 256 256 197 197

314 |

256 256 194 194 195 195

315 |

257 257 194 194 256 256

316 |

194 194 257 257 193 193

317 |

282 282 260 260 261 261

318 |

263 263 244 244 262 262

319 |

264 264 244 244 263 263

320 |

282 282 261 261 244 244

321 |

283 283 260 260 282 282

322 |

282 282 244 244 281 281

323 |

265 265 244 244 264 264

324 |

258 258 283 283 284 284

325 |

188 188 189 189 287 287

326 |

286 286 287 287 189 189

327 |

287 287 187 187 188 188

328 |

187 187 287 287 266 266

329 |

190 190 286 286 189 189

330 |

285 285 286 286 191 191

331 |

258 258 284 284 285 285

332 |

281 281 244 244 213 213

333 |

262 262 244 244 261 261

334 |

199 199 255 255 198 198

335 |

197 197 198 198 255 255

336 |

200 200 255 255 199 199

337 |

268 268 203 203 204 204

338 |

254 254 255 255 200 200

339 |

201 201 254 254 200 200

340 |

269 269 254 254 203 203

341 |

253 253 271 271 252 252

342 |

254 254 269 269 253 253

343 |

252 252 271 271 251 251

344 |

272 272 250 250 251 251

345 |

249 249 250 250 245 245

346 |

202 202 203 203 254 254

347 |

202 202 254 254 201 201

348 |

205 205 268 268 204 204

349 |

266 266 207 207 186 186

350 |

267 267 206 206 207 207

351 |

207 207 266 266 267 267

352 |

268 268 269 269 203 203

353 |

267 267 268 268 205 205

354 |

266 266 186 186 187 187

355 |

269 269 270 270 253 253

356 |

248 248 245 245 247 247

357 |

271 271 253 253 270 270

358 |

247 247 245 245 246 246

359 |

251 251 271 271 272 272

360 |

245 245 248 248 249 249

361 |

273 273 245 245 272 272

362 |

245 245 273 273 225 225

363 |

245 245 250 250 272 272

364 |

205 205 206 206 267 267

365 |

215 215 279 279 214 214

366 |

280 280 214 214 279 279

367 |

215 215 216 216 279 279

368 |

280 280 213 213 214 214

369 |

230 230 211 211 212 212

370 |

213 213 244 244 212 212

371 |

280 280 281 281 213 213

372 |

219 219 277 277 218 218

373 |

278 278 218 218 277 277

374 |

220 220 277 277 219 219

375 |

278 278 216 216 217 217

376 |

278 278 217 217 218 218

377 |

216 216 278 278 279 279

378 |

210 210 211 211 176 176

379 |

230 230 232 232 174 174

380 |

175 175 230 230 174 174

381 |

231 231 232 232 230 230

382 |

174 174 233 233 234 234

383 |

174 174 234 234 173 173

384 |

233 233 174 174 232 232

385 |

235 235 173 173 234 234

386 |

177 177 210 210 176 176

387 |

177 177 178 178 209 209

388 |

177 177 209 209 210 210

389 |

175 175 176 176 230 230

390 |

172 172 173 173 235 235

391 |

172 172 236 236 171 171

392 |

172 172 235 235 236 236

393 |

230 230 176 176 211 211

394 |

244 244 230 230 212 212

395 |

171 171 236 236 170 170

396 |

221 221 276 276 220 220

397 |

277 277 220 220 276 276

398 |

221 221 222 222 276 276

399 |

229 229 208 208 178 178

400 |

209 209 178 178 208 208

401 |

229 229 179 179 228 228

402 |

227 227 228 228 180 180

403 |

275 275 222 222 223 223

404 |

275 275 224 224 274 274

405 |

224 224 275 275 223 223

406 |

276 276 222 222 275 275

407 |

227 227 243 243 226 226

408 |

274 274 224 224 225 225

409 |

245 245 226 226 243 243

410 |

245 245 225 225 226 226

411 |

178 178 179 179 229 229

412 |

243 243 180 180 181 181

413 |

227 227 180 180 243 243

414 |

185 185 170 170 237 237

415 |

236 236 237 237 170 170

416 |

237 237 184 184 185 185

417 |

237 237 238 238 184 184

418 |

182 182 241 241 243 243

419 |

243 243 241 241 242 242

420 |

239 239 182 182 183 183

421 |

181 181 182 182 243 243

422 |

239 239 240 240 182 182

423 |

240 240 241 241 182 182

424 |

238 238 239 239 183 183

425 |

238 238 183 183 184 184

426 |

179 179 180 180 228 228

427 |

274 274 225 225 273 273

428 |

303 303 304 304 288 288

429 |

302 302 316 316 303 303

430 |

304 304 303 303 316 316

431 |

316 316 305 305 304 304

432 |

316 316 306 306 305 305

433 |

307 307 306 306 317 317

434 |

317 317 306 306 316 316

435 |

307 307 317 317 318 318 308 308

436 |

301 301 302 302 289 289

437 |

316 316 302 302 301 301

438 |

316 316 301 301 300 300

439 |

317 317 298 298 297 297 318 318

440 |

317 317 299 299 298 298

441 |

297 297 296 296 318 318

442 |

316 316 300 300 299 299

443 |

316 316 299 299 317 317

444 |

318 318 309 309 308 308

445 |

309 309 318 318 319 319 310 310

446 |

320 320 311 311 310 310 319 319

447 |

318 318 296 296 295 295 319 319

448 |

313 313 312 312 321 321

449 |

313 313 314 314 290 290

450 |

312 312 311 311 320 320

451 |

313 313 321 321 314 314

452 |

319 319 295 295 294 294 320 320

453 |

294 294 293 293 320 320

454 |

292 292 315 315 321 321

455 |

314 314 321 321 315 315

456 |

321 321 293 293 292 292

457 |

315 315 292 292 291 291

458 |

320 320 293 293 321 321

459 |

321 321 312 312 320 320

460 |

345 345 327 327 322 322

461 |

327 327 344 344 343 343 328 328

462 |

329 329 342 342 341 341 330 330

463 |

330 330 341 341 324 324 331 331

464 |

328 328 343 343 342 342 329 329

465 |

323 323 344 344 345 345

466 |

344 344 327 327 345 345

467 |

340 340 333 333 332 332

468 |

333 333 340 340 339 339 334 334

469 |

335 335 338 338 337 337 336 336

470 |

346 346 325 325 336 336

471 |

334 334 339 339 338 338 335 335

472 |

340 340 332 332 324 324

473 |

346 346 337 337 326 326

474 |

337 337 346 346 336 336

475 |

331 331 324 324 332 332

476 |

351 351 362 362 347 347

477 |

362 362 361 361 360 360 363 363

478 |

364 364 359 359 358 358 365 365

479 |

365 365 358 358 357 357 366 366

480 |

363 363 360 360 359 359 364 364

481 |

348 348 361 361 351 351

482 |

361 361 362 362 351 351

483 |

367 367 357 357 356 356 368 368

484 |

368 368 356 356 355 355 369 369

485 |

370 370 354 354 353 353 371 371

486 |

350 350 349 349 371 371

487 |

369 369 355 355 354 354 370 370

488 |

350 350 353 353 352 352

489 |

353 353 350 350 371 371

490 |

366 366 357 357 367 367

491 |

379 379 390 390 372 372

492 |

378 378 400 400 379 379

493 |

390 390 379 379 400 400

494 |

400 400 391 391 390 390

495 |

400 400 392 392 391 391

496 |

393 393 392 392 401 401

497 |

401 401 392 392 400 400

498 |

393 393 401 401 402 402 394 394

499 |

389 389 378 378 373 373

500 |

400 400 378 378 389 389

501 |

400 400 389 389 388 388

502 |

401 401 386 386 385 385 402 402

503 |

401 401 387 387 386 386

504 |

385 385 384 384 402 402

505 |

400 400 388 388 387 387

506 |

400 400 387 387 401 401

507 |

402 402 395 395 394 394

508 |

395 395 402 402 403 403 396 396

509 |

404 404 397 397 396 396 403 403

510 |

402 402 384 384 383 383 403 403

511 |

399 399 398 398 405 405

512 |

399 399 376 376 374 374

513 |

398 398 397 397 404 404

514 |

399 399 405 405 376 376

515 |

403 403 383 383 382 382 404 404

516 |

382 382 381 381 404 404

517 |

380 380 377 377 405 405

518 |

376 376 405 405 377 377

519 |

405 405 381 381 380 380

520 |

377 377 380 380 375 375

521 |

404 404 381 381 405 405

522 |

405 405 398 398 404 404

523 |

421 421 422 422 406 406

524 |

420 420 434 434 421 421

525 |

422 422 421 421 434 434

526 |

434 434 423 423 422 422

527 |

434 434 424 424 423 423

528 |

425 425 424 424 435 435

529 |

435 435 424 424 434 434

530 |

425 425 435 435 436 436 426 426

531 |

419 419 420 420 407 407

532 |

434 434 420 420 419 419

533 |

434 434 419 419 418 418

534 |

435 435 416 416 415 415 436 436

535 |

435 435 417 417 416 416

536 |

415 415 414 414 436 436

537 |

434 434 418 418 417 417

538 |

434 434 417 417 435 435

539 |

436 436 427 427 426 426

540 |

427 427 436 436 437 437 428 428

541 |

438 438 429 429 428 428 437 437

542 |

436 436 414 414 413 413 437 437

543 |

431 431 430 430 439 439

544 |

431 431 432 432 408 408

545 |

430 430 429 429 438 438

546 |

431 431 439 439 432 432

547 |

437 437 413 413 412 412 438 438

548 |

412 412 411 411 438 438

549 |

410 410 433 433 439 439

550 |

432 432 439 439 433 433

551 |

439 439 411 411 410 410

552 |

433 433 410 410 409 409

553 |

438 438 411 411 439 439

554 |

439 439 430 430 438 438

555 |

463 463 443 443 440 440

556 |

443 443 462 462 461 461 444 444

557 |

445 445 460 460 459 459 446 446

558 |

446 446 459 459 458 458 447 447

559 |

444 444 461 461 460 460 445 445

560 |

441 441 462 462 463 463

561 |

462 462 443 443 463 463

562 |

457 457 449 449 448 448

563 |

449 449 457 457 456 456 450 450

564 |

451 451 455 455 454 454 452 452

565 |

464 464 442 442 452 452

566 |

450 450 456 456 455 455 451 451

567 |

457 457 448 448 458 458

568 |

464 464 454 454 453 453

569 |

454 454 464 464 452 452

570 |

447 447 458 458 448 448

571 |

471 471 480 480 465 465

572 |

480 480 479 479 478 478 481 481

573 |

482 482 477 477 476 476 483 483

574 |

483 483 476 476 467 467 484 484

575 |

481 481 478 478 477 477 482 482

576 |

466 466 479 479 471 471

577 |

479 479 480 480 471 471

578 |

485 485 467 467 475 475 486 486

579 |

486 486 475 475 474 474 487 487

580 |

488 488 473 473 472 472 489 489

581 |

470 470 468 468 489 489

582 |

487 487 474 474 473 473 488 488

583 |

470 470 472 472 469 469

584 |

472 472 470 470 489 489

585 |

484 484 467 467 485 485

586 |

497 497 508 508 490 490

587 |

496 496 518 518 497 497

588 |

508 508 497 497 518 518

589 |

518 518 509 509 508 508

590 |

518 518 510 510 509 509

591 |

511 511 510 510 519 519

592 |

519 519 510 510 518 518

593 |

511 511 519 519 520 520 512 512

594 |

507 507 496 496 491 491

595 |

518 518 496 496 507 507

596 |

518 518 507 507 506 506

597 |

519 519 504 504 503 503 520 520

598 |

519 519 505 505 504 504

599 |

503 503 502 502 520 520

600 |

518 518 506 506 505 505

601 |

518 518 505 505 519 519

602 |

520 520 513 513 512 512

603 |

513 513 520 520 521 521 514 514

604 |

522 522 515 515 514 514 521 521

605 |

520 520 502 502 501 501 521 521

606 |

517 517 516 516 523 523

607 |

517 517 494 494 492 492

608 |

516 516 515 515 522 522

609 |

517 517 523 523 494 494

610 |

521 521 501 501 500 500 522 522

611 |

500 500 499 499 522 522

612 |

498 498 495 495 523 523

613 |

494 494 523 523 495 495

614 |

523 523 499 499 498 498

615 |

495 495 498 498 493 493

616 |

522 522 499 499 523 523

617 |

523 523 516 516 522 522

618 |

539 539 525 525 526 526

619 |

526 526 527 527 550 550 551 551

620 |

528 528 529 529 548 548 549 549

621 |

530 530 531 531 546 546 547 547

622 |

529 529 530 530 547 547 548 548

623 |

527 527 528 528 549 549 550 550

624 |

551 551 538 538 539 539

625 |

526 526 551 551 539 539

626 |

546 546 531 531 532 532 545 545

627 |

533 533 544 544 545 545 532 532

628 |

534 534 543 543 544 544 533 533

629 |

536 536 541 541 542 542 535 535

630 |

553 553 537 537 552 552

631 |

536 536 537 537 540 540 541 541

632 |

535 535 542 542 543 543 534 534

633 |

553 553 524 524 540 540

634 |

537 537 553 553 540 540

635 |

559 559 558 558 556 556 554 554

636 |

555 555 557 557 558 558 559 559

637 |

584 584 560 560 564 564

638 |

565 565 566 566 602 602 603 603

639 |

566 566 567 567 601 601 602 602

640 |

564 564 565 565 603 603 604 604

641 |

568 568 569 569 599 599 600 600

642 |

569 569 570 570 598 598 599 599

643 |

571 571 572 572 596 596 597 597

644 |

572 572 573 573 595 595 596 596

645 |

571 571 597 597 598 598 570 570

646 |

568 568 600 600 601 601 567 567

647 |

561 561 584 584 604 604

648 |

604 604 584 584 564 564

649 |

595 595 573 573 574 574 594 594

650 |

575 575 593 593 594 594 574 574

651 |

576 576 577 577 591 591 592 592

652 |

578 578 590 590 591 591 577 577

653 |

575 575 576 576 592 592 593 593

654 |

579 579 580 580 588 588 589 589

655 |

580 580 581 581 587 587 588 588

656 |

583 583 585 585 586 586 582 582

657 |

605 605 583 583 562 562

658 |

582 582 586 586 587 587 581 581

659 |

579 579 589 589 590 590 578 578

660 |

605 605 563 563 585 585

661 |

605 605 585 585 583 583

662 |

610 610 611 611 608 608 606 606

663 |

607 607 609 609 611 611 610 610

664 |

626 626 627 627 612 612

665 |

627 627 625 625 624 624 628 628

666 |

630 630 629 629 623 623 622 622

667 |

631 631 630 630 622 622 621 621

668 |

629 629 628 628 624 624 623 623

669 |

613 613 625 625 626 626

670 |

625 625 627 627 626 626

671 |

621 621 620 620 632 632 631 631

672 |

633 633 632 632 620 620 619 619

673 |

634 634 633 633 619 619 618 618

674 |

636 636 635 635 617 617 616 616

675 |

637 637 614 614 636 636

676 |

635 635 634 634 618 618 617 617

677 |

637 637 616 616 615 615

678 |

616 616 637 637 636 636

679 |

643 643 654 654 638 638

680 |

654 654 653 653 652 652 655 655

681 |

657 657 656 656 651 651 650 650

682 |

658 658 657 657 650 650 649 649

683 |

656 656 655 655 652 652 651 651

684 |

639 639 653 653 643 643

685 |

653 653 654 654 643 643

686 |

649 649 648 648 659 659 658 658

687 |

660 660 659 659 648 648 647 647

688 |

661 661 660 660 647 647 646 646

689 |

663 663 662 662 645 645 644 644

690 |

642 642 640 640 663 663

691 |

662 662 661 661 646 646 645 645

692 |

642 642 644 644 641 641

693 |

644 644 642 642 663 663

694 | 695 | 696 | 697 | 698 | 699 | 700 | 701 | 0.748339 -0.390597 0.536118 139.768430 0.656219 0.318028 -0.684277 -266.057952 0.096776 0.863882 0.494310 190.244530 0.000000 0.000000 0.000000 1.000000 702 | 703 | 704 | 705 | 1.000000 0.000000 0.000000 -1.536154 0.000000 1.000000 0.000000 4.483904 0.000000 0.000000 1.000000 9665.363729 0.000000 0.000000 0.000000 1.000000 706 | 707 | 708 | 709 | 1.000000 0.000000 0.000000 -13.636077 0.000000 0.000000 -1.000000 -80185.933829 0.000000 1.000000 0.000000 51.036493 0.000000 0.000000 0.000000 1.000000 710 | 711 | 712 | 713 | 0.000000 0.000000 1.000000 50711.539250 1.000000 0.000000 0.000000 1.055465 0.000000 1.000000 0.000000 32.061428 0.000000 0.000000 0.000000 1.000000 714 | 715 | 716 | 717 | 718 | 719 | 720 | 721 | 722 | 723 | 724 | 725 | 726 | 727 | 728 | 729 | 730 | -------------------------------------------------------------------------------- /urdf/daes/base.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uArm-Developer/UArmForROS/55efc7d13d782736714ae3f1a3e2c7fa487e4fd9/urdf/daes/base.STL -------------------------------------------------------------------------------- /urdf/model.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | -------------------------------------------------------------------------------- /visualization/visual_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import copy 4 | import math 5 | import sys 6 | 7 | from sensor_msgs.msg import JointState 8 | 9 | from interactive_markers.interactive_marker_server import * 10 | from interactive_markers.menu_handler import * 11 | from visualization_msgs.msg import * 12 | from geometry_msgs.msg import Point 13 | from tf.broadcaster import TransformBroadcaster 14 | from std_msgs.msg import Int32 15 | 16 | from math import sin 17 | import time 18 | from uarm.msg import Angles 19 | from uarm.msg import CoordsWithTime 20 | 21 | server = None 22 | menu_handler = MenuHandler() 23 | br = None 24 | counter = 0 25 | 26 | PI = math.pi 27 | MATH_PI = 3.141592653589793238463 28 | MATH_TRANS = 57.2958 29 | MATH_L1 = (10.645+0.6) 30 | MATH_L2 = 2.117 31 | MATH_L3 = 14.825 32 | MATH_L4 = 16.02 33 | MATH_L43 = MATH_L4/MATH_L3 34 | 35 | # UARM OFFSETS 36 | TopOffset = -1.5 37 | BottomOffset = 1.5 38 | 39 | angle = {} 40 | 41 | def ivsKine(x, y, z): 42 | if z > (MATH_L1 + MATH_L3 + TopOffset): 43 | z = MATH_L1 + MATH_L3 + TopOffset 44 | if z < (MATH_L1 - MATH_L4 + BottomOffset): 45 | z = MATH_L1 - MATH_L4 + BottomOffset 46 | 47 | g_y_in = (-y-MATH_L2)/MATH_L3 48 | g_z_in = (z - MATH_L1) / MATH_L3 49 | g_right_all = (1 - g_y_in*g_y_in - g_z_in*g_z_in - MATH_L43*MATH_L43) / (2 * MATH_L43) 50 | g_sqrt_z_y = math.sqrt(g_z_in*g_z_in + g_y_in*g_y_in) 51 | 52 | if x == 0: 53 | # Calculate value of theta 1 54 | g_theta_1 = 90; 55 | # Calculate value of theta 3 56 | if g_z_in == 0: 57 | g_phi = 90 58 | else: 59 | g_phi = math.atan(-g_y_in / g_z_in)*MATH_TRANS 60 | if g_phi > 0: 61 | g_phi = g_phi - 180 62 | g_theta_3 = math.asin(g_right_all / g_sqrt_z_y)*MATH_TRANS - g_phi 63 | 64 | if g_theta_3<0: 65 | g_theta_3 = 0 66 | # Calculate value of theta 2 67 | g_theta_2 = math.asin((z + MATH_L4*math.sin(g_theta_3 / MATH_TRANS) - MATH_L1) / MATH_L3)*MATH_TRANS 68 | else: 69 | # Calculate value of theta 1 70 | g_theta_1 = math.atan(y / x)*MATH_TRANS 71 | if (y/x) > 0: 72 | g_theta_1 = g_theta_1 73 | if (y/x) < 0: 74 | g_theta_1 = g_theta_1 + 180 75 | if y == 0: 76 | if x > 0: 77 | g_theta_1 = 180 78 | else: 79 | g_theta_1 = 0 80 | # Calculate value of theta 3 81 | g_x_in = (-x / math.cos(g_theta_1 / MATH_TRANS) - MATH_L2) / MATH_L3; 82 | if g_z_in == 0: 83 | g_phi = 90 84 | else: 85 | g_phi = math.atan(-g_x_in / g_z_in)*MATH_TRANS 86 | if g_phi > 0: 87 | g_phi = g_phi - 180 88 | 89 | g_sqrt_z_x = math.sqrt(g_z_in*g_z_in + g_x_in*g_x_in) 90 | 91 | g_right_all_2 = -1 * (g_z_in*g_z_in + g_x_in*g_x_in + MATH_L43*MATH_L43 - 1) / (2 * MATH_L43) 92 | g_theta_3 = math.asin(g_right_all_2 / g_sqrt_z_x)*MATH_TRANS 93 | g_theta_3 = g_theta_3 - g_phi 94 | 95 | if g_theta_3 <0 : 96 | g_theta_3 = 0 97 | # Calculate value of theta 2 98 | g_theta_2 = math.asin(g_z_in + MATH_L43*math.sin(abs(g_theta_3 / MATH_TRANS)))*MATH_TRANS 99 | 100 | g_theta_1 = abs(g_theta_1); 101 | g_theta_2 = abs(g_theta_2); 102 | 103 | angle[1] = g_theta_1 104 | angle[2] = g_theta_2 105 | angle[3] = g_theta_3 106 | angle[4] = 0 107 | return angle 108 | 109 | 110 | 111 | def processFeedback( feedback ): 112 | 113 | current_x = -round(feedback.pose.position.x*1000)/10.0 114 | current_y = -round(feedback.pose.position.y*1000)/10.0 115 | current_z = round(feedback.pose.position.z*1000)/10.0 116 | 117 | print 'x: ' + str(current_x) +'cm y: ' + str(current_y) + 'cm z: '+str(current_z) +'cm' 118 | display(ivsKine(current_x,current_y,current_z)) 119 | 120 | pub3.publish(current_x,current_y,current_z,0) 121 | 122 | server.applyChanges() 123 | 124 | def display(angle): 125 | for i in angle: 126 | angle[i] = angle[i]*math.pi/180 127 | joint_state_send.header.stamp = rospy.Time.now() 128 | joint_state_send.position = [angle[1],angle[2],-angle[2]-angle[3],angle[3],angle[4]] 129 | joint_state_send.position = joint_state_send.position + [-angle[2]-angle[3],angle[3],PI/2-angle[3]] 130 | joint_state_send.position = joint_state_send.position + [angle[2]-PI,angle[2]+angle[3]-PI/2] 131 | joint_state_send.velocity = [0] 132 | joint_state_send.effort = [] 133 | pub.publish(joint_state_send) 134 | 135 | def makeBox( msg ): 136 | marker = Marker() 137 | 138 | marker.type = Marker.SPHERE 139 | 140 | 141 | marker.scale.x = msg.scale * 0.1 142 | marker.scale.y = msg.scale * 0.1 143 | marker.scale.z = msg.scale * 0.1 144 | marker.color.r = 0.5 145 | marker.color.g = 0.5 146 | marker.color.b = 0.5 147 | marker.color.a = 0.1 148 | 149 | return marker 150 | 151 | def makeBoxControl( msg ): 152 | control = InteractiveMarkerControl() 153 | control.always_visible = True 154 | control.markers.append( makeBox(msg) ) 155 | msg.controls.append( control ) 156 | return control 157 | 158 | ##################################################################### 159 | # Marker Creation 160 | 161 | def make6DofMarker( fixed, interaction_mode, position, show_6dof = False): 162 | int_marker = InteractiveMarker() 163 | int_marker.header.frame_id = "base" 164 | int_marker.pose.position = position # Defined the position of the marker 165 | int_marker.scale = 0.1 166 | 167 | int_marker.name = "simple_6dof" 168 | int_marker.description = "uarm_controller" 169 | 170 | # insert a box 171 | makeBoxControl(int_marker) 172 | int_marker.controls[0].interaction_mode = interaction_mode 173 | 174 | if show_6dof: 175 | control = InteractiveMarkerControl() 176 | control.orientation.w = 1 177 | control.orientation.x = 1 178 | control.orientation.y = 0 179 | control.orientation.z = 0 180 | control.name = "move_x" 181 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 182 | if fixed: 183 | control.orientation_mode = InteractiveMarkerControl.FIXED 184 | int_marker.controls.append(control) 185 | 186 | control = InteractiveMarkerControl() 187 | control.orientation.w = 1 188 | control.orientation.x = 0 189 | control.orientation.y = 1 190 | control.orientation.z = 0 191 | control.name = "move_z" 192 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 193 | if fixed: 194 | control.orientation_mode = InteractiveMarkerControl.FIXED 195 | int_marker.controls.append(control) 196 | 197 | control = InteractiveMarkerControl() 198 | control.orientation.w = 1 199 | control.orientation.x = 0 200 | control.orientation.y = 0 201 | control.orientation.z = 1 202 | control.name = "move_y" 203 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 204 | if fixed: 205 | control.orientation_mode = InteractiveMarkerControl.FIXED 206 | int_marker.controls.append(control) 207 | 208 | server.insert(int_marker, processFeedback) 209 | menu_handler.apply( server, int_marker.name ) 210 | 211 | if __name__=="__main__": 212 | 213 | rospy.init_node("visual_control") 214 | pub = rospy.Publisher('joint_states',JointState,queue_size = 10) 215 | pub2 = rospy.Publisher('read_coords',Int32,queue_size = 10) 216 | pub3 = rospy.Publisher('move_to_time',CoordsWithTime,queue_size = 100) 217 | time.sleep(1) 218 | pub2.publish(1) 219 | time.sleep(1) 220 | 221 | 222 | joint_state_send = JointState() 223 | joint_state_send.header = Header() 224 | joint_state_send.name = ['base_to_base_rot','base_rot_to_link_1','link_1_to_link_2' ,'link_2_to_link_3','link_3_to_link_end'] 225 | joint_state_send.name = joint_state_send.name+ ['link_1_to_add_1','link_2_to_add_4','link_3_to_add_2','base_rot_to_add_3','link_2_to_add_5'] 226 | 227 | 228 | br = TransformBroadcaster() 229 | 230 | # create a timer to update the published transforms 231 | 232 | server = InteractiveMarkerServer("uarm_controller") 233 | 234 | menu_handler.insert( "First Entry", callback=processFeedback ) 235 | menu_handler.insert( "Second Entry", callback=processFeedback ) 236 | 237 | ini_x = -rospy.get_param('current_x')/100 238 | ini_y = -rospy.get_param('current_y')/100 239 | ini_z = rospy.get_param('current_z')/100 240 | time.sleep(1) 241 | 242 | position = Point( float(ini_x), float(ini_y), float(ini_z)) 243 | display(ivsKine(float(ini_x),float(ini_y),float(ini_z))) 244 | #position = Point( 0,0 ,0 ) 245 | make6DofMarker( True, InteractiveMarkerControl.NONE, position, True) 246 | server.applyChanges() 247 | 248 | rospy.spin() 249 | 250 | 251 | -------------------------------------------------------------------------------- /visualization/visual_display.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from sensor_msgs.msg import JointState 4 | from std_msgs.msg import Header 5 | import sys 6 | import math 7 | import time 8 | from std_msgs.msg import String 9 | from std_msgs.msg import Int32 10 | 11 | PI = math.pi 12 | 13 | def main_fcn(): 14 | pub = rospy.Publisher('joint_states',JointState,queue_size = 10) 15 | pub2 = rospy.Publisher('read_angles',Int32,queue_size = 10) 16 | pub3 = rospy.Publisher('uarm_status',String,queue_size = 100) 17 | 18 | rospy.init_node('display',anonymous = True) 19 | rate = rospy.Rate(20) 20 | 21 | 22 | joint_state_send = JointState() 23 | joint_state_send.header = Header() 24 | joint_state_send.name = ['base_to_base_rot','base_rot_to_link_1','link_1_to_link_2' ,'link_2_to_link_3','link_3_to_link_end'] 25 | 26 | 27 | joint_state_send.name = joint_state_send.name+ ['link_1_to_add_1','link_2_to_add_4','link_3_to_add_2','base_rot_to_add_3','link_2_to_add_5'] 28 | 29 | angle = {} 30 | 31 | trigger = 1 32 | 33 | while not rospy.is_shutdown(): 34 | joint_state_send.header.stamp = rospy.Time.now() 35 | 36 | pub2.publish(1) 37 | if trigger == 1: 38 | pub3.publish("detach") 39 | time.sleep(1) 40 | trigger = 0 41 | 42 | angle['s1'] = rospy.get_param('servo_1')*math.pi/180 43 | angle['s2'] = rospy.get_param('servo_2')*math.pi/180 44 | angle['s3'] = rospy.get_param('servo_3')*math.pi/180 45 | angle['s4'] = rospy.get_param('servo_4')*math.pi/180 46 | 47 | joint_state_send.position = [angle['s1'],angle['s2'],-angle['s2']-angle['s3'],angle['s3'],angle['s4']] 48 | joint_state_send.position = joint_state_send.position + [-angle['s2']-angle['s3'],angle['s3'],PI/2-angle['s3']] 49 | joint_state_send.position = joint_state_send.position + [angle['s2']-PI,angle['s2']+angle['s3']-PI/2] 50 | joint_state_send.velocity = [0] 51 | joint_state_send.effort = [] 52 | 53 | pub.publish(joint_state_send) 54 | rate.sleep() 55 | 56 | if __name__ == '__main__': 57 | 58 | print 'begin' 59 | main_fcn() 60 | 61 | --------------------------------------------------------------------------------