|
| 1 | +Introduction |
| 2 | +============ |
| 3 | + |
| 4 | +This is a detailed walkthrough on how to configure MoveIt! framework to |
| 5 | +control an uArm Metal mechanical arm. All the needed config files |
| 6 | +are provided by this layer. |
| 7 | + |
| 8 | +Prerequisites |
| 9 | +============= |
| 10 | + |
| 11 | +Your uArm Metal's controller must be programmed to support a small subset |
| 12 | +of G-codes sent via its serial. Particularly it needs to understand the codes: |
| 13 | + |
| 14 | +M203 |
| 15 | + Attach all servos. |
| 16 | + |
| 17 | +M204 |
| 18 | + Detach all servos. |
| 19 | + |
| 20 | +G205 <base_angle> <left_angle> <right_angle> <hand_angle> |
| 21 | + Set position for all four servos. The code accepts four parameters which |
| 22 | + are PWM signal widths for the respective servos in microseconds. |
| 23 | + |
| 24 | +P200 |
| 25 | + Return current servo positions as raw data from the servos' analog pins. |
| 26 | + The command should reply with a line like:: |
| 27 | + |
| 28 | + RET <base_analog_angle> <left_analog_angle> <right_analog_angle> <hand_analog_angle> |
| 29 | + |
| 30 | +As soon as the controller is ready to receive a new G-code it must signal that |
| 31 | +by sending `ok` to serial. |
| 32 | + |
| 33 | +Setting up |
| 34 | +========== |
| 35 | + |
| 36 | +1. Create an image containing the packages `uarmmeta-moveit-config`, |
| 37 | + `python3-pyserial` and `python3-asyncio`, e.g. by putting this line |
| 38 | + in your `local.conf`:: |
| 39 | + |
| 40 | + IMAGE_INSTALL_append = " uarmmeta-moveit-config python3-pyserial python3-asyncio" |
| 41 | + |
| 42 | + and running:: |
| 43 | + |
| 44 | + $ bitbake refkit-image-common |
| 45 | + |
| 46 | +2. Install the image onto your device. See the howto "Installing the Images" |
| 47 | + in IoT Reference OS Kit documentation for details. |
| 48 | + |
| 49 | +3. Plug the mechanical arm in the device's USB port. |
| 50 | + |
| 51 | +4. Run the following script on the device. |
| 52 | + |
| 53 | +.. code:: python |
| 54 | +
|
| 55 | + #!/usr/bin/env python3 |
| 56 | + """ |
| 57 | + TCP robot controller implementing streaming Joint Position Streaming Interface |
| 58 | + for uARM Metal. |
| 59 | +
|
| 60 | + The controller acts as a server for all connections and manages |
| 61 | + the mechanical arm through a serial connection. |
| 62 | +
|
| 63 | + See http://wiki.ros.org/Industrial/Tutorials/create_joint_position_streaming_interface_using_tcp_socket_libraries. |
| 64 | + """ |
| 65 | +
|
| 66 | + import asyncio |
| 67 | + import struct |
| 68 | + import math |
| 69 | +
|
| 70 | + import serial |
| 71 | +
|
| 72 | + # Analog signal for servos' null positions |
| 73 | + BASE_ANALOG_NULL = 350 |
| 74 | + LEFT_ANALOG_NULL = 145 |
| 75 | + RIGHT_ANALOG_NULL = 137 |
| 76 | + HAND_ANALOG_NULL = 282 |
| 77 | +
|
| 78 | + # 1 unit of analog signal in radians |
| 79 | + BASE_ANALOG_RAD = math.pi/(3.111111111*180) |
| 80 | + LEFT_ANALOG_RAD = math.pi/(2.811111111*180) |
| 81 | + RIGHT_ANALOG_RAD = math.pi/(2.655555555*180) |
| 82 | +
|
| 83 | + # 1 PWM microsecond in analog signal units |
| 84 | + BASE_PWM_ANALOG = 0.281 |
| 85 | + LEFT_PWM_ANALOG = 0.280 |
| 86 | + RIGHT_PWM_ANALOG = 0.275 |
| 87 | +
|
| 88 | + # Zero positions in PWM microseconds |
| 89 | + BASE_ZERO_PWM = 1500 |
| 90 | + LEFT_ZERO_PWM = 750 |
| 91 | + RIGHT_ZERO_PWM = 700 |
| 92 | + HAND_ZERO_PWM = 1500 |
| 93 | +
|
| 94 | + class RobotArm(object): |
| 95 | + """Represent the mechanical arm.""" |
| 96 | +
|
| 97 | + def __init__(self, loop): |
| 98 | + """Initialize object.""" |
| 99 | +
|
| 100 | + self.is_inited = False |
| 101 | + self.transport = None |
| 102 | + self.loop = loop |
| 103 | + self.serial_lock = asyncio.Lock(loop=loop) |
| 104 | + self.move_lock = asyncio.Lock(loop=loop) |
| 105 | + self.serial = serial.Serial("/dev/ttyUSB0", 115200, timeout=5) |
| 106 | + loop.add_reader(self.serial, self.__serial_reader) |
| 107 | +
|
| 108 | + @asyncio.coroutine |
| 109 | + def __initialize(self): |
| 110 | + """Initialize robot arm.""" |
| 111 | +
|
| 112 | + self.is_inited = True |
| 113 | + yield from self.__send_to_robot(b"M203\n") |
| 114 | + yield from self.__send_to_robot(b"G205 1500 1300 1300 1500\n") |
| 115 | +
|
| 116 | + def __serial_reader(self): |
| 117 | + line = self.serial.readline() |
| 118 | +
|
| 119 | + if line == b"ok\r\n": |
| 120 | + if self.is_inited: |
| 121 | + self.serial_lock.release() |
| 122 | + else: |
| 123 | + self.loop.create_task(self.__initialize()) |
| 124 | + elif line.startswith(b"RET") and not self.transport.is_closing(): |
| 125 | + (j1, j2, j3, _) = (int(analog_signal) |
| 126 | + for analog_signal in line[4:].split()) |
| 127 | + base_link_to_base_rot = (j1 - BASE_ANALOG_NULL) * BASE_ANALOG_RAD |
| 128 | + base_rot_to_link_1 = (j2 - LEFT_ANALOG_NULL) * LEFT_ANALOG_RAD |
| 129 | + link_2_to_link_3 = (j3 - RIGHT_ANALOG_NULL) * RIGHT_ANALOG_RAD |
| 130 | + link_1_to_link_2 = -base_rot_to_link_1 - link_2_to_link_3 |
| 131 | + link_3_to_link_end = 0 |
| 132 | + message = struct.pack("<Iiiiiffffffffff", 56, 10, 1, 0, 0, |
| 133 | + base_link_to_base_rot, |
| 134 | + base_rot_to_link_1, |
| 135 | + link_1_to_link_2, |
| 136 | + link_2_to_link_3, |
| 137 | + link_3_to_link_end, |
| 138 | + 0.0, 0.0, 0.0, 0.0, 0.0) |
| 139 | + self.transport.write(message) |
| 140 | + elif line.startswith(b"ERROR"): |
| 141 | + print("Got from robot: %s" % line) |
| 142 | +
|
| 143 | + @asyncio.coroutine |
| 144 | + def __send_to_robot(self, command): |
| 145 | + yield from self.serial_lock |
| 146 | + self.serial.write(command) |
| 147 | +
|
| 148 | + @asyncio.coroutine |
| 149 | + def __periodic_p200(self): |
| 150 | + while not self.transport.is_closing(): |
| 151 | + if self.is_inited: |
| 152 | + yield from self.__send_to_robot(b"P200\n") |
| 153 | + yield from asyncio.sleep(0.1) |
| 154 | + self.transport = None |
| 155 | +
|
| 156 | + def deinit(self): |
| 157 | + """Gracefully deinitialize robot arm.""" |
| 158 | +
|
| 159 | + self.loop.run_until_complete(self.__send_to_robot(b"M204\n")) |
| 160 | + self.serial.close() |
| 161 | +
|
| 162 | + def stream_joint_state(self, transport): |
| 163 | + """Start streaming arm's current state to TCP transport.""" |
| 164 | +
|
| 165 | + self.transport = transport |
| 166 | + asyncio.ensure_future(self.__periodic_p200(), loop=self.loop) |
| 167 | +
|
| 168 | + @asyncio.coroutine |
| 169 | + def move_servos(self, base_angle, left_angle, right_angle, hand_angle, |
| 170 | + duration): |
| 171 | + """Move arm's servos to given angles.""" |
| 172 | +
|
| 173 | + base_pwm = int(base_angle / (BASE_ANALOG_RAD * BASE_PWM_ANALOG) + |
| 174 | + BASE_ZERO_PWM) |
| 175 | + left_pwm = int(left_angle / (LEFT_ANALOG_RAD * LEFT_PWM_ANALOG) + |
| 176 | + LEFT_ZERO_PWM) |
| 177 | + right_pwm = int(right_angle / (RIGHT_ANALOG_RAD * RIGHT_PWM_ANALOG) + |
| 178 | + RIGHT_ZERO_PWM) |
| 179 | + hand_pwm = HAND_ZERO_PWM |
| 180 | + message = b"G205 %d %d %d %d\n" % (base_pwm, left_pwm, |
| 181 | + right_pwm, hand_pwm) |
| 182 | + with (yield from self.move_lock): |
| 183 | + print(message[:-1]) |
| 184 | + yield from self.__send_to_robot(message) |
| 185 | + yield from asyncio.sleep(duration) |
| 186 | +
|
| 187 | + class JointPositionStreamProtocol(asyncio.Protocol): |
| 188 | + """Implements joint position streamming protocol.""" |
| 189 | +
|
| 190 | + def __init__(self, loop, robot): |
| 191 | +
|
| 192 | + self.loop = loop |
| 193 | + self.robot = robot |
| 194 | + self.transport = None |
| 195 | +
|
| 196 | + def connection_made(self, transport): |
| 197 | +
|
| 198 | + self.transport = transport |
| 199 | +
|
| 200 | + def data_received(self, data): |
| 201 | +
|
| 202 | + (length, |
| 203 | + msg_id, |
| 204 | + comm_type, |
| 205 | + reply_type, |
| 206 | + seq_num) = struct.unpack("<Iiiii", data[:20]) |
| 207 | + assert msg_id == 11, "MSG_ID is not JOINT_TRAJ_PT" |
| 208 | + assert comm_type == 2, "COMM_TYPE is not REQUEST" |
| 209 | + assert reply_type == 0, "REPLY_TYPE is bogus" |
| 210 | + assert len(data) == (length + 4), "LENGTH is bogus" |
| 211 | + unpacked = struct.unpack("<ffffffffffff", data[20:]) |
| 212 | + response = struct.pack("<Iiiiiffffffffffff", 64, 11, 3, 1, 0, |
| 213 | + 0.0, 0.0, 0.0, 0.0, 0.0, |
| 214 | + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) |
| 215 | + self.transport.write(response) |
| 216 | + print("SEQ_NUM: %s %s" % (seq_num, unpacked)) |
| 217 | + if seq_num > 0: |
| 218 | + self.loop.create_task(self.robot.move_servos(unpacked[0], |
| 219 | + unpacked[1], |
| 220 | + unpacked[3], |
| 221 | + unpacked[4], |
| 222 | + unpacked[11])) |
| 223 | +
|
| 224 | + class JointStateStreamProtocol(asyncio.Protocol): |
| 225 | + """Implements joint states streaming protocol.""" |
| 226 | +
|
| 227 | + def __init__(self, loop, robot): |
| 228 | + self.loop = loop |
| 229 | + self.robot = robot |
| 230 | +
|
| 231 | + def connection_made(self, transport): |
| 232 | + print("A client connected to Joint State Stream") |
| 233 | + self.robot.stream_joint_state(transport) |
| 234 | +
|
| 235 | + def main(): |
| 236 | + """Entry point.""" |
| 237 | +
|
| 238 | + loop = asyncio.get_event_loop() |
| 239 | + robotarm = RobotArm(loop) |
| 240 | +
|
| 241 | + proto_factory = lambda: JointPositionStreamProtocol(loop, robotarm) |
| 242 | + joint_pos_coro = loop.create_server(proto_factory, '0.0.0.0', 11000) |
| 243 | +
|
| 244 | + proto_factory = lambda: JointStateStreamProtocol(loop, robotarm) |
| 245 | + joint_state_coro = loop.create_server(proto_factory, '0.0.0.0', 11002) |
| 246 | +
|
| 247 | + joint_pos_server = loop.run_until_complete(joint_pos_coro) |
| 248 | + joint_state_server = loop.run_until_complete(joint_state_coro) |
| 249 | +
|
| 250 | + # Serve requests until Ctrl+C is pressed |
| 251 | + print('Serving on {}'.format(joint_pos_server.sockets[0].getsockname())) |
| 252 | + print('Serving on {}'.format(joint_state_server.sockets[0].getsockname())) |
| 253 | + try: |
| 254 | + loop.run_forever() |
| 255 | + except KeyboardInterrupt: |
| 256 | + pass |
| 257 | +
|
| 258 | + # Close the server |
| 259 | + joint_pos_server.close() |
| 260 | + joint_state_server.close() |
| 261 | + loop.run_until_complete(asyncio.gather(joint_pos_server.wait_closed(), |
| 262 | + joint_state_server.wait_closed())) |
| 263 | + robotarm.deinit() |
| 264 | + loop.close() |
| 265 | +
|
| 266 | + if __name__ == "__main__": |
| 267 | + main() |
| 268 | +
|
| 269 | +5. Set up ROS environment on the device:: |
| 270 | + |
| 271 | + export ROS_ROOT=/opt/ros |
| 272 | + export ROS_DISTRO=indigo |
| 273 | + export ROS_PACKAGE_PATH=/opt/ros/indigo/share |
| 274 | + export PATH=$PATH:/opt/ros/indigo/bin |
| 275 | + export LD_LIBRARY_PATH=/opt/ros/indigo/lib |
| 276 | + export PYTHONPATH=/opt/ros/indigo/lib/python3.5/site-packages |
| 277 | + export ROS_MASTER_URI=http://localhost:11311 |
| 278 | + export CMAKE_PREFIX_PATH=/opt/ros/indigo |
| 279 | + touch /opt/ros/indigo/.catkin |
| 280 | + |
| 281 | +6. Launch all the needed ROS nodes with the command:: |
| 282 | + |
| 283 | + roslaunch uarmmetal_support refkit-uarm.launch robot_ip:=127.0.0.1 sim:=false |
| 284 | + |
| 285 | +Now your mechanical arm can be controlled through MoveIt's Move Group Interface. |
| 286 | +It's possible to control the arm interactively with a GUI ROS node installed |
| 287 | +on your desktop (i.e. `RViz`_). For that copy the source code of the packages |
| 288 | +`uarmmetal-support` and `uarmmetal-moveit-configs` to your `catkin workspace`_ |
| 289 | +on the desktop as `uarmmetal_support` and `uarmmetal_moveit_configs` ROS packages |
| 290 | +respectively. First initialize ROS environment:: |
| 291 | + |
| 292 | + source <path_to_your_catkin_workspace>/devel/setup.bash |
| 293 | + |
| 294 | +Then run:: |
| 295 | + |
| 296 | + ROS_MASTER_URI=http://<your_device_address>:11311 roslaunch uarmmetal_moveit_config moveit_rviz.launch config:=true |
| 297 | + |
| 298 | +.. _RViz: http://wiki.ros.org/rviz/UserGuide |
| 299 | +.. _catkin workspace: http://wiki.ros.org/catkin/workspaces |
0 commit comments