| … | … | @@ -0,0 +1,301 @@ |
| 1 | import bpy, socket, math, ast, threading, queue |
| 2 | from enum import Enum |
| 3 | |
| 4 | ARM_NAME_PREFIX = 'arm.' |
| 5 | LIGHT_NAME_PREFIX = 'light' |
| 6 | #QUAD_NAME_PREFIX = 'quad.' |
| 7 | BGR_PREFIX = 'bgr-' |
| 8 | LIGHT_PWM_MAX = 65534 |
| 9 | LIGHT_PWM_RED_MAX = 20000 |
| 10 | LIGHT_LUM_MAX = 1.0 |
| 11 | |
| 12 | ConnState = Enum('ConnState', 'none connecting connected') |
| 13 | class Connection: |
| 14 | def __init__(self, socket, state=ConnState.none): |
| 15 | self.socket = socket |
| 16 | self.state = state |
| 17 | |
| 18 | # map from IP address to Connection |
| 19 | sockets = {} |
| 20 | quad_last_command_dict = {} |
| 21 | # ip addresses to be connected to and added to sockets dict |
| 22 | sockets_queue = queue.Queue() |
| 23 | |
| 24 | # connect to address and store socket in persistent global map |
| 25 | def connect_socket(ip, port = 1337): |
| 26 | server_address = (ip, port) |
| 27 | sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) |
| 28 | sock.settimeout(1.0) |
| 29 | |
| 30 | print('connecting to', server_address) |
| 31 | sockets[ip].state = ConnState.connecting |
| 32 | try: |
| 33 | sock.connect(server_address) |
| 34 | print('connected to', server_address) |
| 35 | sock.sendall(b'speed 200gZ78\n') |
| 36 | sockets[ip].socket = sock |
| 37 | sockets[ip].state = ConnState.connected |
| 38 | return sock |
| 39 | |
| 40 | except socket.timeout: |
| 41 | print('error: connection timed out on', ip) |
| 42 | |
| 43 | class SocketThread(threading.Thread): |
| 44 | def __init__(self, name): |
| 45 | self.shouldExit = False |
| 46 | threading.Thread.__init__(self, name=name, daemon=True) |
| 47 | self.start() |
| 48 | |
| 49 | def run(self): |
| 50 | while True: |
| 51 | if self.shouldExit: return |
| 52 | |
| 53 | ip = sockets_queue.get() |
| 54 | if sockets[ip].state != ConnState.connected: |
| 55 | connect_socket(ip) |
| 56 | else: |
| 57 | print('skipping', ip) |
| 58 | sockets_queue.task_done() |
| 59 | |
| 60 | socket_thread = SocketThread('st') |
| 61 | |
| 62 | # find the LAMP attached to this arm |
| 63 | def getLamp(arm): |
| 64 | for child in arm.children: |
| 65 | if child.type == 'LAMP': |
| 66 | return child |
| 67 | |
| 68 | # returns robot arm LED intensity in integer units |
| 69 | def pwmFromLuminosity(lum, pwm_max=LIGHT_PWM_MAX): |
| 70 | pwm = int(round(abs(lum) / LIGHT_LUM_MAX * pwm_max)) |
| 71 | return pwm |
| 72 | |
| 73 | def luminosityFromPWM(pwm, pwm_max=LIGHT_PWM_MAX): |
| 74 | lum = pwm * LIGHT_LUM_MAX / pwm_max |
| 75 | return lum |
| 76 | |
| 77 | # returns a list of arm angles, starting from the base |
| 78 | # units are dynamixel units [200, 824] |
| 79 | def getAngles(arm): |
| 80 | |
| 81 | def dynamixel_from_degrees(angle): |
| 82 | return int(round((angle - 90) * 600.0/180 + 512)) |
| 83 | |
| 84 | # convert to degrees because they are easier to debug |
| 85 | rad_to_deg = 180.0/math.pi |
| 86 | up = arm.pose.bones['base'] |
| 87 | fo = arm.pose.bones['forearm'] |
| 88 | |
| 89 | up_q = up.matrix.to_quaternion() |
| 90 | fo_q = fo.matrix.to_quaternion() |
| 91 | fo_q.rotate(up_q.inverted()) |
| 92 | |
| 93 | # TODO get local orientation somehow instead of adding 180 and 90 |
| 94 | up_a = up.matrix.to_euler().x * rad_to_deg + 180 |
| 95 | fo_a = fo_q.to_euler().z * rad_to_deg + 90 |
| 96 | #up_a = 0 if up_a < 0 else up_a |
| 97 | #print(fo_q.to_euler(), fo_a) |
| 98 | |
| 99 | # convert to dynamixel units |
| 100 | angles = [dynamixel_from_degrees(a) for a in [up_a, fo_a]] |
| 101 | #print(angles) |
| 102 | return angles |
| 103 | |
| 104 | '''upperarm_angles = upperarm.matrix.to_euler() |
| 105 | forearm_angles = (upperarm.matrix.inverted() * forearm.matrix).to_euler() |
| 106 | print('upperarm angles:', upperarm_angles) |
| 107 | print('forearm angles: ', forearm_angles) |
| 108 | s |
| 109 | #angles[0] = arm.pose.bones[0].rotation_quaternion.to_euler()[0] |
| 110 | #angles[1] = arm.pose.bones[1].rotation_quaternion.to_euler()[1] |
| 111 | |
| 112 | angles = [x * 600/math.pi + 512 for x in angles] |
| 113 | ''' |
| 114 | |
| 115 | def robot_anim_handler(scene): |
| 116 | # TODO alter serial protocol to use negative IDs |
| 117 | #baseID = 1 |
| 118 | |
| 119 | # dict mapping ip to (dict mapping 1-based index to pwm string)s |
| 120 | quad_pwm_dicts = {} |
| 121 | |
| 122 | # find light arm objects |
| 123 | for o in bpy.data.objects: |
| 124 | if o.name.startswith(ARM_NAME_PREFIX): |
| 125 | ip = o.name[len(ARM_NAME_PREFIX):] |
| 126 | pwm = pwmFromLuminosity(getLamp(o).data.energy) |
| 127 | angles = getAngles(o) |
| 128 | |
| 129 | # assemble command string |
| 130 | s = 's' |
| 131 | for i in range(len(angles)): |
| 132 | s += ' ' + str(i+baseID) + ':' + str(angles[i]) |
| 133 | |
| 134 | s += '\npwm ' + str(pwm) + '\n' |
| 135 | sendString(ip, s) |
| 136 | |
| 137 | elif o.name.startswith(LIGHT_NAME_PREFIX): |
| 138 | tokens = o.name.split('-') |
| 139 | index = int(tokens[1]) |
| 140 | ip = tokens[2] |
| 141 | |
| 142 | ss = str(pwmFromLuminosity(o.data.energy)) + ' ' |
| 143 | |
| 144 | |
| 145 | try: pwm_dict = quad_pwm_dicts[ip] |
| 146 | except: |
| 147 | pwm_dict = {} |
| 148 | quad_pwm_dicts[ip] = pwm_dict |
| 149 | pwm_dict[index] = ss |
| 150 | |
| 151 | elif o.name.startswith(BGR_PREFIX): |
| 152 | tokens = o.name.split('-') |
| 153 | index = int(tokens[1]) |
| 154 | ip = tokens[2] |
| 155 | |
| 156 | # these will be appended together |
| 157 | ss = str(pwmFromLuminosity(o.data.color[2])) + ' ' |
| 158 | ss += str(pwmFromLuminosity(o.data.color[1])) + ' ' |
| 159 | ss += str(pwmFromLuminosity(o.data.color[0], LIGHT_PWM_RED_MAX)) + ' ' |
| 160 | |
| 161 | try: pwm_dict = quad_pwm_dicts[ip] |
| 162 | except: |
| 163 | pwm_dict = {} |
| 164 | quad_pwm_dicts[ip] = pwm_dict |
| 165 | pwm_dict[index] = ss |
| 166 | |
| 167 | for ip,pwm_dict in quad_pwm_dicts.items(): |
| 168 | # assemble command string in order, starting with index 1 |
| 169 | s = 'pwm ' |
| 170 | for i in range(1, 5): |
| 171 | try: s += pwm_dict[i] |
| 172 | except: s += '0 0 0 ' # not found--use zeros for this light |
| 173 | s += '\n' |
| 174 | |
| 175 | # save the last command sent to an IP address |
| 176 | # sending commands every 30 times a second overwhelms the ESP32 |
| 177 | #global quad_last_command_dict |
| 178 | try: |
| 179 | if s in quad_last_command_dict[ip]: continue |
| 180 | except: pass |
| 181 | quad_last_command_dict[ip] = s |
| 182 | #print(s) |
| 183 | sendString(ip, s) |
| 184 | |
| 185 | def sendString(ip, s): |
| 186 | if not ip: return |
| 187 | #print(s.replace('\n', '_')) |
| 188 | print(ip + ": " + s) |
| 189 | # if this is the first time trying to talk to this device, create an entry |
| 190 | # and connect |
| 191 | conn = None |
| 192 | try: |
| 193 | conn = sockets[ip] |
| 194 | except KeyError: |
| 195 | print('not connected; queueing', ip) |
| 196 | sockets[ip] = Connection(None) |
| 197 | sockets_queue.put(ip) |
| 198 | |
| 199 | if conn and conn.state == ConnState.connected: |
| 200 | try: |
| 201 | #print(s) |
| 202 | conn.socket.sendall(bytes(s, 'UTF-8')) |
| 203 | except (socket.timeout, BrokenPipeError, ConnectionResetError) as e: |
| 204 | print('lost connection; queueing', ip) |
| 205 | conn.state = ConnState.none |
| 206 | sockets_queue.put(ip) |
| 207 | |
| 208 | def saveToFile(filepath): |
| 209 | with open(filepath, 'w') as f: |
| 210 | arms = {} |
| 211 | |
| 212 | for arm in bpy.data.objects: |
| 213 | if not arm.name.startswith(ARM_NAME_PREFIX): continue |
| 214 | |
| 215 | ip = arm.name[len(ARM_NAME_PREFIX):] |
| 216 | angles = getAngles(arm) |
| 217 | light = pwmFromLuminosity(getLamp(arm).data.energy) |
| 218 | |
| 219 | dict = {'angles': angles, 'light': light} |
| 220 | arms[ip] = dict |
| 221 | |
| 222 | text = "{'LightArms': \n " + str(arms) + "\n}\n" |
| 223 | f.write(text) |
| 224 | |
| 225 | def loadFromFile(filepath): |
| 226 | with open(filepath, 'r') as f: |
| 227 | text = f.read() |
| 228 | json = ast.literal_eval(text) |
| 229 | arms = json['LightArms'] |
| 230 | |
| 231 | for ip, data in arms.items(): |
| 232 | name = ARM_NAME_PREFIX + ip |
| 233 | arm = bpy.data.objects[name] |
| 234 | |
| 235 | pwm = data['light'] |
| 236 | getLamp(arm).data.energy = luminosityFromPWM(pwm) |
| 237 | |
| 238 | angles = data['angles'] |
| 239 | # TODO_set angles |
| 240 | |
| 241 | ################################################################################### |
| 242 | # Menu in toolprops region |
| 243 | class RobotPanel(bpy.types.Panel): |
| 244 | bl_label = "Robot Scene" |
| 245 | bl_space_type = "VIEW_3D" |
| 246 | bl_region_type = "TOOL_PROPS" |
| 247 | |
| 248 | def draw(self, context): |
| 249 | self.layout.operator("circus.saver", text='Save Robot Positions') |
| 250 | #self.layout.operator("circus.loader", text='Load Robot Positions') |
| 251 | |
| 252 | class CircusSaver(bpy.types.Operator): |
| 253 | """Export Robot Data""" |
| 254 | bl_idname = "circus.saver" |
| 255 | bl_label = "Save the robots" |
| 256 | filepath = bpy.props.StringProperty(subtype="FILE_PATH") |
| 257 | |
| 258 | @classmethod |
| 259 | def poll(cls, context): |
| 260 | return context.object is not None |
| 261 | |
| 262 | def execute(self, context): |
| 263 | saveToFile(self.filepath) |
| 264 | return {'FINISHED'} |
| 265 | |
| 266 | def invoke(self, context, event): |
| 267 | context.window_manager.fileselect_add(self) |
| 268 | return {'RUNNING_MODAL'} |
| 269 | |
| 270 | class CircusLoader(bpy.types.Operator): |
| 271 | """Export Robot Data""" |
| 272 | bl_idname = "circus.loader" |
| 273 | bl_label = "Load the robots" |
| 274 | filepath = bpy.props.StringProperty(subtype="FILE_PATH") |
| 275 | |
| 276 | @classmethod |
| 277 | def poll(cls, context): |
| 278 | return context.object is not None |
| 279 | |
| 280 | def execute(self, context): |
| 281 | loadFromFile(self.filepath) |
| 282 | return {'FINISHED'} |
| 283 | |
| 284 | def invoke(self, context, event): |
| 285 | context.window_manager.fileselect_add(self) |
| 286 | return {'RUNNING_MODAL'} |
| 287 | |
| 288 | def unregister(): |
| 289 | bpy.utils.unregister_class(RobotPanel) |
| 290 | |
| 291 | if __name__ == "__main__": |
| 292 | # re-register this handler |
| 293 | bpy.app.handlers.frame_change_post.clear() |
| 294 | bpy.app.handlers.frame_change_post.append(robot_anim_handler) |
| 295 | |
| 296 | # create menu |
| 297 | #bpy.utils.register_module(__name__) |
| 298 | bpy.utils.register_class(RobotPanel) |
| 299 | bpy.utils.register_class(CircusLoader) |
| 300 | bpy.utils.register_class(CircusSaver) |
| 301 | |