├── .gitignore ├── LICENSE ├── README.md ├── ReleaseNotes.md ├── doc ├── api │ ├── xarm_api.md │ └── xarm_api_code.md └── tool │ ├── gen_api.py │ └── markdown_doc.py ├── example └── wrapper │ ├── common │ ├── 0000-template.py │ ├── 0001-event_register.py │ ├── 0002-get_property.py │ ├── 0003-api_get.py │ ├── 0004-servo_attach_detach.py │ ├── 1001-move_line.py │ ├── 1002-move_line.py │ ├── 1003-relative_move_line.py │ ├── 1004-move_arc_line.py │ ├── 1005-move_arc_line.py │ ├── 1006-move_tool_line.py │ ├── 1007-counter.py │ ├── 3001-move_circle.py │ ├── 3002-record_trajectory.py │ ├── 3003-playback_trajectory.py │ ├── 5001-get_tgpio_digital.py │ ├── 5002-get_tgpio_analog.py │ ├── 5003-set_tgpio_digital.py │ ├── 5004-set_gripper.py │ ├── 5005-get_cgpio_digital_analog.py │ ├── 5006-set_cgpio_digital_analog.py │ ├── 5007-set_cgpio_input_output_function.py │ ├── 5008-get_cgpio_state.py │ ├── 6001-set_reduced_mode.py │ ├── 6002-set_fense_mode.py │ ├── 7001-servo_j.py │ └── 7002-servo_cartesian.py │ ├── robot.conf │ ├── thridparty │ ├── set_robotiq_gripper.py │ └── set_yinshi_gripper.py │ ├── tool │ ├── blockly_to_python.py │ └── example.xml │ ├── xarm5 │ ├── 2001-move_joint.py │ ├── 2002-move_joint.py │ ├── 2003-move_joint.py │ └── 2004-move_joint.py │ ├── xarm6 │ ├── 2001-move_joint-pybullet-vis.py │ ├── 2001-move_joint-pybullet.py │ ├── 2001-move_joint.py │ ├── 2002-move_joint.py │ ├── 2003-move_joint.py │ ├── 2004-move_joint.py │ ├── loadxarm_sim.py │ ├── xarm_real_ik.py │ └── xarm_sim.py │ └── xarm7 │ ├── 2001-move_joint.py │ ├── 2002-move_joint.py │ ├── 2003-move_joint.py │ ├── 2004-move_joint.py │ ├── loadxarm_sim.py │ ├── meshes │ └── xarm7 │ │ ├── collision │ │ ├── link1.obj │ │ ├── link1_vhacd.obj │ │ ├── link2.obj │ │ ├── link2_vhacd.obj │ │ ├── link3.obj │ │ ├── link3_vhacd.obj │ │ ├── link4.obj │ │ ├── link4_vhacd.obj │ │ ├── link5.obj │ │ ├── link5_vhacd.obj │ │ ├── link6.obj │ │ ├── link6_vhacd.obj │ │ ├── link7.obj │ │ ├── link7_vhacd.obj │ │ ├── linkbase.obj │ │ ├── linkbase_vhacd.obj │ │ └── log.txt │ │ └── visual │ │ ├── link1.STL │ │ ├── link1_smooth.STL │ │ ├── link2.STL │ │ ├── link2_smooth.STL │ │ ├── link3.STL │ │ ├── link3_smooth.STL │ │ ├── link4.STL │ │ ├── link4_smooth.STL │ │ ├── link5.STL │ │ ├── link5_smooth.STL │ │ ├── link6.STL │ │ ├── link6_smooth.STL │ │ ├── link7.STL │ │ ├── link7_smooth.STL │ │ ├── link_base.STL │ │ └── link_base_smooth.STL │ ├── xarm7_robot.urdf │ ├── xarm_real_ik.py │ └── xarm_sim.py ├── requirements.txt ├── setup.py └── xarm ├── __init__.py ├── core ├── __init__.py ├── comm │ ├── __init__.py │ ├── base.py │ ├── serial_port.py │ ├── socket_port.py │ └── uxbus_cmd_protocol.py ├── config │ ├── __init__.py │ ├── x_code.py │ └── x_config.py ├── utils │ ├── __init__.py │ ├── convert.py │ ├── crc16.py │ ├── debug_print.py │ └── log.py ├── version.py └── wrapper │ ├── __init__.py │ ├── uxbus_cmd.py │ ├── uxbus_cmd_ser.py │ └── uxbus_cmd_tcp.py ├── tools ├── __init__.py ├── blockly_tool.py └── list_ports.py ├── version.py ├── wrapper ├── __init__.py └── xarm_api.py └── x3 ├── __init__.py ├── code.py ├── events.py ├── gpio.py ├── gripper.py ├── parse.py ├── record.py ├── servo.py └── utils.py /.gitignore: -------------------------------------------------------------------------------- 1 | files__pycache__/ 2 | .idea 3 | *.py[cod] 4 | *$py.class 5 | build/ 6 | dist/ 7 | *.egg-info/ 8 | example/test/ 9 | example/wrapper/bak/ 10 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/LICENSE -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # xArm-Python-SDK 2 | 3 | ## Overview 4 | xArm Python SDK 5 | 6 | ## Caution 7 | - During use, people should stay away from the robot arm to avoid accidental injury or damage to other items by the robot arm. 8 | - Protect the arm before use. 9 | - Before you exercise, please make sure you don't encounter obstacles. 10 | - Protect the arm before unlocking the motor. 11 | 12 | ## Installation 13 | Install is not necessary, you can run examples without installation.Only Python3 is supported. 14 | - download 15 | 16 | ```bash 17 | git clone https://github.com/xArm-Developer/xArm-Python-SDK.git 18 | ``` 19 | 20 | - install 21 | 22 | ```bash 23 | python setup.py install 24 | ``` 25 | 26 | ## Doc 27 | - #### [API Document](doc/api/xarm_api.md) 28 | 29 | - #### [API Code Document](doc/api/xarm_api_code.md) 30 | 31 | ## Update Summary 32 | 33 | - > ### 1.4.0 34 | - Support servo cartesian interface 35 | - Support for blocky code conversion and operation of xArmStudio1.4.0 36 | 37 | - > ### 1.3.0 38 | 39 | - Added several attributes 40 | - Support tool coordinate system movement 41 | - Support joint range limitation, collision rebound setting 42 | - Support user coordinate system setting 43 | - Support the status of the air pump 44 | - Added counter interface 45 | - Support for blocky code conversion and operation of xArmStudio1.3.0 46 | 47 | - >### [More](ReleaseNotes.md) 48 | 49 | 50 | ## [Example](example/wrapper/) 51 | 52 | __Note: Before running the example, please modify the ip value in the [robot.conf](example/wrapper/robot.conf) file to the robot arm you want to control.__ 53 | 54 | - #### [0000-template](example/wrapper/common/0000-template.py) 55 | 56 | - ##### [0001-event_register](example/wrapper/common/0001-event_register.py) 57 | 58 | - ##### [0002-get_property](example/wrapper/common/0002-get_property.py) 59 | 60 | - ##### [0003-api_get](example/wrapper/common/0003-api_get.py) 61 | 62 | - ##### [0004-servo_attach_detach](example/wrapper/common/0004-servo_attach_detach.py) 63 | 64 | - ##### [1001-move_line](example/wrapper/common/1001-move_line.py) 65 | 66 | - ##### [1002-move_line](example/wrapper/common/1002-move_line.py) 67 | 68 | - ##### [1003-relative_move_line](example/wrapper/common/1003-relative_move_line.py) 69 | 70 | - ##### [1004-move_arc_line](example/wrapper/common/1004-move_arc_line.py) 71 | 72 | - ##### [1005-move_arc_line](example/wrapper/common/1005-move_arc_line.py) 73 | 74 | - ##### [1006-move_tool_line](example/wrapper/common/1006-move_tool_line.py) 75 | 76 | - ##### [1007-counter](example/wrapper/common/1007-counter.py) 77 | 78 | - ##### 2001-move_joint --> [xarm5](example/wrapper/xarm5/2001-move_joint.py) --- [xarm6](example/wrapper/xarm6/2001-move_joint.py) --- [xarm7](example/wrapper/xarm7/2001-move_joint.py) 79 | 80 | - ##### 2002-move_joint --> [xarm5](example/wrapper/xarm5/2002-move_joint.py) --- [xarm6](example/wrapper/xarm6/2002-move_joint.py) --- [xarm7](example/wrapper/xarm7/2002-move_joint.py) 81 | 82 | - ##### 2003-move_joint --> [xarm5](example/wrapper/xarm5/2003-move_joint.py) --- [xarm6](example/wrapper/xarm6/2003-move_joint.py) --- [xarm7](example/wrapper/xarm7/2003-move_joint.py) 83 | 84 | - ##### 2004-move_joint --> [xarm5](example/wrapper/xarm5/2004-move_joint.py) --- [xarm6](example/wrapper/xarm6/2004-move_joint.py) --- [xarm7](example/wrapper/xarm7/2004-move_joint.py) 85 | 86 | - ##### [3001-move_circle](example/wrapper/common/3001-move_circle.py) 87 | 88 | - ##### [3002-record_trajectory](example/wrapper/common/3002-record_trajectory.py) 89 | 90 | - ##### [3003-playback_trajectory](example/wrapper/common/3003-playback_trajectory.py) 91 | 92 | - ##### [5001-get_tgpio_digital](example/wrapper/common/5001-get_tgpio_digital.py) 93 | 94 | - ##### [5002-get_tgpio_analog](example/wrapper/common/5002-get_tgpio_analog.py) 95 | 96 | - ##### [5003-set_tgpio_digital](example/wrapper/common/5003-set_tgpio_digital.py) 97 | 98 | - ##### [5004-set_gripper](example/wrapper/common/5004-set_gripper.py) 99 | 100 | - ##### [5005-get_cgpio_digital_analog](example/wrapper/common/5005-get_cgpio_digital_analog.py) 101 | 102 | - ##### [5006-set_cgpio_digital_analog](example/wrapper/common/5006-set_cgpio_digital_analog.py) 103 | 104 | - ##### [5007-set_cgpio_input_output_function](example/wrapper/common/5007-set_cgpio_input_output_function.py) 105 | 106 | - ##### [5008-get_cgpio_state](example/wrapper/common/5008-get_cgpio_state.py) 107 | 108 | - ##### [6001-set_reduced_mode](example/wrapper/common/6001-set_reduced_mode.py) 109 | 110 | - ##### [6002-set_fense_mode](example/wrapper/common/6002-set_fense_mode.py) 111 | 112 | - ##### [7001-servo_j](example/wrapper/common/7001-servo_j.py) 113 | 114 | - ##### [7002-servo_cartesian](example/wrapper/common/7002-servo_cartesian.py) 115 | 116 | - ##### [blockly_to_python](example/wrapper/tool/blockly_to_python.py) 117 | 118 | 119 | - #### Import 120 | ```python 121 | from xarm.wrapper import XArmAPI 122 | arm = XArmAPI('COM5') 123 | arm = XArmAPI('192.168.1.113') 124 | arm = XArmAPI('192.168.1.113', do_not_open=False) 125 | arm = XArmAPI('192.168.1.113', is_radian=False) 126 | ``` 127 | 128 | - #### Connect/Disconnect 129 | ```python 130 | arm.connect(...) 131 | arm.disconnect() 132 | ``` 133 | 134 | - #### Move 135 | ```python 136 | arm.reset(...) 137 | arm.set_position(...) 138 | arm.set_servo_angle(...) 139 | arm.set_servo_angle_j(...) 140 | arm.move_gohome(...) 141 | arm.move_circle(...) 142 | arm.emergency_stop() 143 | ``` 144 | 145 | - #### Set 146 | ```python 147 | arm.set_servo_attach(...) 148 | arm.set_servo_detach(...) 149 | arm.set_state(...) 150 | arm.set_mode(...) 151 | arm.motion_enable(...) 152 | arm.set_pause_time(...) 153 | ``` 154 | 155 | - #### Get 156 | ```python 157 | arm.get_version() 158 | arm.get_state() 159 | arm.get_is_moving() 160 | arm.get_cmdnum() 161 | arm.get_err_warn_code() 162 | arm.get_position(...) 163 | arm.get_servo_angle(...) 164 | ``` 165 | 166 | - #### Setting 167 | ```python 168 | arm.set_tcp_offset(...) 169 | arm.set_tcp_jerk(...) 170 | arm.set_tcp_maxacc(...) 171 | arm.set_joint_jerk(...) 172 | arm.set_joint_maxacc(...) 173 | arm.set_tcp_load(...) 174 | arm.set_collision_sensitivity(...) 175 | arm.set_teach_sensitivity(...) 176 | arm.set_gravity_direction(...) 177 | arm.clean_conf() 178 | arm.save_conf() 179 | ``` 180 | 181 | - #### Gripper 182 | ```python 183 | arm.set_gripper_enable(...) 184 | arm.set_gripper_mode(...) 185 | arm.set_gripper_speed(...) 186 | arm.set_gripper_position(...) 187 | arm.get_gripper_position() 188 | arm.get_gripper_err_code() 189 | arm.clean_gripper_error() 190 | ``` 191 | 192 | - #### GPIO 193 | ```python 194 | # Tool GPIO 195 | arm.get_tgpio_digital(...) 196 | arm.set_tgpio_digital(...) 197 | arm.get_tgpio_analog(...) 198 | # Controller GPIO 199 | arm.get_cgpio_digital(...) 200 | arm.get_cgpio_analog(...) 201 | arm.set_cgpio_digital(...) 202 | arm.set_cgpio_analog(...) 203 | arm.set_cgpio_digital_input_function(...) 204 | arm.set_cgpio_digital_output_function(...) 205 | arm.get_cgpio_state() 206 | ``` 207 | 208 | - #### Other 209 | ```python 210 | arm.set_pause_time(...) 211 | arm.shutdown_system(...) 212 | arm.clean_error() 213 | arm.clean_warn() 214 | ``` 215 | 216 | - #### Register/Release 217 | ```python 218 | arm.register_report_callback(...) 219 | arm.register_report_location_callback(...) 220 | arm.register_connect_changed_callback(callback) 221 | arm.register_state_changed_callback(callback) 222 | arm.register_mode_changed_callback(callback) 223 | arm.register_mtable_mtbrake_changed_callback(callback) 224 | arm.register_error_warn_changed_callback(callback) 225 | arm.register_cmdnum_changed_callback(callback) 226 | arm.release_report_callback(callback) 227 | arm.release_report_location_callback(callback) 228 | arm.release_connect_changed_callback(callback) 229 | arm.release_state_changed_callback(callback) 230 | arm.release_mode_changed_callback(callback) 231 | arm.release_mtable_mtbrake_changed_callback(callback) 232 | arm.release_error_warn_changed_callback(callback) 233 | arm.release_cmdnum_changed_callback(callback) 234 | ``` 235 | 236 | - #### Property 237 | ```python 238 | arm.connected 239 | arm.default_is_radian 240 | arm.version 241 | arm.position 242 | arm.last_used_position 243 | arm.tcp_speed_limit 244 | arm.tcp_acc_limit 245 | arm.last_used_tcp_speed 246 | arm.last_used_tcp_acc 247 | arm.angles 248 | arm.joint_speed_limit 249 | arm.joint_acc_limit 250 | arm.last_used_angles 251 | arm.last_used_joint_speed 252 | arm.last_used_joint_acc 253 | arm.tcp_offset 254 | arm.state 255 | arm.mode 256 | arm.joints_torque 257 | arm.tcp_load 258 | arm.collision_sensitivity 259 | arm.teach_sensitivity 260 | arm.motor_brake_states 261 | arm.motor_enable_states 262 | arm.has_err_warn 263 | arm.has_error 264 | arm.has_warn 265 | arm.error_code 266 | arm.warn_code 267 | arm.cmd_num 268 | arm.device_type 269 | arm.axis 270 | arm.gravity_direction 271 | ``` 272 | 273 | -------------------------------------------------------------------------------- /ReleaseNotes.md: -------------------------------------------------------------------------------- 1 | # xArm-Python-SDK Release Notes 2 | 3 | ## Update Summary 4 | 5 | - > ### 1.4.0 6 | - Support servo cartesian interface 7 | - Support for blocky code conversion and operation of xArmStudio1.4.0 8 | 9 | - > ### 1.3.0 10 | 11 | - Added several attributes 12 | - Support tool coordinate system movement 13 | - Support joint range limitation, collision rebound setting 14 | - Support user coordinate system setting 15 | - Support the status of the air pump 16 | - Added counter interface 17 | - Support for blocky code conversion and operation of xArmStudio1.3.0 18 | 19 | - > ### 1.2.0 20 | 21 | - Fix the parameters of the control box GPIO analog signal 22 | - Support for more Gcode commands 23 | - Support trajectory recording 24 | - Support for reduced mode settings 25 | - Optimize some interfaces and bug fixes 26 | - Support for blocky code conversion and operation of xArmStudio1.2.0 27 | 28 | - > ### 1.1.0 29 | 30 | - Modify the claw interface 31 | - Modify error code and error message 32 | - Support for blocky code conversion and operation of xArmStudio1.1.0 33 | 34 | - > ### 1.0.0 35 | 36 | - Support for the latest firmware 1.0.0, compatible with old firmware 37 | - Support mount angle setting interface 38 | - Support controller IO interface 39 | - Modify clip and Tool GPIO communication protocol, compatible with standard modbus 40 | - Modify the interface name and parameters of the Tool GPIO, note that the value of the parameter ionum is changed from [1, 2] to [0, 1] 41 | - Support for blocky code conversion and operation of xArmStudio1.0.0 42 | 43 | - > ### 0.2.1 44 | 45 | - Added GPIO example 46 | - Compatible with old reporting protocols using new reporting protocols 47 | - New tools to convert xArmStudio's app code into Python code 48 | 49 | - > ### 0.2.0 50 | 51 | - Support torque detection 52 | - Support drag teaching mode 53 | 54 | - > ### 0.1.1 55 | 56 | - Support GPIO interface 57 | 58 | - > ### 0.1.0 59 | 60 | - Compatible with 5, 6, and 7 axis robot arms 61 | 62 | - > ### 0.0.13 63 | 64 | - Supports trajectory repeat motion interface with circular interpolation, supports repetition times, calibration 65 | - Increase joint limits, attitude angle limits and cmd cache limits 66 | - Exchange the parameters of the attitude angle yaw and the attitude angle pitch, but retain the parameter position 67 | 68 | - > ### 0.0.12 69 | 70 | - By specifying the value of the default parameter of the interface in the instantiation parameter, using angle or radians 71 | - Unify all interfaces by default using angle or radians 72 | - More interface routines 73 | - More detailed interface documentation 74 | - Richer interface 75 | - Set interface alias 76 | - Modified the default values of the default parameters of some interfaces, but support the use of parameters to be compatible when instantiating 77 | 78 | - > ### 0.0.11 79 | 80 | - Support serial port and TCP connection 81 | - Support parameter to enable reporting 82 | - Support callback register and release 83 | - Support Gripper setting 84 | - Support servo setting (Some interfaces are limited to professional debugging) 85 | - Unified return value 86 | - Snaps an exception and returns the specified return value 87 | 88 | -------------------------------------------------------------------------------- /doc/api/xarm_api_code.md: -------------------------------------------------------------------------------- 1 | # xArmSDK API code description 2 | 3 | ## API return value status code 4 | - -9: emergency stop 5 | - -8: out of range 6 | - -7: joint angle limit 7 | - -6: cartesian pos limit 8 | - -5: revesed, no use 9 | - -4: command is not exist 10 | - -3: revesed, no use 11 | - -2: xArm is not ready, may be the motion is not enable or not set state 12 | - -1: xArm is disconnect or not connect 13 | - 0: success 14 | - 1: there are errors that have not been cleared 15 | - 2: there are warnings that have not been cleared 16 | - 3: get response timeout 17 | - 4: tcp reply length error 18 | - 5: tcp reply number error 19 | - 6: tcp protocol flag error 20 | - 7: tcp reply command and send command do not match 21 | - 8: send command error, may be network exception 22 | - 9: reversed, no use 23 | - 10: reversed, no use 24 | - 11: other error 25 | - 12: parameter error 26 | - 31: trajectory read/write failed 27 | - 32: trajectory read/write timeout 28 | - 33: playback trajectory timeout 29 | - 41: wait to set suction cup timeout 30 | 31 | ## Controller warning code 32 | - 11: uxbux que is full 33 | - 12: parameter error 34 | - 13: the instruction does not exist 35 | - 14: command has no solution 36 | 37 | ## Controller error code 38 | - 10: Servo motor error 39 | - 11: Servo motor 1 error 40 | - 12: Servo motor 2 error 41 | - 13: Servo motor 3 error 42 | - 14: Servo motor 4 error 43 | - 15: Servo motor 5 error 44 | - 16: Servo motor 6 error 45 | - 17: Servo motor 7 error 46 | - 19: Gripper Communication Error 47 | - 21: Kinematic Error 48 | - 22: Collision Error 49 | - 23: Joints Angle Exceed Limit 50 | - 24: Speed Exceeds Limit 51 | - 25: Planning Error 52 | - 26: Linux RT Error 53 | - 27: Command Reply Error 54 | - 28: End Module Communication Error 55 | - 29: Other Errors 56 | - 30: Feedback Speed Exceeds limit 57 | - 31: Collision Caused Abnormal Current 58 | - 32: Three-point drawing circle calculation error 59 | - 33: Controller GPIO error 60 | - 34: Recording Timeout 61 | - 35: Safety Boundary Limit 62 | 63 | ## Servo Error Code 64 | 65 | - 10: Current Detection Error 66 | - 11: Joint Current Overlimit 67 | - 12: Joint Speed Overlimit 68 | - 14: Position Command Overlimit 69 | - 15: Joints Overheat 70 | - 16: Encoder Initialization Error 71 | - 17: Single Ring Encoder Error 72 | - 18: Multi-turn Encoder Error 73 | - 19: Low Battery Voltage 74 | - 20: Driver IC Hardware Error 75 | - 21: Driver IC Initialization Error 76 | - 22: Encoder Configuration Error 77 | - 23: Large Motor Position Deviation 78 | - 26: Joint N Positive Overrun 79 | - 27: Joint N Negative Overrun 80 | - 28: Joint Commands Error 81 | - 33: Drive Overloaded 82 | - 34: Motor Overload 83 | - 35: Motor Type Error 84 | - 36: Driver Type Error 85 | - 39: Joint Voltage Overload 86 | - 40: Joint Voltage Insufficient 87 | - 49: EEPROM Read and Write Error 88 | - 52: Motor Angle Initialization Error 89 | 90 | ## Gripper Error Code 91 | 92 | - 9: Gripper Current Detection Error 93 | - 11: Gripper Current Overlimit 94 | - 12: Gripper Speed Overlimit 95 | - 14: Gripper Position Command Overlimit 96 | - 15: Gripper EEPROM Read and Write Error 97 | - 20: Gripper Driver IC Hardware Error 98 | - 21: Gripper Driver IC Initialization Error 99 | - 23: Gripper Large Motor Position Deviation 100 | - 25: Gripper Command Over Software Limit 101 | - 26: Gripper Feedback Position Software Limit 102 | - 33: Gripper Drive Overloaded 103 | - 34: Gripper Motor Overload 104 | - 36: Gripper Driver Type Error 105 | 106 | -------------------------------------------------------------------------------- /doc/tool/gen_api.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2017, UFactory, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Duke Fong 8 | 9 | 10 | import sys 11 | import os 12 | import pydoc 13 | import re 14 | 15 | sys.path.append(os.path.join(os.path.dirname(__file__), '../..')) 16 | 17 | from xarm.wrapper import XArmAPI 18 | 19 | from doc.tool.markdown_doc import MarkdownDoc 20 | 21 | open('../api/xarm_api.md', 'w', encoding='utf-8').write( 22 | pydoc.render_doc(XArmAPI, 23 | title='xArm-Python-SDK API Documentation: %s', 24 | renderer=MarkdownDoc())) 25 | 26 | # docs = pydoc.render_doc(XArmAPI, 27 | # title='xArm-Python-SDK API Documentation: %s', 28 | # renderer=MarkdownDoc()).split('\n') 29 | # d = {'#': 1, '##': 2, '###': 3, '####': 4, '#####': 5, '######': 6} 30 | # pattern = '#+\s' 31 | # 32 | # 33 | # head_id = 0 34 | # targetname = "../api/xarm_api.md" 35 | # with open(targetname, 'w+') as f2: 36 | # for i in docs: 37 | # if not re.match(pattern, i.strip(' \t\n')): 38 | # continue 39 | # i = i.strip(' \t\n') 40 | # head = i.split(' ')[0] 41 | # f2.write('|' + '-----' * (len(head) - 1) + '@[' + i[len(head):].strip(' \t\n') + '](#id' + str( 42 | # head_id) + ') \n') 43 | # head_id += 1 44 | # f2.write('\n') 45 | # head_id = 0 46 | # for i in docs: 47 | # if not re.match(pattern, i.strip(' \t\n')): 48 | # f2.write(i) 49 | # if '```' not in i: 50 | # f2.write(' \t\n') 51 | # else: 52 | # i = i.strip(' \t\n') 53 | # head = i.split(' ')[0] 54 | # if head in d.keys(): 55 | # menu = ''.join( 56 | # ['', i[len(head):].strip(' \t\n').replace('__', ''), ' \n']) 58 | # f2.write(menu) 59 | # head_id += 1 60 | # 61 | # print('done ...') 62 | 63 | -------------------------------------------------------------------------------- /doc/tool/markdown_doc.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | """ 4 | markdowndoc.py 5 | 6 | Written by Patrick Laban and Geremy Condra 7 | 8 | Licensed under GPLv3 9 | 10 | Released 28 April 2009 11 | 12 | This module contains a simple class to output Markdown-style 13 | pydocs. 14 | """ 15 | 16 | import pydoc, inspect, re, builtins 17 | 18 | class MarkdownDoc(pydoc.TextDoc): 19 | 20 | underline = "*" * 40 21 | 22 | def process_docstring(self, obj): 23 | """Get the docstring and turn it into a list.""" 24 | docstring = pydoc.getdoc(obj) 25 | return '```\n{}\n```\n\n'.format(docstring) if docstring else '' 26 | 27 | def process_class_name(self, name, bases, module): 28 | """Format the class's name and bases.""" 29 | title = "## class " + self.bold(name) 30 | if bases: 31 | # get the names of each of the bases 32 | base_titles = [pydoc.classname(base, module) for base in bases] 33 | # if its not just object 34 | if len(base_titles) > 1: 35 | # append the list to the title 36 | title += "(%s)" % ", ".join(base_titles) 37 | return title 38 | 39 | def process_subsection(self, name): 40 | """format the subsection as a header""" 41 | return "### " + name 42 | 43 | def docclass(self, cls, name=None, mod=None): 44 | """Produce text documentation for the class object cls.""" 45 | 46 | # the overall document, as a line-delimited list 47 | document = [] 48 | 49 | # get the object's actual name, defaulting to the passed in name 50 | name = name or cls.__name__ 51 | 52 | # get the object's bases 53 | bases = cls.__bases__ 54 | 55 | # get the object's module 56 | mod = cls.__module__ 57 | 58 | # get the object's MRO 59 | mro = [pydoc.classname(base, mod) for base in inspect.getmro(cls)] 60 | 61 | # get the object's classname, which should be printed 62 | classtitle = self.process_class_name(name, bases, mod) 63 | document.append(classtitle) 64 | document.append(self.underline) 65 | 66 | # get the object's docstring, which should be printed 67 | docstring = self.process_docstring(cls) 68 | document.append(docstring) 69 | 70 | # get all the attributes of the class 71 | attrs = [] 72 | for name, kind, classname, value in pydoc.classify_class_attrs(cls): 73 | if pydoc.visiblename(name): 74 | attrs.append((name, kind, classname, value)) 75 | 76 | # sort them into categories 77 | data, descriptors, methods = [], [], [] 78 | for attr in attrs: 79 | if attr[1] == "data" and not attr[0].startswith("_"): 80 | data.append(attr) 81 | elif attr[1] == "data descriptor" and not attr[0].startswith("_"): 82 | descriptors.append(attr) 83 | elif "method" in attr[1] and not attr[2] is builtins.object: 84 | methods.append(attr) 85 | 86 | if data: 87 | # start the data section 88 | document.append(self.process_subsection(self.bold("data"))) 89 | document.append(self.underline) 90 | 91 | # process your attributes 92 | for name, kind, classname, value in data: 93 | if hasattr(value, '__call__') or inspect.isdatadescriptor(value): 94 | doc = getdoc(value) 95 | else: 96 | doc = None 97 | document.append(self.docother(getattr(cls, name), name, mod, maxlen=70, doc=doc) + '\n') 98 | 99 | if descriptors: 100 | # start the descriptors section 101 | document.append(self.process_subsection(self.bold("Attributes"))) 102 | document.append(self.underline) 103 | 104 | # process your descriptors 105 | for name, kind, classname, value in descriptors: 106 | document.append(self._docdescriptor(name, value, mod)) 107 | 108 | if methods: 109 | # start the methods section 110 | document.append(self.underline) 111 | document.append(self.process_subsection(self.bold("Methods"))) 112 | document.append(self.underline) 113 | 114 | # process your methods 115 | for name, kind, classname, value in methods: 116 | if name == '__init__' or not name.startswith('__'): 117 | document.append(self.document(getattr(cls, name), name, mod, cls)) 118 | 119 | return "\n".join(document) 120 | 121 | def bold(self, text): 122 | """ Formats text as bold in markdown. """ 123 | # return text 124 | if text.startswith('_') and text.endswith('_'): 125 | return "__\%s\__" %text 126 | elif text.startswith('_'): 127 | return "__\%s__" %text 128 | elif text.endswith('_'): 129 | return "__%s\__" %text 130 | else: 131 | return "__%s__" %text 132 | 133 | def indent(self, text, prefix=''): 134 | """Indent text by prepending a given prefix to each line.""" 135 | return "```\n{}\n```".format(text) if text else text 136 | 137 | def section(self, title, contents): 138 | """Format a section with a given heading.""" 139 | clean_contents = self.indent(contents).rstrip() 140 | return "# " + self.bold(title) + '\n\n' + clean_contents + '\n\n' 141 | 142 | def docroutine(self, object, name=None, mod=None, cl=None): 143 | """Produce text documentation for a function or method object.""" 144 | realname = object.__name__ 145 | name = name or realname 146 | note = '' 147 | skipdocs = 0 148 | if inspect.ismethod(object): 149 | object = object.__func__ 150 | if name == realname: 151 | title = self.bold(realname) 152 | else: 153 | if (cl and realname in cl.__dict__ and cl.__dict__[realname] is object): 154 | skipdocs = 1 155 | title = self.bold(name) + ' = ' + realname 156 | if inspect.isfunction(object): 157 | args, varargs, varkw, defaults, kwonlyargs, kwdefaults, ann = inspect.getfullargspec(object) 158 | argspec = inspect.formatargspec( 159 | args, varargs, varkw, defaults, kwonlyargs, kwdefaults, ann, 160 | formatvalue=self.formatvalue, 161 | formatannotation=inspect.formatannotationrelativeto(object)) 162 | if realname == '': 163 | title = self.bold(name) + ' lambda ' 164 | # XXX lambda's won't usually have func_annotations['return'] 165 | # since the syntax doesn't support but it is possible. 166 | # So removing parentheses isn't truly safe. 167 | argspec = argspec[1:-1] # remove parentheses 168 | else: 169 | argspec = '(...)' 170 | decl = "#### " + "def " + title + argspec + ':' + '\n' + note 171 | 172 | if skipdocs: 173 | return decl + '\n' 174 | else: 175 | doc = pydoc.getdoc(object) or '' 176 | return decl + '\n' + (doc and self.indent(doc).rstrip() + '\n') 177 | 178 | def docother(self, object, name=None, mod=None, parent=None, maxlen=None, doc=None): 179 | """Produce text documentation for a data object.""" 180 | line = "#### " + object.__name__ + "\n" 181 | line += super().docother(object, name, mod, parent, maxlen, doc) 182 | return line + "\n" 183 | 184 | def _docdescriptor(self, name, value, mod): 185 | results = "" 186 | if name: results += "#### " + self.bold(name) + "\n" 187 | # doc = pydoc.getdoc(value) or "" 188 | docstring = pydoc.getdoc(value) or '' 189 | if docstring: 190 | doc = '```\n{}\n```'.format(docstring) if docstring else '' 191 | else: 192 | doc = '' 193 | if doc: results += doc + "\n" 194 | return results 195 | -------------------------------------------------------------------------------- /example/wrapper/common/0000-template.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: this is just an example template 11 | 1. Instantiate XArmAPI and specify do_not_open to be true 12 | 2. Registration error callback function 13 | 3. Connect 14 | 4. Enable motion 15 | 5. Setting mode 16 | 6. Setting state 17 | """ 18 | 19 | import os 20 | import sys 21 | import time 22 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 23 | 24 | from xarm.wrapper import XArmAPI 25 | 26 | 27 | ####################################################### 28 | """ 29 | Just for test example 30 | """ 31 | if len(sys.argv) >= 2: 32 | ip = sys.argv[1] 33 | else: 34 | try: 35 | from configparser import ConfigParser 36 | parser = ConfigParser() 37 | parser.read('../robot.conf') 38 | ip = parser.get('xArm', 'ip') 39 | except: 40 | ip = input('Please input the xArm ip address:') 41 | if not ip: 42 | print('input error, exit') 43 | sys.exit(1) 44 | ######################################################## 45 | 46 | 47 | def hangle_err_warn_changed(item): 48 | print('ErrorCode: {}, WarnCode: {}'.format(item['error_code'], item['warn_code'])) 49 | # TODO：Do different processing according to the error code 50 | 51 | 52 | arm = XArmAPI(ip, do_not_open=True) 53 | arm.register_error_warn_changed_callback(hangle_err_warn_changed) 54 | arm.connect() 55 | 56 | # enable motion 57 | arm.motion_enable(enable=True) 58 | # set mode: position control mode 59 | arm.set_mode(0) 60 | # set state: sport state 61 | arm.set_state(state=0) 62 | 63 | time.sleep(10) 64 | 65 | arm.disconnect() 66 | -------------------------------------------------------------------------------- /example/wrapper/common/0001-event_register.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: event callback registration and release 11 | 1. Instantiate XArmAPI and specify do_not_open to be true 12 | 2. Register different event callbacks as needed 13 | 3. Connect 14 | 4. Enable motion 15 | 5. Setting mode 16 | 6. Setting state 17 | """ 18 | 19 | import os 20 | import sys 21 | import time 22 | 23 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 24 | 25 | from xarm.wrapper import XArmAPI 26 | 27 | 28 | ####################################################### 29 | """ 30 | Just for test example 31 | """ 32 | if len(sys.argv) >= 2: 33 | ip = sys.argv[1] 34 | else: 35 | try: 36 | from configparser import ConfigParser 37 | parser = ConfigParser() 38 | parser.read('../robot.conf') 39 | ip = parser.get('xArm', 'ip') 40 | except: 41 | ip = input('Please input the xArm ip address:') 42 | if not ip: 43 | print('input error, exit') 44 | sys.exit(1) 45 | ######################################################## 46 | 47 | 48 | def callback_cmdnum_changed(item): 49 | print('cmdnum changed:', item) 50 | 51 | 52 | def callback_state_changed(item): 53 | print('state changed:', item) 54 | 55 | 56 | def callback_connect_changed(item): 57 | print('connect changed:', item) 58 | 59 | 60 | def callback_error_warn_changed(item): 61 | print('error warn changed:', item) 62 | 63 | 64 | def callback_maable_mtbrake_changed(item): 65 | print('maable mtbrake changed:', item) 66 | 67 | 68 | def callback_report_location(item): 69 | print('location report:', item) 70 | 71 | 72 | arm = XArmAPI(ip, do_not_open=True) 73 | arm.register_cmdnum_changed_callback(callback=callback_cmdnum_changed) 74 | arm.register_state_changed_callback(callback=callback_state_changed) 75 | arm.register_connect_changed_callback(callback=callback_connect_changed) 76 | arm.register_error_warn_changed_callback(callback=callback_error_warn_changed) 77 | arm.register_mtable_mtbrake_changed_callback(callback=callback_maable_mtbrake_changed) 78 | arm.register_report_location_callback(callback=callback_report_location) 79 | 80 | arm.connect() 81 | arm.motion_enable(enable=True) 82 | arm.set_mode(0) 83 | arm.set_state(state=0) 84 | 85 | time.sleep(20) 86 | arm.disconnect() 87 | -------------------------------------------------------------------------------- /example/wrapper/common/0002-get_property.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Get the property of XArmAPI 11 | Note: is_radian is different when instantiating, some attribute values are different 12 | """ 13 | 14 | import os 15 | import sys 16 | import time 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | time.sleep(1) 49 | 50 | print('=' * 50) 51 | print('default_is_radian:', arm.default_is_radian) 52 | print('version:', arm.version) 53 | print('state:', arm.state) 54 | print('mode:', arm.mode) 55 | print('cmdnum:', arm.cmd_num) 56 | print('error_code:', arm.error_code) 57 | print('warn_code:', arm.warn_code) 58 | print('collision_sensitivity:', arm.collision_sensitivity) 59 | print('teach_sensitivity:', arm.teach_sensitivity) 60 | print('world_offset:', arm.world_offset) 61 | print('gravity_direction:', arm.gravity_direction) 62 | 63 | print('============TCP============') 64 | print('* position:', arm.position) 65 | print('* tcp_jerk:', arm.tcp_jerk) 66 | print('* tcp_load:', arm.tcp_load) 67 | print('* tcp_offset:', arm.tcp_offset) 68 | print('* tcp_speed_limit:', arm.tcp_speed_limit) 69 | print('* tcp_acc_limit:', arm.tcp_acc_limit) 70 | 71 | print('===========JOINT===========') 72 | print('* angles:', arm.angles) 73 | print('* joint_jerk:', arm.joint_jerk) 74 | print('* joint_speed_limit:', arm.joint_speed_limit) 75 | print('* joint_acc_limit:', arm.joint_acc_limit) 76 | print('* joints_torque:', arm.joints_torque) 77 | 78 | arm.disconnect() 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /example/wrapper/common/0003-api_get.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Interface for obtaining information 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | 21 | 22 | ####################################################### 23 | """ 24 | Just for test example 25 | """ 26 | if len(sys.argv) >= 2: 27 | ip = sys.argv[1] 28 | else: 29 | try: 30 | from configparser import ConfigParser 31 | parser = ConfigParser() 32 | parser.read('../robot.conf') 33 | ip = parser.get('xArm', 'ip') 34 | except: 35 | ip = input('Please input the xArm ip address:') 36 | if not ip: 37 | print('input error, exit') 38 | sys.exit(1) 39 | ######################################################## 40 | 41 | 42 | arm = XArmAPI(ip) 43 | arm.motion_enable(enable=True) 44 | arm.set_mode(0) 45 | arm.set_state(state=0) 46 | 47 | print('=' * 50) 48 | print('version:', arm.get_version()) 49 | print('state:', arm.get_state()) 50 | print('cmdnum:', arm.get_cmdnum()) 51 | print('err_warn_code:', arm.get_err_warn_code()) 52 | print('position(°):', arm.get_position(is_radian=False)) 53 | print('position(radian):', arm.get_position(is_radian=True)) 54 | print('angles(°):', arm.get_servo_angle(is_radian=False)) 55 | print('angles(radian):', arm.get_servo_angle(is_radian=True)) 56 | print('angles(°)(servo_id=1):', arm.get_servo_angle(servo_id=1, is_radian=False)) 57 | print('angles(radian)(servo_id=1):', arm.get_servo_angle(servo_id=1, is_radian=True)) 58 | 59 | arm.disconnect() 60 | -------------------------------------------------------------------------------- /example/wrapper/common/0004-servo_attach_detach.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Servo attach/detach 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | 21 | 22 | ####################################################### 23 | """ 24 | Just for test example 25 | """ 26 | if len(sys.argv) >= 2: 27 | ip = sys.argv[1] 28 | else: 29 | try: 30 | from configparser import ConfigParser 31 | parser = ConfigParser() 32 | parser.read('../robot.conf') 33 | ip = parser.get('xArm', 'ip') 34 | except: 35 | ip = input('Please input the xArm ip address:') 36 | if not ip: 37 | print('input error, exit') 38 | sys.exit(1) 39 | ######################################################## 40 | 41 | 42 | arm = XArmAPI(ip) 43 | arm.motion_enable(enable=True) 44 | arm.set_mode(0) 45 | arm.set_state(state=0) 46 | 47 | # detach a servo 48 | ret = arm.set_servo_detach(servo_id=1) 49 | print('detach servo-1: {}'.format(ret)) 50 | time.sleep(10) 51 | # attach a servo 52 | ret = arm.set_servo_attach(servo_id=1) 53 | print('attach servo-1: {}'.format(ret)) 54 | time.sleep(5) 55 | arm.disconnect() 56 | -------------------------------------------------------------------------------- /example/wrapper/common/1001-move_line.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move line(linear motion) 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | 21 | 22 | ####################################################### 23 | """ 24 | Just for test example 25 | """ 26 | if len(sys.argv) >= 2: 27 | ip = sys.argv[1] 28 | else: 29 | try: 30 | from configparser import ConfigParser 31 | parser = ConfigParser() 32 | parser.read('../robot.conf') 33 | ip = parser.get('xArm', 'ip') 34 | except: 35 | ip = input('Please input the xArm ip address:') 36 | if not ip: 37 | print('input error, exit') 38 | sys.exit(1) 39 | ######################################################## 40 | 41 | 42 | arm = XArmAPI(ip) 43 | arm.motion_enable(enable=True) 44 | arm.set_mode(0) 45 | arm.set_state(state=0) 46 | 47 | arm.reset(wait=True) 48 | 49 | arm.set_position(x=300, y=0, z=150, roll=-180, pitch=0, yaw=0, speed=100, wait=True) 50 | print(arm.get_position(), arm.get_position(is_radian=True)) 51 | arm.set_position(x=300, y=200, z=250, roll=-180, pitch=0, yaw=0, speed=200, wait=True) 52 | print(arm.get_position(), arm.get_position(is_radian=True)) 53 | arm.set_position(x=500, y=200, z=150, roll=-180, pitch=0, yaw=0, speed=300, wait=True) 54 | print(arm.get_position(), arm.get_position(is_radian=True)) 55 | arm.set_position(x=500, y=-200, z=250, roll=-180, pitch=0, yaw=0, speed=400, wait=True) 56 | print(arm.get_position(), arm.get_position(is_radian=True)) 57 | arm.set_position(x=300, y=-200, z=150, roll=-180, pitch=0, yaw=0, speed=500, wait=True) 58 | print(arm.get_position(), arm.get_position(is_radian=True)) 59 | arm.set_position(x=300, y=0, z=250, roll=-180, pitch=0, yaw=0, speed=600, wait=True) 60 | print(arm.get_position(), arm.get_position(is_radian=True)) 61 | 62 | 63 | arm.reset(wait=True) 64 | 65 | arm.set_position(x=300, y=0, z=150, roll=-3.1415926, pitch=0, yaw=0, speed=100, is_radian=True, wait=True) 66 | print(arm.get_position(), arm.get_position(is_radian=True)) 67 | arm.set_position(x=300, y=200, z=250, roll=-3.1415926, pitch=0, yaw=0, speed=200, is_radian=True, wait=True) 68 | print(arm.get_position(), arm.get_position(is_radian=True)) 69 | arm.set_position(x=500, y=200, z=150, roll=-3.1415926, pitch=0, yaw=0, speed=300, is_radian=True, wait=True) 70 | print(arm.get_position(), arm.get_position(is_radian=True)) 71 | arm.set_position(x=500, y=-200, z=250, roll=-3.1415926, pitch=0, yaw=0, speed=400, is_radian=True, wait=True) 72 | print(arm.get_position(), arm.get_position(is_radian=True)) 73 | arm.set_position(x=300, y=-200, z=150, roll=-3.1415926, pitch=0, yaw=0, speed=500, is_radian=True, wait=True) 74 | print(arm.get_position(), arm.get_position(is_radian=True)) 75 | arm.set_position(x=300, y=0, z=250, roll=-3.1415926, pitch=0, yaw=0, speed=600, is_radian=True, wait=True) 76 | print(arm.get_position(), arm.get_position(is_radian=True)) 77 | 78 | arm.reset(wait=True) 79 | arm.disconnect() 80 | -------------------------------------------------------------------------------- /example/wrapper/common/1002-move_line.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move line(linear motion) 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | 21 | 22 | ####################################################### 23 | """ 24 | Just for test example 25 | """ 26 | if len(sys.argv) >= 2: 27 | ip = sys.argv[1] 28 | else: 29 | try: 30 | from configparser import ConfigParser 31 | parser = ConfigParser() 32 | parser.read('../robot.conf') 33 | ip = parser.get('xArm', 'ip') 34 | except: 35 | ip = input('Please input the xArm ip address:') 36 | if not ip: 37 | print('input error, exit') 38 | sys.exit(1) 39 | ######################################################## 40 | 41 | 42 | arm = XArmAPI(ip, is_radian=True) 43 | arm.motion_enable(enable=True) 44 | arm.set_mode(0) 45 | arm.set_state(state=0) 46 | 47 | arm.reset(wait=True) 48 | 49 | arm.set_position(x=300, y=0, z=150, roll=-180, pitch=0, yaw=0, speed=100, is_radian=False, wait=True) 50 | print(arm.get_position(), arm.get_position(is_radian=False)) 51 | arm.set_position(x=300, y=200, z=250, roll=-180, pitch=0, yaw=0, speed=200, is_radian=False, wait=True) 52 | print(arm.get_position(), arm.get_position(is_radian=False)) 53 | arm.set_position(x=500, y=200, z=150, roll=-180, pitch=0, yaw=0, speed=300, is_radian=False, wait=True) 54 | print(arm.get_position(), arm.get_position(is_radian=False)) 55 | arm.set_position(x=500, y=-200, z=250, roll=-180, pitch=0, yaw=0, speed=400, is_radian=False, wait=True) 56 | print(arm.get_position(), arm.get_position(is_radian=False)) 57 | arm.set_position(x=300, y=-200, z=150, roll=-180, pitch=0, yaw=0, speed=500, is_radian=False, wait=True) 58 | print(arm.get_position(), arm.get_position(is_radian=False)) 59 | arm.set_position(x=300, y=0, z=250, roll=-180, pitch=0, yaw=0, speed=600, is_radian=False, wait=True) 60 | print(arm.get_position(), arm.get_position(is_radian=False)) 61 | 62 | 63 | arm.reset(wait=True) 64 | 65 | arm.set_position(x=300, y=0, z=150, roll=-3.1415926, pitch=0, yaw=0, speed=100, wait=True) 66 | print(arm.get_position(), arm.get_position(is_radian=False)) 67 | arm.set_position(x=300, y=200, z=250, roll=-3.1415926, pitch=0, yaw=0, speed=200, wait=True) 68 | print(arm.get_position(), arm.get_position(is_radian=False)) 69 | arm.set_position(x=500, y=200, z=150, roll=-3.1415926, pitch=0, yaw=0, speed=300, wait=True) 70 | print(arm.get_position(), arm.get_position(is_radian=False)) 71 | arm.set_position(x=500, y=-200, z=250, roll=-3.1415926, pitch=0, yaw=0, speed=400, wait=True) 72 | print(arm.get_position(), arm.get_position(is_radian=False)) 73 | arm.set_position(x=300, y=-200, z=150, roll=-3.1415926, pitch=0, yaw=0, speed=500, wait=True) 74 | print(arm.get_position(), arm.get_position(is_radian=False)) 75 | arm.set_position(x=300, y=0, z=250, roll=-3.1415926, pitch=0, yaw=0, speed=600, wait=True) 76 | print(arm.get_position(), arm.get_position(is_radian=False)) 77 | 78 | arm.reset(wait=True) 79 | arm.disconnect() 80 | -------------------------------------------------------------------------------- /example/wrapper/common/1003-relative_move_line.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move line(linear motion), relative move 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | 21 | 22 | ####################################################### 23 | """ 24 | Just for test example 25 | """ 26 | if len(sys.argv) >= 2: 27 | ip = sys.argv[1] 28 | else: 29 | try: 30 | from configparser import ConfigParser 31 | parser = ConfigParser() 32 | parser.read('../robot.conf') 33 | ip = parser.get('xArm', 'ip') 34 | except: 35 | ip = input('Please input the xArm ip address:') 36 | if not ip: 37 | print('input error, exit') 38 | sys.exit(1) 39 | ######################################################## 40 | 41 | 42 | arm = XArmAPI(ip, is_radian=True) 43 | arm.motion_enable(enable=True) 44 | arm.set_mode(0) 45 | arm.set_state(state=0) 46 | 47 | arm.reset(wait=True) 48 | 49 | arm.set_position(x=100, relative=True, wait=True) 50 | print(arm.get_position(), arm.get_position(is_radian=False)) 51 | arm.set_position(y=200, z=100, relative=True, wait=True) 52 | print(arm.get_position(), arm.get_position(is_radian=False)) 53 | arm.set_position(x=200, z=-100, relative=True, wait=True) 54 | print(arm.get_position(), arm.get_position(is_radian=False)) 55 | arm.set_position(y=-400, z=100, relative=True, wait=True) 56 | print(arm.get_position(), arm.get_position(is_radian=False)) 57 | arm.set_position(x=-200, z=-100, relative=True, wait=True) 58 | print(arm.get_position(), arm.get_position(is_radian=False)) 59 | arm.set_position(y=200, z=100, relative=True, wait=True) 60 | print(arm.get_position(), arm.get_position(is_radian=False)) 61 | 62 | arm.reset(wait=True) 63 | arm.disconnect() 64 | -------------------------------------------------------------------------------- /example/wrapper/common/1004-move_arc_line.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Arc line(linear arc motion) 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | 21 | 22 | ####################################################### 23 | """ 24 | Just for test example 25 | """ 26 | if len(sys.argv) >= 2: 27 | ip = sys.argv[1] 28 | else: 29 | try: 30 | from configparser import ConfigParser 31 | parser = ConfigParser() 32 | parser.read('../robot.conf') 33 | ip = parser.get('xArm', 'ip') 34 | except: 35 | ip = input('Please input the xArm ip address:') 36 | if not ip: 37 | print('input error, exit') 38 | sys.exit(1) 39 | ######################################################## 40 | 41 | 42 | arm = XArmAPI(ip, is_radian=True) 43 | arm.motion_enable(enable=True) 44 | arm.set_mode(0) 45 | arm.set_state(state=0) 46 | 47 | arm.reset(wait=True) 48 | 49 | paths = [ 50 | [300, 0, 150, -180, 0, 0], 51 | [300, 200, 250, -180, 0, 0], 52 | [500, 200, 150, -180, 0, 0], 53 | [500, -200, 250, -180, 0, 0], 54 | [300, -200, 150, -180, 0, 0], 55 | [300, 0, 250, -180, 0, 0], 56 | [300, 200, 350, -180, 0, 0], 57 | [500, 200, 250, -180, 0, 0], 58 | [500, -200, 350, -180, 0, 0], 59 | [300, -200, 250, -180, 0, 0], 60 | [300, 0, 350, -180, 0, 0], 61 | ] 62 | 63 | arm.set_position(*paths[0], wait=True) 64 | _, angles = arm.get_servo_angle() 65 | arm.set_pause_time(0.2) 66 | 67 | 68 | def move(): 69 | ret = arm.set_servo_angle(angle=angles, is_radian=False, speed=50, wait=False) 70 | if ret < 0: 71 | print('set_servo_angle, ret={}'.format(ret)) 72 | return -1 73 | for path in paths: 74 | ret = arm.set_position(*path[:6], radius=0, is_radian=False, wait=False, speed=300) 75 | if ret < 0: 76 | print('set_position, ret={}'.format(ret)) 77 | return -1 78 | return 0 79 | 80 | for i in range(10): 81 | if move() != 0: 82 | break 83 | 84 | arm.reset(wait=True) 85 | arm.disconnect() 86 | -------------------------------------------------------------------------------- /example/wrapper/common/1005-move_arc_line.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Arc line(linear arc motion) 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | 21 | 22 | ####################################################### 23 | """ 24 | Just for test example 25 | """ 26 | if len(sys.argv) >= 2: 27 | ip = sys.argv[1] 28 | else: 29 | try: 30 | from configparser import ConfigParser 31 | parser = ConfigParser() 32 | parser.read('../robot.conf') 33 | ip = parser.get('xArm', 'ip') 34 | except: 35 | ip = input('Please input the xArm ip address:') 36 | if not ip: 37 | print('input error, exit') 38 | sys.exit(1) 39 | ######################################################## 40 | 41 | 42 | arm = XArmAPI(ip, is_radian=True) 43 | arm.motion_enable(enable=True) 44 | arm.set_mode(0) 45 | arm.set_state(state=0) 46 | 47 | arm.reset(wait=True) 48 | 49 | paths = [ 50 | [300, 0, 150, -180, 0, 0], 51 | [300, 200, 250, -180, 0, 0], 52 | [500, 200, 150, -180, 0, 0], 53 | [500, -200, 250, -180, 0, 0], 54 | [300, -200, 150, -180, 0, 0], 55 | [300, 0, 250, -180, 0, 0], 56 | [300, 200, 350, -180, 0, 0], 57 | [500, 200, 250, -180, 0, 0], 58 | [500, -200, 350, -180, 0, 0], 59 | [300, -200, 250, -180, 0, 0], 60 | [300, 0, 350, -180, 0, 0], 61 | ] 62 | 63 | arm.move_arc_lines(paths, is_radian=False, speed=300, times=10, wait=True) 64 | 65 | arm.reset(wait=True) 66 | arm.disconnect() 67 | -------------------------------------------------------------------------------- /example/wrapper/common/1006-move_tool_line.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Tool Line 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | 21 | 22 | ####################################################### 23 | """ 24 | Just for test example 25 | """ 26 | if len(sys.argv) >= 2: 27 | ip = sys.argv[1] 28 | else: 29 | try: 30 | from configparser import ConfigParser 31 | parser = ConfigParser() 32 | parser.read('../robot.conf') 33 | ip = parser.get('xArm', 'ip') 34 | except: 35 | ip = input('Please input the xArm ip address:') 36 | if not ip: 37 | print('input error, exit') 38 | sys.exit(1) 39 | ######################################################## 40 | 41 | 42 | arm = XArmAPI(ip, is_radian=True) 43 | arm.motion_enable(enable=True) 44 | arm.set_mode(0) 45 | arm.set_state(state=0) 46 | 47 | arm.reset(wait=True) 48 | 49 | arm.set_tool_position(x=100, y=0, z=0, roll=0, pitch=0, yaw=0, speed=100, is_radian=False, wait=True) 50 | print(arm.get_position(), arm.get_position(is_radian=False)) 51 | arm.set_tool_position(x=0, y=200, z=0, roll=0, pitch=0, yaw=0, speed=200, is_radian=False, wait=True) 52 | print(arm.get_position(), arm.get_position(is_radian=False)) 53 | arm.set_tool_position(x=200, y=0, z=0, roll=0, pitch=0, yaw=0, speed=300, is_radian=False, wait=True) 54 | print(arm.get_position(), arm.get_position(is_radian=False)) 55 | arm.set_tool_position(x=0, y=-400, z=0, roll=0, pitch=0, yaw=0, speed=400, is_radian=False, wait=True) 56 | print(arm.get_position(), arm.get_position(is_radian=False)) 57 | arm.set_tool_position(x=-200, y=0, z=0, roll=0, pitch=0, yaw=0, speed=500, is_radian=False, wait=True) 58 | print(arm.get_position(), arm.get_position(is_radian=False)) 59 | arm.set_tool_position(x=0, y=200, z=0, roll=0, pitch=0, yaw=0, speed=600, is_radian=False, wait=True) 60 | print(arm.get_position(), arm.get_position(is_radian=False)) 61 | 62 | arm.reset(wait=True) 63 | arm.disconnect() 64 | -------------------------------------------------------------------------------- /example/wrapper/common/1007-counter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Arc line(linear arc motion) 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | 21 | 22 | ####################################################### 23 | """ 24 | Just for test example 25 | """ 26 | if len(sys.argv) >= 2: 27 | ip = sys.argv[1] 28 | else: 29 | try: 30 | from configparser import ConfigParser 31 | parser = ConfigParser() 32 | parser.read('../robot.conf') 33 | ip = parser.get('xArm', 'ip') 34 | except: 35 | ip = input('Please input the xArm ip address:') 36 | if not ip: 37 | print('input error, exit') 38 | sys.exit(1) 39 | ######################################################## 40 | 41 | 42 | arm = XArmAPI(ip, is_radian=True) 43 | arm.motion_enable(enable=True) 44 | arm.set_mode(0) 45 | arm.set_state(state=0) 46 | 47 | 48 | def callback_count_changed(item): 49 | print('counter val: {}'.format(item['count'])) 50 | 51 | arm.register_count_changed_callback(callback_count_changed) 52 | 53 | 54 | arm.reset(wait=True) 55 | 56 | paths = [ 57 | [300, 0, 150, -180, 0, 0], 58 | [300, 200, 250, -180, 0, 0], 59 | [500, 200, 150, -180, 0, 0], 60 | [500, -200, 250, -180, 0, 0], 61 | [300, -200, 150, -180, 0, 0], 62 | [300, 0, 250, -180, 0, 0], 63 | [300, 200, 350, -180, 0, 0], 64 | [500, 200, 250, -180, 0, 0], 65 | [500, -200, 350, -180, 0, 0], 66 | [300, -200, 250, -180, 0, 0], 67 | [300, 0, 350, -180, 0, 0], 68 | ] 69 | 70 | arm.set_position(*paths[0], wait=True) 71 | _, angles = arm.get_servo_angle() 72 | arm.set_pause_time(0.2) 73 | 74 | 75 | def move(): 76 | ret = arm.set_servo_angle(angle=angles, is_radian=False, speed=50, wait=False) 77 | if ret < 0: 78 | print('set_servo_angle, ret={}'.format(ret)) 79 | return -1 80 | for path in paths: 81 | ret = arm.set_position(*path[:6], radius=0, is_radian=False, wait=False, speed=300) 82 | if ret < 0: 83 | print('set_position, ret={}'.format(ret)) 84 | return -1 85 | return 0 86 | 87 | arm.set_counter_reset() 88 | 89 | for i in range(100): 90 | if move() != 0: 91 | break 92 | arm.set_counter_increase() 93 | 94 | arm.reset(wait=True) 95 | arm.disconnect() 96 | -------------------------------------------------------------------------------- /example/wrapper/common/3001-move_circle.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Circle 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | 21 | 22 | ####################################################### 23 | """ 24 | Just for test example 25 | """ 26 | if len(sys.argv) >= 2: 27 | ip = sys.argv[1] 28 | else: 29 | try: 30 | from configparser import ConfigParser 31 | parser = ConfigParser() 32 | parser.read('../robot.conf') 33 | ip = parser.get('xArm', 'ip') 34 | except: 35 | ip = input('Please input the xArm ip address:') 36 | if not ip: 37 | print('input error, exit') 38 | sys.exit(1) 39 | ######################################################## 40 | 41 | 42 | arm = XArmAPI(ip, is_radian=True) 43 | arm.motion_enable(enable=True) 44 | arm.set_mode(0) 45 | arm.set_state(state=0) 46 | 47 | arm.reset(wait=True) 48 | 49 | poses = [ 50 | [300, 0, 100, -3.1416, 0, 0], 51 | [300, 100, 100, -3.1416, 0, 0], 52 | [400, 100, 100, -3.1416, 0, 0], 53 | [400, -100, 100, -3.1416, 0, 0], 54 | [300, 0, 300, -3.1416, 0, 0] 55 | ] 56 | 57 | ret = arm.set_position(*poses[0], speed=50, mvacc=100, wait=True) 58 | print('set_position, ret: {}'.format(ret)) 59 | 60 | ret = arm.move_circle(pose1=poses[1], pose2=poses[2], percent=50, speed=200, mvacc=1000, wait=True) 61 | print('move_circle, ret: {}'.format(ret)) 62 | 63 | ret = arm.move_circle(pose1=poses[3], pose2=poses[4], percent=200, speed=200, mvacc=1000, wait=True) 64 | print('move_circle, ret: {}'.format(ret)) 65 | 66 | arm.reset(wait=True) 67 | arm.disconnect() 68 | -------------------------------------------------------------------------------- /example/wrapper/common/3002-record_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Record trajectory 11 | 1. requires firmware 1.2.0 and above support 12 | """ 13 | 14 | import os 15 | import sys 16 | import time 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip, is_radian=True) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | 49 | # Turn on manual mode before recording 50 | arm.set_mode(2) 51 | arm.set_state(0) 52 | 53 | arm.start_record_trajectory() 54 | 55 | # Analog recording process, here with delay instead 56 | time.sleep(20) 57 | 58 | arm.stop_record_trajectory() 59 | arm.save_record_trajectory('test.traj') 60 | 61 | time.sleep(1) 62 | 63 | # Turn off manual mode after recording 64 | arm.set_mode(0) 65 | arm.set_state(0) 66 | -------------------------------------------------------------------------------- /example/wrapper/common/3003-playback_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: playback trajectory 11 | 1. requires firmware 1.2.0 and above support 12 | 2. need to record the trajectory in advance 13 | """ 14 | 15 | import os 16 | import sys 17 | import time 18 | 19 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 20 | 21 | from xarm.wrapper import XArmAPI 22 | 23 | 24 | ####################################################### 25 | """ 26 | Just for test example 27 | """ 28 | if len(sys.argv) >= 2: 29 | ip = sys.argv[1] 30 | else: 31 | try: 32 | from configparser import ConfigParser 33 | parser = ConfigParser() 34 | parser.read('../robot.conf') 35 | ip = parser.get('xArm', 'ip') 36 | except: 37 | ip = input('Please input the xArm ip address:') 38 | if not ip: 39 | print('input error, exit') 40 | sys.exit(1) 41 | ######################################################## 42 | 43 | 44 | arm = XArmAPI(ip, is_radian=True) 45 | arm.motion_enable(enable=True) 46 | arm.set_mode(0) 47 | arm.set_state(state=0) 48 | 49 | 50 | arm.load_trajectory('test.traj') 51 | arm.playback_trajectory() 52 | 53 | 54 | -------------------------------------------------------------------------------- /example/wrapper/common/5001-get_tgpio_digital.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Example: Get GPIO Digital 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 17 | 18 | from xarm.wrapper import XArmAPI 19 | from configparser import ConfigParser 20 | parser = ConfigParser() 21 | parser.read('../robot.conf') 22 | try: 23 | ip = parser.get('xArm', 'ip') 24 | except: 25 | ip = input('Please input the xArm ip address[192.168.1.194]:') 26 | if not ip: 27 | ip = '192.168.1.194' 28 | 29 | 30 | arm = XArmAPI(ip) 31 | time.sleep(0.5) 32 | if arm.warn_code != 0: 33 | arm.clean_warn() 34 | if arm.error_code != 0: 35 | arm.clean_error() 36 | 37 | last_digitals = [-1, -1] 38 | while arm.connected and arm.error_code != 19 and arm.error_code != 28: 39 | code, digitals = arm.get_tgpio_digital() 40 | if code == 0: 41 | if digitals[0] == 1 and digitals[0] != last_digitals[0]: 42 | print('IO0 input high level') 43 | if digitals[1] == 1 and digitals[1] != last_digitals[1]: 44 | print('IO1 input high level') 45 | last_digitals = digitals 46 | time.sleep(0.1) 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /example/wrapper/common/5002-get_tgpio_analog.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Example: Get Tool GPIO Analog 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 17 | 18 | from xarm.wrapper import XArmAPI 19 | from configparser import ConfigParser 20 | parser = ConfigParser() 21 | parser.read('../robot.conf') 22 | try: 23 | ip = parser.get('xArm', 'ip') 24 | except: 25 | ip = input('Please input the xArm ip address[192.168.1.194]:') 26 | if not ip: 27 | ip = '192.168.1.194' 28 | 29 | arm = XArmAPI(ip) 30 | time.sleep(0.5) 31 | if arm.warn_code != 0: 32 | arm.clean_warn() 33 | if arm.error_code != 0: 34 | arm.clean_error() 35 | 36 | while arm.connected and arm.error_code != 19 and arm.error_code != 28: 37 | code, analogs = arm.get_tgpio_analog() 38 | print('code: {}, analogs={}'.format(code, analogs)) 39 | time.sleep(1) 40 | code, analog = arm.get_tgpio_analog(0) 41 | print('code: {}, analog(io0)={}'.format(code, analog)) 42 | time.sleep(0.2) 43 | code, analog = arm.get_tgpio_analog(1) 44 | print('code: {}, analog(io1)={}'.format(code, analog)) 45 | time.sleep(0.5) 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /example/wrapper/common/5003-set_tgpio_digital.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Example: Set Tool GPIO Digital 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 17 | 18 | from xarm.wrapper import XArmAPI 19 | from configparser import ConfigParser 20 | parser = ConfigParser() 21 | parser.read('../robot.conf') 22 | try: 23 | ip = parser.get('xArm', 'ip') 24 | except: 25 | ip = input('Please input the xArm ip address[192.168.1.194]:') 26 | if not ip: 27 | ip = '192.168.1.194' 28 | 29 | 30 | arm = XArmAPI(ip, check_robot_sn=False) 31 | time.sleep(0.5) 32 | if arm.warn_code != 0: 33 | arm.clean_warn() 34 | if arm.error_code != 0: 35 | arm.clean_error() 36 | 37 | print('set IO0 high level') 38 | arm.set_tgpio_digital(0, 1) 39 | time.sleep(2) 40 | print('set IO1 high level') 41 | arm.set_tgpio_digital(1, 1) 42 | time.sleep(2) 43 | print('set IO0 low level') 44 | arm.set_tgpio_digital(0, 0) 45 | print('set IO1 low level') 46 | arm.set_tgpio_digital(1, 0) 47 | 48 | 49 | -------------------------------------------------------------------------------- /example/wrapper/common/5004-set_gripper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Example: Gripper Control 11 | Please make sure that the gripper is attached to the end. 12 | """ 13 | 14 | import os 15 | import sys 16 | import time 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | from configparser import ConfigParser 21 | parser = ConfigParser() 22 | parser.read('../robot.conf') 23 | try: 24 | ip = parser.get('xArm', 'ip') 25 | except: 26 | ip = input('Please input the xArm ip address[192.168.1.194]:') 27 | if not ip: 28 | ip = '192.168.1.194' 29 | 30 | 31 | arm = XArmAPI(ip) 32 | time.sleep(0.5) 33 | if arm.warn_code != 0: 34 | arm.clean_warn() 35 | if arm.error_code != 0: 36 | arm.clean_error() 37 | 38 | code = arm.set_gripper_mode(0) 39 | print('set gripper mode: location mode, code={}'.format(code)) 40 | 41 | code = arm.set_gripper_enable(True) 42 | print('set gripper enable, code={}'.format(code)) 43 | 44 | code = arm.set_gripper_speed(5000) 45 | print('set gripper speed, code={}'.format(code)) 46 | 47 | code = arm.set_gripper_position(600, wait=True) 48 | print('[wait]set gripper pos, code={}'.format(code)) 49 | 50 | code = arm.set_gripper_position(300, wait=True, speed=8000) 51 | print('[no wait]set gripper pos, code={}'.format(code)) 52 | 53 | -------------------------------------------------------------------------------- /example/wrapper/common/5005-get_cgpio_digital_analog.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Example: Get Controller GPIO Digital/Anglog 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 17 | 18 | from xarm.wrapper import XArmAPI 19 | from configparser import ConfigParser 20 | parser = ConfigParser() 21 | parser.read('../robot.conf') 22 | try: 23 | ip = parser.get('xArm', 'ip') 24 | except: 25 | ip = input('Please input the xArm ip address[192.168.1.194]:') 26 | if not ip: 27 | ip = '192.168.1.194' 28 | 29 | 30 | arm = XArmAPI(ip) 31 | time.sleep(0.5) 32 | if arm.warn_code != 0: 33 | arm.clean_warn() 34 | if arm.error_code != 0: 35 | arm.clean_error() 36 | 37 | code, digitals = arm.get_cgpio_digital() 38 | print('get_cgpio_digital, code={}, digitals={}'.format(code, digitals)) 39 | 40 | code, analogs = arm.get_cgpio_analog() 41 | print('get_cgpio_analog, code={}, analogs={}'.format(code, analogs)) 42 | 43 | 44 | -------------------------------------------------------------------------------- /example/wrapper/common/5006-set_cgpio_digital_analog.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Example: Set Controller GPIO Digital/Analog 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 17 | 18 | from xarm.wrapper import XArmAPI 19 | from configparser import ConfigParser 20 | parser = ConfigParser() 21 | parser.read('../robot.conf') 22 | try: 23 | ip = parser.get('xArm', 'ip') 24 | except: 25 | ip = input('Please input the xArm ip address[192.168.1.194]:') 26 | if not ip: 27 | ip = '192.168.1.194' 28 | 29 | arm = XArmAPI(ip) 30 | time.sleep(0.5) 31 | if arm.warn_code != 0: 32 | arm.clean_warn() 33 | if arm.error_code != 0: 34 | arm.clean_error() 35 | 36 | value = 0 37 | for i in range(8): 38 | code = arm.set_cgpio_digital(i, value) 39 | print('set_cgpio_digital({}, {}), code={}'.format(i, value, code)) 40 | time.sleep(0.5) 41 | 42 | value = 1 43 | for i in range(8): 44 | code = arm.set_cgpio_digital(i, value) 45 | print('set_cgpio_digital({}, {}), code={}'.format(i, value, code)) 46 | time.sleep(0.5) 47 | 48 | value = 2.6 49 | code = arm.set_cgpio_analog(0, value) 50 | print('set_cgpio_analog(0, {}), code={}'.format(value, code)) 51 | 52 | value = 3.6 53 | code = arm.set_cgpio_analog(1, value) 54 | print('set_cgpio_analog(1, {}), code={}'.format(value, code)) 55 | -------------------------------------------------------------------------------- /example/wrapper/common/5007-set_cgpio_input_output_function.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Example: Set Controller GPIO input/output function 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 17 | 18 | from xarm.wrapper import XArmAPI 19 | from configparser import ConfigParser 20 | parser = ConfigParser() 21 | parser.read('../robot.conf') 22 | try: 23 | ip = parser.get('xArm', 'ip') 24 | except: 25 | ip = input('Please input the xArm ip address[192.168.1.194]:') 26 | if not ip: 27 | ip = '192.168.1.194' 28 | 29 | 30 | arm = XArmAPI(ip) 31 | time.sleep(0.5) 32 | if arm.warn_code != 0: 33 | arm.clean_warn() 34 | if arm.error_code != 0: 35 | arm.clean_error() 36 | 37 | value = 2 38 | for i in range(4): 39 | code = arm.set_cgpio_digital_input_function(i, value) 40 | print('set_cgpio_digital_input_function({}, {}), code={}'.format(i, value, code)) 41 | # Reset: 255 42 | for i in range(4): 43 | code = arm.set_cgpio_digital_input_function(i, 255) 44 | print('set_cgpio_digital_input_function({}, {}), code={}'.format(i, value, code)) 45 | 46 | 47 | value = 0 48 | for i in range(4, 8): 49 | code = arm.set_cgpio_digital_output_function(i, value) 50 | print('set_cgpio_digital_output_function({}, {}), code={}'.format(i, value, code)) 51 | # Reset: 255 52 | for i in range(4, 8): 53 | code = arm.set_cgpio_digital_output_function(i, 255) 54 | print('set_cgpio_digital_output_function({}, {}), code={}'.format(i, value, code)) 55 | -------------------------------------------------------------------------------- /example/wrapper/common/5008-get_cgpio_state.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Example: Get Controller GPIO Digital/Anglog 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 17 | 18 | from xarm.wrapper import XArmAPI 19 | from configparser import ConfigParser 20 | parser = ConfigParser() 21 | parser.read('../robot.conf') 22 | try: 23 | ip = parser.get('xArm', 'ip') 24 | except: 25 | ip = input('Please input the xArm ip address[192.168.1.194]:') 26 | if not ip: 27 | ip = '192.168.1.194' 28 | 29 | 30 | arm = XArmAPI(ip) 31 | time.sleep(0.5) 32 | if arm.warn_code != 0: 33 | arm.clean_warn() 34 | if arm.error_code != 0: 35 | arm.clean_error() 36 | 37 | code, states = arm.get_cgpio_state() 38 | print('get_cgpio_state, code={}'.format(code)) 39 | print('GPIO state: {}'.format(states[0])) 40 | print('GPIO error code: {}'.format(states[1])) 41 | print('Digital->Input->FunctionalIO: {}'.format([states[2] >> i & 0x0001 for i in range(8)])) 42 | print('Digital->Input->ConfiguringIO: {}'.format([states[3] >> i & 0x0001 for i in range(8)])) 43 | print('Digital->Output->FunctionalIO: {}'.format([states[4] >> i & 0x0001 for i in range(8)])) 44 | print('Digital->Output->ConfiguringIO: {}'.format([states[5] >> i & 0x0001 for i in range(8)])) 45 | print('Analog->Input: {}'.format(states[6:8])) 46 | print('Analog->Output: {}'.format(states[8:10])) 47 | print('Digital->Input->Conf: {}'.format(states[10])) 48 | print('Digital->Output->Conf: {}'.format(states[11])) 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /example/wrapper/common/6001-set_reduced_mode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Example: Set reduced mode 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 17 | 18 | from xarm.wrapper import XArmAPI 19 | from configparser import ConfigParser 20 | parser = ConfigParser() 21 | parser.read('../robot.conf') 22 | try: 23 | ip = parser.get('xArm', 'ip') 24 | except: 25 | ip = input('Please input the xArm ip address[192.168.1.194]:') 26 | if not ip: 27 | ip = '192.168.1.194' 28 | 29 | 30 | arm = XArmAPI(ip) 31 | time.sleep(0.5) 32 | if arm.warn_code != 0: 33 | arm.clean_warn() 34 | if arm.error_code != 0: 35 | arm.clean_error() 36 | 37 | code = arm.set_reduced_max_joint_speed(100) 38 | print('set_reduced_max_joint_speed, code={}'.format(code)) 39 | arm.set_reduced_max_tcp_speed(500) 40 | print('set_reduced_max_tcp_speed, code={}'.format(code)) 41 | arm.set_reduced_mode(True) 42 | print('set_reduced_mode, code={}'.format(code)) 43 | 44 | 45 | -------------------------------------------------------------------------------- /example/wrapper/common/6002-set_fense_mode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Example: Set reduced mode 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 17 | 18 | from xarm.wrapper import XArmAPI 19 | from configparser import ConfigParser 20 | parser = ConfigParser() 21 | parser.read('../robot.conf') 22 | try: 23 | ip = parser.get('xArm', 'ip') 24 | except: 25 | ip = input('Please input the xArm ip address[192.168.1.194]:') 26 | if not ip: 27 | ip = '192.168.1.194' 28 | 29 | 30 | arm = XArmAPI(ip) 31 | time.sleep(0.5) 32 | if arm.warn_code != 0: 33 | arm.clean_warn() 34 | if arm.error_code != 0: 35 | arm.clean_error() 36 | 37 | x_max, x_min, y_max, y_min, z_max, z_min = 500, -500, 600, -600, 400, -400 38 | code = arm.set_reduced_tcp_boundary([x_max, x_min, y_max, y_min, z_max, z_min]) 39 | print('set_reduced_tcp_boundary, code={}'.format(code)) 40 | code = arm.set_fense_mode(True) 41 | print('set_fense_mode, code={}'.format(code)) 42 | -------------------------------------------------------------------------------- /example/wrapper/common/7001-servo_j.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Servo j 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | 21 | 22 | ####################################################### 23 | """ 24 | Just for test example 25 | """ 26 | if len(sys.argv) >= 2: 27 | ip = sys.argv[1] 28 | else: 29 | try: 30 | from configparser import ConfigParser 31 | parser = ConfigParser() 32 | parser.read('../robot.conf') 33 | ip = parser.get('xArm', 'ip') 34 | except: 35 | ip = input('Please input the xArm ip address:') 36 | if not ip: 37 | print('input error, exit') 38 | sys.exit(1) 39 | ######################################################## 40 | 41 | 42 | arm = XArmAPI(ip) 43 | arm.motion_enable(enable=True) 44 | arm.set_mode(0) 45 | arm.set_state(state=0) 46 | 47 | arm.reset(wait=True) 48 | 49 | arm.set_mode(1) 50 | arm.set_state(0) 51 | time.sleep(0.1) 52 | 53 | while arm.connected and arm.state != 4: 54 | for i in range(100): 55 | angles = [i, 0, 0, 0, 0, 0, 0] 56 | ret = arm.set_servo_angle_j(angles) 57 | print('set_servo_angle_j, ret={}'.format(ret)) 58 | time.sleep(0.01) 59 | for i in range(100): 60 | angles = [100-i, 0, 0, 0, 0, 0, 0] 61 | ret = arm.set_servo_angle_j(angles) 62 | print('set_servo_angle_j, ret={}'.format(ret)) 63 | time.sleep(0.01) 64 | 65 | arm.disconnect() 66 | -------------------------------------------------------------------------------- /example/wrapper/common/7002-servo_cartesian.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Servo cartesian 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | 21 | 22 | ####################################################### 23 | """ 24 | Just for test example 25 | """ 26 | if len(sys.argv) >= 2: 27 | ip = sys.argv[1] 28 | else: 29 | try: 30 | from configparser import ConfigParser 31 | parser = ConfigParser() 32 | parser.read('../robot.conf') 33 | ip = parser.get('xArm', 'ip') 34 | except: 35 | ip = input('Please input the xArm ip address:') 36 | if not ip: 37 | print('input error, exit') 38 | sys.exit(1) 39 | ######################################################## 40 | 41 | 42 | arm = XArmAPI(ip) 43 | arm.motion_enable(enable=True) 44 | arm.set_mode(0) 45 | arm.set_state(state=0) 46 | 47 | arm.reset(wait=True) 48 | 49 | arm.set_position(*[200, 0, 200, 180, 0, 0], wait=True) 50 | 51 | arm.set_mode(1) 52 | arm.set_state(0) 53 | time.sleep(0.1) 54 | 55 | while arm.connected and arm.state != 4: 56 | for i in range(300): 57 | x = 200 + i 58 | mvpose = [x, 0, 200, 180, 0, 0] 59 | ret = arm.set_servo_cartesian(mvpose, speed=100, mvacc=2000) 60 | print('set_servo_cartesian, ret={}'.format(ret)) 61 | time.sleep(0.01) 62 | for i in range(300): 63 | x = 500 - i 64 | mvpose = [x, 0, 200, 180, 0, 0] 65 | ret = arm.set_servo_cartesian(mvpose, speed=100, mvacc=2000) 66 | print('set_servo_cartesian, ret={}'.format(ret)) 67 | time.sleep(0.01) 68 | 69 | arm.disconnect() 70 | -------------------------------------------------------------------------------- /example/wrapper/robot.conf: -------------------------------------------------------------------------------- 1 | [xArm] 2 | ip = 10.0.0.113 3 | -------------------------------------------------------------------------------- /example/wrapper/thridparty/set_robotiq_gripper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Example: Robotiq Gripper Control 11 | Please make sure that the gripper is attached to the end. 12 | """ 13 | 14 | import os 15 | import sys 16 | import time 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | from configparser import ConfigParser 21 | parser = ConfigParser() 22 | parser.read('../robot.conf') 23 | try: 24 | ip = parser.get('xArm', 'ip') 25 | except: 26 | ip = input('Please input the xArm ip address[192.168.1.194]:') 27 | if not ip: 28 | ip = '192.168.1.194' 29 | 30 | 31 | arm = XArmAPI(ip) 32 | time.sleep(0.5) 33 | if arm.warn_code != 0: 34 | arm.clean_warn() 35 | if arm.error_code != 0: 36 | arm.clean_error() 37 | 38 | ret = arm.core.set_modbus_timeout(5) 39 | print('set modbus timeout, ret = %d' % (ret[0])) 40 | 41 | ret = arm.core.set_modbus_baudrate(115200) 42 | print('set modbus baudrate, ret = %d' % (ret[0])) 43 | 44 | # robotiq open/close test 45 | data_frame = [0x09, 0x10, 0x03, 0xE8, 0x00, 0x03, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00] 46 | ret = arm.core.tgpio_set_modbus(data_frame, len(data_frame)) 47 | print('set modbus, ret = %d' % (ret[0])) 48 | time.sleep(0.1) 49 | 50 | data_frame = [0x09, 0x10, 0x03, 0xE8, 0x00, 0x03, 0x06, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00] 51 | ret = arm.core.tgpio_set_modbus(data_frame, len(data_frame)) 52 | print('set modbus, ret = %d' % (ret[0])) 53 | 54 | arm.disconnect() 55 | -------------------------------------------------------------------------------- /example/wrapper/thridparty/set_yinshi_gripper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Example: yinshi gripper Control 11 | Please make sure that the gripper is attached to the end. 12 | """ 13 | 14 | import os 15 | import sys 16 | import time 17 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 18 | 19 | from xarm.wrapper import XArmAPI 20 | from configparser import ConfigParser 21 | parser = ConfigParser() 22 | parser.read('../robot.conf') 23 | try: 24 | ip = parser.get('xArm', 'ip') 25 | except: 26 | ip = input('Please input the xArm ip address[192.168.1.194]:') 27 | if not ip: 28 | ip = '192.168.1.194' 29 | 30 | 31 | arm = XArmAPI(ip) 32 | time.sleep(0.5) 33 | if arm.warn_code != 0: 34 | arm.clean_warn() 35 | if arm.error_code != 0: 36 | arm.clean_error() 37 | 38 | arm.motion_enable(True) 39 | arm.set_mode(0) 40 | arm.set_state(0) 41 | 42 | ret = arm.core.set_modbus_timeout(20) 43 | print('set modbus timeout, ret = %d' % (ret[0])) 44 | 45 | ret = arm.core.set_modbus_baudrate(2000000) 46 | print('set modbus baudrate, ret = %d' % (ret[0])) 47 | time.sleep(2) 48 | 49 | while arm.connected: 50 | # yinshi open/close test 51 | data_frame = [0x01, 0x06, 0x00, 0x0A, 0x00, 0x03] 52 | ret = arm.core.tgpio_set_modbus(data_frame, len(data_frame)) 53 | print('set modbus, ret = %d' % (ret[0])) 54 | time.sleep(2) 55 | 56 | data_frame = [0x01, 0x06, 0x00, 0x0A, 0x03, 0x60] 57 | ret = arm.core.tgpio_set_modbus(data_frame, len(data_frame)) 58 | print('set modbus, ret = %d' % (ret[0])) 59 | time.sleep(1) 60 | -------------------------------------------------------------------------------- /example/wrapper/tool/blockly_to_python.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Example: Generate Python code from the app generated by xArmStudio software 11 | """ 12 | 13 | import os 14 | import sys 15 | sys.path.append(os.path.join(os.path.dirname(__file__), '../..')) 16 | 17 | from xarm.tools.blockly_tool import BlocklyTool 18 | 19 | # blockly app path 20 | source_path = './example.xml' 21 | # the path is the python code to save 22 | target_path = './blockly_app.py' 23 | blockly = BlocklyTool(source_path) 24 | blockly.to_python(target_path) 25 | -------------------------------------------------------------------------------- /example/wrapper/tool/example.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | i 4 | 5 | 6 | 7 | 8 | forward 9 | TRUE 10 | 100 11 | 12 | 13 | left 14 | TRUE 15 | 100 16 | 17 | 18 | forward 19 | TRUE 20 | 100 21 | 22 | 23 | right 24 | TRUE 25 | 200 26 | 27 | 28 | up 29 | TRUE 30 | 100 31 | 32 | 33 | left 34 | TRUE 35 | 100 36 | 37 | 38 | down 39 | TRUE 40 | 100 41 | 42 | 43 | backward 44 | TRUE 45 | 100 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /example/wrapper/xarm5/2001-move_joint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | arm.reset(wait=True) 49 | 50 | speed = 50 51 | arm.set_servo_angle(angle=[90, 0, 0, 0, 0], speed=speed, wait=True) 52 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 53 | arm.set_servo_angle(angle=[90, 0, -60, 0, 0], speed=speed, wait=True) 54 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 55 | arm.set_servo_angle(angle=[90, -30, -60, 0, 0], speed=speed, wait=True) 56 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 57 | arm.set_servo_angle(angle=[0, -30, -60, 0, 0], speed=speed, wait=True) 58 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 59 | arm.set_servo_angle(angle=[0, 0, -60, 0, 0], speed=speed, wait=True) 60 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 61 | arm.set_servo_angle(angle=[0, 0, 0, 0, 0], speed=speed, wait=True) 62 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 63 | 64 | 65 | arm.reset(wait=True) 66 | speed = math.radians(50) 67 | arm.set_servo_angle(angle=[math.radians(90), 0, 0, 0, 0], speed=speed, is_radian=True, wait=True) 68 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 69 | arm.set_servo_angle(angle=[math.radians(90), 0, math.radians(-60), 0, 0], speed=speed, is_radian=True, wait=True) 70 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 71 | arm.set_servo_angle(angle=[math.radians(90), math.radians(-30), math.radians(-60), 0, 0], speed=speed, is_radian=True, wait=True) 72 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 73 | arm.set_servo_angle(angle=[0, math.radians(-30), math.radians(-60), 0, 0], speed=speed, is_radian=True, wait=True) 74 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 75 | arm.set_servo_angle(angle=[0, 0, math.radians(-60), 0, 0], speed=speed, is_radian=True, wait=True) 76 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 77 | arm.set_servo_angle(angle=[0, 0, 0, 0, 0], speed=speed, is_radian=True, wait=True) 78 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 79 | 80 | 81 | arm.reset(wait=True) 82 | arm.disconnect() 83 | -------------------------------------------------------------------------------- /example/wrapper/xarm5/2002-move_joint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip, is_radian=True) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | arm.reset(wait=True) 49 | 50 | speed = 50 51 | arm.set_servo_angle(angle=[90, 0, 0, 0, 0], speed=speed, is_radian=False, wait=True) 52 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 53 | arm.set_servo_angle(angle=[90, 0, -60, 0, 0], speed=speed, is_radian=False, wait=True) 54 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 55 | arm.set_servo_angle(angle=[90, -30, -60, 0, 0], speed=speed, is_radian=False, wait=True) 56 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 57 | arm.set_servo_angle(angle=[0, -30, -60, 0, 0], speed=speed, is_radian=False, wait=True) 58 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 59 | arm.set_servo_angle(angle=[0, 0, -60, 0, 0], speed=speed, is_radian=False, wait=True) 60 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 61 | arm.set_servo_angle(angle=[0, 0, 0, 0, 0], speed=speed, is_radian=False, wait=True) 62 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 63 | 64 | 65 | arm.reset(wait=True) 66 | speed = math.radians(50) 67 | arm.set_servo_angle(angle=[math.radians(90), 0, 0, 0, 0], speed=speed, wait=True) 68 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 69 | arm.set_servo_angle(angle=[math.radians(90), 0, math.radians(-60), 0, 0], speed=speed, wait=True) 70 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 71 | arm.set_servo_angle(angle=[math.radians(90), math.radians(-30), math.radians(-60), 0, 0], speed=speed, wait=True) 72 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 73 | arm.set_servo_angle(angle=[0, math.radians(-30), math.radians(-60), 0, 0], speed=speed, wait=True) 74 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 75 | arm.set_servo_angle(angle=[0, 0, math.radians(-60), 0, 0], speed=speed, wait=True) 76 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 77 | arm.set_servo_angle(angle=[0, 0, 0, 0, 0], speed=speed, wait=True) 78 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 79 | 80 | 81 | arm.reset(wait=True) 82 | arm.disconnect() 83 | -------------------------------------------------------------------------------- /example/wrapper/xarm5/2003-move_joint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | arm.reset(wait=True) 49 | 50 | speed = 50 51 | arm.set_servo_angle(servo_id=1, angle=90, speed=speed, wait=True) 52 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 53 | arm.set_servo_angle(servo_id=3, angle=-60, speed=speed, wait=True) 54 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 55 | arm.set_servo_angle(servo_id=2, angle=-30, speed=speed, wait=True) 56 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 57 | arm.set_servo_angle(servo_id=1, angle=0, speed=speed, wait=True) 58 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 59 | arm.set_servo_angle(servo_id=2, angle=0, speed=speed, wait=True) 60 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 61 | arm.set_servo_angle(servo_id=3, angle=0, speed=speed, wait=True) 62 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 63 | 64 | arm.reset(wait=True) 65 | 66 | speed = math.radians(50) 67 | arm.set_servo_angle(servo_id=1, angle=math.radians(90), speed=speed, is_radian=True, wait=True) 68 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 69 | arm.set_servo_angle(servo_id=3, angle=math.radians(-60), speed=speed, is_radian=True, wait=True) 70 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 71 | arm.set_servo_angle(servo_id=2, angle=math.radians(-30), speed=speed, is_radian=True, wait=True) 72 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 73 | arm.set_servo_angle(servo_id=1, angle=0, speed=speed, is_radian=True, wait=True) 74 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 75 | arm.set_servo_angle(servo_id=2, angle=0, speed=speed, is_radian=True, wait=True) 76 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 77 | arm.set_servo_angle(servo_id=3, angle=0, speed=speed, is_radian=True, wait=True) 78 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 79 | 80 | 81 | arm.reset(wait=True) 82 | arm.disconnect() 83 | -------------------------------------------------------------------------------- /example/wrapper/xarm5/2004-move_joint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip, is_radian=True) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | arm.reset(wait=True) 49 | 50 | speed = 50 51 | arm.set_servo_angle(servo_id=1, angle=90, speed=speed, is_radian=False, wait=True) 52 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 53 | arm.set_servo_angle(servo_id=3, angle=-60, speed=speed, is_radian=False, wait=True) 54 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 55 | arm.set_servo_angle(servo_id=2, angle=-30, speed=speed, is_radian=False, wait=True) 56 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 57 | arm.set_servo_angle(servo_id=1, angle=0, speed=speed, is_radian=False, wait=True) 58 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 59 | arm.set_servo_angle(servo_id=2, angle=0, speed=speed, is_radian=False, wait=True) 60 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 61 | arm.set_servo_angle(servo_id=3, angle=0, speed=speed, is_radian=False, wait=True) 62 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 63 | 64 | arm.reset(wait=True) 65 | 66 | speed = math.radians(50) 67 | arm.set_servo_angle(servo_id=1, angle=math.radians(90), speed=speed, wait=True) 68 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 69 | arm.set_servo_angle(servo_id=3, angle=math.radians(-60), speed=speed, wait=True) 70 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 71 | arm.set_servo_angle(servo_id=2, angle=math.radians(-30), speed=speed, wait=True) 72 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 73 | arm.set_servo_angle(servo_id=1, angle=0, speed=speed, wait=True) 74 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 75 | arm.set_servo_angle(servo_id=2, angle=0, speed=speed, wait=True) 76 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 77 | arm.set_servo_angle(servo_id=3, angle=0, speed=speed, wait=True) 78 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 79 | 80 | 81 | arm.reset(wait=True) 82 | arm.disconnect() 83 | -------------------------------------------------------------------------------- /example/wrapper/xarm6/2001-move_joint-pybullet-vis.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | import pybullet as p 19 | import pybullet_data as pd 20 | p.connect(p.GUI)#, options="--background_color_red=1.0 --background_color_blue=1.0 --background_color_green=1.0") 21 | p.setAdditionalSearchPath(pd.getDataPath()) 22 | 23 | useFixedBase = True 24 | flags = p.URDF_INITIALIZE_SAT_FEATURES#0#p.URDF_USE_SELF_COLLISION 25 | 26 | #plane_pos = [0,0,0] 27 | #plane = p.loadURDF("plane.urdf", plane_pos, flags = flags, useFixedBase=useFixedBase) 28 | table_pos = [0,0,-0.625] 29 | table = p.loadURDF("table/table.urdf", table_pos, flags = flags, useFixedBase=useFixedBase) 30 | xarm = p.loadURDF("xarm/xarm6_robot.urdf", flags = flags, useFixedBase=useFixedBase) 31 | 32 | jointIds = [] 33 | paramIds = [] 34 | 35 | for j in range(p.getNumJoints(xarm)): 36 | p.changeDynamics(xarm, j, linearDamping=0, angularDamping=0) 37 | info = p.getJointInfo(xarm, j) 38 | #print(info) 39 | jointName = info[1] 40 | jointType = info[2] 41 | if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE): 42 | jointIds.append(j) 43 | paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, 0)) 44 | 45 | skip_cam_frames = 10 46 | 47 | 48 | 49 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 50 | 51 | from xarm.wrapper import XArmAPI 52 | 53 | 54 | ####################################################### 55 | """ 56 | Just for test example 57 | """ 58 | if len(sys.argv) >= 2: 59 | ip = sys.argv[1] 60 | else: 61 | try: 62 | from configparser import ConfigParser 63 | parser = ConfigParser() 64 | parser.read('../robot.conf') 65 | ip = parser.get('xArm', 'ip') 66 | except: 67 | ip = input('Please input the xArm ip address:') 68 | if not ip: 69 | print('input error, exit') 70 | sys.exit(1) 71 | ######################################################## 72 | 73 | 74 | arm = XArmAPI(ip) 75 | arm.motion_enable(enable=True) 76 | arm.set_mode(0) 77 | 78 | arm.set_state(state=0) 79 | 80 | arm.reset(wait=True) 81 | speed = math.radians(50) 82 | 83 | 84 | while (p.isConnected()): 85 | p.stepSimulation() 86 | targetPositions = arm.get_servo_angle(is_radian=True)[1] 87 | #print("targetPositions=",targetPositions) 88 | for i in range(len(paramIds)): 89 | targetPos = targetPositions [i] 90 | p.setJointMotorControl2(xarm, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.) 91 | skip_cam_frames -= 1 92 | #if (skip_cam_frames<0): 93 | 94 | # p.getCameraImage(320,200, renderer=p.ER_BULLET_HARDWARE_OPENGL ) 95 | # skip_cam_frames = 10 96 | #time.sleep(1./240.) 97 | 98 | 99 | arm.reset(wait=True) 100 | arm.disconnect() 101 | -------------------------------------------------------------------------------- /example/wrapper/xarm6/2001-move_joint-pybullet.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | import pybullet as p 19 | import pybullet_data as pd 20 | p.connect(p.GUI)#, options="--background_color_red=1.0 --background_color_blue=1.0 --background_color_green=1.0") 21 | p.setAdditionalSearchPath(pd.getDataPath()) 22 | 23 | useFixedBase = True 24 | flags = p.URDF_INITIALIZE_SAT_FEATURES#0#p.URDF_USE_SELF_COLLISION 25 | 26 | #plane_pos = [0,0,0] 27 | #plane = p.loadURDF("plane.urdf", plane_pos, flags = flags, useFixedBase=useFixedBase) 28 | table_pos = [0,0,-0.625] 29 | table = p.loadURDF("table/table.urdf", table_pos, flags = flags, useFixedBase=useFixedBase) 30 | xarm = p.loadURDF("xarm/xarm6_robot.urdf", flags = flags, useFixedBase=useFixedBase) 31 | 32 | jointIds = [] 33 | paramIds = [] 34 | 35 | for j in range(p.getNumJoints(xarm)): 36 | p.changeDynamics(xarm, j, linearDamping=0, angularDamping=0) 37 | info = p.getJointInfo(xarm, j) 38 | #print(info) 39 | jointName = info[1] 40 | jointType = info[2] 41 | if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE): 42 | jointIds.append(j) 43 | paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, 0)) 44 | 45 | skip_cam_frames = 10 46 | 47 | 48 | 49 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 50 | 51 | from xarm.wrapper import XArmAPI 52 | 53 | 54 | ####################################################### 55 | """ 56 | Just for test example 57 | """ 58 | if len(sys.argv) >= 2: 59 | ip = sys.argv[1] 60 | else: 61 | try: 62 | from configparser import ConfigParser 63 | parser = ConfigParser() 64 | parser.read('../robot.conf') 65 | ip = parser.get('xArm', 'ip') 66 | except: 67 | ip = input('Please input the xArm ip address:') 68 | if not ip: 69 | print('input error, exit') 70 | sys.exit(1) 71 | ######################################################## 72 | 73 | 74 | arm = XArmAPI(ip) 75 | arm.motion_enable(enable=True) 76 | arm.set_mode(0) 77 | arm.set_state(state=0) 78 | arm.reset(wait=True) 79 | arm.set_mode(1) 80 | arm.set_state(state=0) 81 | speed = math.radians(5) 82 | 83 | 84 | while (p.isConnected()): 85 | p.stepSimulation() 86 | targetPositions=[] 87 | for i in range(len(paramIds)): 88 | c = paramIds[i] 89 | targetPos = p.readUserDebugParameter(c) 90 | p.setJointMotorControl2(xarm, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.) 91 | targetPositions.append(targetPos) 92 | skip_cam_frames -= 1 93 | arm.set_servo_angle_j(angles=targetPositions, speed=speed, is_radian=True) 94 | if (skip_cam_frames<0): 95 | p.getCameraImage(320,200, renderer=p.ER_BULLET_HARDWARE_OPENGL ) 96 | skip_cam_frames = 20 97 | time.sleep(1./240.) 98 | 99 | 100 | arm.reset(wait=True) 101 | arm.disconnect() 102 | -------------------------------------------------------------------------------- /example/wrapper/xarm6/2001-move_joint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | arm.reset(wait=True) 49 | 50 | speed = 50 51 | arm.set_servo_angle(angle=[90, 0, 0, 0, 0, 0], speed=speed, wait=True) 52 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 53 | arm.set_servo_angle(angle=[90, 0, -60, 0, 0, 0], speed=speed, wait=True) 54 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 55 | arm.set_servo_angle(angle=[90, -30, -60, 0, 0, 0], speed=speed, wait=True) 56 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 57 | arm.set_servo_angle(angle=[0, -30, -60, 0, 0, 0], speed=speed, wait=True) 58 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 59 | arm.set_servo_angle(angle=[0, 0, -60, 0, 0, 0], speed=speed, wait=True) 60 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 61 | arm.set_servo_angle(angle=[0, 0, 0, 0, 0, 0], speed=speed, wait=True) 62 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 63 | 64 | 65 | arm.reset(wait=True) 66 | speed = math.radians(50) 67 | arm.set_servo_angle(angle=[math.radians(90), 0, 0, 0, 0, 0], speed=speed, is_radian=True, wait=True) 68 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 69 | arm.set_servo_angle(angle=[math.radians(90), 0, math.radians(-60), 0, 0, 0], speed=speed, is_radian=True, wait=True) 70 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 71 | arm.set_servo_angle(angle=[math.radians(90), math.radians(-30), math.radians(-60), 0, 0, 0], speed=speed, is_radian=True, wait=True) 72 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 73 | arm.set_servo_angle(angle=[0, math.radians(-30), math.radians(-60), 0, 0, 0], speed=speed, is_radian=True, wait=True) 74 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 75 | arm.set_servo_angle(angle=[0, 0, math.radians(-60), 0, 0, 0], speed=speed, is_radian=True, wait=True) 76 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 77 | arm.set_servo_angle(angle=[0, 0, 0, 0, 0, 0], speed=speed, is_radian=True, wait=True) 78 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 79 | 80 | 81 | arm.reset(wait=True) 82 | arm.disconnect() 83 | -------------------------------------------------------------------------------- /example/wrapper/xarm6/2002-move_joint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip, is_radian=True) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | arm.reset(wait=True) 49 | 50 | speed = 50 51 | arm.set_servo_angle(angle=[90, 0, 0, 0, 0, 0], speed=speed, is_radian=False, wait=True) 52 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 53 | arm.set_servo_angle(angle=[90, 0, -60, 0, 0, 0], speed=speed, is_radian=False, wait=True) 54 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 55 | arm.set_servo_angle(angle=[90, -30, -60, 0, 0, 0], speed=speed, is_radian=False, wait=True) 56 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 57 | arm.set_servo_angle(angle=[0, -30, -60, 0, 0, 0], speed=speed, is_radian=False, wait=True) 58 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 59 | arm.set_servo_angle(angle=[0, 0, -60, 0, 0, 0], speed=speed, is_radian=False, wait=True) 60 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 61 | arm.set_servo_angle(angle=[0, 0, 0, 0, 0, 0], speed=speed, is_radian=False, wait=True) 62 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 63 | 64 | 65 | arm.reset(wait=True) 66 | speed = math.radians(50) 67 | arm.set_servo_angle(angle=[math.radians(90), 0, 0, 0, 0, 0], speed=speed, wait=True) 68 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 69 | arm.set_servo_angle(angle=[math.radians(90), 0, math.radians(-60), 0, 0, 0], speed=speed, wait=True) 70 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 71 | arm.set_servo_angle(angle=[math.radians(90), math.radians(-30), math.radians(-60), 0, 0, 0], speed=speed, wait=True) 72 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 73 | arm.set_servo_angle(angle=[0, math.radians(-30), math.radians(-60), 0, 0, 0], speed=speed, wait=True) 74 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 75 | arm.set_servo_angle(angle=[0, 0, math.radians(-60), 0, 0, 0], speed=speed, wait=True) 76 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 77 | arm.set_servo_angle(angle=[0, 0, 0, 0, 0, 0], speed=speed, wait=True) 78 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 79 | 80 | 81 | arm.reset(wait=True) 82 | arm.disconnect() 83 | -------------------------------------------------------------------------------- /example/wrapper/xarm6/2003-move_joint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | arm.reset(wait=True) 49 | 50 | speed = 50 51 | arm.set_servo_angle(servo_id=1, angle=90, speed=speed, wait=True) 52 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 53 | arm.set_servo_angle(servo_id=3, angle=-60, speed=speed, wait=True) 54 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 55 | arm.set_servo_angle(servo_id=2, angle=-30, speed=speed, wait=True) 56 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 57 | arm.set_servo_angle(servo_id=1, angle=0, speed=speed, wait=True) 58 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 59 | arm.set_servo_angle(servo_id=2, angle=0, speed=speed, wait=True) 60 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 61 | arm.set_servo_angle(servo_id=3, angle=0, speed=speed, wait=True) 62 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 63 | 64 | arm.reset(wait=True) 65 | 66 | speed = math.radians(50) 67 | arm.set_servo_angle(servo_id=1, angle=math.radians(90), speed=speed, is_radian=True, wait=True) 68 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 69 | arm.set_servo_angle(servo_id=3, angle=math.radians(-60), speed=speed, is_radian=True, wait=True) 70 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 71 | arm.set_servo_angle(servo_id=2, angle=math.radians(-30), speed=speed, is_radian=True, wait=True) 72 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 73 | arm.set_servo_angle(servo_id=1, angle=0, speed=speed, is_radian=True, wait=True) 74 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 75 | arm.set_servo_angle(servo_id=2, angle=0, speed=speed, is_radian=True, wait=True) 76 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 77 | arm.set_servo_angle(servo_id=3, angle=0, speed=speed, is_radian=True, wait=True) 78 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 79 | 80 | 81 | arm.reset(wait=True) 82 | arm.disconnect() 83 | -------------------------------------------------------------------------------- /example/wrapper/xarm6/2004-move_joint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip, is_radian=True) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | arm.reset(wait=True) 49 | 50 | speed = 50 51 | arm.set_servo_angle(servo_id=1, angle=90, speed=speed, is_radian=False, wait=True) 52 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 53 | arm.set_servo_angle(servo_id=3, angle=-60, speed=speed, is_radian=False, wait=True) 54 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 55 | arm.set_servo_angle(servo_id=2, angle=-30, speed=speed, is_radian=False, wait=True) 56 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 57 | arm.set_servo_angle(servo_id=1, angle=0, speed=speed, is_radian=False, wait=True) 58 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 59 | arm.set_servo_angle(servo_id=2, angle=0, speed=speed, is_radian=False, wait=True) 60 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 61 | arm.set_servo_angle(servo_id=3, angle=0, speed=speed, is_radian=False, wait=True) 62 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 63 | 64 | arm.reset(wait=True) 65 | 66 | speed = math.radians(50) 67 | arm.set_servo_angle(servo_id=1, angle=math.radians(90), speed=speed, wait=True) 68 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 69 | arm.set_servo_angle(servo_id=3, angle=math.radians(-60), speed=speed, wait=True) 70 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 71 | arm.set_servo_angle(servo_id=2, angle=math.radians(-30), speed=speed, wait=True) 72 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 73 | arm.set_servo_angle(servo_id=1, angle=0, speed=speed, wait=True) 74 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 75 | arm.set_servo_angle(servo_id=2, angle=0, speed=speed, wait=True) 76 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 77 | arm.set_servo_angle(servo_id=3, angle=0, speed=speed, wait=True) 78 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 79 | 80 | 81 | arm.reset(wait=True) 82 | arm.disconnect() 83 | -------------------------------------------------------------------------------- /example/wrapper/xarm6/loadxarm_sim.py: -------------------------------------------------------------------------------- 1 | import pybullet as p 2 | import pybullet_data as pd 3 | import math 4 | import time 5 | import numpy as np 6 | import xarm_sim 7 | 8 | p.connect(p.GUI) 9 | p.setAdditionalSearchPath(pd.getDataPath()) 10 | 11 | timeStep=1./60. 12 | p.setTimeStep(timeStep) 13 | p.setGravity(0,0,-9.8) 14 | 15 | xarm = xarm_sim.XArm6Sim(p,[0,0,0]) 16 | while (1): 17 | xarm.step() 18 | p.stepSimulation() 19 | time.sleep(timeStep) 20 | 21 | -------------------------------------------------------------------------------- /example/wrapper/xarm6/xarm_real_ik.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import pybullet as p 4 | import pybullet_data as pd 5 | import math 6 | import time 7 | import numpy as np 8 | import xarm_sim 9 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 10 | from xarm.wrapper import XArmAPI 11 | ####################################################### 12 | """ 13 | Just for test example 14 | """ 15 | if len(sys.argv) >= 2: 16 | ip = sys.argv[1] 17 | else: 18 | try: 19 | from configparser import ConfigParser 20 | parser = ConfigParser() 21 | parser.read('../robot.conf') 22 | ip = parser.get('xArm', 'ip') 23 | except: 24 | ip = input('Please input the xArm ip address:') 25 | if not ip: 26 | print('input error, exit') 27 | sys.exit(1) 28 | ######################################################## 29 | 30 | p.connect(p.GUI) 31 | p.setAdditionalSearchPath(pd.getDataPath()) 32 | arm = XArmAPI(ip) 33 | arm.motion_enable(enable=True) 34 | arm.set_mode(0) 35 | arm.set_state(state=0) 36 | 37 | arm.reset(wait=True) 38 | 39 | arm.set_mode(1) 40 | arm.set_state(state=0) 41 | 42 | speed = math.radians(150) 43 | 44 | 45 | timeStep=1./60. 46 | p.setTimeStep(timeStep) 47 | p.setGravity(0,0,-9.8) 48 | 49 | xarm = xarm_sim.XArm6Sim(p,[0,0,0]) 50 | while (p.isConnected()): 51 | xarm.step() 52 | arm.set_servo_angle_j(angles=xarm.jointPoses, speed=speed, is_radian=True) 53 | p.stepSimulation() 54 | time.sleep(timeStep) 55 | 56 | arm.reset(wait=True) 57 | arm.disconnect() 58 | -------------------------------------------------------------------------------- /example/wrapper/xarm6/xarm_sim.py: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy as np 3 | import math 4 | 5 | useNullSpace = 0 6 | useDynamics = 1 7 | useIKFast = 0 #ikfast doesn't get solutions and is actually slower than pybullet IK (300us versus 150us), probably a configuration issue 8 | if useIKFast: 9 | import ikfastpy 10 | 11 | ikSolver = 0 12 | xarmEndEffectorIndex = 6 13 | xarmNumDofs = 6 14 | 15 | ll = [-17]*xarmNumDofs 16 | #upper limits for null space (todo: set them to proper range) 17 | ul = [17]*xarmNumDofs 18 | #joint ranges for null space (todo: set them to proper range) 19 | jr = [17]*xarmNumDofs 20 | #restposes for null space 21 | jointPositions=[0,0,0,0,0,0] 22 | rp = jointPositions 23 | jointPoses = jointPositions 24 | class XArm6Sim(object): 25 | def __init__(self, bullet_client, offset): 26 | if useIKFast: 27 | # Initialize kinematics for UR5 robot arm 28 | self.xarm_kin = ikfastpy.PyKinematics() 29 | self.n_joints = self.xarm_kin.getDOF() 30 | print("numJoints IKFast=", self.n_joints) 31 | 32 | self.bullet_client = bullet_client 33 | self.offset = np.array(offset) 34 | self.jointPoses = [0]*xarmNumDofs 35 | #print("offset=",offset) 36 | flags = self.bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES 37 | legos=[] 38 | self.bullet_client.loadURDF("tray/traybox.urdf", [0.4+offset[0], offset[1], offset[2]], [0,0,0,1], flags=flags) 39 | legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.3, 0.1, 0.3])+self.offset, flags=flags)) 40 | legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.3, -0.1, 0.3])+self.offset, flags=flags)) 41 | legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.5, 0.1, 0.3])+self.offset, flags=flags)) 42 | sphereId = self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0.4, 0, 0.3])+self.offset, flags=flags) 43 | self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0.3, 0, 0.3])+self.offset, flags=flags) 44 | self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0.5, 0, 0.3])+self.offset, flags=flags) 45 | orn=[0,0,0,1] 46 | self.xarm = self.bullet_client.loadURDF("xarm/xarm6_robot.urdf", np.array([0,0,0])+self.offset, orn, useFixedBase=True, flags=flags) 47 | index = 0 48 | for j in range(self.bullet_client.getNumJoints(self.xarm)): 49 | self.bullet_client.changeDynamics(self.xarm, j, linearDamping=0, angularDamping=0) 50 | info = self.bullet_client.getJointInfo(self.xarm, j) 51 | 52 | jointName = info[1] 53 | jointType = info[2] 54 | if (jointType == self.bullet_client.JOINT_PRISMATIC): 55 | 56 | self.bullet_client.resetJointState(self.xarm, j, jointPositions[index]) 57 | index=index+1 58 | if (jointType == self.bullet_client.JOINT_REVOLUTE): 59 | self.bullet_client.resetJointState(self.xarm, j, jointPositions[index]) 60 | index=index+1 61 | self.t = -math.pi/4.0 62 | def reset(self): 63 | pass 64 | 65 | def step(self): 66 | t = self.t 67 | self.t += 1./60. 68 | 69 | pos = [0.407+self.offset[0]+0.2 * math.sin(1.5 * t), 0.0+self.offset[1]+0.2 * math.cos(1.5 * t), 0.12+self.offset[2]] 70 | #orn = self.bullet_client.getQuaternionFromEuler([math.pi/2.,0.,0.]) 71 | orn = [1,0,0,0] 72 | 73 | if useIKFast: 74 | 75 | #mat = self.bullet_client.getMatrixFromQuaternion(orn) 76 | #ee_pose = np.array([mat[0],mat[1],mat[2],pos[0], 77 | # mat[3],mat[4],mat[5],pos[1], 78 | # mat[6],mat[7],mat[8],pos[2]]) 79 | #ee_pose = np.asarray(ee_pose).reshape(3,4) 80 | joint_angles = [0.39,0,0,0,0,0]#-3.1,-1.6,1.6,-1.6,-1.6,0.] # in radians 81 | self.bullet_client.submitProfileTiming("ikfast.forwardDynamics") 82 | ee_pose = self.xarm_kin.forward(joint_angles) 83 | self.bullet_client.submitProfileTiming() 84 | ee_pose = np.asarray(ee_pose).reshape(3,4) 85 | #print("ee_pose=",ee_pose) 86 | self.bullet_client.submitProfileTiming("ikfast.inverseDynamics") 87 | joint_configs = self.xarm_kin.inverse(ee_pose.reshape(-1).tolist()) 88 | self.bullet_client.submitProfileTiming() 89 | n_solutions = int(len(joint_configs)/self.n_joints) 90 | print("%d solutions found:"%(n_solutions)) 91 | joint_configs = np.asarray(joint_configs).reshape(n_solutions,self.n_joints) 92 | for joint_config in joint_configs: 93 | print(joint_config) 94 | self.jointPoses = [0]*self.n_joints 95 | 96 | else: 97 | if useNullSpace: 98 | jointPoses = self.bullet_client.calculateInverseKinematics(self.xarm,xarmEndEffectorIndex, pos, orn,lowerLimits=ll, 99 | upperLimits=ul,jointRanges=jr, restPoses=np.array(self.jointPoses).tolist(),residualThreshold=1e-5, maxNumIterations=50) 100 | self.jointPoses = [0,0,jointPoses[2],jointPoses[3],jointPoses[4],jointPoses[5]] 101 | else: 102 | self.jointPoses = self.bullet_client.calculateInverseKinematics(self.xarm,xarmEndEffectorIndex, pos, orn, maxNumIterations=50) 103 | #print("jointPoses=",jointPoses) 104 | if useDynamics: 105 | for i in range(xarmNumDofs): 106 | self.bullet_client.setJointMotorControl2(self.xarm, i+1, self.bullet_client.POSITION_CONTROL, self.jointPoses[i],force=5 * 240.) 107 | else: 108 | for i in range(xarmNumDofs): 109 | self.bullet_client.resetJointState(self.xarm, i+1, jointPoses[i]) 110 | ls = self.bullet_client.getLinkState(self.xarm, xarmEndEffectorIndex, computeForwardKinematics=True) 111 | linkComPos=np.array(ls[0]) 112 | linkComOrn=ls[1] 113 | linkUrdfPos=np.array(ls[4]) 114 | linkUrdfOrn=ls[5] 115 | #print("linkComPos=",linkComPos) 116 | #print("linkUrdfOrn=",linkUrdfOrn) 117 | mat = self.bullet_client.getMatrixFromQuaternion(linkUrdfOrn) 118 | #print("mat=",mat) 119 | self.bullet_client.addUserDebugLine(pos, linkUrdfPos, [1,0,0],lifeTime=100) 120 | diff = linkUrdfPos-np.array(pos) 121 | #print("diff=",diff) 122 | 123 | 124 | -------------------------------------------------------------------------------- /example/wrapper/xarm7/2001-move_joint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | arm.reset(wait=True) 49 | 50 | speed = 50 51 | arm.set_servo_angle(angle=[90, 0, 0, 0, 0, 0, 0], speed=speed, wait=True) 52 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 53 | arm.set_servo_angle(angle=[90, -60, 0, 0, 0, 0, 0], speed=speed, wait=True) 54 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 55 | arm.set_servo_angle(angle=[90, -60, -30, 0, 0, 0, 0], speed=speed, wait=True) 56 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 57 | arm.set_servo_angle(angle=[0, -60, -30, 0, 0, 0, 0], speed=speed, wait=True) 58 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 59 | arm.set_servo_angle(angle=[0, -60, 0, 0, 0, 0, 0], speed=speed, wait=True) 60 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 61 | arm.set_servo_angle(angle=[0, 0, 0, 0, 0, 0, 0], speed=speed, wait=True) 62 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 63 | 64 | 65 | arm.reset(wait=True) 66 | speed = math.radians(50) 67 | arm.set_servo_angle(angle=[math.radians(90), 0, 0, 0, 0, 0, 0], speed=speed, is_radian=True, wait=True) 68 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 69 | arm.set_servo_angle(angle=[math.radians(90), math.radians(-60), 0, 0, 0, 0, 0], speed=speed, is_radian=True, wait=True) 70 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 71 | arm.set_servo_angle(angle=[math.radians(90), math.radians(-60), math.radians(-30), 0, 0, 0, 0], speed=speed, is_radian=True, wait=True) 72 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 73 | arm.set_servo_angle(angle=[0, math.radians(-60), math.radians(-30), 0, 0, 0, 0], speed=speed, is_radian=True, wait=True) 74 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 75 | arm.set_servo_angle(angle=[0, math.radians(-60), 0, 0, 0, 0, 0], speed=speed, is_radian=True, wait=True) 76 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 77 | arm.set_servo_angle(angle=[0, 0, 0, 0, 0, 0], speed=speed, is_radian=True, wait=True) 78 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 79 | 80 | 81 | arm.reset(wait=True) 82 | arm.disconnect() 83 | -------------------------------------------------------------------------------- /example/wrapper/xarm7/2002-move_joint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip, is_radian=True) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | arm.reset(wait=True) 49 | 50 | speed = 50 51 | arm.set_servo_angle(angle=[90, 0, 0, 0, 0, 0, 0], speed=speed, is_radian=False, wait=True) 52 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 53 | arm.set_servo_angle(angle=[90, -60, 0, 0, 0, 0, 0], speed=speed, is_radian=False, wait=True) 54 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 55 | arm.set_servo_angle(angle=[90, -60, -30, 0, 0, 0, 0], speed=speed, is_radian=False, wait=True) 56 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 57 | arm.set_servo_angle(angle=[0, -60, -30, 0, 0, 0, 0], speed=speed, is_radian=False, wait=True) 58 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 59 | arm.set_servo_angle(angle=[0, -60, 0, 0, 0, 0, 0], speed=speed, is_radian=False, wait=True) 60 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 61 | arm.set_servo_angle(angle=[0, 0, 0, 0, 0, 0], speed=speed, is_radian=False, wait=True) 62 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 63 | 64 | 65 | arm.reset(wait=True) 66 | speed = math.radians(50) 67 | arm.set_servo_angle(angle=[math.radians(90), 0, 0, 0, 0, 0, 0], speed=speed, wait=True) 68 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 69 | arm.set_servo_angle(angle=[math.radians(90), math.radians(-60), 0, 0, 0, 0, 0], speed=speed, wait=True) 70 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 71 | arm.set_servo_angle(angle=[math.radians(90), math.radians(-60), math.radians(-30), 0, 0, 0, 0], speed=speed, wait=True) 72 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 73 | arm.set_servo_angle(angle=[0, math.radians(-60), math.radians(-30), 0, 0, 0, 0], speed=speed, wait=True) 74 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 75 | arm.set_servo_angle(angle=[0, math.radians(-60), 0, 0, 0, 0, 0], speed=speed, wait=True) 76 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 77 | arm.set_servo_angle(angle=[0, 0, 0, 0, 0, 0], speed=speed, wait=True) 78 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 79 | 80 | 81 | arm.reset(wait=True) 82 | arm.disconnect() 83 | -------------------------------------------------------------------------------- /example/wrapper/xarm7/2003-move_joint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | arm.reset(wait=True) 49 | 50 | speed = 50 51 | arm.set_servo_angle(servo_id=1, angle=90, speed=speed, wait=True) 52 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 53 | arm.set_servo_angle(servo_id=2, angle=-60, speed=speed, wait=True) 54 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 55 | arm.set_servo_angle(servo_id=3, angle=-30, speed=speed, wait=True) 56 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 57 | arm.set_servo_angle(servo_id=1, angle=0, speed=speed, wait=True) 58 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 59 | arm.set_servo_angle(servo_id=3, angle=0, speed=speed, wait=True) 60 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 61 | arm.set_servo_angle(servo_id=2, angle=0, speed=speed, wait=True) 62 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 63 | 64 | arm.reset(wait=True) 65 | 66 | speed = math.radians(50) 67 | arm.set_servo_angle(servo_id=1, angle=math.radians(90), speed=speed, is_radian=True, wait=True) 68 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 69 | arm.set_servo_angle(servo_id=2, angle=math.radians(-60), speed=speed, is_radian=True, wait=True) 70 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 71 | arm.set_servo_angle(servo_id=3, angle=math.radians(-30), speed=speed, is_radian=True, wait=True) 72 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 73 | arm.set_servo_angle(servo_id=1, angle=0, speed=speed, is_radian=True, wait=True) 74 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 75 | arm.set_servo_angle(servo_id=3, angle=0, speed=speed, is_radian=True, wait=True) 76 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 77 | arm.set_servo_angle(servo_id=2, angle=0, speed=speed, is_radian=True, wait=True) 78 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 79 | 80 | 81 | arm.reset(wait=True) 82 | arm.disconnect() 83 | -------------------------------------------------------------------------------- /example/wrapper/xarm7/2004-move_joint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | """ 10 | Description: Move Joint 11 | """ 12 | 13 | import os 14 | import sys 15 | import time 16 | import math 17 | 18 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 19 | 20 | from xarm.wrapper import XArmAPI 21 | 22 | 23 | ####################################################### 24 | """ 25 | Just for test example 26 | """ 27 | if len(sys.argv) >= 2: 28 | ip = sys.argv[1] 29 | else: 30 | try: 31 | from configparser import ConfigParser 32 | parser = ConfigParser() 33 | parser.read('../robot.conf') 34 | ip = parser.get('xArm', 'ip') 35 | except: 36 | ip = input('Please input the xArm ip address:') 37 | if not ip: 38 | print('input error, exit') 39 | sys.exit(1) 40 | ######################################################## 41 | 42 | 43 | arm = XArmAPI(ip, is_radian=True) 44 | arm.motion_enable(enable=True) 45 | arm.set_mode(0) 46 | arm.set_state(state=0) 47 | 48 | arm.reset(wait=True) 49 | 50 | speed = 50 51 | arm.set_servo_angle(servo_id=1, angle=90, speed=speed, is_radian=False, wait=True) 52 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 53 | arm.set_servo_angle(servo_id=2, angle=-60, speed=speed, is_radian=False, wait=True) 54 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 55 | arm.set_servo_angle(servo_id=3, angle=-30, speed=speed, is_radian=False, wait=True) 56 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 57 | arm.set_servo_angle(servo_id=1, angle=0, speed=speed, is_radian=False, wait=True) 58 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 59 | arm.set_servo_angle(servo_id=3, angle=0, speed=speed, is_radian=False, wait=True) 60 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 61 | arm.set_servo_angle(servo_id=2, angle=0, speed=speed, is_radian=False, wait=True) 62 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 63 | 64 | arm.reset(wait=True) 65 | 66 | speed = math.radians(50) 67 | arm.set_servo_angle(servo_id=1, angle=math.radians(90), speed=speed, wait=True) 68 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 69 | arm.set_servo_angle(servo_id=2, angle=math.radians(-60), speed=speed, wait=True) 70 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 71 | arm.set_servo_angle(servo_id=3, angle=math.radians(-30), speed=speed, wait=True) 72 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 73 | arm.set_servo_angle(servo_id=1, angle=0, speed=speed, wait=True) 74 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 75 | arm.set_servo_angle(servo_id=3, angle=0, speed=speed, wait=True) 76 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 77 | arm.set_servo_angle(servo_id=2, angle=0, speed=speed, wait=True) 78 | print(arm.get_servo_angle(), arm.get_servo_angle(is_radian=True)) 79 | 80 | 81 | arm.reset(wait=True) 82 | arm.disconnect() 83 | -------------------------------------------------------------------------------- /example/wrapper/xarm7/loadxarm_sim.py: -------------------------------------------------------------------------------- 1 | import pybullet as p 2 | import pybullet_data as pd 3 | import math 4 | import time 5 | import numpy as np 6 | import xarm_sim 7 | 8 | options = ""#"--mp4=\"video.mp4\"" 9 | 10 | p.connect(p.GUI, options=options) 11 | p.setAdditionalSearchPath(pd.getDataPath()) 12 | 13 | timeStep=1./60. 14 | p.setTimeStep(timeStep) 15 | p.setGravity(0,0,-9.8) 16 | 17 | xarm = xarm_sim.XArm7Sim(p,[0,0,0]) 18 | while (1): 19 | xarm.step() 20 | p.stepSimulation() 21 | time.sleep(timeStep) 22 | 23 | -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link1.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link1_smooth.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link1_smooth.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link2.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link2_smooth.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link2_smooth.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link3.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link3.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link3_smooth.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link3_smooth.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link4.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link4.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link4_smooth.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link4_smooth.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link5.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link5.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link5_smooth.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link5_smooth.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link6.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link6.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link6_smooth.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link6_smooth.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link7.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link7.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link7_smooth.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link7_smooth.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link_base.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link_base.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/meshes/xarm7/visual/link_base_smooth.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/example/wrapper/xarm7/meshes/xarm7/visual/link_base_smooth.STL -------------------------------------------------------------------------------- /example/wrapper/xarm7/xarm_real_ik.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import pybullet as p 4 | import pybullet_data as pd 5 | import math 6 | import time 7 | import numpy as np 8 | import xarm_sim 9 | sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) 10 | from xarm.wrapper import XArmAPI 11 | ####################################################### 12 | """ 13 | Just for test example 14 | """ 15 | if len(sys.argv) >= 2: 16 | ip = sys.argv[1] 17 | else: 18 | try: 19 | from configparser import ConfigParser 20 | parser = ConfigParser() 21 | parser.read('../robot.conf') 22 | ip = parser.get('xArm', 'ip') 23 | except: 24 | ip = input('Please input the xArm ip address:') 25 | if not ip: 26 | print('input error, exit') 27 | sys.exit(1) 28 | ######################################################## 29 | 30 | p.connect(p.GUI) 31 | p.setAdditionalSearchPath(pd.getDataPath()) 32 | arm = XArmAPI(ip) 33 | arm.motion_enable(enable=True) 34 | arm.set_mode(0) 35 | arm.set_state(state=0) 36 | 37 | arm.reset(wait=True) 38 | 39 | arm.set_mode(1) 40 | arm.set_state(state=0) 41 | 42 | speed = math.radians(150) 43 | 44 | 45 | timeStep=1./60. 46 | p.setTimeStep(timeStep) 47 | p.setGravity(0,0,-9.8) 48 | 49 | xarm = xarm_sim.XArm7Sim(p,[0,0,0]) 50 | while (p.isConnected()): 51 | xarm.step() 52 | arm.set_servo_angle_j(angles=xarm.jointPoses, speed=speed, is_radian=True) 53 | p.stepSimulation() 54 | time.sleep(timeStep) 55 | 56 | arm.reset(wait=True) 57 | arm.disconnect() 58 | -------------------------------------------------------------------------------- /example/wrapper/xarm7/xarm_sim.py: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy as np 3 | import math 4 | 5 | useNullSpace = 1 6 | useDynamics = 1 7 | ikMaxNumIterations=50 8 | ikSolver = 0 9 | xarmEndEffectorIndex = 7 10 | xarmNumDofs = 7 11 | 12 | ll = [-17]*xarmNumDofs 13 | #upper limits for null space (todo: set them to proper range) 14 | ul = [17]*xarmNumDofs 15 | #joint ranges for null space (todo: set them to proper range) 16 | jr = [17]*xarmNumDofs 17 | #restposes for null space 18 | jointPositions=[0,0,0,0,0,0,0] 19 | rp = jointPositions 20 | jointPoses = jointPositions 21 | class XArm7Sim(object): 22 | def __init__(self, bullet_client, offset): 23 | 24 | self.bullet_client = bullet_client 25 | self.offset = np.array(offset) 26 | self.jointPoses = [0]*xarmNumDofs 27 | #print("offset=",offset) 28 | flags = self.bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES 29 | legos=[] 30 | self.bullet_client.loadURDF("tray/traybox.urdf", [0.4+offset[0], offset[1], offset[2]], [0,0,0,1], flags=flags) 31 | legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.3, 0.1, 0.3])+self.offset, flags=flags)) 32 | legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.3, -0.1, 0.3])+self.offset, flags=flags)) 33 | legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.5, 0.1, 0.3])+self.offset, flags=flags)) 34 | sphereId = self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0.4, 0, 0.3])+self.offset, flags=flags) 35 | self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0.3, 0, 0.3])+self.offset, flags=flags) 36 | self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0.5, 0, 0.3])+self.offset, flags=flags) 37 | orn=[0,0,0,1] 38 | self.xarm = self.bullet_client.loadURDF("xarm7_robot.urdf", np.array([0,0,0])+self.offset, orn, useFixedBase=True, flags=flags) 39 | index = 0 40 | for j in range(self.bullet_client.getNumJoints(self.xarm)): 41 | self.bullet_client.changeDynamics(self.xarm, j, linearDamping=0, angularDamping=0) 42 | info = self.bullet_client.getJointInfo(self.xarm, j) 43 | 44 | jointName = info[1] 45 | jointType = info[2] 46 | if (jointType == self.bullet_client.JOINT_PRISMATIC): 47 | 48 | self.bullet_client.resetJointState(self.xarm, j, jointPositions[index]) 49 | index=index+1 50 | if (jointType == self.bullet_client.JOINT_REVOLUTE): 51 | self.bullet_client.resetJointState(self.xarm, j, jointPositions[index]) 52 | index=index+1 53 | self.t = -math.pi/4.0 54 | def reset(self): 55 | pass 56 | 57 | def step(self): 58 | t = self.t 59 | self.t += 1./60. 60 | 61 | pos = [0.407+self.offset[0]+0.2 * math.sin(1.5 * t), 0.0+self.offset[1]+0.2 * math.cos(1.5 * t), 0.12+self.offset[2]] 62 | #orn = self.bullet_client.getQuaternionFromEuler([math.pi/2.,0.,0.]) 63 | orn = [1,0,0,0] 64 | 65 | if useNullSpace: 66 | restPoses = [0]*xarmNumDofs 67 | jointPoses = self.bullet_client.calculateInverseKinematics(self.xarm,xarmEndEffectorIndex, pos, orn,lowerLimits=ll, 68 | upperLimits=ul,jointRanges=jr, restPoses=np.array(restPoses).tolist(),residualThreshold=1e-5, maxNumIterations=ikMaxNumIterations) 69 | self.jointPoses = jointPoses 70 | else: 71 | self.jointPoses = self.bullet_client.calculateInverseKinematics(self.xarm,xarmEndEffectorIndex, pos, orn, maxNumIterations=ikMaxNumIterations) 72 | if useDynamics: 73 | for i in range(xarmNumDofs): 74 | pose = self.jointPoses[i] 75 | self.bullet_client.setJointMotorControl2(self.xarm, i+1, self.bullet_client.POSITION_CONTROL, pose,force=5 * 240.) 76 | else: 77 | for i in range(xarmNumDofs): 78 | self.bullet_client.resetJointState(self.xarm, i+1, jointPoses[i]) 79 | ls = self.bullet_client.getLinkState(self.xarm, xarmEndEffectorIndex, computeForwardKinematics=True) 80 | linkComPos=np.array(ls[0]) 81 | linkComOrn=ls[1] 82 | linkUrdfPos=np.array(ls[4]) 83 | linkUrdfOrn=ls[5] 84 | #print("linkComPos=",linkComPos) 85 | #print("linkUrdfOrn=",linkUrdfOrn) 86 | mat = self.bullet_client.getMatrixFromQuaternion(linkUrdfOrn) 87 | #print("mat=",mat) 88 | self.bullet_client.addUserDebugLine(pos, linkUrdfPos, [1,0,0],lifeTime=100) 89 | diff = linkUrdfPos-np.array(pos) 90 | #print("diff=",diff) 91 | 92 | 93 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | pyserial==3.4 -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2017, UFactory, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import os 10 | from distutils.util import convert_path 11 | try: 12 | from setuptools import setup, find_packages 13 | except ImportError: 14 | from distutils.core import setup 15 | 16 | def find_packages(base_path='.'): 17 | base_path = convert_path(base_path) 18 | found = [] 19 | for root, dirs, files in os.walk(base_path, followlinks=True): 20 | dirs[:] = [d for d in dirs if d[0] != '.' and d not in ('ez_setup', '__pycache__')] 21 | relpath = os.path.relpath(root, base_path) 22 | parent = relpath.replace(os.sep, '.').lstrip('.') 23 | if relpath != '.' and parent not in found: 24 | # foo.bar package but no foo package, skip 25 | continue 26 | for dir in dirs: 27 | if os.path.isfile(os.path.join(root, dir, '__init__.py')): 28 | package = '.'.join((parent, dir)) if parent else dir 29 | found.append(package) 30 | return found 31 | 32 | main_ns = {} 33 | ver_path = convert_path('xarm/version.py') 34 | with open(os.path.join(os.getcwd(), ver_path)) as ver_file: 35 | exec(ver_file.read(), main_ns) 36 | 37 | version = main_ns['__version__'] 38 | 39 | # long_description = open('README.rst').read() 40 | long_description = 'long description for xArm-Python-SDK' 41 | 42 | with open(os.path.join(os.getcwd(), 'requirements.txt')) as f: 43 | requirements = f.read().splitlines() 44 | 45 | setup( 46 | name='xArm-Python-SDK', 47 | version=version, 48 | author='Vinman', 49 | description='Python SDK for xArm', 50 | packages=find_packages(), 51 | author_email='vinman@ufactory.cc', 52 | install_requires=requirements, 53 | long_description=long_description, 54 | license='MIT', 55 | zip_safe=False 56 | ) 57 | -------------------------------------------------------------------------------- /xarm/__init__.py: -------------------------------------------------------------------------------- 1 | from .wrapper import XArmAPI 2 | from .version import __version__ 3 | -------------------------------------------------------------------------------- /xarm/core/__init__.py: -------------------------------------------------------------------------------- 1 | from .config.x_code import ControllerWarn, ControllerError, ServoError 2 | from .config.x_config import XCONF 3 | -------------------------------------------------------------------------------- /xarm/core/comm/__init__.py: -------------------------------------------------------------------------------- 1 | try: 2 | from .serial_port import SerialPort 3 | except: 4 | import warnings 5 | warnings.warn('serial module is not found, if you want to connect to xArm with serial, please `pip install pyserial==3.4`') 6 | SerialPort = object 7 | from .socket_port import SocketPort -------------------------------------------------------------------------------- /xarm/core/comm/base.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (MIT License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | 10 | import threading 11 | import queue 12 | import socket 13 | import time 14 | from ..utils.log import logger 15 | 16 | 17 | class Port(threading.Thread): 18 | def __init__(self, rxque_max): 19 | super(Port, self).__init__() 20 | self.daemon = True 21 | self.rx_que = queue.Queue(rxque_max) 22 | self.write_lock = threading.Lock() 23 | self._connected = False 24 | self.com = None 25 | self.rx_parse = -1 26 | self.com_read = None 27 | self.com_write = None 28 | self.port_type = '' 29 | self.buffer_size = 1 30 | self.heartbeat_thread = None 31 | self.alive = True 32 | 33 | @property 34 | def connected(self): 35 | return self._connected 36 | 37 | def run(self): 38 | self.recv_proc() 39 | 40 | def close(self): 41 | self.alive = False 42 | if 'socket' in self.port_type: 43 | try: 44 | self.com.shutdown(socket.SHUT_RDWR) 45 | except: 46 | pass 47 | self.com.close() 48 | # start_time = time.time() 49 | # while self.connected and time.time() - start_time < 5: 50 | # time.sleep(0.01) 51 | 52 | def flush(self, fromid=-1, toid=-1): 53 | if not self.connected: 54 | return -1 55 | while not(self.rx_que.empty()): 56 | self.rx_que.queue.clear() 57 | if self.rx_parse != -1: 58 | self.rx_parse.flush(fromid, toid) 59 | return 0 60 | 61 | def write(self, data): 62 | if not self.connected: 63 | return -1 64 | try: 65 | with self.write_lock: 66 | logger.verbose('send: {}'.format(data)) 67 | self.com_write(data) 68 | return 0 69 | except Exception as e: 70 | self._connected = False 71 | logger.error("{} send: {}".format(self.port_type, e)) 72 | return -1 73 | 74 | def read(self, timeout=None): 75 | if not self.connected: 76 | return -1 77 | if not self.rx_que.empty(): 78 | buf = self.rx_que.get(timeout=timeout) 79 | logger.verbose('recv: {}'.format(buf)) 80 | return buf 81 | else: 82 | return -1 83 | 84 | def recv_proc(self): 85 | self.alive = True 86 | logger.debug('{} recv thread start'.format(self.port_type)) 87 | try: 88 | failed_read_count = 0 89 | timeout_count = 0 90 | while self.connected and self.alive: 91 | if self.port_type == 'main-socket': 92 | try: 93 | rx_data = self.com_read(self.buffer_size) 94 | except socket.timeout: 95 | continue 96 | if len(rx_data) == 0: 97 | failed_read_count += 1 98 | if failed_read_count > 5: 99 | self._connected = False 100 | break 101 | time.sleep(0.1) 102 | continue 103 | elif self.port_type == 'report-socket': 104 | try: 105 | rx_data = self.com_read(self.buffer_size) 106 | except socket.timeout: 107 | timeout_count += 1 108 | if timeout_count > 3: 109 | self._connected = False 110 | break 111 | continue 112 | if len(rx_data) == 0: 113 | failed_read_count += 1 114 | if failed_read_count > 5: 115 | self._connected = False 116 | break 117 | time.sleep(0.1) 118 | continue 119 | elif self.port_type == 'main-serial': 120 | rx_data = self.com_read(self.com.in_waiting or self.buffer_size) 121 | else: 122 | break 123 | timeout_count = 0 124 | failed_read_count = 0 125 | if -1 == self.rx_parse: 126 | if self.rx_que.full(): 127 | self.rx_que.get() 128 | self.rx_que.put(rx_data) 129 | else: 130 | self.rx_parse.put(rx_data) 131 | except Exception as e: 132 | if self.alive: 133 | logger.error('{}: {}'.format(self.port_type, e)) 134 | finally: 135 | self.close() 136 | logger.debug('{} recv thread had stopped'.format(self.port_type)) 137 | self._connected = False 138 | # if self.heartbeat_thread: 139 | # try: 140 | # self.heartbeat_thread.join() 141 | # except: 142 | # pass 143 | 144 | -------------------------------------------------------------------------------- /xarm/core/comm/serial_port.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Jimy Zhang 8 | # Author: Vinman 9 | 10 | 11 | import serial 12 | from ..utils.log import logger 13 | from .base import Port 14 | from .uxbus_cmd_protocol import Ux2HexProtocol 15 | from ..config.x_config import XCONF 16 | 17 | 18 | class SerialPort(Port): 19 | def __init__(self, port, baud=XCONF.SerialConf.SERIAL_BAUD, 20 | rxque_max=XCONF.SerialConf.UXBUS_RXQUE_MAX, protocol=XCONF.SerialConf.UX2_HEX_PROTOCOL): 21 | super(SerialPort, self).__init__(rxque_max) 22 | self.port_type = 'main-serial' 23 | try: 24 | self.com = serial.Serial(port=port, baudrate=baud) 25 | if not self.com.isOpen(): 26 | self._connected = False 27 | raise Exception('serial is not open') 28 | logger.info('{} connect {}:{} success'.format(self.port_type, port, baud)) 29 | 30 | self._connected = True 31 | 32 | self.buffer_size = 1 33 | 34 | if protocol == XCONF.SerialConf.UX2_HEX_PROTOCOL: 35 | self.rx_parse = Ux2HexProtocol(self.rx_que, 36 | XCONF.SerialConf.UXBUS_DEF_FROMID, 37 | XCONF.SerialConf.UXBUS_DEF_TOID) 38 | else: 39 | self.rx_parse = -1 40 | self.com_read = self.com.read 41 | self.com_write = self.com.write 42 | self.start() 43 | except Exception as e: 44 | logger.info('{} connect {}:{} failed, {}'.format(self.port_type, port, baud, e)) 45 | self._connected = False 46 | 47 | -------------------------------------------------------------------------------- /xarm/core/comm/socket_port.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Jimy Zhang 8 | # Author: Vinman 9 | 10 | 11 | import queue 12 | import socket 13 | import threading 14 | import time 15 | from ..utils.log import logger 16 | from .base import Port 17 | from ..config.x_config import XCONF 18 | 19 | 20 | class HeartBeatThread(threading.Thread): 21 | def __init__(self, sock_class): 22 | threading.Thread.__init__(self) 23 | self.sock_class = sock_class 24 | self.daemon = True 25 | 26 | def run(self): 27 | logger.debug('{} heartbeat thread start'.format(self.sock_class.port_type)) 28 | heat_data = bytes([0, 0, 0, 1, 0, 2, 0, 0]) 29 | 30 | while self.sock_class.connected: 31 | if self.sock_class.write(heat_data) == -1: 32 | break 33 | time.sleep(1) 34 | logger.debug('{} heartbeat thread had stopped'.format(self.sock_class.port_type)) 35 | 36 | 37 | class SocketPort(Port): 38 | def __init__(self, server_ip, server_port, rxque_max=XCONF.SocketConf.TCP_RX_QUE_MAX, heartbeat=False, 39 | buffer_size=XCONF.SocketConf.TCP_CONTROL_BUF_SIZE): 40 | super(SocketPort, self).__init__(rxque_max) 41 | if server_port == XCONF.SocketConf.TCP_CONTROL_PORT: 42 | self.port_type = 'main-socket' 43 | # self.com.setsockopt(socket.SOL_SOCKET, socket.SO_RCVTIMEO, 5) 44 | else: 45 | self.port_type = 'report-socket' 46 | try: 47 | socket.setdefaulttimeout(1) 48 | self.com = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 49 | self.com.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 50 | self.com.setblocking(True) 51 | self.com.settimeout(1) 52 | self.com.connect((server_ip, server_port)) 53 | logger.info('{} connect {} success'.format(self.port_type, server_ip)) 54 | # logger.info('{} connect {}:{} success'.format(self.port_type, server_ip, server_port)) 55 | 56 | self._connected = True 57 | self.buffer_size = buffer_size 58 | # time.sleep(1) 59 | 60 | self.rx_parse = -1 61 | self.com_read = self.com.recv 62 | self.com_write = self.com.send 63 | self.write_lock = threading.Lock() 64 | self.start() 65 | if heartbeat: 66 | self.heartbeat_thread = HeartBeatThread(self) 67 | self.heartbeat_thread.start() 68 | except Exception as e: 69 | logger.info('{} connect {} failed, {}'.format(self.port_type, server_ip, e)) 70 | # logger.error('{} connect {}:{} failed, {}'.format(self.port_type, server_ip, server_port, e)) 71 | self._connected = False 72 | 73 | -------------------------------------------------------------------------------- /xarm/core/comm/uxbus_cmd_protocol.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Jimy Zhang 8 | # Author: Vinman 9 | 10 | 11 | from ..utils import crc16 12 | from ..utils.log import logger 13 | 14 | # ux2_hex_protocol define 15 | UX2HEX_RXSTART_FROMID = 0 16 | UX2HEX_RXSTART_TOID = 1 17 | UX2HEX_RXSTATE_LEN = 2 18 | UX2HEX_RXSTATE_DATA = 3 19 | UX2HEX_RXSTATE_CRC1 = 4 20 | UX2HEX_RXSTATE_CRC2 = 5 21 | UX2HEX_RXLEN_MAX = 50 22 | 23 | 24 | class Ux2HexProtocol(object): 25 | """ 26 | fromid and toid: broadcast address is 0xFF 27 | """ 28 | def __init__(self, rx_que, fromid, toid): 29 | self.rx_que = rx_que 30 | self.rxstate = UX2HEX_RXSTART_FROMID 31 | self.data_idx = 0 32 | self.len = 0 33 | self.fromid = fromid 34 | self.toid = toid 35 | self.rxbuf = None 36 | 37 | # wipe cache , set from_id and to_id 38 | def flush(self, fromid=-1, toid=-1): 39 | self.rxstate = UX2HEX_RXSTART_FROMID 40 | self.data_idx = 0 41 | self.len = 0 42 | if fromid != -1: 43 | self.fromid = fromid 44 | if toid != -1: 45 | self.toid = toid 46 | 47 | def put(self, rxstr, length=0): 48 | if length == 0: 49 | length = len(rxstr) 50 | if len(rxstr) < length: 51 | logger.error('len(rxstr) < length') 52 | 53 | for i in range(length): 54 | rxch = bytes([rxstr[i]]) 55 | # print_hex(self.DB_FLG, rxch, 1) 56 | # print('state:%d' % (self.rxstate)) 57 | if UX2HEX_RXSTART_FROMID == self.rxstate: 58 | if self.toid == rxch[0] or 255 == self.toid: 59 | self.rxbuf = rxch 60 | self.rxstate = UX2HEX_RXSTART_TOID 61 | 62 | elif UX2HEX_RXSTART_TOID == self.rxstate: 63 | if self.fromid == rxch[0] or self.fromid == 0xFF: 64 | self.rxbuf += rxch 65 | self.rxstate = UX2HEX_RXSTATE_LEN 66 | else: 67 | self.rxstate = UX2HEX_RXSTART_FROMID 68 | 69 | elif UX2HEX_RXSTATE_LEN == self.rxstate: 70 | if rxch[0] < UX2HEX_RXLEN_MAX: 71 | self.rxbuf += rxch 72 | self.len = rxch[0] 73 | self.data_idx = 0 74 | self.rxstate = UX2HEX_RXSTATE_DATA 75 | else: 76 | self.rxstate = UX2HEX_RXSTART_FROMID 77 | 78 | elif UX2HEX_RXSTATE_DATA == self.rxstate: 79 | if self.data_idx < self.len: 80 | self.rxbuf += rxch 81 | self.data_idx += 1 82 | if self.data_idx == self.len: 83 | self.rxstate = UX2HEX_RXSTATE_CRC1 84 | else: 85 | self.rxstate = UX2HEX_RXSTART_FROMID 86 | 87 | elif UX2HEX_RXSTATE_CRC1 == self.rxstate: 88 | self.rxbuf += rxch 89 | self.rxstate = UX2HEX_RXSTATE_CRC2 90 | 91 | elif UX2HEX_RXSTATE_CRC2 == self.rxstate: 92 | self.rxbuf += rxch 93 | self.rxstate = UX2HEX_RXSTART_FROMID 94 | crc = crc16.crc_modbus(self.rxbuf[:self.len + 3]) 95 | if crc[0] == self.rxbuf[self.len + 3] and crc[1] == self.rxbuf[self.len + 4]: 96 | if self.rx_que.full(): 97 | self.rx_que.get() 98 | self.rx_que.put(self.rxbuf) 99 | # print(self.rxbuf) 100 | 101 | -------------------------------------------------------------------------------- /xarm/core/config/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/xarm/core/config/__init__.py -------------------------------------------------------------------------------- /xarm/core/config/x_config.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import math 10 | 11 | 12 | class XCONF(object): 13 | ARM_AXIS_NUM = 7 14 | GRIPPER_ID = 8 15 | TGPIO_ID = 9 16 | MAX_CMD_NUM = 1000 17 | 18 | def __init__(self): 19 | pass 20 | 21 | class Robot: 22 | class Axis: 23 | XARM5 = 5 24 | XARM6 = 6 25 | XARM7 = 7 26 | 27 | class Type: 28 | XARM6_X1 = 1 29 | XARM7_X2 = 2 30 | XARM7_X3 = 3 31 | XARM7_X3MIR = 4 32 | XARM5_X4 = 5 33 | XARM6_X4 = 6 34 | XARM7_X4 = 7 35 | 36 | JOINT_LIMITS = { 37 | Axis.XARM5: { 38 | Type.XARM5_X4: [ 39 | (-2 * math.pi, 2 * math.pi), 40 | (-2.059488, 2.094395), # (-2.18, 2.18), 41 | (-3.92699, 0.191986), # (-4.01, 0.1), 42 | (-1.692969, math.pi), # (-1.75, math.pi), 43 | (-2 * math.pi, 2 * math.pi) 44 | ], 45 | }, 46 | Axis.XARM6: { 47 | Type.XARM6_X4: [ 48 | (-2 * math.pi, 2 * math.pi), 49 | (-2.059488, 2.094395), # (-2.18, 2.18), 50 | (-3.92699, 0.191986), # (-4.01, 0.1), 51 | (-2 * math.pi, 2 * math.pi), 52 | (-1.692969, math.pi), # (-1.75, math.pi), 53 | (-2 * math.pi, 2 * math.pi) 54 | ], 55 | }, 56 | Axis.XARM7: { 57 | Type.XARM7_X3: [ 58 | (-2 * math.pi, 2 * math.pi), 59 | (-2.059488, 2.094395), # (-2.18, 2.18), 60 | (-2 * math.pi, 2 * math.pi), 61 | (-3.92699, 0.191986), # (-4.01, 0.1), 62 | (-2 * math.pi, 2 * math.pi), 63 | (-1.692969, math.pi), # (-1.75, math.pi), 64 | (-2 * math.pi, 2 * math.pi) 65 | ], 66 | Type.XARM7_X4: [ 67 | (-2 * math.pi, 2 * math.pi), 68 | (-2.059488, 2.094395), # (-2.18, 2.18), 69 | (-2 * math.pi, 2 * math.pi), 70 | (-0.191986, 3.92699), # (-0.1, 4.01), 71 | (-2 * math.pi, 2 * math.pi), 72 | (-1.692969, math.pi), # (-1.75, math.pi), 73 | (-2 * math.pi, 2 * math.pi) 74 | ], 75 | } 76 | } 77 | TCP_LIMITS = { 78 | Axis.XARM5: { 79 | Type.XARM5_X4: [ 80 | (-750, 750), 81 | (-750, 750), 82 | (-400, 1000), 83 | (math.pi, math.pi), 84 | (0, 0), 85 | (-math.pi, math.pi) 86 | ], 87 | }, 88 | Axis.XARM6: { 89 | Type.XARM6_X1: [ 90 | (-750, 750), 91 | (-750, 750), 92 | (-400, 1000), 93 | (-math.pi, math.pi), 94 | (-math.pi, math.pi), 95 | (-math.pi, math.pi) 96 | ], 97 | Type.XARM6_X4: [ 98 | (-750, 750), 99 | (-750, 750), 100 | (-400, 1000), 101 | (-math.pi, math.pi), 102 | (-math.pi, math.pi), 103 | (-math.pi, math.pi) 104 | ], 105 | }, 106 | Axis.XARM7: { 107 | Type.XARM7_X3: [ 108 | (-750, 750), 109 | (-750, 750), 110 | (-400, 1000), 111 | (-math.pi, math.pi), 112 | (-math.pi, math.pi), 113 | (-math.pi, math.pi) 114 | ], 115 | Type.XARM7_X4: [ 116 | (-750, 750), 117 | (-750, 750), 118 | (-400, 1000), 119 | (-math.pi, math.pi), 120 | (-math.pi, math.pi), 121 | (-math.pi, math.pi) 122 | ], 123 | } 124 | } 125 | 126 | class SerialConf: 127 | SERIAL_BAUD = 2000000 # 921600 128 | UXBUS_RXQUE_MAX = 10 129 | UXBUS_DEF_FROMID = 0xAA 130 | UXBUS_DEF_TOID = 0x55 131 | UX2_HEX_PROTOCOL = 1 132 | UX2_STR_PROTOCOL = 2 133 | UX1_HEX_PROTOCOL = 3 134 | UX1_STR_PROTOCOL = 4 135 | 136 | class SocketConf: 137 | TCP_CONTROL_PORT = 502 138 | TCP_REPORT_NORM_PORT = 30001 139 | TCP_REPORT_RICH_PORT = 30002 140 | TCP_REPORT_REAL_PORT = 30003 141 | TCP_RX_QUE_MAX = 1024 142 | TCP_CONTROL_BUF_SIZE = 1024 143 | TCP_REPORT_REAL_BUF_SIZE = 87 144 | TCP_REPORT_NORMAL_BUF_SIZE = 133 145 | TCP_REPORT_RICH_BUF_SIZE = 233 146 | 147 | class UxbusReg: 148 | GET_VERSION = 1 149 | GET_ROBOT_SN = 2 150 | CHECK_VERIFY = 3 151 | RELOAD_DYNAMICS = 4 152 | SHUTDOWN_SYSTEM = 10 153 | MOTION_EN = 11 154 | SET_STATE = 12 155 | GET_STATE = 13 156 | GET_CMDNUM = 14 157 | GET_ERROR = 15 158 | CLEAN_ERR = 16 159 | CLEAN_WAR = 17 160 | SET_BRAKE = 18 161 | SET_MODE = 19 162 | 163 | MOVE_LINE = 21 164 | MOVE_LINEB = 22 165 | MOVE_JOINT = 23 166 | MOVE_HOME = 25 167 | SLEEP_INSTT = 26 168 | MOVE_CIRCLE = 27 169 | MOVE_LINE_TOOL = 28 170 | MOVE_SERVOJ = 29 171 | MOVE_SERVO_CART = 30 172 | 173 | SET_TCP_JERK = 31 174 | SET_TCP_MAXACC = 32 175 | SET_JOINT_JERK = 33 176 | SET_JOINT_MAXACC = 34 177 | SET_TCP_OFFSET = 35 178 | SET_LOAD_PARAM = 36 179 | SET_COLLIS_SENS = 37 180 | SET_TEACH_SENS = 38 181 | CLEAN_CONF = 39 182 | SAVE_CONF = 40 183 | 184 | GET_TCP_POSE = 41 185 | GET_JOINT_POS = 42 186 | GET_IK = 43 187 | GET_FK = 44 188 | IS_JOINT_LIMIT = 45 189 | IS_TCP_LIMIT = 46 190 | 191 | SET_REDUCED_TRSV = 47 192 | SET_REDUCED_P2PV = 48 193 | GET_REDUCED_MODE = 49 194 | SET_REDUCED_MODE = 50 195 | SET_GRAVITY_DIR = 51 196 | SET_LIMIT_XYZ = 52 197 | GET_REDUCED_STATE = 53 198 | 199 | SET_SERVOT = 54 200 | GET_JOINT_TAU = 55 201 | SET_SAFE_LEVEL = 56 202 | GET_SAFE_LEVEL = 57 203 | 204 | SET_REDUCED_JRANGE = 58 205 | SET_FENSE_ON = 59 206 | SET_COLLIS_REB = 60 207 | 208 | SET_TRAJ_RECORD = 61 209 | SAVE_TRAJ = 62 210 | LOAD_TRAJ = 63 211 | PLAY_TRAJ = 64 212 | GET_TRAJ_RW_STATUS = 65 213 | 214 | SET_TIMER = 71 215 | CANCEL_TIMER = 72 216 | SET_WORLD_OFFSET = 73 217 | CNTER_RESET = 74 218 | CNTER_PLUS = 75 219 | 220 | SERVO_W16B = 101 221 | SERVO_R16B = 102 222 | SERVO_W32B = 103 223 | SERVO_R32B = 104 224 | SERVO_ZERO = 105 225 | SERVO_DBMSG = 106 226 | 227 | TGPIO_MB_TIOUT = 123 228 | TGPIO_MODBUS = 124 229 | TGPIO_ERR = 125 230 | TGPIO_W16B = 127 231 | TGPIO_R16B = 128 232 | TGPIO_W32B = 129 233 | TGPIO_R32B = 130 234 | 235 | CGPIO_GET_DIGIT = 131 236 | CGPIO_GET_ANALOG1 = 132 237 | CGPIO_GET_ANALOG2 = 133 238 | CGPIO_SET_DIGIT = 134 239 | CGPIO_SET_ANALOG1 = 135 240 | CGPIO_SET_ANALOG2 = 136 241 | CGPIO_SET_IN_FUN = 137 242 | CGPIO_SET_OUT_FUN = 138 243 | CGPIO_GET_STATE = 139 244 | 245 | GET_HD_TYPES = 141 246 | 247 | class UxbusConf: 248 | SET_TIMEOUT = 1000 # ms 249 | GET_TIMEOUT = 1000 # ms 250 | 251 | class ServoConf: 252 | CON_EN = 0x0100 253 | CON_MODE = 0x0101 254 | CON_DIR = 0x0102 255 | 256 | SV3MOD_POS = 0 257 | SV3MOD_SPD = 1 258 | SV3MOD_FOS = 2 259 | SV3_SAVE = 0x1000 260 | 261 | BRAKE = 0x0104 262 | GET_TEMP = 0x000E 263 | ERR_CODE = 0x000F 264 | OVER_TEMP = 0x0108 265 | CURR_CURR = 0x0001 266 | POS_KP = 0x0200 267 | POS_FWDKP = 0x0201 268 | POS_PWDTC = 0x0202 269 | SPD_KP = 0x0203 270 | SPD_KI = 0x0204 271 | CURR_KP = 0x090C 272 | CURR_KI = 0x090D 273 | SPD_IFILT = 0x030C 274 | SPD_OFILT = 0x030D 275 | POS_CMDILT = 0x030E 276 | CURR_IFILT = 0x0401 277 | POS_KD = 0x0205 278 | POS_ACCT = 0x0300 279 | POS_DECT = 0x0301 280 | POS_STHT = 0x0302 281 | POS_SPD = 0x0303 282 | MT_ID = 0x1600 283 | BAUDRATE = 0x0601 284 | SOFT_REBOOT = 0x0607 285 | TAGET_TOQ = 0x050a 286 | CURR_TOQ = 0x050c 287 | TOQ_SPD = 0x050e 288 | TAGET_POS = 0x0700 289 | CURR_POS = 0x0702 290 | HARD_VER = 0x0800 291 | SOFT_VER = 0x0801 292 | MT_TYPE = 0x0802 293 | MT_ZERO = 0x0817 294 | RESET_PVL = 0x0813 295 | CAL_ZERO = 0x080C 296 | ERR_SWITCH = 0x0910 297 | RESET_ERR = 0x0109 298 | SV3_BRO_ID = 0xFF 299 | 300 | MODBUS_BAUDRATE = 0x0A0B 301 | TOOL_MB_TIMEOUT = 0x0A0E 302 | DIGITAL_IN = 0x0A14 303 | DIGITAL_OUT = 0x0A15 304 | ANALOG_IO1 = 0x0A16 305 | ANALOG_IO2 = 0x0A17 306 | 307 | class UxbusState: 308 | ERR_CODE = 1 # 有尚未清除的错误 309 | WAR_CODE = 2 # 有尚未清除的警告 310 | ERR_TOUT = 3 # 获取结果超时 311 | ERR_LENG = 4 # TCP回复长度错误 312 | ERR_NUM = 5 # TCP回复序号错误 313 | ERR_PROT = 6 # TCP协议标志错误 314 | ERR_FUN = 7 # TCP回复指令和发送指令不匹配 315 | ERR_NOTTCP = 8 # 发送错误 316 | ERR_OTHER = 11 # 其它错误 317 | ERR_PARAM = 12 # 参数错误 318 | 319 | class TrajState: 320 | IDLE = 0 321 | LOADING = 1 322 | LOAD_SUCCESS = 2 323 | LOAD_FAIL = 3 324 | SAVING = 4 325 | SAVE_SUCCESS = 5 326 | SAVE_FAIL = 6 327 | 328 | 329 | 330 | -------------------------------------------------------------------------------- /xarm/core/utils/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: UTF-8 -*- 3 | 4 | -------------------------------------------------------------------------------- /xarm/core/utils/convert.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: UTF-8 -*- 3 | 4 | # 5 | # ./common_modules/common_type.py 6 | # Copyright (C) 2018.4 - UFactory. 7 | # Author: Jimy Zhang 8 | # 9 | # 10 | 11 | import struct 12 | 13 | 14 | def fp32_to_bytes(data): 15 | """小端字节序""" 16 | return bytes(struct.pack(" 0 27 | ret = int32_to_bytes(data[0]) 28 | for i in range(1, n): 29 | ret += int32_to_bytes(data[i]) 30 | return ret 31 | 32 | 33 | def bytes_to_fp32(data): 34 | """小端字节序""" 35 | byte = bytes([data[0]]) 36 | byte += bytes([data[1]]) 37 | byte += bytes([data[2]]) 38 | byte += bytes([data[3]]) 39 | ret = struct.unpack(" 0 46 | ret = fp32_to_bytes(data[0]) 47 | for i in range(1, n): 48 | ret += fp32_to_bytes(data[i]) 49 | return ret 50 | 51 | 52 | def bytes_to_fp32s(data, n): 53 | """小端字节序""" 54 | ret = [0] * n 55 | for i in range(n): 56 | ret[i] = bytes_to_fp32(data[i * 4:i * 4 + 4]) 57 | return ret 58 | 59 | 60 | def u16_to_bytes(data): 61 | """大端字节序""" 62 | bts = bytes([data // 256 % 256]) 63 | bts += bytes([data % 256]) 64 | return bts 65 | 66 | 67 | def u16s_to_bytes(data, num): 68 | """大端字节序""" 69 | bts = b'' 70 | if num != 0: 71 | bts = u16_to_bytes(data[0]) 72 | for i in range(1, num): 73 | bts += u16_to_bytes(data[i]) 74 | return bts 75 | 76 | 77 | def bytes_to_u16(data): 78 | """大端字节序""" 79 | data_u16 = data[0] << 8 | data[1] 80 | return data_u16 81 | 82 | 83 | def bytes_to_u16s(data, n): 84 | """大端字节序""" 85 | ret = [0] * n 86 | for i in range(n): 87 | ret[i] = bytes_to_u16(data[i * 2: i * 2 + 2]) 88 | return ret 89 | 90 | 91 | def bytes_to_16s(data, n): 92 | """大端字节序""" 93 | ret = [0] * n 94 | for i in range(n): 95 | ret[i] = struct.unpack(">h", bytes(data[i * 2: i * 2 + 2]))[0] 96 | return ret 97 | 98 | 99 | def bytes_to_u32(data): 100 | """大端字节序""" 101 | data_u32 = data[0] << 24 | data[1] << 16 | data[2] << 8 | data[3] 102 | return data_u32 103 | 104 | 105 | def bytes_to_long_big(data): 106 | """大端字节序""" 107 | byte = bytes([data[0]]) 108 | byte += bytes([data[1]]) 109 | byte += bytes([data[2]]) 110 | byte += bytes([data[3]]) 111 | ret = struct.unpack(">l", byte) 112 | return ret[0] 113 | -------------------------------------------------------------------------------- /xarm/core/utils/crc16.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # 4 | # ./common_modules/crc16.py 5 | # Copyright (C) 2018.4 - UFactory. 6 | # Author: Jimy Zhang 7 | # 8 | # 9 | 10 | 11 | CRC_TABLE_H = ( 12 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 13 | 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 14 | 0x40, 0x01, 0xC0, 15 | 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 16 | 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 17 | 0xC0, 0x80, 0x41, 18 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 19 | 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 20 | 0x41, 0x01, 0xC0, 21 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 22 | 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 23 | 0xC1, 0x81, 0x40, 24 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 25 | 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 26 | 0x40, 0x01, 0xC0, 27 | 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 28 | 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 29 | 0xC0, 0x80, 0x41, 30 | 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 31 | 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 32 | 0x40, 0x01, 0xC0, 33 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 34 | 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 35 | 0xC0, 0x80, 0x41, 36 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 37 | 0xC1, 0x81, 0x40) 38 | 39 | CRC_TABLE_L = ( 40 | 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 41 | 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 42 | 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 43 | 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 44 | 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 45 | 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 46 | 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 47 | 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 48 | 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 49 | 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 50 | 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 51 | 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 52 | 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 53 | 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 54 | 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 55 | 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 56 | 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 57 | 0x80, 0x40) 58 | 59 | 60 | def crc_modbus(data): 61 | leng = len(data) 62 | init_crch = 0xFF 63 | init_crcl = 0xFF 64 | i = 0 65 | 66 | while leng > 0: 67 | index = init_crch ^ data[i] 68 | i += 1 69 | init_crch = init_crcl ^ CRC_TABLE_H[index] 70 | init_crcl = CRC_TABLE_L[index] 71 | leng -= 1 72 | s = init_crch << 8 | init_crcl 73 | crc = bytes([s // 256 % 256]) 74 | crc += bytes([s % 256]) 75 | return crc 76 | 77 | -------------------------------------------------------------------------------- /xarm/core/utils/debug_print.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: UTF-8 -*- 3 | 4 | # 5 | # ./common_modules/debug_print.py 6 | # Copyright (C) 2018.4 - UFactory. 7 | # Author: Jimy Zhang 8 | # 9 | # 10 | 11 | 12 | def print_hex(str, data, len): 13 | for i in range(len): 14 | str += ('%0.2X ' % data[i]) 15 | print(str) 16 | 17 | 18 | def print_nvect(str, data, len): 19 | for i in range(len): 20 | str += ('%0.3f ' % data[i]) 21 | print(str) 22 | -------------------------------------------------------------------------------- /xarm/core/utils/log.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import logging 10 | import functools 11 | import sys 12 | import os 13 | 14 | log_path = os.path.join(os.path.expanduser('~'), '.UFACTORY', 'log', 'xarm', 'sdk') 15 | if not os.path.exists(log_path): 16 | os.makedirs(log_path) 17 | 18 | logging.VERBOSE = 5 19 | logging.addLevelName(logging.VERBOSE, 'VERBOSE') 20 | 21 | 22 | class Logger(logging.Logger): 23 | logger_fmt = '{}[%(levelname)s][%(asctime)s][%(filename)s:%(lineno)d] - - %(message)s' 24 | logger_date_fmt = '%Y-%m-%d %H:%M:%S' 25 | stream_handler_fmt = logger_fmt.format('[SDK]') 26 | stream_handler_date_fmt = logger_date_fmt 27 | stream_handler = logging.StreamHandler(sys.stdout) 28 | stream_handler.setLevel(logging.VERBOSE) 29 | stream_handler.setFormatter(logging.Formatter(stream_handler_fmt, stream_handler_date_fmt)) 30 | 31 | logger = logging.Logger(__name__) 32 | logger.setLevel(logging.VERBOSE) 33 | logger.addHandler(stream_handler) 34 | 35 | def __new__(cls, *args, **kwargs): 36 | if not hasattr(cls, 'logger'): 37 | cls.logger = super(Logger, cls).__new__(cls, *args, **kwargs) 38 | return cls.logger 39 | 40 | logger = Logger(__name__) 41 | logger.setLevel(logging.WARNING) 42 | 43 | logger.VERBOSE = logging.VERBOSE 44 | logger.DEBUG = logging.DEBUG 45 | logger.INFO = logging.INFO 46 | logger.WARN = logging.WARN 47 | logger.WARNING = logging.WARNING 48 | logger.ERROR = logging.ERROR 49 | logger.CRITICAL = logging.CRITICAL 50 | 51 | logger.verbose = functools.partial(logger.log, logger.VERBOSE) 52 | 53 | colors = { 54 | 'none': '{}', 55 | 'white': '\033[30m{}\033[0m', 56 | 'red': '\033[31m{}\033[0m', 57 | 'green': '\033[32m{}\033[0m', 58 | 'orange': '\033[33m{}\033[0m', 59 | 'blue': '\033[34m{}\033[0m', 60 | 'purple': '\033[35m{}\033[0m', 61 | 'cyan': '\033[36m{}\033[0m', 62 | 'light_gray': '\033[37m{}\033[0m', 63 | 'dark_gray': '\033[90m{}\033[0m', 64 | 'light_red': '\033[91m{}\033[0m', 65 | 'light_green': '\033[92m{}\033[0m', 66 | 'yellow': '\033[93m{}\033[0m', 67 | 'light_blue': '\033[94m{}\033[0m', 68 | 'pink': '\033[95m{}\033[0m', 69 | 'light_cyan': '\033[96m{}\033[0m', 70 | } 71 | 72 | 73 | def pretty_print(*args, sep=' ', end='\n', file=None, color='none'): 74 | msg = '' 75 | for arg in args: 76 | msg += arg + sep 77 | msg = msg.rstrip(sep) 78 | # msg = colors.get(color, '{}').format(msg) 79 | print(msg, end=end, file=file) 80 | 81 | 82 | -------------------------------------------------------------------------------- /xarm/core/version.py: -------------------------------------------------------------------------------- 1 | __version__ = '0.1.0' 2 | -------------------------------------------------------------------------------- /xarm/core/wrapper/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (MIT License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | 10 | from .uxbus_cmd_ser import UxbusCmdSer 11 | from .uxbus_cmd_tcp import UxbusCmdTcp 12 | -------------------------------------------------------------------------------- /xarm/core/wrapper/uxbus_cmd_ser.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Jimy Zhang 8 | # Author: Vinman 9 | 10 | 11 | import time 12 | from ..utils import crc16 13 | from .uxbus_cmd import UxbusCmd 14 | from ..config.x_config import XCONF 15 | 16 | 17 | class UxbusCmdSer(UxbusCmd): 18 | def __init__(self, arm_port, fromid=XCONF.SerialConf.UXBUS_DEF_FROMID, toid=XCONF.SerialConf.UXBUS_DEF_TOID): 19 | super(UxbusCmdSer, self).__init__() 20 | self.arm_port = arm_port 21 | self.fromid = fromid 22 | self.toid = toid 23 | arm_port.flush(fromid, toid) 24 | self._has_err_warn = False 25 | 26 | @property 27 | def has_err_warn(self): 28 | return self._has_err_warn 29 | 30 | @has_err_warn.setter 31 | def has_err_warn(self, value): 32 | self._has_err_warn = value 33 | 34 | def check_xbus_prot(self, data, funcode=0): 35 | if data[3] & 0x40: 36 | self._has_err_warn = True 37 | return XCONF.UxbusState.ERR_CODE 38 | elif data[3] & 0x20: 39 | self._has_err_warn = True 40 | return XCONF.UxbusState.WAR_CODE 41 | else: 42 | self._has_err_warn = False 43 | return 0 44 | 45 | def send_pend(self, funcode, num, timeout): 46 | if num == -1: 47 | ret = [0] * 254 48 | else: 49 | ret = [0] * (num + 1) 50 | times = int(timeout) 51 | ret[0] = XCONF.UxbusState.ERR_TOUT 52 | while times > 0: 53 | times -= 1 54 | rx_data = self.arm_port.read() 55 | if rx_data != -1 and len(rx_data) > 5: 56 | ret[0] = self.check_xbus_prot(rx_data) 57 | if num == -1: 58 | num = rx_data[2] 59 | for i in range(num): 60 | ret[i + 1] = rx_data[i + 4] 61 | return ret 62 | time.sleep(0.001) 63 | return ret 64 | 65 | def send_xbus(self, reg, txdata, num): 66 | send_data = bytes([self.fromid, self.toid, num + 1, reg]) 67 | for i in range(num): 68 | send_data += bytes([txdata[i]]) 69 | send_data += crc16.crc_modbus(send_data) 70 | self.arm_port.flush() 71 | return self.arm_port.write(send_data) 72 | -------------------------------------------------------------------------------- /xarm/core/wrapper/uxbus_cmd_tcp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Jimy Zhang 8 | # Author: Vinman 9 | 10 | 11 | import time 12 | from ..utils import convert 13 | from .uxbus_cmd import UxbusCmd 14 | from ..config.x_config import XCONF 15 | 16 | 17 | TX2_PROT_CON = 2 # tcp cmd prot 18 | TX2_PROT_HEAT = 1 # tcp heat prot 19 | TX2_BUS_FLAG_MIN = 1 # cmd序号 起始值 20 | TX2_BUS_FLAG_MAX = 5000 # cmd序号 最大值 21 | 22 | 23 | class UxbusCmdTcp(UxbusCmd): 24 | def __init__(self, arm_port): 25 | super(UxbusCmdTcp, self).__init__() 26 | self.arm_port = arm_port 27 | self.bus_flag = TX2_BUS_FLAG_MIN 28 | self.prot_flag = TX2_PROT_CON 29 | self._has_err_warn = False 30 | 31 | @property 32 | def has_err_warn(self): 33 | return self._has_err_warn 34 | 35 | @has_err_warn.setter 36 | def has_err_warn(self, value): 37 | self._has_err_warn = value 38 | 39 | def check_xbus_prot(self, data, funcode): 40 | num = convert.bytes_to_u16(data[0:2]) 41 | prot = convert.bytes_to_u16(data[2:4]) 42 | length = convert.bytes_to_u16(data[4:6]) 43 | fun = data[6] 44 | state = data[7] 45 | 46 | bus_flag = self.bus_flag 47 | if bus_flag == TX2_BUS_FLAG_MIN: 48 | bus_flag = TX2_BUS_FLAG_MAX 49 | else: 50 | bus_flag -= 1 51 | if num != bus_flag: 52 | return XCONF.UxbusState.ERR_NUM 53 | if prot != TX2_PROT_CON: 54 | return XCONF.UxbusState.ERR_PROT 55 | if fun != funcode: 56 | return XCONF.UxbusState.ERR_FUN 57 | if state & 0x40: 58 | self._has_err_warn = True 59 | return XCONF.UxbusState.ERR_CODE 60 | if state & 0x20: 61 | self._has_err_warn = True 62 | return XCONF.UxbusState.WAR_CODE 63 | if len(data) != length + 6: 64 | return XCONF.UxbusState.ERR_LENG 65 | self._has_err_warn = False 66 | return 0 67 | 68 | def send_pend(self, funcode, num, timeout): 69 | if num == -1: 70 | ret = [0] * 254 71 | else: 72 | ret = [0] * (num + 1) 73 | times = int(timeout) 74 | ret[0] = XCONF.UxbusState.ERR_TOUT 75 | while times > 0: 76 | times -= 1 77 | rx_data = self.arm_port.read() 78 | if rx_data != -1 and len(rx_data) > 7: 79 | # print('recv:', end=' ') 80 | # for i in range(len(rx_data)): 81 | # print(hex(rx_data[i]), end=',') 82 | # print() 83 | ret[0] = self.check_xbus_prot(rx_data, funcode) 84 | if ret[0] in [0, XCONF.UxbusState.ERR_CODE, XCONF.UxbusState.WAR_CODE]: 85 | if num == -1: 86 | num = rx_data[5] - 2 87 | ret1 = [ret[0]] * (num + 1) 88 | for i in range(num): 89 | ret1[i + 1] = rx_data[i + 8] 90 | return ret1 91 | else: 92 | for i in range(num): 93 | ret[i + 1] = rx_data[i + 8] 94 | return ret 95 | time.sleep(0.001) 96 | return ret 97 | 98 | def send_xbus(self, funcode, datas, num): 99 | send_data = convert.u16_to_bytes(self.bus_flag) 100 | send_data += convert.u16_to_bytes(self.prot_flag) 101 | send_data += convert.u16_to_bytes(num + 1) 102 | send_data += bytes([funcode]) 103 | if type(datas) == str: 104 | send_data += datas.encode() 105 | else: 106 | for i in range(num): 107 | send_data += bytes([datas[i]]) 108 | self.arm_port.flush() 109 | # print('send:', end=' ') 110 | # for i in range(len(send_data)): 111 | # print(hex(send_data[i]), end=',') 112 | # print() 113 | ret = self.arm_port.write(send_data) 114 | if ret != 0: 115 | return -1 116 | self.bus_flag += 1 117 | if self.bus_flag > TX2_BUS_FLAG_MAX: 118 | self.bus_flag = TX2_BUS_FLAG_MIN 119 | return 0 120 | -------------------------------------------------------------------------------- /xarm/tools/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erwincoumans/xArm-Python-SDK/9f03fa38fff5e60a71071d396181ae04f0f961fd/xarm/tools/__init__.py -------------------------------------------------------------------------------- /xarm/tools/list_ports.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (MIT License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | from serial.tools import list_ports 10 | 11 | def _dump_port(d): 12 | print('-' * 80) 13 | print(' device : {}'.format(d.device)) 14 | print(' hwid : {}'.format(d.hwid)) 15 | print(' product : {}'.format(d.hwid)) 16 | print(' description: {}'.format(d.hwid)) 17 | print('-' * 80) 18 | 19 | def get_ports(is_dump=True): 20 | ports = [] 21 | for i in list_ports.comports(): 22 | # if i.pid is not None and '{:04x}:{:04x}'.format(i.vid, i.pid) == vidpid: 23 | if i.pid is not None: 24 | if is_dump: 25 | _dump_port(i) 26 | ports.append({ 27 | 'pid': '{:04x}'.format(i.pid), 28 | 'vid': '{:04x}'.format(i.vid), 29 | 'device': i.device, 30 | 'serial_number': i.serial_number, 31 | 'hwid': i.hwid, 32 | 'name': i.name, 33 | 'description': i.description, 34 | 'interface': i.interface, 35 | 'location': i.location, 36 | 'manufacturer': i.manufacturer, 37 | 'product': i.product 38 | }) 39 | return ports -------------------------------------------------------------------------------- /xarm/version.py: -------------------------------------------------------------------------------- 1 | __version__ = '1.4.0' 2 | -------------------------------------------------------------------------------- /xarm/wrapper/__init__.py: -------------------------------------------------------------------------------- 1 | from .xarm_api import XArmAPI 2 | -------------------------------------------------------------------------------- /xarm/x3/code.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | 10 | from ..core.config.x_config import XCONF 11 | 12 | 13 | class APIState(object): 14 | NOT_CONNECTED = -1 # 已断开或未连接 15 | NOT_READY = -2 # 未使能或者设置状态 16 | API_EXCEPTION = -3 # 接口异常，可能是参数错误 17 | CMD_NOT_EXIST = -4 # 命令不存在 18 | TCP_LIMIT = -6 # 笛卡尔限位 19 | JOINT_LIMIT = -7 # 关节角度限位 20 | OUT_OF_RANGE = -8 # 超出范围 21 | EMERGENCY_STOP = -9 # 紧急停止 22 | SERVO_NOT_EXIST = -10 # 不存在此ID的关节 23 | CONVERT_FAILED = -11 # 转换Blockly失败 24 | NORMAL = 0 # 正常 25 | HAS_ERROR = XCONF.UxbusState.ERR_CODE # 有尚未清除的错误 26 | HAS_WARN = XCONF.UxbusState.WAR_CODE # 有尚未清除的警告 27 | RES_TIMEOUT = XCONF.UxbusState.ERR_TOUT # 命令回复超时 28 | RES_LENGTH_ERROR = XCONF.UxbusState.ERR_LENG # TCP长度错误 29 | CMD_NUM_ERROR = XCONF.UxbusState.ERR_NUM # TCP序号错误 30 | CMD_PROT_ERROR = XCONF.UxbusState.ERR_PROT # TCP协议标志错误 31 | FUN_ERROR = XCONF.UxbusState.ERR_FUN # TCP回复指令和发送指令不匹配 32 | NO_TCP = XCONF.UxbusState.ERR_NOTTCP # 写数据异常 33 | OTHER = XCONF.UxbusState.ERR_OTHER # 其它错误 34 | PARAM_ERROR = XCONF.UxbusState.ERR_PARAM # 参数错误 35 | TRAJ_RW_FAILED = 31 # 读写轨迹失败(加载轨迹或保存轨迹) 36 | TRAJ_RW_TOUT = 32 # 读写轨迹等待超时(加载轨迹或保存轨迹) 37 | TRAJ_PLAYBACK_TOUT = 33 # 回放轨迹超时(多种情况) 38 | SUCTION_CUP_TOUT = 41 # 等待吸泵设置超时 39 | 40 | 41 | -------------------------------------------------------------------------------- /xarm/x3/events.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | REPORT_ID = 'REPORT' 10 | REPORT_LOCATION_ID = 'LOCATION' 11 | REPORT_CONNECT_CHANGED_ID = 'REPORT_CONNECT_CHANGED' 12 | REPORT_STATE_CHANGED_ID = 'REPORT_STATE_CHANGED' 13 | REPORT_MODE_CHANGED_ID = 'REPORT_MODE_CHANGED' 14 | REPORT_MTABLE_MTBRAKE_CHANGED_ID = 'REPORT_MTABLE_MTBRAKE_CHANGED' 15 | REPORT_ERROR_WARN_CHANGED_ID = 'REPORT_ERROR_WARN_CHANGED' 16 | REPORT_CMDNUM_CHANGED_ID = 'REPORT_CMDNUM_CHANGED' 17 | REPORT_TEMPERATURE_CHANGED_ID = 'REPORT_TEMPERATURE_CHANGED' 18 | REPORT_COUNT_CHANGED_ID = 'REPORT_COUNT_CHANGED' 19 | 20 | 21 | class Events(object): 22 | def __init__(self): 23 | self._report_callbacks = { 24 | REPORT_ID: [], 25 | REPORT_LOCATION_ID: [], 26 | REPORT_CONNECT_CHANGED_ID: [], 27 | REPORT_ERROR_WARN_CHANGED_ID: [], 28 | REPORT_STATE_CHANGED_ID: [], 29 | REPORT_MODE_CHANGED_ID: [], 30 | REPORT_MTABLE_MTBRAKE_CHANGED_ID: [], 31 | REPORT_CMDNUM_CHANGED_ID: [], 32 | } 33 | 34 | def _register_report_callback(self, report_id, callback): 35 | if report_id not in self._report_callbacks.keys(): 36 | self._report_callbacks[report_id] = [] 37 | if (callable(callback) or isinstance(callback, dict)) and callback not in self._report_callbacks[report_id]: 38 | self._report_callbacks[report_id].append(callback) 39 | return True 40 | elif not (callable(callback) or isinstance(callback, dict)): 41 | return False 42 | else: 43 | return True 44 | 45 | def _release_report_callback(self, report_id, callback): 46 | if report_id in self._report_callbacks.keys() and callback: 47 | if callback is None: 48 | self._report_callbacks[report_id].clear() 49 | return True 50 | elif callback: 51 | for cb in self._report_callbacks[report_id]: 52 | if callback == cb: 53 | self._report_callbacks[report_id].remove(callback) 54 | return True 55 | elif isinstance(cb, dict): 56 | if cb['callback'] == callback: 57 | self._report_callbacks[report_id].remove(cb) 58 | return True 59 | return False 60 | 61 | def register_report_callback(self, callback=None, report_cartesian=True, report_joints=True, 62 | report_state=True, report_error_code=True, report_warn_code=True, 63 | report_mtable=True, report_mtbrake=True, report_cmd_num=True): 64 | return self._register_report_callback(REPORT_ID, { 65 | 'callback': callback, 66 | 'cartesian': report_cartesian, 67 | 'joints': report_joints, 68 | 'error_code': report_error_code, 69 | 'warn_code': report_warn_code, 70 | 'state': report_state, 71 | 'mtable': report_mtable, 72 | 'mtbrake': report_mtbrake, 73 | 'cmdnum': report_cmd_num 74 | }) 75 | 76 | def register_report_location_callback(self, callback=None, report_cartesian=True, report_joints=False): 77 | ret = self._register_report_callback(REPORT_LOCATION_ID, { 78 | 'callback': callback, 79 | 'cartesian': report_cartesian, 80 | 'joints': report_joints, 81 | }) 82 | return ret 83 | 84 | def register_connect_changed_callback(self, callback=None): 85 | return self._register_report_callback(REPORT_CONNECT_CHANGED_ID, callback) 86 | 87 | def register_state_changed_callback(self, callback=None): 88 | return self._register_report_callback(REPORT_STATE_CHANGED_ID, callback) 89 | 90 | def register_mode_changed_callback(self, callback=None): 91 | return self._register_report_callback(REPORT_MODE_CHANGED_ID, callback) 92 | 93 | def register_mtable_mtbrake_changed_callback(self, callback=None): 94 | return self._register_report_callback(REPORT_MTABLE_MTBRAKE_CHANGED_ID, callback) 95 | 96 | def register_error_warn_changed_callback(self, callback=None): 97 | return self._register_report_callback(REPORT_ERROR_WARN_CHANGED_ID, callback) 98 | 99 | def register_cmdnum_changed_callback(self, callback=None): 100 | return self._register_report_callback(REPORT_CMDNUM_CHANGED_ID, callback) 101 | 102 | def register_temperature_changed_callback(self, callback=None): 103 | return self._register_report_callback(REPORT_TEMPERATURE_CHANGED_ID, callback) 104 | 105 | def register_count_changed_callback(self, callback=None): 106 | return self._register_report_callback(REPORT_COUNT_CHANGED_ID, callback) 107 | 108 | def release_report_callback(self, callback=None): 109 | return self._release_report_callback(REPORT_ID, callback) 110 | 111 | def release_report_location_callback(self, callback=None): 112 | return self._release_report_callback(REPORT_LOCATION_ID, callback) 113 | 114 | def release_connect_changed_callback(self, callback=None): 115 | return self._release_report_callback(REPORT_CONNECT_CHANGED_ID, callback) 116 | 117 | def release_state_changed_callback(self, callback=None): 118 | return self._release_report_callback(REPORT_STATE_CHANGED_ID, callback) 119 | 120 | def release_mode_changed_callback(self, callback=None): 121 | return self._release_report_callback(REPORT_MODE_CHANGED_ID, callback) 122 | 123 | def release_mtable_mtbrake_changed_callback(self, callback=None): 124 | return self._release_report_callback(REPORT_MTABLE_MTBRAKE_CHANGED_ID, callback) 125 | 126 | def release_error_warn_changed_callback(self, callback=None): 127 | return self._release_report_callback(REPORT_ERROR_WARN_CHANGED_ID, callback) 128 | 129 | def release_cmdnum_changed_callback(self, callback=None): 130 | return self._release_report_callback(REPORT_CMDNUM_CHANGED_ID, callback) 131 | 132 | def release_temperature_changed_callback(self, callback=None): 133 | return self._release_report_callback(REPORT_TEMPERATURE_CHANGED_ID, callback) 134 | 135 | def release_count_changed_callback(self, callback=None): 136 | return self._release_report_callback(REPORT_COUNT_CHANGED_ID, callback) -------------------------------------------------------------------------------- /xarm/x3/parse.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (MIT License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import re 10 | 11 | GCODE_PARAM_X = 'X' # TCP-X 12 | GCODE_PARAM_Y = 'Y' # TCP-Y 13 | GCODE_PARAM_Z = 'Z' # TCP-Z 14 | GCODE_PARAM_A = 'A' # TCP-Roll 15 | GCODE_PARAM_B = 'B' # TCP-Pitch 16 | GCODE_PARAM_C = 'C' # TCP-Yaw 17 | GCODE_PARAM_R = 'R' # TCP-Radius 18 | GCODE_PARAM_I = 'I' # Joint-1 19 | GCODE_PARAM_J = 'J' # Joint-2 20 | GCODE_PARAM_K = 'K' # Joint-3 21 | GCODE_PARAM_L = 'L' # Joint-4 22 | GCODE_PARAM_M = 'M' # Joint-5 23 | GCODE_PARAM_N = 'N' # Joint-6 24 | GCODE_PARAM_O = 'O' # Joint-7 25 | GCODE_PARAM_F = 'F' # Move-Speed 26 | GCODE_PARAM_Q = 'Q' # Move-Acc 27 | GCODE_PARAM_T = 'T' # Move-Time 28 | GCODE_PARAM_V = 'V' # Value 29 | GCODE_PARAM_D = 'D' # Addr 30 | 31 | 32 | class GcodeParser: 33 | def __init__(self): 34 | self._int_val = 0 35 | self._float_val = 0.0 36 | 37 | @staticmethod 38 | def __get_value(string, ch, return_type, default=None): 39 | pattern = '{}(\-?\d+\.?\d*)'.format(ch) 40 | data = re.findall(pattern, string) 41 | if len(data) > 0: 42 | return return_type(data[0]) 43 | return default 44 | 45 | @staticmethod 46 | def __get_hex_value(string, ch, default=None): 47 | pattern = '{}(-?\w{{3,4}})'.format(ch) 48 | data = re.findall(pattern, string) 49 | if len(data) > 0: 50 | return int(data[0], base=16) 51 | return default 52 | 53 | def _get_int_value(self, string, ch, default=None): 54 | return self.__get_value(string, ch, int, default=default) 55 | 56 | def _get_float_value(self, string, ch, default=None): 57 | return self.__get_value(string, ch, float, default=default) 58 | 59 | def get_int_value(self, string, default=None): 60 | if default is None: 61 | default = self._int_val 62 | self._int_val = self._get_int_value(string, GCODE_PARAM_V, default=default) 63 | return self._int_val 64 | else: 65 | return self._get_int_value(string, GCODE_PARAM_V, default=default) 66 | 67 | def get_float_value(self, string, default=0): 68 | return self._get_float_value(string, GCODE_PARAM_V, default=default) 69 | 70 | def get_addr(self, string, default=0): 71 | return self.__get_hex_value(string, GCODE_PARAM_D, default=default) 72 | 73 | def get_gcode_cmd_num(self, string, ch): 74 | return self._get_int_value(string, ch, default=-1) 75 | 76 | def get_mvvelo(self, string, default=None): 77 | return self._get_float_value(string, GCODE_PARAM_F, default=default) 78 | 79 | def get_mvacc(self, string, default=None): 80 | return self._get_float_value(string, GCODE_PARAM_Q, default=default) 81 | 82 | def get_mvtime(self, string, default=None): 83 | return self._get_float_value(string, GCODE_PARAM_T, default=default) 84 | 85 | def get_mvradius(self, string, default=None): 86 | return self._get_float_value(string, GCODE_PARAM_R, default=default) 87 | 88 | def get_id_num(self, string, default=None): 89 | return self._get_int_value(string, GCODE_PARAM_I, default=default) 90 | 91 | def get_poses(self, string, default=None): 92 | pose = [None] * 6 93 | pose[0] = self._get_float_value(string[2:], GCODE_PARAM_X, default=default) 94 | pose[1] = self._get_float_value(string[2:], GCODE_PARAM_Y, default=default) 95 | pose[2] = self._get_float_value(string[2:], GCODE_PARAM_Z, default=default) 96 | pose[3] = self._get_float_value(string[2:], GCODE_PARAM_A, default=default) 97 | pose[4] = self._get_float_value(string[2:], GCODE_PARAM_B, default=default) 98 | pose[5] = self._get_float_value(string[2:], GCODE_PARAM_C, default=default) 99 | return pose 100 | 101 | def get_joints(self, string, default=None): 102 | joints = [None] * 7 103 | joints[0] = self._get_float_value(string[2:], GCODE_PARAM_I, default=default) 104 | joints[1] = self._get_float_value(string[2:], GCODE_PARAM_J, default=default) 105 | joints[2] = self._get_float_value(string[2:], GCODE_PARAM_K, default=default) 106 | joints[3] = self._get_float_value(string[2:], GCODE_PARAM_L, default=default) 107 | joints[4] = self._get_float_value(string[2:], GCODE_PARAM_M, default=default) 108 | joints[5] = self._get_float_value(string[2:], GCODE_PARAM_N, default=default) 109 | joints[6] = self._get_float_value(string[2:], GCODE_PARAM_O, default=default) 110 | return joints 111 | -------------------------------------------------------------------------------- /xarm/x3/record.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import json 10 | import time 11 | from urllib import request 12 | from .utils import xarm_is_connected 13 | from .code import APIState 14 | from ..core.config.x_config import XCONF 15 | from ..core.utils.log import logger 16 | 17 | 18 | class Record(object): 19 | def __init__(self): 20 | pass 21 | 22 | @xarm_is_connected(_type='get') 23 | def get_trajectories(self, ip=None): 24 | if ip is None: 25 | url = 'http://{}:18333/cmd'.format(self._port) 26 | else: 27 | url = 'http://{}:18333/cmd'.format(ip) 28 | try: 29 | data = {'cmd': 'xarm_list_trajs'} 30 | req = request.Request(url, headers={'Content-Type': 'application/json'}, data=json.dumps(data).encode('utf-8')) 31 | res = request.urlopen(req) 32 | if res.code == 200: 33 | result = json.loads(res.read().decode('utf-8')) 34 | return result['res'][0], [{'name': item['name'], 'duration': item['count'] / 100} for item in result['res'][1]] 35 | else: 36 | return APIState.API_EXCEPTION, [] 37 | except Exception as e: 38 | return APIState.API_EXCEPTION, [] 39 | 40 | @xarm_is_connected(_type='set') 41 | def start_record_trajectory(self): 42 | ret = self.arm_cmd.set_record_traj(1) 43 | logger.info('API -> start_record_trajectory -> ret={}'.format(ret[0])) 44 | return ret[0] 45 | 46 | @xarm_is_connected(_type='set') 47 | def stop_record_trajectory(self, filename=None): 48 | ret = self.arm_cmd.set_record_traj(0) 49 | if isinstance(filename, str) and filename.strip(): 50 | ret2 = self.save_record_trajectory(filename, wait=True, timeout=10) 51 | if ret2 != 0: 52 | return ret2 53 | logger.info('API -> stop_record_trajectory -> ret={}'.format(ret[0])) 54 | return ret[0] 55 | 56 | @xarm_is_connected(_type='set') 57 | def save_record_trajectory(self, filename, wait=True, timeout=2): 58 | assert isinstance(filename, str) and filename.strip() 59 | filename = filename.strip() 60 | if not filename.endswith('.traj'): 61 | full_filename = '{}.traj'.format(filename) 62 | else: 63 | full_filename = filename 64 | ret = self.arm_cmd.save_traj(full_filename, wait_time=0) 65 | logger.info('API -> save_record_trajectory -> ret={}'.format(ret[0])) 66 | if ret[0] in [0, XCONF.UxbusState.ERR_CODE, XCONF.UxbusState.WAR_CODE]: 67 | if wait: 68 | start_time = time.time() 69 | while time.time() - start_time < timeout: 70 | code, status = self.get_trajectory_rw_status() 71 | if code in [0, XCONF.UxbusState.ERR_CODE, XCONF.UxbusState.WAR_CODE]: 72 | if status == XCONF.TrajState.IDLE: 73 | logger.info('Save {} failed'.format(filename)) 74 | return APIState.TRAJ_RW_FAILED 75 | elif status == XCONF.TrajState.SAVE_SUCCESS: 76 | logger.info('Save {} success'.format(filename)) 77 | return 0 78 | elif status == XCONF.TrajState.SAVE_FAIL: 79 | logger.error('Save {} failed'.format(filename)) 80 | return APIState.TRAJ_RW_FAILED 81 | time.sleep(0.1) 82 | logger.warning('Save {} timeout'.format(filename)) 83 | return APIState.TRAJ_RW_TOUT 84 | else: 85 | return ret[0] 86 | logger.error('Save {} failed, ret={}'.format(filename, ret)) 87 | return ret[0] 88 | 89 | @xarm_is_connected(_type='set') 90 | def load_trajectory(self, filename, wait=True, timeout=10): 91 | assert isinstance(filename, str) and filename.strip() 92 | filename = filename.strip() 93 | if not filename.endswith('.traj'): 94 | full_filename = '{}.traj'.format(filename) 95 | else: 96 | full_filename = filename 97 | ret = self.arm_cmd.load_traj(full_filename, wait_time=0) 98 | logger.info('API -> load_trajectory -> ret={}'.format(ret[0])) 99 | if ret[0] == 0: 100 | if wait: 101 | start_time = time.time() 102 | while time.time() - start_time < timeout: 103 | code, status = self.get_trajectory_rw_status() 104 | if code == 0: 105 | if status == XCONF.TrajState.IDLE: 106 | logger.info('Load {} failed'.format(filename)) 107 | return APIState.TRAJ_RW_FAILED 108 | elif status == XCONF.TrajState.LOAD_SUCCESS: 109 | logger.info('Load {} success'.format(filename)) 110 | return 0 111 | elif status == XCONF.TrajState.LOAD_FAIL: 112 | logger.error('Load {} failed'.format(filename)) 113 | return APIState.TRAJ_RW_FAILED 114 | time.sleep(0.1) 115 | logger.warning('Load {} timeout'.format(filename)) 116 | return APIState.TRAJ_RW_TOUT 117 | else: 118 | return ret[0] 119 | logger.error('Load {} failed, ret={}'.format(filename, ret)) 120 | return ret[0] 121 | 122 | @xarm_is_connected(_type='set') 123 | def playback_trajectory(self, times=1, filename=None, wait=False, double_speed=1): 124 | assert isinstance(times, int) 125 | times = times if times > 0 else -1 126 | if isinstance(filename, str) and filename.strip(): 127 | ret = self.load_trajectory(filename, wait=True, timeout=10) 128 | if ret != 0: 129 | return ret 130 | if self.state == 4: 131 | return APIState.NOT_READY 132 | if self.version_is_ge_1_2_11: 133 | ret = self.arm_cmd.playback_traj(times, double_speed) 134 | else: 135 | ret = self.arm_cmd.playback_traj_old(times) 136 | logger.info('API -> playback_trajectory -> ret={}'.format(ret[0])) 137 | if ret[0] == 0 and wait: 138 | start_time = time.time() 139 | while self.state != 1: 140 | if self.state == 4: 141 | return APIState.NOT_READY 142 | if time.time() - start_time > 5: 143 | return APIState.TRAJ_PLAYBACK_TOUT 144 | time.sleep(0.1) 145 | max_count = int((time.time() - start_time) / 0.1) 146 | max_count = max_count if max_count > 10 else 10 147 | start_time = time.time() 148 | while self.mode != 11: 149 | if self.state == 1: 150 | start_time = time.time() 151 | time.sleep(0.1) 152 | continue 153 | if self.state == 4: 154 | return APIState.NOT_READY 155 | if time.time() - start_time > 5: 156 | return APIState.TRAJ_PLAYBACK_TOUT 157 | time.sleep(0.1) 158 | time.sleep(0.1) 159 | count = 0 160 | while self.state != 4: 161 | if self.state == 2: 162 | if times == 1: 163 | break 164 | count += 1 165 | else: 166 | count = 0 167 | if count > max_count: 168 | break 169 | time.sleep(0.1) 170 | # while self.state != 4 and self.state != 2: 171 | # time.sleep(0.1) 172 | if self.state != 4: 173 | self.set_mode(0) 174 | self.set_state(0) 175 | return ret[0] 176 | 177 | @xarm_is_connected(_type='get') 178 | def get_trajectory_rw_status(self): 179 | ret = self.arm_cmd.get_traj_rw_status() 180 | return ret[0], ret[1] 181 | -------------------------------------------------------------------------------- /xarm/x3/utils.py: -------------------------------------------------------------------------------- 1 | # !/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import time 10 | import functools 11 | from ..core.utils.log import logger 12 | from .code import APIState 13 | 14 | 15 | def xarm_is_connected(_type='set'): 16 | def _xarm_is_connected(func): 17 | @functools.wraps(func) 18 | def decorator(*args, **kwargs): 19 | if args[0].connected: 20 | return func(*args, **kwargs) 21 | else: 22 | logger.error('xArm is not connect') 23 | return APIState.NOT_CONNECTED if _type == 'set' else (APIState.NOT_CONNECTED, 'xArm is not connect') 24 | return decorator 25 | return _xarm_is_connected 26 | 27 | 28 | def xarm_is_ready(_type='set'): 29 | def _xarm_is_ready(func): 30 | @functools.wraps(func) 31 | def decorator(*args, **kwargs): 32 | if args[0].connected and kwargs.get('auto_enable', False): 33 | if not args[0].ready: 34 | args[0].motion_enable(enable=True) 35 | args[0].set_mode(0) 36 | args[0].set_state(0) 37 | if args[0].connected: 38 | if not args[0]._check_is_ready or args[0].ready: 39 | return func(*args, **kwargs) 40 | else: 41 | logger.error('xArm is not ready') 42 | logger.info('Please check the arm for errors. If so, please clear the error first. ' 43 | 'Then enable the motor, set the mode and set the state') 44 | return APIState.NOT_READY if _type == 'set' else (APIState.NOT_READY, 'xArm is not ready') 45 | else: 46 | logger.error('xArm is not connect') 47 | return APIState.NOT_CONNECTED if _type == 'set' else (APIState.NOT_CONNECTED, 'xArm is not connect') 48 | return decorator 49 | return _xarm_is_ready 50 | 51 | 52 | def xarm_is_pause(_type='set'): 53 | def _xarm_is_pause(func): 54 | @functools.wraps(func) 55 | def decorator(*args, **kwargs): 56 | args[0].check_is_pause() 57 | return func(*args, **kwargs) 58 | # try: 59 | # if args[0].connected: 60 | # return func(*args, **kwargs) 61 | # else: 62 | # logger.error('xArm is not connect') 63 | # return APIState.NOT_CONNECTED if _type == 'set' else APIState.NOT_CONNECTED, 'xArm is not connect' 64 | # except Exception as e: 65 | # logger.error('{} - {} - {}'.format(type(e).__name__, func.__name__, e)) 66 | # return APIState.API_EXCEPTION if _type == 'set' else APIState.API_EXCEPTION, str(e) 67 | return decorator 68 | return _xarm_is_pause 69 | 70 | 71 | # def xarm_is_connected(_type='set'): 72 | # def _xarm_is_connected(func): 73 | # @functools.wraps(func) 74 | # def decorator(*args, **kwargs): 75 | # try: 76 | # if args[0].connected: 77 | # return func(*args, **kwargs) 78 | # else: 79 | # logger.error('xArm is not connect') 80 | # return APIState.NOT_CONNECTED if _type == 'set' else APIState.NOT_CONNECTED, 'xArm is not connect' 81 | # except Exception as e: 82 | # logger.error('{} - {} - {}'.format(type(e).__name__, func.__name__, e)) 83 | # return APIState.API_EXCEPTION if _type == 'set' else APIState.API_EXCEPTION, str(e) 84 | # return decorator 85 | # return _xarm_is_connected 86 | # 87 | # 88 | # def xarm_is_ready(_type='set'): 89 | # def _xarm_is_ready(func): 90 | # @functools.wraps(func) 91 | # def decorator(*args, **kwargs): 92 | # try: 93 | # if args[0].connected and kwargs.get('auto_enable', False): 94 | # args[0].motion_enable(enable=True) 95 | # args[0].set_state(0) 96 | # if args[0].connected and args[0].ready: 97 | # return func(*args, **kwargs) 98 | # elif not args[0].connected: 99 | # logger.error('xArm is not connect') 100 | # return APIState.NOT_CONNECTED if _type == 'set' else APIState.NOT_CONNECTED, 'xArm is not connect' 101 | # else: 102 | # logger.error('xArm is not ready') 103 | # return APIState.NOT_READY if _type == 'set' else APIState.NOT_READY, 'xArm is not ready' 104 | # except Exception as e: 105 | # logger.error('{} - {} - {}'.format(type(e).__name__, func.__name__, e)) 106 | # return APIState.API_EXCEPTION if _type == 'set' else APIState.API_EXCEPTION, str(e) 107 | # return decorator 108 | # return _xarm_is_ready 109 | 110 | def compare_time(time1, time2): 111 | try: 112 | s_time = time.mktime(time.strptime(time1, '%Y-%m-%d')) 113 | e_time = time.mktime(time.strptime(time2, '%Y-%m-%d')) 114 | return int(s_time) - int(e_time) > 0 115 | except: 116 | return False 117 | 118 | 119 | def compare_version(v1, v2): 120 | for i in range(3): 121 | if v1[i] > v2[i]: 122 | return True 123 | elif v1[i] < v2[i]: 124 | return False 125 | return False 126 | --------------------------------------------------------------------------------