diff --git a/.gitignore b/.gitignore index 47ba013..838257f 100644 --- a/.gitignore +++ b/.gitignore @@ -135,3 +135,6 @@ dmypy.json *.pc *.tp .DS_Store + +# VSCODE +.vs/* \ No newline at end of file diff --git a/README.md b/README.md index 1b91b01..a068817 100644 --- a/README.md +++ b/README.md @@ -1,87 +1,3 @@ -# cpsFanucInterface +# cps-fanuc-api -## Acknowledgements -* The HTTP interface is adapted based on the [Ellis](https://github.com/afeldman/Ellis) repository by Anton Feldmann. -* We would also like to thank Bernd Klein from FANUC for his guidance and support in development. - -## ROBOGUIDE Setup -* Create New Project - Default options are okay except for the following steps: - * Step 2: Workcell Name -- Choose a name - * Step 6: Group 1 Robot Model -- Purchased robot is H753 LR Mate 200iD/7L (R-30iB+ Mate Controller) - * Step 8: Robot Options -- Add the following - * KAREL (R632) - * User Socket Msg (R648) - * Dyn Path Modifier (R739) - -## Setting the Teach Pendant to Wide View -When navigating the Teach Pendant (in either ROBOGUIDE or the real robot), it is helpful to set it to wide view mode. It can be done as follows. -* On the Teach Pendant, tap or click in the area highlighted in purple as shown. - ![alt text](readme-images/wideview.png) -* Click or tap on `[VIEW]` and select Wide - - -## Compiling & Installing KAREL programs - -Programs can be compiled and loaded onto virtual robot controllers in ROBOGUIDE as follows. Once compiled, they can be copied onto a flash drive and uploaded to real physical robots. - -### Compiling and Installing in Roboguide -* Open the target robot project -* Add the source files (.kl for KAREL code, .ls for Teach Pendant code) by right clicking Files -> Add in the cell browser -![alt text](readme-images/add.png) - * For the HTTP interface, add all the KAREL wrapper files in `karel/http` and the motion backend `MOTION.LS` from `karel/motion`. - ![alt text](readme-images/files.png) -* Right click Files -> Build -> All. There should be no errors. -* (Optional) Ensure that the programs are installed by looking for them in the Teach Pendant. Press SELECT to bring up the program menu. Ensure KAREL programs are desplayed by selecting `[TYPE]` -> ALL. -![alt text](readme-images/verify.png) -* The HTTP interface should now be working - -### In Real Robot -The compiled programs need to be uploaded to the real robots, which can be done as follows. Ensure the programs are built, as described in the `Compiling and Installing in Roboguide` section. -* Copy the compiled `.pc` files (for KAREL) and `.TP` file (For Teach Pendant) onto a FAT32 flash drive. The compiled files can be found next to the source files (in `karel/http` and `karel/motion`). -* Once the transfer is complete, attach the flash drive to the physical Teach Pendant. -* On the Teach Pendant, select MENU -> FILE -> FILE -* Select `[UTIL]` -> Set Device -> USB on TP -* Select LOAD -> (all files) -* Select all the copied `.pc` and `.TP` files to load then. -* (Optional) Ensure that the programs are installed by looking for them in the Teach Pendant. Press SELECT to bring up the program menu. Ensure KAREL programs are desplayed by selecting `[TYPE]` -> ALL. -![alt text](readme-images/verify.png) -* The HTTP interface should now be working on the physical robot - - -## HTTP Interface -Commands can be sent to the HTTP interface using HTTP requests sent from a browser or Python script specifying the IP address of the robot (127.0.0.1 works if the robot is running in ROBOGUIDE on the same machine and will be used in these examples) as follows. - -`127.0.0.1/KAREL/[command]` - -where `[command]` can be the following. -* `appreset` - Initiate a fault reset (For example, after an unreachable position was sent to the robot). This is equivalent to clicking the fault reset button in ROBOGUIDE or pressing shift + reset on the Teach Pendant. -* `appmove` - Update position register target, `appmotion` must be called before to start the motion backend, and the Teach Pendant must be switched off. For more details, see the readme of [Ellis](https://github.com/afeldman/Ellis). - * To send joint commands, use the `jja` option, as follows: - `http://127.0.0.1/KAREL/appmove?motion_t=jja&coord1=0.0&coord2=0.0&coord3=0.0&coord4=0.0&coord5=-90.0&coord6=0.0` - ![alt text](readme-images/jja.png) - * To send commands in world frame (absolute position offsets in mm from the robot frame origin), use the `lca` option, as follows. - `http://127.0.0.1/KAREL/appmove?motion_t=lca&coord1=730&coord2=-150.0&coord3=800&coord4=-90.0&coord5=-45.0&coord6=30.0` - ![alt text](readme-images/lca.png) - * It should be noted that currently, the wrapper will return a successful status even if the motion was unreachable. An unreachable position can be found via the error state returned by `appmonitor` or by looking at the teach Pendant. The robot should be reset (via 'appreset' or the teach Pendant) if the robot reaches a fault state. -* `appmotion` - Starts the motion backend -* `appmonitor` - Retrieves the robot position and error state in JSON format - * A normal state response looks like the following: `{ "joint": [ 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -9.00000E+01, 0.00000E+00 ], "position": [ 5.40000E+02, -1.50000E+02, 5.50000E+02, -1.80000E+02, 2.50448E-06, 0.00000E+00 ], "error": [ 0,"R E S E T " ] }` - * An error state response looks like the following: `{ "joint": [ 0.00000E+00, 0.00000E+00, 0.00000E+00, 0.00000E+00, -9.00000E+01, 0.00000E+00 ], "position": [ 5.40000E+02, -1.50000E+02, 5.50000E+02, -1.80000E+02, 2.50448E-06, 0.00000E+00 ], "error": [ 15018,"MOTN-018 Position not reachable (G:1) " ] }` -* `appabort` - Stops any running robot tasks, such as the motion backend. This is equivalent to pressing FCTN -> ABORT(ALL) on the Teach Pendant. - - -If you are prompted to login when invoking HTTP requests, it is required to set the HTTP authentication to unlocked, which can be done as follows. -* Open Teach Pendant -* Menu -> Setup -> Host Comm (Should be under window 2 number 9 if no additional options were chosen in Setup Step 8) -* Select HTTP -* Set Karel to UNLOCK - -## Setup DPM -* Follow the DPM and Host Comm sections under [Configuration guide](https://github.com/gavanderhoorn/fanuc_dpm_mouse_demo) from the FANUC DPM Mouse Demo -* Set U Frame - * Menu -> 6. Setup -> 4. Frames - * Choose [Other] -> 3. User Frames - * Select SETIND and select 1 - -## Requesting Commands via Socket Messaging -WIP \ No newline at end of file +WIP Repository for CPS API in Fanuc KAREL diff --git a/displacementloctest.py b/displacementloctest.py deleted file mode 100644 index 9e1cfcc..0000000 --- a/displacementloctest.py +++ /dev/null @@ -1,45 +0,0 @@ -import pygame -import driver.fanucfrontend as ffe - -MOUSE_LEFT = 1 -MOUSE_MIDDLE = 2 -MOUSE_RIGHT = 3 - - -def main(): - frontend = ffe.FANUCFrontEnd("http://10.162.12.191") - frontend.reset() - frontend.stop_motion() - frontend.start_motion() - status = frontend.get_status() - pos = status['position'] - pygame.display.set_mode((500, 500)) - dimension = 0 - while True: - for event in pygame.event.get(): - if event.type == pygame.QUIT: - frontend.stop_motion() - exit(0) - elif event.type == pygame.MOUSEBUTTONDOWN: - pygame.event.set_grab(True) - dimension = event.button - print("Pressed") - elif event.type == pygame.MOUSEBUTTONUP: - pygame.event.set_grab(False) - print("Released") - elif event.type == pygame.MOUSEMOTION: - if pygame.event.get_grab(): - if dimension == MOUSE_LEFT: # X, Y - pos[0] += event.rel[0] - pos[1] += event.rel[1] - elif dimension == MOUSE_MIDDLE: # Y, Z - pos[1] += event.rel[0] - pos[2] += event.rel[1] - elif dimension == MOUSE_RIGHT: # X, Z - pos[0] += event.rel[0] - pos[2] += event.rel[1] - frontend.send_motion_request(pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], "lca") - - -if __name__ == "__main__": - main() diff --git a/driver/__init__.py b/driver/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/driver/djangofrontend.py b/driver/djangofrontend.py deleted file mode 100644 index 3996ef5..0000000 --- a/driver/djangofrontend.py +++ /dev/null @@ -1,21 +0,0 @@ -import json -from driver.httpfrontend import HTTPFrontEnd - - -# This class handles all fetching from the Django upstream server -class DjangoFrontEnd(HTTPFrontEnd): - - def __init__(self, endpoint): - HTTPFrontEnd.__init__(self, endpoint + "/") - - def make_request(self): - initparams = {"format": "json"} - response = super().invoke_program("robot_position/", initparams) - command = json.loads(response.content) - return command - - def send_response(self, position_id, success): - initparams = {"successful": str(success).lower(), "position": position_id} - super().post("robot_response/", initparams) - - diff --git a/driver/fanucfrontend.py b/driver/fanucfrontend.py deleted file mode 100644 index 2fb367d..0000000 --- a/driver/fanucfrontend.py +++ /dev/null @@ -1,52 +0,0 @@ -from driver.httpfrontend import HTTPFrontEnd -import json - - -# This class handles all invocations to the FANUC robot -class FANUCFrontEnd(HTTPFrontEnd): - - def __init__(self, endpoint): - if endpoint[-1] == '/': - super().__init__(endpoint + "KAREL/") - else: - super().__init__(endpoint + "/KAREL/") - - def start_motion(self): - response = super().invoke_program("appmotion") - return self.__validateSuccess(response) - - def stop_motion(self): - response = super().invoke_program("appabort") - return self.__validateSuccess(response) - - def send_motion_request(self, pos1, pos2, pos3, pos4, pos5, pos6, type): - initparams = { - "motion_t": type, - "coord1": pos1, - "coord2": pos2, - "coord3": pos3, - "coord4": pos4, - "coord5": pos5, - "coord6": pos6 - } - response = super().invoke_program("appmove", initparams) - return self.__validateSuccess(response) - - def reset(self): - response = super().invoke_program("appreset") - return self.__validateSuccess(response) - - def get_status(self): - response = super().invoke_program("appmonitor") - return json.loads(response.text) - - def rotate(self): - initparams = {"task": "rotate"} - super().invoke_program("appstart", initparams) - - def __validateSuccess(self, response): - try: - response_json = json.loads(response.text) - return response_json["result"] == "success" - except: - return False \ No newline at end of file diff --git a/driver/httpfrontend.py b/driver/httpfrontend.py deleted file mode 100644 index 6e18da1..0000000 --- a/driver/httpfrontend.py +++ /dev/null @@ -1,30 +0,0 @@ -import requests -import logging -from urllib.parse import urlencode -from urllib.request import Request, urlopen - - -class HTTPFrontEnd: - __endpoint = None - - def __init__(self, endpoint): - if type(endpoint) is not str: - raise TypeError('endpoint needs to be a string') - self.__endpoint = endpoint - - def invoke_program(self, name, params=None): - if type(name) is not str: - raise TypeError('program name needs to be a string') - r = requests.get(url=self.__endpoint + name, params=params) - logging.debug(r.url) - return r - - def post(self, name, params=None): - if type(name) is not str: - raise TypeError('program name needs to be a string') - request = Request(self.__endpoint + name, urlencode(params).encode()) - logging.debug(request.full_url + " data: " + str(request.data)) - urlopen(request).read().decode() - - def get_endpoint(self): - return self.__endpoint \ No newline at end of file diff --git a/fdpm_mdemo.py b/fdpm_mdemo.py deleted file mode 100644 index 0f9a0bb..0000000 --- a/fdpm_mdemo.py +++ /dev/null @@ -1,104 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2018, G.A. vd. Hoorn -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -# -# NOTICE: THIS IS A DEMO, AND NOT MEANT FOR ANY KIND OF PRODUCTION USE. -# THIS PROGRAM LACKS ANY AND ALL ERROR CHECKING AND HANDLING. -# USAGE OTHER THAN WITH ROBOGUIDE IS COMPLETELY UNTESTED. -# THE AUTHOR DENIES ANY RESPONSIBILITY FOR DAMAGE DONE TO -# ROBOTS, CONTROLLERS, OTHER EQUIPMENT OR PEOPLE THAT IS THE -# RESULT OF IGNORING THE PREVIOUS WARNING. -# REFER ALSO TO THE APACHE 2.0 LICENSE INCLUDED WITH THIS SOFTWARE. -# - - -import pygame -import struct -import socket -import time - - -controller_ip = '127.0.0.1' -tcp_port = 11010 - -# makes motions a bit more intuitive if looking at robot 'from the side' -mirror_y = True - - -# nothing to change below this line - - -IDX_X=0 -IDX_Y=1 - -MOUSE_LEFT = 1 -MOUSE_RIGHT = 3 - -M_NONE = 0 -M_Z = 1 -M_XY = 2 - - -running = True -m_dir = M_NONE -mult_y = -1 if not mirror_y else 1 - - -# open socket to controller -s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) -s.connect((controller_ip, tcp_port)) - -print ("Connected, move mouse to control EEF.") -print ("Left mouse button : X-Y") -print ("Right mouse button: Z") - -screen = pygame.display.set_mode((320, 240)) - -while running: - for event in pygame.event.get(): - if event.type == pygame.QUIT: - m_dir = M_NONE - running = 0 - elif event.type == pygame.MOUSEBUTTONDOWN: - if event.button == MOUSE_LEFT: - m_dir = M_XY - pygame.event.set_grab(True) - print ("EEF: grabbed X-Y") - elif event.button == MOUSE_RIGHT: - m_dir = M_Z - pygame.event.set_grab(True) - print ("EEF: grabbed Z") - elif event.type == pygame.MOUSEBUTTONUP: - if event.button in [MOUSE_LEFT, MOUSE_RIGHT]: - m_dir = M_NONE - pygame.event.set_grab(False) - print ("EEF: released") - elif event.type == pygame.MOUSEMOTION: - if pygame.event.get_grab(): - if m_dir == M_XY: - data = struct.pack('<3f', event.rel[IDX_X], mult_y * event.rel[IDX_Y], 0.0) - s.send(data) - elif m_dir == M_Z: - data = struct.pack('<3f', 0.0, 0.0, event.rel[IDX_Y]) - s.send(data) - - time.sleep(0.01) - - screen.fill((0, 0, 0)) - pygame.display.flip() - -print ("Disconnecting ..") diff --git a/karel/console/print_utils.kl b/karel/console/print_utils.kl deleted file mode 100644 index ea680f5..0000000 --- a/karel/console/print_utils.kl +++ /dev/null @@ -1,36 +0,0 @@ - - ---PROGRAM util ---%NOLOCKGROUP ---%NOPAUSE= COMMAND + TPENABLE + ERROR -CONST - TP_WIDTH = 40 - ---BEGIN ---END util - -ROUTINE wrapPrint(wrapPrintText : STRING) -VAR - len : INTEGER - printIdx : INTEGER - subPrintStr : STRING[TP_WIDTH] -BEGIN - len = STR_LEN(wrapPrintText) - printIdx = 1 - WHILE printIdx <= len DO - subPrintStr = SUB_STR(wrapPrintText, printIdx, TP_WIDTH) - WRITE (subPrintStr, CR) - printIdx = printIdx + TP_WIDTH - ENDWHILE -END wrapPrint - ---ROUTINE intToString(int : INTEGER) : STRING FROM klutils - -ROUTINE checkError(operation : STRING; status : INTEGER) -BEGIN - IF status <> 0 THEN - WRITE ('ERROR: ' + operation,CR) - WRITE ('STATUS: ', status, CR) - status = 0 - ENDIF -END checkError diff --git a/karel/core/core_grip.kl b/karel/core/core_grip.kl new file mode 100644 index 0000000..fa443db --- /dev/null +++ b/karel/core/core_grip.kl @@ -0,0 +1,28 @@ +ROUTINE CORE_GRIP(width: REAL; force: REAL): STRING + +CONST + R_WIDTH = 81 + R_FORCE = 82 + +BEGIN + -- abort current gripper instruction + ABORT_TASK('gripper_control', TRUE, TRUE, STATUS) + -- DELAY(1) -- yield to be able to aqcuire motion group lock + SET_REAL_REG(R_WIDTH, width_real, STATUS) + SET_REAL_REG(R_FORCE, force_real, STATUS) + + IF (STATUS <> 0) THEN + RETURN ( '{"result":"failed", "reason":"Could not set gripper register "}') + + ENDIF + + RUN_TASK( 'gripper_control', 1, FALSE, TRUE, 1 OR 2 OR 4, STATUS ) + -- DELAY(1) + + IF (STATUS <> 0) THEN + RETURN ( '{"result":"failed", "reason":"Could not start gripper_control TP routine"}') + ELSE + RETURN ( '{"result":"success"}') + ENDIF + +END CORE_GRIP \ No newline at end of file diff --git a/karel/core/core_gripper_info.kl b/karel/core/core_gripper_info.kl new file mode 100644 index 0000000..511da41 --- /dev/null +++ b/karel/core/core_gripper_info.kl @@ -0,0 +1,64 @@ +ROUTINE CORE_GRIPPER_INFO(reponse_file: FILE) + +CONST + R_ABSDEPTH = 71 + R_RELDEPTH = 72 + R_WIDTH = 73 + R_ISBUSY = 74 + R_ISGRIP = 75 + R_ISSAFETY = 76 + R_ISCONNECT = 77 + +VAR + temp_real: REAL + temp_int: INTEGER + + abs_depth: REAL + rel_depth: REAL + width: REAL + + is_busy: INTEGER + is_grip: INTEGER + is_safety: INTEGER + is_connect: INTEGER + +BEGIN + + -- abort current TP process + ABORT_TASK('gripper_info', TRUE, TRUE, STATUS) + RUN_TASK( 'gripper_info', 1, FALSE, TRUE, 1 OR 2 OR 4, STATUS ) + DELAY(1) --yield to let TP fill registers + + IF (STATUS <> 0) THEN + WRITE response_file( '{"result":"failed", "reason":"Could not start gripper_info TP routine"}') + RETURN + ENDIF + + -- ignore status here for performance reasons + GET_REG(R_ABSDEPTH, TRUE, temp_int, temp_real, STATUS) + abs_depth = temp_real + GET_REG(R_RELDEPTH, TRUE, temp_int, temp_real, STATUS) + rel_depth = temp_real + GET_REG(R_WIDTH, TRUE, temp_int, temp_real, STATUS) + width= temp_real + + GET_REG(R_ISBUSY, FALSE, temp_int, temp_real, STATUS) + is_busy = temp_int + GET_REG(R_ISGRIP, FALSE, temp_int, temp_real, STATUS) + is_grip = temp_int + GET_REG(R_ISSAFETY, FALSE, temp_int, temp_real, STATUS) + is_safety = temp_int + GET_REG(R_ISCONNECT, FALSE, temp_int, temp_real, STATUS) + is_connect = temp_int + + WRITE response_file( '{"result":"success"', + ', "abs_depth":', abs_depth, + ', "rel_depth":', rel_depth, + ', "width":', width, + ', "is_busy":', is_busy, + ', "is_grip":', is_grip, + ', "is_safety":', is_safety, + ', "is_connect":', is_connect, + '}') + +END CORE_GRIPPER_INFO \ No newline at end of file diff --git a/karel/core/core_motion_abort.kl b/karel/core/core_motion_abort.kl new file mode 100644 index 0000000..a877d87 --- /dev/null +++ b/karel/core/core_motion_abort.kl @@ -0,0 +1,15 @@ +ROUTINE CORE_MOTION_ABORT: INTEGER + +VAR + + STATUS: INTEGER + +BEGIN + + -- terminate motion TP task + ABORT_TASK('hs_motion_backend', TRUE, TRUE, STATUS) + ABORT_TASK('motion_backend', TRUE, TRUE, STATUS) + + RETURN (STATUS) + +END CORE_MOTION_ABORT \ No newline at end of file diff --git a/karel/core/core_motion_start.kl b/karel/core/core_motion_start.kl new file mode 100644 index 0000000..440d4ae --- /dev/null +++ b/karel/core/core_motion_start.kl @@ -0,0 +1,22 @@ +ROUTINE CORE_MOTION_START(hs_flag: BOOLEAN): INTEGER + +VAR + + STATUS: INTEGER + +BEGIN + -- terminate motion TP task + ABORT_TASK('motion_backend', TRUE, TRUE, STATUS) + ABORT_TASK('hs_motion_backend', TRUE, TRUE, STATUS) + DELAY(1000) -- delay to be able to restart TP routine without fault + + -- start new motion TP task + IF(hs_flag) THEN + RUN_TASK( 'hs_motion_backend', 1, FALSE, TRUE, 1 OR 2 OR 4, STATUS ) + ELSE + RUN_TASK( 'motion_backend', 1, FALSE, TRUE, 1 OR 2 OR 4, STATUS ) + ENDIF + + RETURN (STATUS) + +END CORE_MOTION_START \ No newline at end of file diff --git a/karel/core/core_move.kl b/karel/core/core_move.kl new file mode 100644 index 0000000..b411b46 --- /dev/null +++ b/karel/core/core_move.kl @@ -0,0 +1,75 @@ +ROUTINE UPDATE_MOTION_BACKEND(target_pos: XYZWPR; +linear_path: BOOLEAN; +interrupt_current_motion: BOOLEAN) + +CONST + PR_XYZWPR = 41 + PR_XYZWPR_SEC = 42 + R_LIN = 6 + R_SKIP = 5 + R_NEW = 7 + +BEGIN + +IF(interrupt_current_motion) THEN + SET_INT_REG(R_SKIP, 1, STATUS) + ELSE + SET_INT_REG(R_SKIP, 0, STATUS) + ENDIF + + IF(linear_path) THEN + SET_INT_REG(R_LIN, 1, STATUS) + ELSE + SET_INT_REG(R_LIN, 0, STATUS) + ENDIF + + SET_POS_REG(PR_XYZWPR, target_pos, STATUS) + SET_INT_REG(R_NEW, 1, STATUS) + +END UPDATE_MOTION_BACKEND + +ROUTINE CORE_MOVE(coords_6d: ARRAY[*] OF REAL; +linear_path: BOOLEAN; +interrupt_current_motion: BOOLEAN; +response_file: FILE) + +VAR + target_pos: XYZWPR + current_pos: XYZWPR + temp_int: INTEGER + temp_real: REAL + +BEGIN + $GROUP[1].$UTOOL = $MNUTOOL[1,1] --uTool 1 + $GROUP[1].$UFRAME = $MNUFRAME[1,1] --uFrame 1 + current_pos = CURPOS(0,0); + + -- start from old position instance + target_pos = current_pos + -- overwrite specific value + target_pos.x = coords_6d[1] + target_pos.y = coords_6d[2] + target_pos.z = coords_6d[3] + target_pos.w = coords_6d[4] + target_pos.p = coords_6d[5] + target_pos.r = coords_6d[6] + + UPDATE_MOTION_BACKEND(target_pos, linear_path, interrupt_current_motion) + + IF (STATUS <> 0) THEN + WRITE response_file ( '{"result":"failed", "reason":"Could not update target position register"}') + ELSE + GET_REG(8, FALSE, temp_int, temp_real, STATUS) + WRITE response_file ( '{"result":"success"', + ', "x":', current_pos.x, + ', "y":', current_pos.y, + ', "z":', current_pos.z, + ', "w":', current_pos.w, + ', "p":', current_pos.p, + ', "r":', current_pos.r, + ', "count":', temp_int, + '}') + ENDIF + +END CORE_MOVE + diff --git a/karel/core/core_position.kl b/karel/core/core_position.kl new file mode 100644 index 0000000..7a70735 --- /dev/null +++ b/karel/core/core_position.kl @@ -0,0 +1,20 @@ +ROUTINE CORE_POSITION(response_file: FILE) + +VAR + current_pos: XYZWPR + +BEGIN + $GROUP[1].$UTOOL = $MNUTOOL[1,1] --uTool 1 + $GROUP[1].$UFRAME = $MNUFRAME[1,1] --uFrame 1 + current_pos = CURPOS(0,0) + + WRITE response_file ( '{"result":"success"', + ', "x":', current_pos.x, + ', "y":', current_pos.y, + ', "z":', current_pos.z, + ', "w":', current_pos.w, + ', "p":', current_pos.p, + ', "r":', current_pos.r, + '}') + +END CORE_POSITION \ No newline at end of file diff --git a/karel/dpm/fdpm_mdemo.kl b/karel/dpm/fdpm_mdemo.kl deleted file mode 100644 index a1c59dd..0000000 --- a/karel/dpm/fdpm_mdemo.kl +++ /dev/null @@ -1,348 +0,0 @@ - --- Copyright (c) 2018, G.A. vd. Hoorn --- --- Licensed under the Apache License, Version 2.0 (the "License"); --- you may not use this file except in compliance with the License. --- You may obtain a copy of the License at --- --- http://www.apache.org/licenses/LICENSE-2.0 --- --- Unless required by applicable law or agreed to in writing, software --- distributed under the License is distributed on an "AS IS" BASIS, --- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. --- See the License for the specific language governing permissions and --- limitations under the License. - - -PROGRAM fdpm_mdemo --------------------------------------------------------------------------------- --- --- A simple Dynamic Path Modification demo using Model DPM to control a Fanuc --- with a mouse in 3D. --- --- author: G.A. vd. Hoorn --- --------------------------------------------------------------------------------- -%ALPHABETIZE -%COMMENT = 'FDPMMD//r0b' -%NOLOCKGROUP -%NOPAUSE = COMMAND + TPENABLE + ERROR - - - - --------------------------------------------------------------------------------- --- --- remote types & constants --- --------------------------------------------------------------------------------- -%INCLUDE include\libssock.th - - - - --------------------------------------------------------------------------------- --- --- local types & constants --- --------------------------------------------------------------------------------- -TYPE - dpmm_cfg_t = STRUCTURE - f_tp_ready : INTEGER - f_krl_ready : INTEGER - s_tcp_port : INTEGER -- TCP port to listen on - s_tag_nr : INTEGER -- server TAG number to use - um_clear : BOOLEAN -- clear user menu on start - ENDSTRUCTURE - - - dpmm_t = STRUCTURE - sock : ssock_t -- server socket instance - shutdwn_req : BOOLEAN -- program abort requested status - ENDSTRUCTURE - - - - -CONST - LOG_PFIX = 'FDMD ' - - COND_AH = 4242 -- ABORT handler id (randomly chosen, but less - -- likely to clash than simply '1') - - CFG_OK = 0 -- config ok - CFG_NOTDONE = -1 -- configuration not checked: user action required - - FILE_ILL_PRM = 2032 -- FILE-032 Illegal parameter - HOST_CTAG_ER = 67144 -- HOST-144 Comm Tag error - SEV_ABORT = 2 -- ABORT severity - - RACCEPT_DLAY = 1000 -- 'return to sock_accept' delay - - - - --------------------------------------------------------------------------------- --- --- local variables --- --------------------------------------------------------------------------------- -VAR - cfg_ IN SHADOW : dpmm_cfg_t -- configuration - this_ : dpmm_t -- prog instance - - sock_fd_ : FILE -- file descriptor associated with srvr socket - stat_ : INTEGER -- status variable - - - - --------------------------------------------------------------------------------- --- --- remote routine prototypes --- --------------------------------------------------------------------------------- -%INCLUDE include\libssock.h -%INCLUDE include\libind_log.h - - - - --------------------------------------------------------------------------------- --- --- local routine prototypes --- --------------------------------------------------------------------------------- -ROUTINE hndl_client_(this : dpmm_t; fd : FILE) : INTEGER FROM fdpm_mdemo - -ROUTINE check_cfg_(cfg : dpmm_cfg_t) : INTEGER FROM fdpm_mdemo - -ROUTINE set_offs_(dx : REAL; dy : REAL; dz : REAL) : INTEGER FROM fdpm_mdemo - -ROUTINE is_dpm_mpty_ : BOOLEAN FROM fdpm_mdemo - - - - --------------------------------------------------------------------------------- --- --- Main program --- --------------------------------------------------------------------------------- -BEGIN - -- check config - stat_ = check_cfg_(cfg_) - IF (stat_ <> CFG_OK) THEN - log_error_a(LOG_PFIX + 'cfg error:', stat_) - log_error(LOG_PFIX + 'check cfg') - -- errors with config always force user to log window - log_force - POST_ERR(FILE_ILL_PRM, '', 0, SEV_ABORT) - RETURN - ENDIF - - - -- TODO: should check DPM config is ok - - - stat_ = 0 - this_.shutdwn_req = FALSE - - - -- enable log output - IF (cfg_.um_clear) THEN log_clear; ENDIF - - - -- init server socket - stat_ = ssock_ctor(this_.sock, cfg_.s_tcp_port, cfg_.s_tag_nr) - IF (stat_ <> 0) THEN - IF (stat_ = TAG_CONF_ERR) THEN - log_error_a(LOG_PFIX + 'cfg err, TAG idx:', cfg_.s_tag_nr) - ELSE - log_error_a(LOG_PFIX + 'ssock_ctor err:', stat_) - ENDIF - -- nothing we can do, abort - POST_ERR(HOST_CTAG_ER, '', 0, SEV_ABORT) - ENDIF - - - -- make sure socket is closed (don't care about result) - stat_ = ssock_dconnf(this_.sock) - - - log_info(LOG_PFIX + 'init done') - - -- handshake with TP prog: - -- 1) wait for TP program to tell us that it's ready - log_info(LOG_PFIX + 'wait TP') - WAIT FOR (FLG[cfg_.f_tp_ready] = ON) - - - -- TODO: make sure and wait for DPM to be ready - - - -- 2) tell TP prog we are ready - log_info(LOG_PFIX + 'TP is ready') - FLG[cfg_.f_krl_ready] = ON - - - -- handle clients - WHILE (NOT this_.shutdwn_req) DO - -- inform user - log_info(LOG_PFIX + 'wait client') - - -- we need binary mode (UnFormatted) - SET_FILE_ATR(sock_fd_, ATR_UF) - - -- wait for connection - CLR_IO_STAT(sock_fd_) - stat_ = ssock_accpt2(this_.sock, sock_fd_) - IF (stat_ <> 0) THEN - -- can't continue on any error - log_error_a(LOG_PFIX + 'sock_accept err:', stat_) - this_.shutdwn_req = TRUE - GOTO lbl_m_break1 - ENDIF - - -- inform user - log_info(LOG_PFIX + 'connected') - - -- client connected, 'spin-off' connection handler - stat_ = hndl_client_(this_, sock_fd_) - - -- TODO: handle client errors properly - - lbl_m_break1:: - stat_ = ssock_dconn2(this_.sock, sock_fd_) - - lbl_m_cont0:: - log_info(LOG_PFIX + 'disconnected') - - -- always delay returning to accept() a bit, to make sure user can - -- escape runaway loops - DELAY (RACCEPT_DLAY) - - -- /WHILE (NOT this.shutdwn_req) - ENDWHILE - - -- make sure socket is closed (don t care about result) - stat_ = ssock_dconnf(this_.sock) - log_info(LOG_PFIX + 'exit') -END fdpm_mdemo - - - - -ROUTINE hndl_client_ -VAR - stat__ : INTEGER - dx__ : REAL - dy__ : REAL - dz__ : REAL -BEGIN - stat__ = 0 - dx__ = 0.0 - dy__ = 0.0 - dz__ = 0.0 - - log_info(LOG_PFIX + 'ready for mouse input') - - -- service connected client - WHILE (NOT this.shutdwn_req) DO - - -- wait for new offsets (essentially 'forever' as READ blocks) - READ fd(dx__, dy__, dz__) - stat__ = IO_STATUS(fd) - IF (stat__ <> 0) THEN - log_error_a(LOG_PFIX + 'sock err:', stat__) - GOTO lbl_hc_break - ENDIF - - IF (NOT is_dpm_mpty_) THEN - -- DPM hasn't copied previous offsets yet, try again next iteration - -- with new offsets (net effect: ignore the currently received - -- offsets, but since everything is relative, this shouldn't - -- matter that much for user experience) - GOTO lbl_hc_cont1 - ENDIF - - -- push offsets into sysvars - stat__ = set_offs_(dx__, dy__, dz__) - IF (stat__ <> 0) THEN POST_ERR(-stat__, '', 0, SEV_ABORT); ENDIF - - lbl_hc_cont1:: - -- just yield - DELAY (1) - - -- /WHILE (NOT this.shutdwn_req) - ENDWHILE - -lbl_hc_break:: - log_info(LOG_PFIX + 'hndl_client exit') - - RETURN (-ABS(stat__)) -END hndl_client_ - - - - -ROUTINE check_cfg_ -BEGIN - - cfg.f_tp_ready = 1 - cfg.f_krl_ready = 2 - cfg.s_tcp_port = 11010 - cfg.s_tag_nr = 3 - cfg.um_clear = TRUE - - -- check for any uninitialised entries - IF (UNINIT(cfg.f_tp_ready )) THEN RETURN (CFG_NOTDONE); ENDIF - IF (UNINIT(cfg.f_krl_ready)) THEN RETURN (CFG_NOTDONE); ENDIF - IF (UNINIT(cfg.s_tcp_port )) THEN RETURN (CFG_NOTDONE); ENDIF - IF (UNINIT(cfg.s_tag_nr )) THEN RETURN (CFG_NOTDONE); ENDIF - IF (UNINIT(cfg.um_clear )) THEN RETURN (CFG_NOTDONE); ENDIF - - -- all ok - RETURN (CFG_OK) -END check_cfg_ - - - - -ROUTINE set_offs_ -VAR - stat__ : INTEGER -BEGIN - stat__ = 0 - - SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[1].$INI_OFS', dx, stat__) - IF (stat__ <> 0) THEN GOTO lbl_so_exit; ENDIF - SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[2].$INI_OFS', dy, stat__) - IF (stat__ <> 0) THEN GOTO lbl_so_exit; ENDIF - SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[3].$INI_OFS', dz, stat__) - IF (stat__ <> 0) THEN GOTO lbl_so_exit; ENDIF - -lbl_so_exit:: - RETURN (-ABS(stat__)) -END set_offs_ - - - - -ROUTINE is_dpm_mpty_ -VAR - stat__ : INTEGER - temp0__ : REAL - temp1__ : REAL - temp2__ : REAL -BEGIN - stat__ = 0 - GET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[1].$INI_OFS', temp0__, stat__) - IF (stat__ <> 0) THEN POST_ERR(stat__, '', 0, SEV_ABORT); ENDIF - GET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[2].$INI_OFS', temp1__, stat__) - IF (stat__ <> 0) THEN POST_ERR(stat__, '', 0, SEV_ABORT); ENDIF - GET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[3].$INI_OFS', temp2__, stat__) - IF (stat__ <> 0) THEN POST_ERR(stat__, '', 0, SEV_ABORT); ENDIF - - RETURN ((temp0__ = 0.0) AND (temp1__ = 0.0) AND (temp2__ = 0.0)) -END is_dpm_mpty_ diff --git a/karel/dpm/include/libind_log.h.kl b/karel/dpm/include/libind_log.h.kl deleted file mode 100644 index 881332a..0000000 --- a/karel/dpm/include/libind_log.h.kl +++ /dev/null @@ -1,137 +0,0 @@ --- Software License Agreement (BSD License) --- --- Copyright (c) 2012-2015, TU Delft Robotics Institute --- All rights reserved. --- --- Redistribution and use in source and binary forms, with or without --- modification, are permitted provided that the following conditions --- are met: --- --- * Redistributions of source code must retain the above copyright --- notice, this list of conditions and the following disclaimer. --- * Redistributions in binary form must reproduce the above --- copyright notice, this list of conditions and the following --- disclaimer in the documentation and/or other materials provided --- with the distribution. --- * Neither the name of the TU Delft Robotics Institute nor the names --- of its contributors may be used to endorse or promote products --- derived from this software without specific prior written --- permission. --- --- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS --- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT --- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS --- FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE --- COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, --- INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, --- BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; --- LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER --- CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT --- LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN --- ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE --- POSSIBILITY OF SUCH DAMAGE. - - - --------------------------------------------------------------------------------- --- --- libind_log routine prototypes --- --- author: G.A. vd. Hoorn (TU Delft Robotics Institute) --- --------------------------------------------------------------------------------- - - - - --------------------------------------------------------------------------------- --- --- Clear and force the USER menu screen --- --------------------------------------------------------------------------------- -ROUTINE log_clear FROM libind_log - - - - --------------------------------------------------------------------------------- --- --- Force the USER menu screen --- --------------------------------------------------------------------------------- -ROUTINE log_force FROM libind_log - - - - --------------------------------------------------------------------------------- --- --- Write INFO level string to the logging fd --- --- [in ] msg : the msg to print --- --------------------------------------------------------------------------------- -ROUTINE log_info(msg : STRING) FROM libind_log - - - - --------------------------------------------------------------------------------- --- --- Write INFO level string and 1 integer parameter to the logging fd --- --- [in ] msg : the msg to print --- [in ] prm : the parameter to print --- --------------------------------------------------------------------------------- -ROUTINE log_info_a(msg : STRING; prm : INTEGER) FROM libind_log - - - - --------------------------------------------------------------------------------- --- --- Write WARN level string to the logging fd --- --- [in ] msg : the msg to print --- --------------------------------------------------------------------------------- -ROUTINE log_warn(msg : STRING) FROM libind_log - - - - --------------------------------------------------------------------------------- --- --- Write WARN level string and 1 integer parameter to the logging fd --- --- [in ] msg : the msg to print --- [in ] prm : the parameter to print --- --------------------------------------------------------------------------------- -ROUTINE log_warn_a(msg : STRING; prm : INTEGER) FROM libind_log - - - - --------------------------------------------------------------------------------- --- --- Write ERROR level string to the logging fd --- --- [in ] msg : the msg to print --- --------------------------------------------------------------------------------- -ROUTINE log_error(msg : STRING) FROM libind_log - - - - --------------------------------------------------------------------------------- --- --- Write ERROR level string and 1 integer parameter to the logging fd --- --- [in ] msg : the msg to print --- [in ] prm : the parameter to print --- --------------------------------------------------------------------------------- -ROUTINE log_error_a(msg : STRING; prm : INTEGER) FROM libind_log diff --git a/karel/dpm/include/libssock.h.kl b/karel/dpm/include/libssock.h.kl deleted file mode 100644 index b16b1d6..0000000 --- a/karel/dpm/include/libssock.h.kl +++ /dev/null @@ -1,147 +0,0 @@ --- Software License Agreement (BSD License) --- --- Copyright (c) 2012-2015, TU Delft Robotics Institute --- All rights reserved. --- --- Redistribution and use in source and binary forms, with or without --- modification, are permitted provided that the following conditions --- are met: --- --- * Redistributions of source code must retain the above copyright --- notice, this list of conditions and the following disclaimer. --- * Redistributions in binary form must reproduce the above --- copyright notice, this list of conditions and the following --- disclaimer in the documentation and/or other materials provided --- with the distribution. --- * Neither the name of the TU Delft Robotics Institute nor the names --- of its contributors may be used to endorse or promote products --- derived from this software without specific prior written --- permission. --- --- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS --- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT --- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS --- FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE --- COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, --- INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, --- BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; --- LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER --- CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT --- LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN --- ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE --- POSSIBILITY OF SUCH DAMAGE. - - - --------------------------------------------------------------------------------- --- --- libssock routine prototypes --- --- author: G.A. vd. Hoorn (TU Delft Robotics Institute) --- --------------------------------------------------------------------------------- - - - - --------------------------------------------------------------------------------- --- --- Ping a host on the network. No connected socket necessary. --- --- NB: hostname needs to be known to robot (setup DNS and / or 'Host Entry --- table') --- --------------------------------------------------------------------------------- -ROUTINE ssock_ping(host : STRING) : INTEGER FROM libssock - - - - --------------------------------------------------------------------------------- --- --- Constructor: sets up member variables and initialises related robot --- system variable. --- --------------------------------------------------------------------------------- -ROUTINE ssock_ctor(this : ssock_t; tcp_port : INTEGER; tag_idx : INTEGER) : INTEGER FROM libssock - - - - --------------------------------------------------------------------------------- --- --- Dump string rep of the given instance to to file descriptor 'fd'. --- --------------------------------------------------------------------------------- -ROUTINE ssock_dump(this : ssock_t; fd : FILE) FROM libssock - - - - --------------------------------------------------------------------------------- --- --- Wait for a connection on the given socket instance, if not already connected --- --------------------------------------------------------------------------------- -ROUTINE ssock_accept(this : ssock_t) : INTEGER FROM libssock - - - - --------------------------------------------------------------------------------- --- --- Disconnect the socket, if connected. --- --------------------------------------------------------------------------------- -ROUTINE ssock_dconn(this : ssock_t) : INTEGER FROM libssock - - - - --------------------------------------------------------------------------------- --- --- Open a file descriptor on the given socket instance --- --------------------------------------------------------------------------------- -ROUTINE ssock_ofd(this : ssock_t; fd : FILE; mode : STRING) : INTEGER FROM libssock - - - - --------------------------------------------------------------------------------- --- --- Close the given file descriptor (on the given socket instance) --- --------------------------------------------------------------------------------- -ROUTINE ssock_cfd(this : ssock_t; fd : FILE) : INTEGER FROM libssock - - - - --------------------------------------------------------------------------------- --- --- Disconnect the socket, unconditionally. --- --------------------------------------------------------------------------------- -ROUTINE ssock_dconnf(this : ssock_t) : INTEGER FROM libssock - - - - --------------------------------------------------------------------------------- --- --- Wait for a connection on the given socket instance. If successful, --- open given file descriptor on it. --- --------------------------------------------------------------------------------- -ROUTINE ssock_accpt2(this : ssock_t; fd : FILE) : INTEGER FROM libssock - - - - --------------------------------------------------------------------------------- --- --- Disconnect the socket, if connected. Then close given file descriptor. --- --------------------------------------------------------------------------------- -ROUTINE ssock_dconn2(this : ssock_t; fd : FILE) : INTEGER FROM libssock diff --git a/karel/dpm/include/libssock.th.kl b/karel/dpm/include/libssock.th.kl deleted file mode 100644 index 601bdfb..0000000 --- a/karel/dpm/include/libssock.th.kl +++ /dev/null @@ -1,69 +0,0 @@ --- Software License Agreement (BSD License) --- --- Copyright (c) 2012-2015, TU Delft Robotics Institute --- All rights reserved. --- --- Redistribution and use in source and binary forms, with or without --- modification, are permitted provided that the following conditions --- are met: --- --- * Redistributions of source code must retain the above copyright --- notice, this list of conditions and the following disclaimer. --- * Redistributions in binary form must reproduce the above --- copyright notice, this list of conditions and the following --- disclaimer in the documentation and/or other materials provided --- with the distribution. --- * Neither the name of the TU Delft Robotics Institute nor the names --- of its contributors may be used to endorse or promote products --- derived from this software without specific prior written --- permission. --- --- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS --- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT --- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS --- FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE --- COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, --- INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, --- BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; --- LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER --- CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT --- LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN --- ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE --- POSSIBILITY OF SUCH DAMAGE. - - - --------------------------------------------------------------------------------- --- --- libssock shared type / constant decls --- --- author: G.A. vd. Hoorn (TU Delft Robotics Institute) --- --------------------------------------------------------------------------------- - -type - ssock_t FROM libssock = STRUCTURE - connected_ : BOOLEAN -- connection status - status_ : INTEGER -- multi-use status var (output status for KAREL statements) - port_ : INTEGER -- TCP port number to listen on - tag_idx_ : INTEGER -- index into the 'Server Tags' list - tag_str_ : STRING[3] -- tag string (created with tag_idx_) - hcfg_str_ : STRING[30] -- complete host config system variable name - ENDSTRUCTURE - - - - -const - TAG_CONF_ERR = -1 - S_OPEN_ERR = -2 - S_FD_ERR = -3 - S_ACPT_ERR = -4 -- socket accept error: already connected, for instance - S_CONN_ERR = -5 -- socket connect error: something went wrong waiting for clients to connect - S_DISC_ERR = -6 -- socket disconnect error: not connected, for instance - S_NOTCON_ERR = -7 -- general 'not connected' error - - -- TAG status constants - TAG_UNDEFIND = 1 -- an 'undefined' TAG - TAG_DEFINED = 2 -- a 'defined' TAG - TAG_STARTED = 3 -- a 'started' TAG diff --git a/karel/dpm/libind_log.kl b/karel/dpm/libind_log.kl deleted file mode 100644 index 703371c..0000000 --- a/karel/dpm/libind_log.kl +++ /dev/null @@ -1,215 +0,0 @@ --- Software License Agreement (BSD License) --- --- Copyright (c) 2012-2015, TU Delft Robotics Institute --- All rights reserved. --- --- Redistribution and use in source and binary forms, with or without --- modification, are permitted provided that the following conditions --- are met: --- --- * Redistributions of source code must retain the above copyright --- notice, this list of conditions and the following disclaimer. --- * Redistributions in binary form must reproduce the above --- copyright notice, this list of conditions and the following --- disclaimer in the documentation and/or other materials provided --- with the distribution. --- * Neither the name of the TU Delft Robotics Institute nor the names --- of its contributors may be used to endorse or promote products --- derived from this software without specific prior written --- permission. --- --- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS --- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT --- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS --- FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE --- COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, --- INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, --- BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; --- LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER --- CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT --- LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN --- ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE --- POSSIBILITY OF SUCH DAMAGE. - - -PROGRAM libind_log --------------------------------------------------------------------------------- --- --- Simple logging routines --- --- TODO: --- - use mutex / semaphore in 'log_clear' with init bool --- --- author: G.A. vd. Hoorn (TU Delft Robotics Institute) --- --------------------------------------------------------------------------------- -%ALPHABETIZE -%COMMENT = 'ROS//r3' -%NOBUSYLAMP -%NOLOCKGROUP -%NOPAUSE = COMMAND + TPENABLE + ERROR - - - - --------------------------------------------------------------------------------- --- --- remote types & constants --- --------------------------------------------------------------------------------- -%INCLUDE klevccdf - - - - --------------------------------------------------------------------------------- --- --- local types & constants --- --------------------------------------------------------------------------------- -CONST - -- use klevccdf constants for colours from rosconsole.cpp - ROS_WARN = cc_red - ROS_ERR = cc_red - ROS_DEBUG = cc_green - - -- - ROS_ERR_T = ' E ' - ROS_WARN_T = ' W ' - ROS_INFO_T = ' I ' - ROS_DEBUG_T = ' D ' - - - - --------------------------------------------------------------------------------- --- --- local routine prototypes --- --------------------------------------------------------------------------------- -%INCLUDE include\libind_log.h -%INCLUDE klrdutil - - - - --------------------------------------------------------------------------------- --- --- Empty body --- --------------------------------------------------------------------------------- -BEGIN -END libind_log - - - - --------------------------------------------------------------------------------- --- --- Print the current time on the current line (as prefix for log lines) --- --------------------------------------------------------------------------------- -ROUTINE prnt_time_ -VAR - time_ : INTEGER - tmp_ : STRING[30] -BEGIN - -- - GET_TIME(time_) - -- keep only lower 16 bits: HH:MM:SS - time_ = time_ AND 65535 - -- convert it to a string (raw, not using CNV_TIME_STR) - CNV_INT_STR(time_, 4, 10, tmp_) - -- write on current line - WRITE TPDISPLAY(tmp_) -END prnt_time_ - - - - -ROUTINE log_clear -VAR - stat__ : INTEGER -BEGIN - -- from klrdutil - clear_win(TPDISPLAY, stat__) - log_force -END log_clear - - - - -ROUTINE log_force -BEGIN - FORCE_SPMENU(TP_PANEL, SPI_TPUSER, 1) -END log_force - - - - -ROUTINE log_info -BEGIN - prnt_time_ - WRITE TPDISPLAY(ROS_INFO_T, msg, CR) -END log_info - - - - -ROUTINE log_info_a -BEGIN - prnt_time_ - WRITE TPDISPLAY(ROS_INFO_T, msg, prm, CR) -END log_info_a - - - - -ROUTINE log_warn -BEGIN - prnt_time_ - WRITE TPDISPLAY( - CHR(cc_fgcolor), CHR(ROS_WARN), - ROS_WARN_T, msg, - CHR(cc_fgcolor), CHR(cc_dfcolor), - CR) -END log_warn - - - - -ROUTINE log_warn_a -BEGIN - prnt_time_ - WRITE TPDISPLAY( - CHR(cc_fgcolor), CHR(ROS_WARN), - ROS_WARN_T, msg, prm, - CHR(cc_fgcolor), CHR(cc_dfcolor), - CR) -END log_warn_a - - - - -ROUTINE log_error -BEGIN - prnt_time_ - WRITE TPDISPLAY( - CHR(cc_fgcolor), CHR(ROS_ERR), - ROS_ERR_T, msg, - CHR(cc_fgcolor), CHR(cc_dfcolor), - CR) -END log_error - - - - -ROUTINE log_error_a -BEGIN - prnt_time_ - WRITE TPDISPLAY( - CHR(cc_fgcolor), CHR(ROS_ERR), - ROS_ERR_T, msg, prm, - CHR(cc_fgcolor), CHR(cc_dfcolor), - CR) -END log_error_a diff --git a/karel/dpm/libssock.kl b/karel/dpm/libssock.kl deleted file mode 100644 index 5cd3a9d..0000000 --- a/karel/dpm/libssock.kl +++ /dev/null @@ -1,294 +0,0 @@ --- Software License Agreement (BSD License) --- --- Copyright (c) 2012-2015, TU Delft Robotics Institute --- All rights reserved. --- --- Redistribution and use in source and binary forms, with or without --- modification, are permitted provided that the following conditions --- are met: --- --- * Redistributions of source code must retain the above copyright --- notice, this list of conditions and the following disclaimer. --- * Redistributions in binary form must reproduce the above --- copyright notice, this list of conditions and the following --- disclaimer in the documentation and/or other materials provided --- with the distribution. --- * Neither the name of the TU Delft Robotics Institute nor the names --- of its contributors may be used to endorse or promote products --- derived from this software without specific prior written --- permission. --- --- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS --- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT --- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS --- FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE --- COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, --- INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, --- BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; --- LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER --- CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT --- LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN --- ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE --- POSSIBILITY OF SUCH DAMAGE. - - -PROGRAM libssock --------------------------------------------------------------------------------- --- --- Convenience wrapper around the KAREL Socket Programming extensions. Only --- works for Server TAGs right now. --- --- See also KAREL Reference Manual, MARRC75KR07091E Rev C, chap. 11 --- --- --- author: G.A. vd. Hoorn (TU Delft Robotics Institute) --- --------------------------------------------------------------------------------- -%ALPHABETIZE -%COMMENT = 'ROS//r1' -%NOBUSYLAMP -%NOLOCKGROUP -%NOPAUSE = COMMAND + TPENABLE + ERROR - - - - --------------------------------------------------------------------------------- --- --- local types & constants --- --------------------------------------------------------------------------------- -%INCLUDE include\libssock.th - - - - --------------------------------------------------------------------------------- --- --- local routine prototypes --- --------------------------------------------------------------------------------- -%INCLUDE include\libssock.h - - - - --------------------------------------------------------------------------------- --- --- Empty body --- --------------------------------------------------------------------------------- -BEGIN -END libssock - - - - -ROUTINE ssock_ping -VAR - stat__ : INTEGER -BEGIN - MSG_PING(host, stat__) - RETURN (-stat__) -END ssock_ping - - - - -ROUTINE ssock_ctor -VAR - tmp_str__ : STRING[1] -- temp string for tag_nr to str conversion - entry__ : INTEGER -- dummy var for set_var - tag_state__ : INTEGER -- TAG state -BEGIN - -- setup instance variables - this.connected_ = FALSE - this.status_ = 0 - this.port_ = tcp_port - this.tag_idx_ = tag_idx - this.tag_str_ = '' - this.hcfg_str_ = '' - - -- check - IF (this.tag_idx_ < 1) OR (this.tag_idx_ > 8) THEN - -- cannot open TAGs that don't exist - RETURN (S_OPEN_ERR) - ENDIF - - -- construct server_port variable string (ascii 'trick') - tmp_str__ = CHR(this.tag_idx_ + ORD('0', 1)) - - -- and set system variable - this.hcfg_str_ = '$HOSTS_CFG[' + tmp_str__ + ']' - - -- construct server tag string - this.tag_str_ = 'S' + tmp_str__ + ':' - - -- check state of tag, make sure it is STARTED - GET_VAR(entry__, '*SYSTEM*', this.hcfg_str_ + '.$STATE', tag_state__, this.status_) - IF (this.status_ <> 0) OR (tag_state__ <> TAG_STARTED) THEN - -- TAG config not correct, or couldn't read it - RETURN (TAG_CONF_ERR) - ENDIF - - -- set port of TAG - SET_VAR(entry__, '*SYSTEM*', this.hcfg_str_ + '.$SERVER_PORT', this.port_, this.status_) - IF this.status_ <> 0 THEN - -- couldn't update TAG config - RETURN (TAG_CONF_ERR) - ENDIF - - -- done - RETURN (-this.status_) -END ssock_ctor - - - - -ROUTINE ssock_dump -BEGIN - WRITE fd('Sock instance:', CR, & - ' connected :', this.connected_, CR, & - ' port :', this.port_, CR, & - ' tag_idx :', this.tag_idx_, CR, & - ' tag_str : ', this.tag_str_, CR, & - ' hcfg_str : ', this.hcfg_str_, CR) -END ssock_dump - - - - -ROUTINE ssock_accept -VAR - stat__ : INTEGER -BEGIN - stat__ = 0 - IF this.connected_ THEN - RETURN (S_ACPT_ERR) - ENDIF - - -- msg_connect only returns after a client connects to the port - MSG_CONNECT(this.tag_str_, stat__) - - IF stat__ = 0 THEN - this.connected_ = TRUE - ENDIF - - RETURN (-stat__) -END ssock_accept - - - - -ROUTINE ssock_dconn -BEGIN - IF this.connected_ = FALSE THEN - RETURN (S_NOTCON_ERR) - ENDIF - - -- use other routine - RETURN (ssock_dconnf(this)) -END ssock_dconn - - - - -ROUTINE ssock_dconnf -VAR - stat__ : INTEGER -BEGIN - stat__ = 0 - MSG_DISCO(this.tag_str_, stat__) - - this.connected_ = FALSE - - RETURN (-stat__) -END ssock_dconnf - - - - -ROUTINE ssock_ofd -VAR - tmp__ : STRING[3] - stat__ : INTEGER -BEGIN - IF this.connected_ = FALSE THEN - RETURN (S_NOTCON_ERR) - ENDIF - - -- TODO: why is this temporary variable necessary? - tmp__ = this.tag_str_ - OPEN FILE fd(mode, tmp__) - - stat__ = IO_STATUS(fd) - RETURN (-stat__) -END ssock_ofd - - - - -ROUTINE ssock_cfd -VAR - stat__ : INTEGER -BEGIN - IF this.connected_ = FALSE THEN - RETURN (S_NOTCON_ERR) - ENDIF - - -- close file desc - CLOSE FILE fd - - -- according to Karel Ref Man, A.4.12, this will always return 0 - stat__ = IO_STATUS(fd) - RETURN (-stat__) -END ssock_cfd - - - - -ROUTINE ssock_accpt2 -VAR - stat__ : INTEGER -BEGIN - stat__ = 0 - -- use other routine - stat__ = ssock_accept(this) - - -- make sure it succeeded - IF stat__ <> 0 THEN - -- stat already < 0 from ssock_accept - RETURN (stat__) - ENDIF - - -- now open fd, read / write - stat__ = ssock_ofd(this, fd, 'RW') - - -- done (stat already < 0 from ssock_ofd) - RETURN (stat__) -END ssock_accpt2 - - - - -ROUTINE ssock_dconn2 -VAR - stat__ : INTEGER -BEGIN - stat__ = 0 - IF this.connected_ = FALSE THEN - RETURN (S_NOTCON_ERR) - ENDIF - - -- close fd first - -- TODO: fail on failure to close fd? - stat__ = ssock_cfd(this, fd) - - -- now socket - MSG_DISCO(this.tag_str_, stat__) - - this.connected_ = FALSE - - -- done - RETURN (-stat__) -END ssock_dconn2 diff --git a/karel/dpm/statrackx.ls b/karel/dpm/statrackx.ls deleted file mode 100644 index d7b15e2..0000000 --- a/karel/dpm/statrackx.ls +++ /dev/null @@ -1,65 +0,0 @@ -/PROG STATRACKX -/ATTR -OWNER = MNEDITOR; -COMMENT = "FDPMMD//r0b"; -PROG_SIZE = 1050; -CREATE = DATE 18-01-21 TIME 12:00:00; -MODIFIED = DATE 18-01-21 TIME 12:00:00; -FILE_NAME = ; -VERSION = 0; -LINE_COUNT = 30; -MEMORY_SIZE = 1310; -PROTECT = READ_WRITE; -TCD: STACK_SIZE = 0, - TASK_PRIORITY = 50, - TIME_SLICE = 0, - BUSY_LAMP_OFF = 0, - ABORT_REQUEST = 0, - PAUSE_REQUEST = 0; -DEFAULT_GROUP = 1,*,*,*,*; -CONTROL_CODE = 00000000 00000000; -/MN - : !if your robot needs ; - : !a specific toolframe ; - : !selected, add it here ; - : ; - : !Init handshaking: ; - : ! TP not ready ; - : ! Karel not ready ; - : F[1]=(OFF) ; - : F[2]=(OFF) ; - : ; - : !Slowly go to initial ; - : !position ; - :J P[1] 10% FINE ; - : ; - : !Start karel program ; - : RUN FDPM_MDEMO ; - : ; - : !Handshake: ; - : ! TP is ready ; - : F[1]=(ON) ; - : ! Karel ready ? ; - : WAIT (F[2]=ON) ; - : ; - : !Start stat tracking ; - : Track DPM[1] ; - :L P[2] 100mm/sec FINE ; - : ; - : !Stop tracking ; - : !(will never reach this) ; - : Track End ; -/POS -P[1]{ - GP1: - UF : 1, UT : 1, - J1= 1.000 deg, J2= 1.000 deg, J3= 1.000 deg, - J4= 1.000 deg, J5= -90.000 deg, J6= 1.000 deg -}; -P[2]{ - GP1: - UF : 1, UT : 1, CONFIG : 'N U T, 0, 0, 0', - X = 470.000 mm, Y = 0.000 mm, Z = -20.000 mm, - W = 000.000 deg, P = 000.000 deg, R = 00.000 deg -}; -/END diff --git a/karel/fieldview_client.kl b/karel/fieldview_client.kl deleted file mode 100644 index 9af7445..0000000 --- a/karel/fieldview_client.kl +++ /dev/null @@ -1,68 +0,0 @@ -PROGRAM fv_client -%STACKSIZE = 4000 -%NOLOCKGROUP -%NOPAUSE=ERROR+COMMAND+TPENABLE -%ENVIRONMENT uif -%ENVIRONMENT sysdef -%ENVIRONMENT memo -%ENVIRONMENT kclop -%ENVIRONMENT bynam -%ENVIRONMENT fdev -%ENVIRONMENT flbt -%INCLUDE klevccdf -%INCLUDE klevkeys -%INCLUDE klevkmsk --------------------------------------------------------------------------------- -VAR - target_pos : POSITION - target_id : INTEGER - connection : FILE - status : INTEGER - entry : INTEGER - pick_now : BOOLEAN -- the system has 2 states: picking or handling - main_loop : BOOLEAN --------------------------------------------------------------------------------- -BEGIN - SET_FILE_ATR(connection, ATR_IA) --force reads to completion - -- SET_VAR(entry, ’*SYSTEM*’,’$HOSTC_CFG[2].$SERVER_PORT’,59002,status) -- not sure about this, variable is never used again - pick_now = TRUE - WHILE main_loop = TRUE DO - IF pick_now = TRUE THEN - -- TODO point camera at the working area - -- Connect the tag - WRITE(’Connecting..’,cr) - MSG_CONNECT(’C2:’,status) -- C2 is a client tag connected to the REST service on the AI controller - WRITE(’ Connect Status = ’,status,cr) - loop1 = TRUE - IF status = 0 THEN - WHILE pick_now = TRUE DO - WRITE(’Opening File..’,cr) - OPEN FILE connection(’ro’,’C2:’) - status = IO_STATUS(connection) - IF status = 0 THEN - -- TODO read JSON and parse picking positions - IF target_id = 0 THEN - -- pick first object id and set it in target_id - ENDIF - -- extract picking position for object with id = target_id - -- TODO loop through picking positions - -- TODO if picking position is ok (compatible with safety, robot extention etc) - -- TODO update target_pos variable and pass it to the active motion command - WRITE(’Closed File’,cr) - CLOSE FILE connection - ELSE - WRITE(’Error Opening File’,cr) - main_loop = FALSE - ENDIF - -- TODO if motion finished and set pick_now to false and target_id to 0 - ENDWHILE - WRITE(’Disconnecting..’,cr) - MSG_DISCO(’C2:’,status) - WRITE(’Done.’,cr) - pick_now = FALSE - ELSE - -- TODO we have picked the object perform specific handling - pick_now = TRUE - ENDIF - ENDWHILE -END diff --git a/karel/gripper/close-gripper.kl b/karel/gripper/close-gripper.kl deleted file mode 100644 index 221b104..0000000 --- a/karel/gripper/close-gripper.kl +++ /dev/null @@ -1,7 +0,0 @@ -PROGRAM close_gripper - -%INCLUDE gripper-utils - -BEGIN - closeGripper -END close_gripper \ No newline at end of file diff --git a/karel/gripper/gripper-utils.kl b/karel/gripper/gripper-utils.kl deleted file mode 100644 index 8269205..0000000 --- a/karel/gripper/gripper-utils.kl +++ /dev/null @@ -1,13 +0,0 @@ -ROUTINE openGripper -BEGIN - RDO[8] = OFF - DELAY 15 - RDO[7] = ON -END openGripper - -ROUTINE closeGripper -BEGIN - RDO[7] = OFF - DELAY 15 - RDO[8] = ON -END closeGripper \ No newline at end of file diff --git a/karel/gripper/open-gripper.kl b/karel/gripper/open-gripper.kl deleted file mode 100644 index 404ca59..0000000 --- a/karel/gripper/open-gripper.kl +++ /dev/null @@ -1,7 +0,0 @@ -PROGRAM open_gripper - -%INCLUDE gripper-utils - -BEGIN - openGripper -END open_gripper \ No newline at end of file diff --git a/karel/http/app_abort.kl b/karel/http/app_abort.kl deleted file mode 100644 index dcba687..0000000 --- a/karel/http/app_abort.kl +++ /dev/null @@ -1,22 +0,0 @@ -PROGRAM appabort - -%COMMENT = 'appabort v1' -%NOLOCKGROUP -%NOBUSYLAMP - -VAR - rf_ :FILE - STATUS: INTEGER - -BEGIN - - OPEN FILE rf_ ('RW', 'RD:RESPONSE.HTM') - STATUS = IO_STATUS(rf_) - IF (STATUS <> 0) THEN - WRITE rf_ ( '{"result":"failed", "program":"appabort", "reason":',STATUS,'}') - ELSE - WRITE rf_ ( '{"result":"success", "program":"appabort", "reason":',STATUS,'}') - ENDIF - CLOSE FILE rf_ - ABORT_TASK('*ALL*', TRUE, TRUE, STATUS) -END appabort \ No newline at end of file diff --git a/karel/http/app_monitor.kl b/karel/http/app_monitor.kl deleted file mode 100644 index 6c74bff..0000000 --- a/karel/http/app_monitor.kl +++ /dev/null @@ -1,66 +0,0 @@ -PROGRAM appmonitor -%COMMENT = 'appmonitor v1' -%NOLOCKGROUP -%NOPAUSE=ERROR+COMMAND+TPENABLE -%NOBUSYLAMP - -VAR - - STATUS: INTEGER - responseFile: FILE - - error_code: INTEGER - error_string: STRING[127] - cause_code: INTEGER - cause_string: STRING[127] - time_int: INTEGER - severity: INTEGER - prog_nam: STRING[127] - - - currentPos: XYZWPREXT - currentPosJ: JOINTPOS - valuePosJ: ARRAY[6] OF REAL - -BEGIN - ERR_DATA(MAXINT, error_code, error_string, cause_code, cause_string, time_int, severity, prog_nam) - - $GROUP[1].$UTOOL = $MNUTOOL[1,1] --uTool 1 - $GROUP[1].$UFRAME = $MNUFRAME[1,1] --uFrame 1 - - currentPos = CURPOS(0,0) - currentPosJ = CURJPOS(0,0) - - CNV_JPOS_REL( currentPosJ, valuePosJ, STATUS ) - IF (STATUS <> 0 ) THEN - WRITE ('CNV_JPOS_REL is failed, STATUS = ', STATUS, CR) - ENDIF - - OPEN FILE responseFile ('RW', 'RD:RESPONSE.HTM') - - WRITE responseFile( '{', CR ) - WRITE responseFile( '"joint": [' ) - WRITE responseFile( valuePosJ[1],',') - WRITE responseFile( valuePosJ[2],',') - WRITE responseFile( valuePosJ[3],',') - WRITE responseFile( valuePosJ[4],',') - WRITE responseFile( valuePosJ[5],',') - WRITE responseFile( valuePosJ[6] ) - WRITE responseFile( ' ],', CR ) - WRITE responseFile( '"position": [' ) - WRITE responseFile( currentPos.x,',') - WRITE responseFile( currentPos.y,',') - WRITE responseFile( currentPos.z,',') - WRITE responseFile( currentPos.w,',') - WRITE responseFile( currentPos.p,',') - WRITE responseFile( currentPos.r ) - WRITE responseFile( ' ],', CR ) - WRITE responseFile( '"error": [' ) - WRITE responseFile( error_code,',' ) - WRITE responseFile( '"',error_string,'"' ) - WRITE responseFile( ' ] ', CR ) - WRITE responseFile( '}') - - CLOSE FILE responseFile - -END appmonitor \ No newline at end of file diff --git a/karel/http/app_motion.kl b/karel/http/app_motion.kl deleted file mode 100644 index e548e96..0000000 --- a/karel/http/app_motion.kl +++ /dev/null @@ -1,32 +0,0 @@ -PROGRAM appmotion -%COMMENT = 'appmotion v1' -%NOLOCKGROUP -%NOPAUSE=ERROR+COMMAND+TPENABLE -%NOBUSYLAMP - -VAR - response: FILE - STATUS: INTEGER - prog_idx: INTEGER - - --motion_ctrl: INTEGER -BEGIN - $GROUP[1].$UTOOL = $MNUTOOL[1,1] --uTool 1 - $GROUP[1].$UFRAME = $MNUFRAME[1,1] --uFrame 1 - OPEN FILE response ('RW', 'RD:RESPONSE.HTM') - STATUS = IO_STATUS(response) - IF (STATUS <> 0) THEN - WRITE response('{"result":"failed", "program":"appmotion", "reason":',STATUS,'}') - ELSE - --motion_ctrl = 1 -- Semaphore to allow motion control - --CLEAR_SEMA (motion_ctrl) -- makes sure semaphore is zero before using it. - RUN_TASK( 'motion', 1, FALSE, FALSE, 1 OR 2 OR 4, STATUS ) - --POST_SEMA (motion_ctrl) -- makes motion_ctrl available immediately - IF (STATUS <> 0) THEN - WRITE response('{"result":"failed", "program":"appmotion", "reason":',STATUS,'}') - ELSE - WRITE response ( '{"result":"success", "program":"appmotion", "reason":',STATUS,'}') - ENDIF - ENDIF - CLOSE FILE response -END appmotion diff --git a/karel/http/app_move.kl b/karel/http/app_move.kl deleted file mode 100644 index 30a36ce..0000000 --- a/karel/http/app_move.kl +++ /dev/null @@ -1,275 +0,0 @@ -PROGRAM appmove - -%COMMENT = 'appmove v1' -%NOLOCKGROUP -%NOPAUSE=ERROR+COMMAND+TPENABLE -%NOBUSYLAMP - -CONST - JJR_MOTION = 1 -- joint relative joint interpolation - JJA_MOTION = 2 -- joint absolut joint interpolation - - JCA_MOTION = 3 -- cartesian absolut joint interpolation - LCA_MOTION = 4 -- cartesian absolut linear interpolation - JCR_MOTION = 5 -- cartesian relative joint interpolation - LCR_MOTION = 6 -- cartesian relative linear interpolation - - WAIT_MOTION = 7 -- wait for motion - - COORD_LEN = 9 - - --register ids - PR_JOINTS = 40 - PR_XYZWPR = 41 - - R_COORD1 = 51 - R_COORD2 = 52 - R_COORD3 = 53 - R_COORD4 = 54 - R_COORD5 = 55 - R_COORD6 = 56 - - -VAR ---get parameters - motion_t: STRING[3] - ---program parameter - rf_ :FILE - STATUS: INTEGER - --- option type is related to mt url parameter - opt: INTEGER - - coord1: STRING[127] - coord2: STRING[127] - coord3: STRING[127] - coord4: STRING[127] - coord5: STRING[127] - coord6: STRING[127] - - coord: ARRAY[COORD_LEN] OF REAL - - cur_jpos: JOINTPOS - cur_pos: XYZWPR - - new_jpos: JOINTPOS - new_pos: XYZWPR - - -ROUTINE check_param_ : INTEGER FROM appmove -ROUTINE set_reg_ : INTEGER FROM appmove - -BEGIN - - cur_pos = CURPOS(0,0); - cur_jpos = CURJPOS(0,0); - - OPEN FILE rf_ ('RW', 'RD:RESPONSE.HTM') - STATUS = IO_STATUS(rf_) - IF (STATUS <> 0) THEN - WRITE rf_ ( '{"result":"failed", "program":"appmotion", "reason":',STATUS,'}') - ENDIF - - opt = check_param_ - SELECT opt OF - - CASE (JJR_MOTION): - -- if the register cannot be set - IF (set_reg_ <> 0) THEN - WRITE rf_ ( '{"result":"failed", "program":"appmove", "reason":',STATUS,'}') - ELSE - SET_JPOS_REG(PR_JOINTS, cur_jpos, STATUS) - IF (STATUS <> 0 ) THEN - WRITE rf_ ( '{"result":"failed", "program":"appmove", "reason":',STATUS,'}') - ELSE - WRITE rf_ ( '{"result":"success", "program":"appmove", "reason":',STATUS,'}') - FLG[1] = ON - FLG[7] = ON - ENDIF - ENDIF - - CASE (JJA_MOTION): - CNV_REL_JPOS(coord, new_jpos, STATUS) - IF (STATUS <> 0 ) THEN - WRITE rf_ ( '{"result":"failed", "program":"appmove", "reason":',STATUS,'}') - ELSE - SET_JPOS_REG(PR_JOINTS, new_jpos, STATUS) -- set the joint motion positions register - IF (STATUS <> 0 ) THEN - WRITE rf_ ( '{"result":"failed", "program":"appmove", "reason":',STATUS,'}') - ELSE - WRITE rf_ ( '{"result":"success", "program":"appmove", "reason":',STATUS,'}') - FLG[2] = ON - FLG[7] = ON - ENDIF - ENDIF - - CASE (JCA_MOTION): - new_pos = cur_pos --Keep the current configuration - - new_pos.x = coord[1] - new_pos.y = coord[2] - new_pos.z = coord[3] - new_pos.w = coord[4] - new_pos.p = coord[5] - new_pos.r = coord[6] - - SET_POS_REG(PR_XYZWPR, new_pos, STATUS) - IF (STATUS <> 0 ) THEN - WRITE rf_ ( '{"result":"failed", "program":"appmove", "reason":',STATUS,'}') - ELSE - SET_POS_REG(PR_XYZWPR, cur_pos, STATUS) - IF (STATUS <> 0 ) THEN - WRITE rf_ ( '{"result":"failed", "program":"appmove", "reason":',STATUS,'}') - ELSE - WRITE rf_ ( '{"result":"success", "program":"appmove", "reason":',STATUS,'}') - FLG[3] = ON - FLG[7] = ON - ENDIF - ENDIF - - CASE (LCA_MOTION): - - new_pos = cur_pos --Keep the current configuration - - new_pos.x = coord[1] - new_pos.y = coord[2] - new_pos.z = coord[3] - new_pos.w = coord[4] - new_pos.p = coord[5] - new_pos.r = coord[6] - - SET_POS_REG(PR_XYZWPR, new_pos, STATUS) -- set positions register - IF (STATUS <> 0 ) THEN - WRITE rf_ ( '{"result":"failed", "program":"appmove", "reason":',STATUS,'}') - ELSE - WRITE rf_ ( '{"result":"success", "program":"appmove", "reason":',STATUS,'}') - FLG[4] = ON - FLG[7] = ON - ENDIF - - CASE (JCR_MOTION): - -- if the register cannot be set - IF (set_reg_ <> 0) THEN - WRITE rf_ ( '{"result":"failed", "program":"appmove", "reason":',STATUS,'}') - ELSE - WRITE rf_ ( '{"result":"success", "program":"appmove", "reason":',STATUS,'}') - FLG[5] = ON - FLG[7] = ON - ENDIF - - CASE (LCR_MOTION): - -- if the register cannot be set - IF (set_reg_ <> 0) THEN - WRITE rf_ ( '{"result":"failed", "program":"appmove", "reason":',STATUS,'}') - ELSE - WRITE rf_ ( '{"result":"success", "program":"appmove", "reason":',STATUS,'}') - FLG[6] = ON - FLG[7] = ON - ENDIF - - ELSE: - WRITE rf_ ( '{"result":"failed", "program":"appmove", "reason":',opt,'}') - ENDSELECT - - CLOSE FILE rf_ - -END appmove - --------------------------------------- ROUTINE ---------------------------------------- - -ROUTINE check_param_ -VAR - stat_: INTEGER -BEGIN - - stat_ = -1 - - IF (UNINIT(motion_t)) THEN RETURN (-1); ENDIF - - IF (motion_t = 'jjr') THEN stat_ = JJR_MOTION; ENDIF - IF (motion_t = 'jja') THEN stat_ = JJA_MOTION; ENDIF - - IF (motion_t = 'jcr') THEN stat_ = JCR_MOTION; ENDIF - IF (motion_t = 'jca') THEN stat_ = JCA_MOTION; ENDIF - IF (motion_t = 'lcr') THEN stat_ = LCR_MOTION; ENDIF - IF (motion_t = 'lca') THEN stat_ = LCA_MOTION; ENDIF - - WRITE ('status while parameter check ', stat_ ,CR) - - IF (UNINIT(coord1) OR - UNINIT(coord2) OR - UNINIT(coord3) OR - UNINIT(coord4) OR - UNINIT(coord5) OR - UNINIT(coord6)) THEN - WRITE ('uninitialized variable',CR) - RETURN (-1) - ELSE - - IF (stat_ <> -1) THEN - - IF ((coord1 = '') OR - (coord2 = '') OR - (coord3 = '') OR - (coord4 = '') OR - (coord5 = '') OR - (coord6 = '')) THEN - WRITE ('coordinate empty',CR) - RETURN(-1) - ENDIF - - -- convert the positions to real numbers - CNV_STR_REAL(coord1, coord[1]) - CNV_STR_REAL(coord2, coord[2]) - CNV_STR_REAL(coord3, coord[3]) - CNV_STR_REAL(coord4, coord[4]) - CNV_STR_REAL(coord5, coord[5]) - CNV_STR_REAL(coord6, coord[6]) - coord[7] = 0.0 - coord[8] = 0.0 - coord[9] = 0.0 - WRITE ('coordinate set',CR) - RETURN (stat_) - ENDIF - - ENDIF - - RETURN(-1) -END check_param_ - -ROUTINE set_reg_ -BEGIN - --set registers - SET_REAL_REG( R_COORD1, coord[1], STATUS) - IF (STATUS <> 0 ) THEN - RETURN (-1) - ENDIF - - SET_REAL_REG( R_COORD2, coord[2], STATUS) - IF (STATUS <> 0 ) THEN - RETURN (-1) - ENDIF - - SET_REAL_REG( R_COORD3, coord[3], STATUS) - IF (STATUS <> 0 ) THEN - RETURN (-1) - ENDIF - - SET_REAL_REG( R_COORD4, coord[4], STATUS) - IF (STATUS <> 0 ) THEN - RETURN (-1) - ENDIF - - SET_REAL_REG( R_COORD5, coord[5], STATUS) - IF (STATUS <> 0 ) THEN - RETURN (-1) - ENDIF - - SET_REAL_REG( R_COORD6, coord[6], STATUS) - IF (STATUS <> 0 ) THEN - RETURN (-1) - ENDIF - - RETURN (STATUS) -END set_reg_ \ No newline at end of file diff --git a/karel/http/app_reset.kl b/karel/http/app_reset.kl deleted file mode 100644 index ec8ed65..0000000 --- a/karel/http/app_reset.kl +++ /dev/null @@ -1,26 +0,0 @@ -PROGRAM appreset - -%NOLOCKGROUP - -VAR - success : BOOLEAN - - response : FILE - STATUS: INTEGER - -BEGIN - RESET(success) - OPEN FILE response('RW', 'RD:RESPONSE.HTM') - STATUS = IO_STATUS(response) - IF (STATUS <> 0) THEN - WRITE response('{"result":"failed", "program":"appreset", "reason":',STATUS,'}') - ELSE - IF success THEN - WRITE response('{"result":"success", "program":"appreset", "reason":',STATUS,'}') - ELSE - WRITE response('{"result":"failed", "program":"appreset", "reason":',STATUS,'}') - ENDIF - ENDIF - CLOSE FILE response -END appreset - diff --git a/karel/http_api_client/api_client.kl b/karel/http_api_client/api_client.kl new file mode 100644 index 0000000..e7bbd11 --- /dev/null +++ b/karel/http_api_client/api_client.kl @@ -0,0 +1,173 @@ +PROGRAM api_client + +%NOLOCKGROUP +%NOPAUSE=ERROR+COMMAND+TPENABLE +%NOBUSYLAMP + +-- type definitions in seperate file +%INCLUDE ../xml/xml_types +-- KAREL built-in xml routines +%INCLUDE klerxmlf + +CONST + + -- variable names as stings (required for xml parsing) + PROGRAM_NAME = 'api_client' + PP_BUFFER_NAME = 'pp_buffer' + OBJ_BUFFER_NAME = 'obj_buffer' + + pipe_path = 'PIP:pipe.dt' + CONNECTION_TAG = 'C3:' + + relative_pos = FALSE + +VAR + + coords_6d: ARRAY[6] OF REAL + + -- buffers for xml parsing, named according to consts above + obj_buffer: picking_object_t + pp_buffer: picking_pose_t + + -- array of fixed length for storage of API response data + obj_array: ARRAY [16] OF picking_object_t + pp_array: ARRAY [16] OF picking_pose_t + + -- storage of target selection + target_obj_id: STRING[32] + target_pp_id: STRING[32] + + -- socket file for API queries + socket_file: FILE + -- output file for API queries + pipe_file: FILE + + current_pos: XYZWPR + target_pos: XYZWPR + + -- key value pairs for API calls + pp_query_keys: ARRAY[2] OF STRING[32] + pp_query_values: ARRAY[2] OF STRING[32] + + query: STRING[254] + query_success: BOOLEAN + + STATUS: INTEGER + n: INTEGER + +-- dependencies (also transitive dependencies of included files) +%INCLUDE ../util/generic_util +%INCLUDE ../util/socket_util +%INCLUDE ../util/file_util +%INCLUDE ../xml/obj_parser +%INCLUDE ../xml/pp_parser +%INCLUDE ../core/core_move +%INCLUDE ../core/core_motion_start +%INCLUDE ../core/core_motion_abort +%INCLUDE ../util/http_client_util + +BEGIN + +-- disconnect if previous program termination was faulty +DISCONNECT_FROM_SERVER(CONNECTION_TAG, socket_file) + +--------------------------- +-- Fetch picking objects +--------------------------- + +-- use raw query +query = 'GET /objects' +MAKE_HTTP_REQUEST(query,CONNECTION_TAG, socket_file, pipe_file, pipe_path) + +n = PARSE_OBJ_LIST(pipe_file, pipe_path, obj_buffer, OBJ_BUFFER_NAME, PROGRAM_NAME, obj_array) + +IF(n = 0) THEN + RETURN +ENDIF + +-- choose first +target_obj_id = obj_array[1].id + +--------------------------- +-- Fetch picking pose +--------------------------- + +pp_query_keys[1] = 'obj_id' +pp_query_keys[2] = 'relative' + +pp_query_values[1] = target_obj_id +pp_query_values[2] = BOOL_TO_STR(relative_pos) + +-- use query builder for convenience +query = BUILD_PARAM_REQUEST('GET', 'picking-poses', pp_query_keys, pp_query_values) +MAKE_HTTP_REQUEST(query,CONNECTION_TAG, socket_file, pipe_file, pipe_path) + +n = PARSE_PP_LIST(pipe_file, pipe_path, pp_buffer, PP_BUFFER_NAME, PROGRAM_NAME, pp_array) + +IF(n = 0) THEN + RETURN +ENDIF + +-- choose first +target_pp_id = pp_array[1].id + +-- set correct coordinate frame (for CURPOS routine) +$GROUP[1].$UTOOL = $MNUTOOL[1,1] --uTool 1 +$GROUP[1].$UFRAME = $MNUFRAME[1,1] --uFrame 1 + +-- start from current_pos +current_pos = CURPOS(0,0) +target_pos = current_pos +-- overwrite specific values +target_pos.x = pp_array[1].x +target_pos.y = pp_array[1].y +target_pos.z = pp_array[1].z +target_pos.w = pp_array[1].w +target_pos.p = pp_array[1].p +target_pos.r = pp_array[1].r + +-- start robot movement +STATUS = CORE_MOTION_START(FALSE) + +--------------------------- +-- Track selected picking pose +--------------------------- +REPEAT + + -- update motion target + UPDATE_MOTION_BACKEND(target_pos, FALSE, FALSE) + -- query API twice per second + DELAY(500) + + pp_query_keys[1] = 'pp_id' + pp_query_keys[2] = 'relative' + + pp_query_values[1] = target_pp_id + pp_query_values[2] = BOOL_TO_STR(relative_pos) + + -- use query builder for convenience + query = BUILD_PARAM_REQUEST('GET', 'track-picking-pose', pp_query_keys, pp_query_values) + MAKE_HTTP_REQUEST(query,CONNECTION_TAG, socket_file, pipe_file, pipe_path) + + query_success = PARSE_PP_SINGLE(pipe_file, pipe_path, pp_buffer, PP_BUFFER_NAME, PROGRAM_NAME) + + IF(NOT query_success) THEN + RETURN + ENDIF + + -- update target position variable + target_pos = current_pos + target_pos.x = pp_buffer.x + target_pos.y = pp_buffer.y + target_pos.z = pp_buffer.z + target_pos.w = pp_buffer.w + target_pos.p = pp_buffer.p + target_pos.r = pp_buffer.r + +-- check for equality within 1mm and 0.1 degrees +UNTIL (XYZWPR_ALMOST_EQUAL(target_pos, current_pos, 1, 0.1)) + +--terminate robot movement +STATUS = CORE_MOTION_ABORT + +END api_client diff --git a/karel/http_rpc_server/remote_control.kl b/karel/http_rpc_server/remote_control.kl new file mode 100644 index 0000000..09c937b --- /dev/null +++ b/karel/http_rpc_server/remote_control.kl @@ -0,0 +1,114 @@ +PROGRAM remotecontrol + +%COMMENT = ' Remote Control' +%NOLOCKGROUP +%NOPAUSE=ERROR+COMMAND+TPENABLE +%NOBUSYLAMP + +VAR + response_file :FILE + STATUS: INTEGER + + --remote_grip URL params + gripper_width: STRING[127] + gripper_force: STRING[127] + + --remote_move URL params + x: STRING[127] + y: STRING[127] + z: STRING[127] + w: STRING[127] + p: STRING[127] + r: STRING[127] + linear_path: STRING[127] + interrupt: STRING[127] + + -- remote_grip internal vars + width_real: REAL + force_real: REAL + + -- remote_move internal vars + coords_6d: ARRAY[6] OF REAL + linear_flag: BOOLEAN + interrupt_flag: BOOLEAN + + %INCLUDE ../util/generic_util + %INCLUDE ../core/core_gripper_info + %INCLUDE ../core/core_grip + %INCLUDE ../core/core_position + %INCLUDE ../core/core_move + +BEGIN + OPEN FILE response_file ('RW', 'RD:RESPONSE.HTM') + STATUS = IO_STATUS(response_file) + IF (STATUS <> 0) THEN + RETURN + ENDIF + + -- check url params (bool flag can be omitted) + IF (UNINIT(x) OR + UNINIT(y) OR + UNINIT(z) OR + UNINIT(w) OR + UNINIT(p) OR + UNINIT(r) OR + UNINIT(gripper_width) OR + UNINIT(gripper_force))THEN + WRITE response_file ( '{"result":"failed", "reason":','uninitialized parameter','}') + RETURN + ELSE + IF ((x = '') OR + (y = '') OR + (z = '') OR + (w = '') OR + (p = '') OR + (r = '') OR + (gripper_width = '') OR + (gripper_force = ''))THEN + WRITE response_file ( '{"result":"failed", "reason":','empty parameter','}') + RETURN + ENDIF + ENDIF + + -- REMOTE_MOVE + WRITE response_file('{"remote_move":') + CNV_STR_REAL(x, coords_6d[1]) + CNV_STR_REAL(y, coords_6d[2]) + CNV_STR_REAL(z, coords_6d[3]) + CNV_STR_REAL(w, coords_6d[4]) + CNV_STR_REAL(p, coords_6d[5]) + CNV_STR_REAL(r, coords_6d[6]) + + -- parse optional paramters or set to default + IF(UNINIT(linear_path)) THEN + linear_flag = TRUE + ELSE + linear_flag = STR_TO_BOOL(linear_path) + ENDIF + + IF(UNINIT(interrupt)) THEN + interrupt_flag = FALSE + ELSE + interrupt_flag = STR_TO_BOOL(interrupt) + ENDIF + + WRITE response_file(CORE_MOVE(coords_6d, linear_flag, interrupt_flag)) + + -- REMOTE_GRIP + WRITE response_file(',"remote_grip":') + CNV_STR_REAL(gripper_width, width_real) + CNV_STR_REAL(gripper_force, force_real) + + WRITE response_file(CORE_GRIP(width_real, force_real)) + + -- REMOTE_GRIPPER_INFO + WRITE response_file(',"remote_gripper_info":') + CORE_GRIPPER_INFO(response_file) + + -- REMOTE_POSITION + WRITE response_file(',"remote_position":') + CORE_POSITION(response_file) + + WRITE response_file('}') + CLOSE FILE response_file +END remotecontrol \ No newline at end of file diff --git a/karel/http_rpc_server/remote_grip.kl b/karel/http_rpc_server/remote_grip.kl new file mode 100644 index 0000000..66cc604 --- /dev/null +++ b/karel/http_rpc_server/remote_grip.kl @@ -0,0 +1,49 @@ +PROGRAM remotegrip + +%COMMENT = ' Remote Grip' +%NOLOCKGROUP +%NOPAUSE=ERROR+COMMAND+TPENABLE +%NOBUSYLAMP + +VAR + + response_file :FILE + STATUS: INTEGER + + width: STRING[127] + force: STRING[127] + + width_real: REAL + force_real: REAL + + %INCLUDE ../util/generic_util + %INCLUDE ../core/core_grip + +BEGIN + OPEN FILE response_file ('RW', 'RD:RESPONSE.HTM') + STATUS = IO_STATUS(response_file) + IF (STATUS <> 0) THEN + RETURN + ENDIF + + -- check url params + IF (UNINIT(width) OR + UNINIT(force)) THEN + WRITE response_file ( '{"result":"failed", "reason":','uninitialized parameter','}') + RETURN + ELSE + IF ((width = '') OR + (force = ''))THEN + WRITE response_file ( '{"result":"failed", "reason":','empty parameter','}') + RETURN + ENDIF + ENDIF + + CNV_STR_REAL(width, width_real) + CNV_STR_REAL(force, force_real) + + WRITE response_file(CORE_GRIP(width_real, force_real)) + + CLOSE FILE response_file + +END remotegrip \ No newline at end of file diff --git a/karel/http_rpc_server/remote_gripper_info.kl b/karel/http_rpc_server/remote_gripper_info.kl new file mode 100644 index 0000000..49073b2 --- /dev/null +++ b/karel/http_rpc_server/remote_gripper_info.kl @@ -0,0 +1,24 @@ +PROGRAM remotegripperinfo + +%COMMENT = ' Remote Gripper Info' +%NOLOCKGROUP +%NOPAUSE=ERROR+COMMAND+TPENABLE +%NOBUSYLAMP + +VAR + response_file :FILE + STATUS: INTEGER + + %INCLUDE ../core/core_gripper_info + +BEGIN + OPEN FILE response_file ('RW', 'RD:RESPONSE.HTM') + STATUS = IO_STATUS(response_file) + IF (STATUS <> 0) THEN + RETURN + ELSE + CORE_GRIPPER_INFO(response_file) + ENDIF + + CLOSE FILE response_file +END remotegripperinfo \ No newline at end of file diff --git a/karel/http_rpc_server/remote_motion_abort.kl b/karel/http_rpc_server/remote_motion_abort.kl new file mode 100644 index 0000000..99f5a6f --- /dev/null +++ b/karel/http_rpc_server/remote_motion_abort.kl @@ -0,0 +1,31 @@ +PROGRAM remotemotionabort + +%COMMENT = 'Remote Motion Abort' +%NOLOCKGROUP +%NOBUSYLAMP + +VAR + response_file :FILE + STATUS: INTEGER + +%INCLUDE ../core/core_motion_abort + +BEGIN + + OPEN FILE response_file ('RW', 'RD:RESPONSE.HTM') + STATUS = IO_STATUS(response_file) + IF (STATUS <> 0) THEN + RETURN + ENDIF + + STATUS = CORE_MOTION_ABORT + + -- 3016 is task wasnt even active to begin with + IF ((STATUS <> 0) AND (STATUS <> 3016) ) THEN + WRITE response_file ( '{"result":"failed", "reason":"Could not terminate motion_backend TP routine"}') + ELSE + WRITE response_file ( '{"result":"success"}') + ENDIF + + CLOSE FILE response_file +END remotemotionabort \ No newline at end of file diff --git a/karel/http_rpc_server/remote_motion_start.kl b/karel/http_rpc_server/remote_motion_start.kl new file mode 100644 index 0000000..0aee153 --- /dev/null +++ b/karel/http_rpc_server/remote_motion_start.kl @@ -0,0 +1,46 @@ +PROGRAM remotemotionstart + +%COMMENT = 'Remote Motion Abort' +%NOLOCKGROUP +%NOBUSYLAMP + +VAR + response_file :FILE + STATUS: INTEGER + hs: STRING[127] + hs_flag: BOOLEAN + +%INCLUDE ../util/generic_util +%INCLUDE ../core/core_motion_start + +BEGIN + + OPEN FILE response_file ('RW', 'RD:RESPONSE.HTM') + STATUS = IO_STATUS(response_file) + IF (STATUS <> 0) THEN + RETURN + ENDIF + + -- terminate motion TP task + ABORT_TASK('motion_backend', TRUE, TRUE, STATUS) + ABORT_TASK('hs_motion_backend', TRUE, TRUE, STATUS) + DELAY(1000) -- yield to be able to restart TP routine + -- start new motion TP task + + IF(UNINIT(hs)) THEN + hs_flag = FALSE + ELSE + hs_flag = STR_TO_BOOL(hs) + ENDIF + + SET_INT_REG(8, 0, STATUS) + STATUS = CORE_MOTION_START(hs_flag) + + IF (STATUS <> 0 ) THEN + WRITE response_file ( '{"result":"failed", "reason":"Could not start motion_backend TP routine"}') + ELSE + WRITE response_file ( '{"result":"success"}') + ENDIF + + CLOSE FILE response_file +END remotemotionstart \ No newline at end of file diff --git a/karel/http_rpc_server/remote_move.kl b/karel/http_rpc_server/remote_move.kl new file mode 100644 index 0000000..feeab28 --- /dev/null +++ b/karel/http_rpc_server/remote_move.kl @@ -0,0 +1,81 @@ +PROGRAM remotemove + +%COMMENT = ' Remote Move' +%NOLOCKGROUP +%NOPAUSE=ERROR+COMMAND+TPENABLE +%NOBUSYLAMP + +VAR + + response_file :FILE + STATUS: INTEGER + + x: STRING[127] + y: STRING[127] + z: STRING[127] + w: STRING[127] + p: STRING[127] + r: STRING[127] + linear_path: STRING[127] + interrupt: STRING[127] + + coords_6d: ARRAY[6] OF REAL + linear_flag: BOOLEAN + interrupt_flag: BOOLEAN + + %INCLUDE ../util/generic_util + %INCLUDE ../core/core_move + +BEGIN + OPEN FILE response_file ('RW', 'RD:RESPONSE.HTM') + STATUS = IO_STATUS(response_file) + IF (STATUS <> 0) THEN + RETURN + ENDIF + + -- check url params (bool flags can be omitted) + IF (UNINIT(x) OR + UNINIT(y) OR + UNINIT(z) OR + UNINIT(w) OR + UNINIT(p) OR + UNINIT(r)) THEN + WRITE response_file ( '{"result":"failed", "reason":','uninitialized parameter','}') + RETURN + ELSE + IF ((x = '') OR + (y = '') OR + (z = '') OR + (w = '') OR + (p = '') OR + (r = ''))THEN + WRITE response_file ( '{"result":"failed", "reason":','empty parameter','}') + RETURN + ENDIF + ENDIF + + -- convert the positions to real numbers + CNV_STR_REAL(x, coords_6d[1]) + CNV_STR_REAL(y, coords_6d[2]) + CNV_STR_REAL(z, coords_6d[3]) + CNV_STR_REAL(w, coords_6d[4]) + CNV_STR_REAL(p, coords_6d[5]) + CNV_STR_REAL(r, coords_6d[6]) + + -- parse optional paramters or set to default + IF(UNINIT(linear_path)) THEN + linear_flag = TRUE + ELSE + linear_flag = STR_TO_BOOL(linear_path) + ENDIF + + IF(UNINIT(interrupt)) THEN + interrupt_flag = FALSE + ELSE + interrupt_flag = STR_TO_BOOL(interrupt) + ENDIF + + CORE_MOVE(coords_6d, linear_flag, interrupt_flag, response_file) + + CLOSE FILE response_file +END remotemove \ No newline at end of file diff --git a/karel/http_rpc_server/remote_position.kl b/karel/http_rpc_server/remote_position.kl new file mode 100644 index 0000000..179a185 --- /dev/null +++ b/karel/http_rpc_server/remote_position.kl @@ -0,0 +1,22 @@ +PROGRAM remoteposition +%COMMENT = ' Remote Position' +%NOLOCKGROUP +%NOBUSYLAMP + +VAR + response_file :FILE + STATUS: INTEGER + + %INCLUDE ./core_position + +BEGIN + OPEN FILE response_file ('RW', 'RD:RESPONSE.HTM') + STATUS = IO_STATUS(response_file) + IF (STATUS <> 0) THEN + RETURN + ENDIF + + CORE_POSITION(response_file) + + CLOSE FILE response_file +END remoteposition \ No newline at end of file diff --git a/karel/http_rpc_server/remote_target_check.kl b/karel/http_rpc_server/remote_target_check.kl new file mode 100644 index 0000000..9b23aaf --- /dev/null +++ b/karel/http_rpc_server/remote_target_check.kl @@ -0,0 +1,37 @@ +PROGRAM remotetargetcheck +%COMMENT = 'Remote Target Check' +%NOLOCKGROUP +%NOBUSYLAMP + +CONST + PR_XYZWPR = 41 + error_margin = 0.1 + +VAR + response_file :FILE + STATUS: INTEGER + current_pos: XYZWPR + target_pos: XYZWPR + +%INCLUDE ../util/generic_util + +BEGIN + $GROUP[1].$UTOOL = $MNUTOOL[1,1] --uTool 1 + $GROUP[1].$UFRAME = $MNUFRAME[1,1] --uFrame 1 + current_pos = CURPOS(0,0) + target_pos = GET_POS_REG(PR_XYZWPR, STATUS) + + OPEN FILE response_file ('RW', 'RD:RESPONSE.HTM') + STATUS = IO_STATUS(response_file) + IF (STATUS <> 0) THEN + RETURN + ENDIF + + IF(XYZWPR_ALMOST_EQUAL(current_pos, target_pos, error_margin, error_margin)) THEN + WRITE response_file('{"result":"success", "target_reached":', 1, '}') + ELSE + WRITE response_file('{"result":"success", "target_reached":', 0, '}') + ENDIF + + CLOSE FILE response_file +END remotetargetcheck \ No newline at end of file diff --git a/karel/motion/MOTION.LS b/karel/legacy/MOTION.LS similarity index 94% rename from karel/motion/MOTION.LS rename to karel/legacy/MOTION.LS index 4967b49..6d279aa 100644 --- a/karel/motion/MOTION.LS +++ b/karel/legacy/MOTION.LS @@ -51,22 +51,22 @@ CONTROL_CODE = 00000000 00000000; 30: PR[40,5]=PR[40,5]+R[55] ; 31: PR[40,6]=PR[40,6]+R[56] ; 32:J PR[40] 100% CNT100 ; - 33: JMP LBL[999] ; + 33: JMP LBL[666] ; 34: ; 35: ! absolut joint moition with join ; 36: LBL[2] ; 37:J PR[40] 100% CNT100 ; - 38: JMP LBL[999] ; + 38: JMP LBL[666] ; 39: ; 40: ! absolut cartesian moition with ; 41: LBL[3] ; 42:J PR[41] 100% CNT100 ; - 43: JMP LBL[999] ; + 43: JMP LBL[666] ; 44: ; 45: ! absolut cartesian moition with ; 46: LBL[4] ; 47:L PR[41] 5000mm/sec CNT100 ; - 48: JMP LBL[999] ; + 48: JMP LBL[666] ; 49: ; 50: ! relative cartesian motion with ; 51: LBL[5] ; @@ -77,7 +77,7 @@ CONTROL_CODE = 00000000 00000000; 56: PR[41,5]=PR[41,5]+R[55] ; 57: PR[41,6]=PR[41,6]+R[56] ; 58:J PR[41] 100% CNT100 ; - 59: JMP LBL[999] ; + 59: JMP LBL[666] ; 60: ; 61: ! relative cartesian motion with ; 62: LBL[6] ; @@ -88,7 +88,7 @@ CONTROL_CODE = 00000000 00000000; 67: PR[41,5]=PR[41,5]+R[55] ; 68: PR[41,6]=PR[41,6]+R[56] ; 69:L PR[41] 5000mm/sec CNT100 ; - 70: JMP LBL[999] ; + 70: JMP LBL[666] ; 71: ; 72: ! default setting ; 73: JMP LBL[999] ; @@ -108,5 +108,6 @@ CONTROL_CODE = 00000000 00000000; 87: R[55]=0 ; 88: R[56]=0 ; 89: JMP LBL[7] ; + 90: LBL[666] ; /POS /END diff --git a/karel/legacy/STATIC_TRACK.ls b/karel/legacy/STATIC_TRACK.ls new file mode 100644 index 0000000..a13ed9b --- /dev/null +++ b/karel/legacy/STATIC_TRACK.ls @@ -0,0 +1,29 @@ +/PROG STATIC_TRACK +/ATTR +OWNER = MNEDITOR; +COMMENT = "DPM Static Tracking"; +PROG_SIZE = 1050; +CREATE = DATE 18-01-21 TIME 12:00:00; +MODIFIED = DATE 18-01-21 TIME 12:00:00; +FILE_NAME = ; +VERSION = 0; +LINE_COUNT = 30; +MEMORY_SIZE = 1310; +PROTECT = READ_WRITE; +TCD: STACK_SIZE = 0, + TASK_PRIORITY = 50, + TIME_SLICE = 0, + BUSY_LAMP_OFF = 0, + ABORT_REQUEST = 0, + PAUSE_REQUEST = 0; +DEFAULT_GROUP = 1,*,*,*,*; +CONTROL_CODE = 00000000 00000000; +/MN + : !Start stat tracking ; + : Track DPM[1] ; + :L PR[40] 100mm/sec FINE ; + : ; + : !Stop tracking ; + : !(will never reach this) ; + : Track End ; +/END diff --git a/karel/legacy/motion_util.kl b/karel/legacy/motion_util.kl new file mode 100644 index 0000000..1978bc2 --- /dev/null +++ b/karel/legacy/motion_util.kl @@ -0,0 +1,46 @@ +-- TIMED_LINEAR_MOVEMENT ... Move TCP linearly to target for specified duration +-- +-- Routine Signature description: +-- +-- target_pos: XYZWPR ... Desired target position +-- timeout_ms: INTEGER ... Timeout in ms after which motion is terminated +ROUTINE TIMED_LINEAR_MOVEMENT(target_pos: XYZWPR; timeout_ms: INTEGER) +CONST + -- position register associated with motion task + PR_XYZWPR = 41 + +BEGIN + RUN_TASK( 'motion', 1, FALSE, TRUE, 1 OR 2 OR 4, STATUS ) -- queue motion task + SET_POS_REG(PR_XYZWPR, target_pos, STATUS) -- set positions register + IF (STATUS <> 0 ) THEN + ELSE + FLG[4] = ON -- flag 4 sets linear (TCP) interpolation + FLG[7] = ON -- flag 7 starts movement + ENDIF + DELAY(timeout_ms) + ABORT_TASK('motion', TRUE, TRUE, STATUS) -- terminate motion task & associated movement +END TIMED_LINEAR_MOVEMENT + + +-- TIMED_JOINT_MOVEMENT ... Move TCP in arbitrary trajectory to target for specified duration +-- +-- Routine Signature description: +-- +-- target_pos: XYZWPR ... Desired target position +-- timeout_ms: INTEGER ... Timeout in ms after which motion is terminated +ROUTINE TIMED_JOINT_MOVEMENT(target_pos: XYZWPR; timeout_ms: INTEGER) +CONST + -- position register associated with motion task + PR_XYZWPR = 41 + +BEGIN + RUN_TASK( 'motion', 1, FALSE, TRUE, 1 OR 2 OR 4, STATUS ) -- queue motion task + SET_POS_REG(PR_XYZWPR, target_pos, STATUS) -- set positions register + IF (STATUS <> 0 ) THEN + ELSE + FLG[3] = ON -- flag 3 sets joint interpolation + FLG[7] = ON -- flag 7 starts movement + ENDIF + DELAY(timeout_ms) + ABORT_TASK('motion', TRUE, TRUE, STATUS) -- terminate motion task & associated movement +END TIMED_JOINT_MOVEMENT diff --git a/karel/legacy/remote_move.kl b/karel/legacy/remote_move.kl new file mode 100644 index 0000000..6b9ac05 --- /dev/null +++ b/karel/legacy/remote_move.kl @@ -0,0 +1,111 @@ +PROGRAM remotemove + +%COMMENT = ' Remote Move' +%NOLOCKGROUP +%NOPAUSE=ERROR+COMMAND+TPENABLE +%NOBUSYLAMP + +CONST + --register id + PR_XYZWPR = 41 + +VAR + + rf_ :FILE + STATUS: INTEGER + + x: STRING[127] + y: STRING[127] + z: STRING[127] + w: STRING[127] + p: STRING[127] + r: STRING[127] + lin: STRING[127] + + coord: ARRAY[6] OF REAL + + + cur_pos: XYZWPR + new_pos: XYZWPR + + %INCLUDE ../util/generic_util + +BEGIN + OPEN FILE rf_ ('RW', 'RD:RESPONSE.HTM') + STATUS = IO_STATUS(rf_) + IF (STATUS <> 0) THEN + WRITE rf_ ( '{"result":"failed", "reason":',STATUS,'}') + RETURN + ENDIF + + -- check url params (bool flag can be omitted) + IF (UNINIT(x) OR + UNINIT(y) OR + UNINIT(z) OR + UNINIT(w) OR + UNINIT(p) OR + UNINIT(r)) THEN + WRITE rf_ ( '{"result":"failed", "reason":','uninitialized parameter','}') + RETURN + ELSE + IF ((x = '') OR + (y = '') OR + (z = '') OR + (w = '') OR + (p = '') OR + (r = ''))THEN + WRITE rf_ ( '{"result":"failed", "reason":','empty parameter','}') + RETURN + ENDIF + ENDIF + + -- convert the positions to real numbers + CNV_STR_REAL(x, coord[1]) + CNV_STR_REAL(y, coord[2]) + CNV_STR_REAL(z, coord[3]) + CNV_STR_REAL(w, coord[4]) + CNV_STR_REAL(p, coord[5]) + CNV_STR_REAL(r, coord[6]) + + -- abort all other movement + ABORT_TASK('motion', TRUE, TRUE, STATUS) + ABORT_TASK('static_track', TRUE, TRUE, STATUS) + -- yield to be able to aqcuire motion group lock + DELAY(10) + + RUN_TASK( 'motion', 1, FALSE, TRUE, 1 OR 2 OR 4, STATUS ) + + $GROUP[1].$UTOOL = $MNUTOOL[1,1] --uTool 1 + $GROUP[1].$UFRAME = $MNUFRAME[1,1] --uFrame 1 + cur_pos = CURPOS(0,0); + + -- start from old position instance + new_pos = cur_pos + + -- overwrite specific values + new_pos.x = coord[1] + new_pos.y = coord[2] + new_pos.z = coord[3] + new_pos.w = coord[4] + new_pos.p = coord[5] + new_pos.r = coord[6] + + -- set positions register + SET_POS_REG(PR_XYZWPR, new_pos, STATUS) + + IF (STATUS <> 0 ) THEN + WRITE rf_ ( '{"result":"failed", "reason":',STATUS,'}') + ELSE + IF(STR_TO_BOOL(lin)) THEN + FLG[4] = ON + ELSE + FLG[3] = ON + ENDIF + -- unlock motion task + FLG[7] = ON + WRITE rf_ ( '{"result":"success"}') + ENDIF + + CLOSE FILE rf_ + +END remotemove \ No newline at end of file diff --git a/karel/legacy/remote_tracking_start.kl b/karel/legacy/remote_tracking_start.kl new file mode 100644 index 0000000..b585534 --- /dev/null +++ b/karel/legacy/remote_tracking_start.kl @@ -0,0 +1,68 @@ +PROGRAM REMOTETRACKINGSTART + +%COMMENT = ' Remote Tracking Start' +%NOLOCKGROUP +%NOPAUSE=ERROR+COMMAND+TPENABLE +%NOBUSYLAMP + +CONST + PR_STATIC_TRACK_XYZWPR = 40 + +VAR + + rf_ :FILE + STATUS: INTEGER + + current_pos: XYZWPR + +BEGIN + + -- set system vars for some reason + $GROUP[1].$UTOOL = $MNUTOOL[1,1] --uTool 1 + $GROUP[1].$UFRAME = $MNUFRAME[1,1] --uFrame 1 + + current_pos = CURPOS(0,0) + -- offset slightly to start static tracking with non zero motion + current_pos.x = current_pos.x + 0.1 + current_pos.y = current_pos.y + 0.1 + current_pos.z = current_pos.z + 0.1 + current_pos.w = current_pos.w + 0.1 + current_pos.p = current_pos.p + 0.1 + current_pos.r = current_pos.r + 0.1 + + OPEN FILE rf_ ('RW', 'RD:RESPONSE.HTM') + STATUS = IO_STATUS(rf_) + IF (STATUS <> 0) THEN + WRITE rf_ ( '{"result":"failed", "reason":',STATUS,'}') + RETURN + ENDIF + + SET_POS_REG(PR_STATIC_TRACK_XYZWPR, current_pos, STATUS) + IF (STATUS <> 0 ) THEN + WRITE rf_ ( '{"result":"failed", "reason":',STATUS,'}') + ENDIF + + -- abort all other movement + ABORT_TASK('motion', TRUE, TRUE, STATUS) + ABORT_TASK('static_track', TRUE, TRUE, STATUS) + -- yield to be able to aqcuire motion group lock + DELAY(10) + + -- reset system variables + SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[1].$INI_OFS', 0.0, STATUS) + SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[2].$INI_OFS', 0.0, STATUS) + SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[3].$INI_OFS', 0.0, STATUS) + SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[4].$INI_OFS', 0.0, STATUS) + SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[5].$INI_OFS', 0.0, STATUS) + SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[6].$INI_OFS', 0.0, STATUS) + -- start tracking task + RUN_TASK( 'static_track', 1, FALSE, TRUE, 1 OR 2 OR 4, STATUS ) + IF (STATUS <> 0 ) THEN + WRITE rf_ ( '{"result":"failed", "reason":',STATUS,'}') + ELSE + WRITE rf_ ( '{"result":"success"}') + ENDIF + + CLOSE FILE rf_ + +END REMOTETRACKINGSTART \ No newline at end of file diff --git a/karel/legacy/remote_tracking_update.kl b/karel/legacy/remote_tracking_update.kl new file mode 100644 index 0000000..fe078a1 --- /dev/null +++ b/karel/legacy/remote_tracking_update.kl @@ -0,0 +1,83 @@ +PROGRAM REMOTETRACKINGUPDATE + +%COMMENT = ' Remote Tracking Update' +%NOLOCKGROUP +%NOPAUSE=ERROR+COMMAND+TPENABLE +%NOBUSYLAMP + +CONST + --register id + PR_XYZWPR = 41 + +VAR + + rf_ :FILE + STATUS: INTEGER + + x: STRING[127] + y: STRING[127] + z: STRING[127] + w: STRING[127] + p: STRING[127] + r: STRING[127] + + coord: ARRAY[6] OF REAL + + + cur_pos: XYZWPR + new_pos: XYZWPR + + %INCLUDE ../util/generic_util + +BEGIN + OPEN FILE rf_ ('RW', 'RD:RESPONSE.HTM') + STATUS = IO_STATUS(rf_) + IF (STATUS <> 0) THEN + WRITE rf_ ( '{"result":"failed", "reason":',STATUS,'}') + RETURN + ENDIF + + -- check url params (bool flag can be omitted) + IF (UNINIT(x) OR + UNINIT(y) OR + UNINIT(z) OR + UNINIT(w) OR + UNINIT(p) OR + UNINIT(r)) THEN + WRITE rf_ ( '{"result":"failed", "reason":','uninitialized parameter','}') + RETURN + ELSE + IF ((x = '') OR + (y = '') OR + (z = '') OR + (w = '') OR + (p = '') OR + (r = ''))THEN + WRITE rf_ ( '{"result":"failed", "reason":','empty parameter','}') + RETURN + ENDIF + ENDIF + + -- convert the positions to real numbers + CNV_STR_REAL(x, coord[1]) + CNV_STR_REAL(y, coord[2]) + CNV_STR_REAL(z, coord[3]) + CNV_STR_REAL(w, coord[4]) + CNV_STR_REAL(p, coord[5]) + CNV_STR_REAL(r, coord[6]) + + SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[1].$INI_OFS', coord[1], STATUS) + SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[2].$INI_OFS', coord[2], STATUS) + SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[3].$INI_OFS', coord[3], STATUS) + SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[4].$INI_OFS', coord[4], STATUS) + SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[5].$INI_OFS', coord[5], STATUS) + SET_VAR(0, '*SYSTEM*', '$DPM_SCH[1].$GRP[1].$OFS[6].$INI_OFS', coord[6], STATUS) + IF (STATUS <> 0) THEN + WRITE rf_ ( '{"result":"failed", "reason":',STATUS,'}') + ELSE + WRITE rf_ ( '{"result":"success"}') + ENDIF + + CLOSE FILE rf_ + +END REMOTETRACKINGUPDATE \ No newline at end of file diff --git a/karel/main.kl b/karel/main.kl deleted file mode 100644 index 1eba1dd..0000000 --- a/karel/main.kl +++ /dev/null @@ -1,54 +0,0 @@ -PROGRAM main - -%NOLOCKGROUP -%NOPAUSE=ERROR+COMMAND+TPENABLE -%NOBUSYLAMP - -%INCLUDE types - -VAR - connFile : FILE - pipeFile : FILE - STATUS : INTEGER - fetched_picking_point: object_position - - -- only for debug - cur_pos : XYZWPR - -%INCLUDE console/print_utils -%INCLUDE parsers/position_parser -%INCLUDE socket/socket_utils -%INCLUDE motion/motion_utils - -BEGIN - connectServ('C3:') - STATUS = requestData('PIP:read.dt') - checkError('requestData', STATUS) - CLOSE FILE connFile - - -- debug printing the saved pipeFile - OPEN FILE pipeFile('RO', 'PIP:read.dt') - STATUS = IO_STATUS(pipeFile) - checkError('Open pipeFile', STATUS) - - SET_FILE_ATR(pipeFile, ATR_PIPWAIT, WAIT_USED) -- force READS to wait - SET_FILE_ATR(pipeFile, ATR_TIMEOUT, (1*1000)) -- READs timeout after 1*1000ms - - fetched_picking_point = parsePosition(pipeFile) - WRITE('fetched_picking_point ', fetched_picking_point.coordinates, CR) - - STATUS = startMotionListener - checkError('Start Motion Listener', STATUS) - - --fetched_picking_point.coordinates.x = 500 - updatePositionTarget(fetched_picking_point.coordinates) - - DELAY(5000) - - STATUS = stopMotionListener - checkError('Stop Motion Listener', STATUS) - - -- parseMultiplePickingPositions(pipeFile) - MSG_DISCO('C3:', STATUS) - -END main diff --git a/karel/motion/motion_utils.kl b/karel/motion/motion_utils.kl deleted file mode 100644 index e3b165e..0000000 --- a/karel/motion/motion_utils.kl +++ /dev/null @@ -1,35 +0,0 @@ -ROUTINE startMotionListener : INTEGER -VAR - status : INTEGER -BEGIN - $GROUP[1].$UTOOL = $MNUTOOL[1,1] --uTool 1 - $GROUP[1].$UFRAME = $MNUFRAME[1,1] --uFrame 1 - RUN_TASK( 'motion', 1, FALSE, TRUE, 1 OR 2 OR 4, status) - RETURN (status) -END startMotionListener - -ROUTINE stopMotionListener : INTEGER -VAR - status : INTEGER -BEGIN - ABORT_TASK('motion', TRUE, TRUE, status) - RETURN (status) -END stopMotionListener - -ROUTINE updatePositionTarget (target : XYZWPR) -VAR - status : INTEGER -BEGIN - -- If position is not set this way, and unintialized data exception will occur - cur_pos = CURPOS(0,0) - cur_pos.x = target.x - cur_pos.y = target.y - cur_pos.z = target.z - cur_pos.w = target.w - cur_pos.p = target.p - cur_pos.r = target.r - SET_POS_REG(41, cur_pos, STATUS) - checkError('Update Position Target', STATUS) - FLG[3] = TRUE - FLG[7] = TRUE -END updatePositionTarget \ No newline at end of file diff --git a/karel/parsers/digital_twin_parser.kl b/karel/parsers/digital_twin_parser.kl deleted file mode 100644 index d381933..0000000 --- a/karel/parsers/digital_twin_parser.kl +++ /dev/null @@ -1,87 +0,0 @@ -ROUTINE parseDigitalTwin(pipeFile : FILE) --- Example of json to parse: --- [{"id":1,"picking_points":[{"x":2.0,"y":3.0,"z":4.0,"u":-0.0,"v":0.0,"n":-0.0},{"x":6.0,"y":7.0,"z":8.0,"u":1.308996938995747,"v":1.5707963267948966,"n":0.0}], --- "last_update":"2020-05-29T08:29:37.092123Z","age":0,"last_known_position":{"id":4,"x":1.0,"y":1.0,"z":1.0,"u":0.0,"v":0.0,"n":0.0}, --- "item_name":"Test name"}] - -%INCLUDE parsers/position_parser -%INCLUDE parsers/string_parser - -VAR - status: INTEGER - count: INTEGER - c : STRING[1] - picking_points: ARRAY[30] OF picking_point - parsed_id = INTEGER - parsed_picking_positions: ARRAY[30] OF object_position - parsed_last_known_position: object_position - parsed_class: STRING - key: STRING[10] - parsing_key: BOOLEAN - key_found: BOOLEAN - -BEGIN - c='' - key = '' - reading_key = FALSE - key = '' - key_found = FALSE - -- read file. call single parser at the first { - BYTES_AHEAD(pipeFile, count, status) - checkError('Bytes Ahead 0', status) - -- parser will return when it finds }; if next char is , then repeat. if next char is ] return an array of picking_points - WHILE ((count > 0) AND (c <> ']')) DO - -- if we find " and reading_key is false, it means we found quotes before the key starts. Thus we start reading - IF ((c = '"') AND (reading_key = FALSE)) THEN - reading_key = TRUE - ENDIF - - -- if we find " and reading_key is true, it means we found quotes after the key end. Thus we stop reading and we have our key - IF ((c = '"') AND (reading_key = TRUE)) THEN - reading_key = FALSE - key_found = FALSE - ENDIF - - -- we are reading our key: we add the new character to the key string - IF (reading_key)THEN - READ pipeFile(c::1) - BYTES_AHEAD(pipeFile, count, status) - key = key + c - ENDIF - - -- we just read a new character since we are not in the key business described above - IF ((c <> '"') AND (reading_key = FALSE)) THEN - READ pipeFile(c::1) - BYTES_AHEAD(pipeFile, count, status) - ENDIF - - -- when key_found is TRUE, it meand we are ready to parse - IF (key_found) THEN - -- for saftey: we don't want to start parsing from the column diving hey and value - IF (c = ':') THEN - READ pipeFile(c::1) - BYTES_AHEAD(pipeFile, count, status) - ENDIF - - IF key = 'id' THEN - parsed_id = parseStringOrNumber(pipeFile, TRUE) - ENDIF - - IF key = 'picking_points' THEN - parsed_picking_positions = parseMultiplePositions(pipeFile) - ENDIF - - IF key = 'last_known_position' THEN - parsed_last_known_position = parsePosition(pipeFile) - ENDIF - - IF key = 'item_name' THEN - parsed_class = parseStringOrNumber(pipeFile, FALSE) - ENDIF - - key = '' - key_found = FALSE - ENDIF - ENDWHILE - -END parseDigitalTwin \ No newline at end of file diff --git a/karel/parsers/position_parser.kl b/karel/parsers/position_parser.kl deleted file mode 100644 index 31b7401..0000000 --- a/karel/parsers/position_parser.kl +++ /dev/null @@ -1,137 +0,0 @@ -ROUTINE parsePosition(pipeFile : FILE) : object_position -VAR - status: INTEGER - count: INTEGER - read_str: STRING[100] - x : REAL - y : REAL - z : REAL - w : REAL - p : REAL - r : REAL - c : STRING[1] - c_1 : STRING[50] - key_length : INTEGER - string_value : STRING[20] - quote_count : INTEGER - searching : BOOLEAN - candidate : STRING[52] - reading : BOOLEAN - v : REAL - ret_val : object_position -BEGIN - BYTES_AHEAD(pipeFile, count, status) - checkError('Bytes Ahead 0', status) - searching = TRUE - quote_count = 0 - candidate = '' - reading = FALSE - string_value = '' - c = '' - v = 0 - key_length = 0 - - WHILE ((count > 0) AND (c <> '}')) DO -- parser will return when it finds } - READ pipeFile(c::1) - BYTES_AHEAD(pipeFile, count, status) - checkError('Bytes Ahead inside loop', status) - - IF c = '{' THEN - READ pipeFile(c::1) - BYTES_AHEAD(pipeFile, count, status) - ENDIF - - IF searching = TRUE THEN - IF ((((quote_count = 0) OR (quote_count=1)) AND (c = '"'))) THEN - candidate = candidate + c - quote_count = quote_count + 1 - ENDIF - IF ((quote_count = 1) AND (c <> '"')) THEN - candidate = candidate + c - key_length = key_length + 1 - ENDIF - IF quote_count = 2 THEN - searching = FALSE - c_1 = SUB_STR(candidate, 2, key_length) - quote_count = 0 - key_length = 0 - ENDIF - ELSE - IF (reading = TRUE) THEN - IF (c <> ' ') AND (c <> ',') AND (c <> '}') THEN - string_value = string_value + c - ELSE - CNV_STR_REAL(string_value, v) - string_value = '' - reading = FALSE - ENDIF - ENDIF - IF c = ':' THEN - reading = TRUE - ENDIF - IF (c = ',') OR (c = '}') THEN - searching = TRUE - reading = FALSE - IF c_1 = 'x' THEN - x = v - ENDIF - IF c_1 = 'y' THEN - y = v - ENDIF - IF c_1 = 'z' THEN - z = v - ENDIF - IF c_1 = 'phi' THEN - w = v - ENDIF - IF c_1 = 'theta' THEN - p = v - ENDIF - IF c_1 = 'psi' THEN - r = v - ENDIF - candidate = '' - ENDIF - ENDIF - ENDWHILE - - -- initialize picking_point structure with parsed data - ret_val.coordinates.x = x - ret_val.coordinates.y = y - ret_val.coordinates.z = z - ret_val.coordinates.w = w - ret_val.coordinates.p = p - ret_val.coordinates.r = r - RETURN (ret_val) - -END parsePosition - -ROUTINE parseMultiplePositions(pipeFile : FILE) -VAR - status: INTEGER - count: INTEGER - c : STRING[1] - picking_points: ARRAY[30] OF object_position - array_index : INTEGER - parsed_picking_position: object_position -BEGIN - array_index = 1 - c='' - -- read file. call single parser at the first { - BYTES_AHEAD(pipeFile, count, status) - checkError('Bytes Ahead 0', status) - -- parser will return when it finds }; if next char is , then repeat. if next char is ] return an array of picking_points - WHILE ((count > 0) AND (c <> ']')) DO - READ pipeFile(c::1) - BYTES_AHEAD(pipeFile, count, status) - IF ((c=',') OR (c='[')) THEN - READ pipeFile(c::1) - BYTES_AHEAD(pipeFile, count, status) - parsed_picking_position = parsePosition(pipeFile) - picking_points[array_index] = parsed_picking_position - WRITE('array elements', picking_points[array_index].coordinates, CR) - array_index = array_index + 1 - ENDIF - ENDWHILE - -END parseMultiplePositions \ No newline at end of file diff --git a/karel/parsers/string_parser.kl b/karel/parsers/string_parser.kl deleted file mode 100644 index 1267387..0000000 --- a/karel/parsers/string_parser.kl +++ /dev/null @@ -1,52 +0,0 @@ -ROUTINE parseStringOrNumber(pipeFile : FILE, number: BOOLEAN) : STRING[200] -VAR -status: INTEGER - count: INTEGER - c : STRING[1] - quote_count : INTEGER - candidate : STRING[200] - v : REAL -BEGIN - BYTES_AHEAD(pipeFile, count, status) - checkError('Bytes Ahead 0', status) - quote_count = 0 - candidate = '' - c = '' - v = 0 - - WHILE ((count > 0) AND (c <> '}')) DO -- parser will return when it finds } - READ pipeFile(c::1) - BYTES_AHEAD(pipeFile, count, status) - checkError('Bytes Ahead inside loop', status) - - IF c = '{' THEN - READ pipeFile(c::1) - BYTES_AHEAD(pipeFile, count, status) - ENDIF - - IF ((((quote_count = 0) AND (c = '"')) OR (number))) THEN - quote_count = quote_count + 1 - ELSE - IF ((quote_count = 1) AND (c <> '"')) THEN - candidate = candidate + c - ENDIF - ELSE - IF ((quote_count = 1) AND ((c = '"') OR (c = ',')) THEN - candidate = candidate + c - ENDIF - ELSE - IF quote_count = 2 THEN - count = 0 - ENDIF - ENDIF - ENDWHILE - - -- if we are parsing a number, we go from string to real and we return the result - IF number THEN - CNV_STR_REAL(candidate, v) - return(v) - ENDIF - - RETURN (candidate) - -END parseString \ No newline at end of file diff --git a/karel/socket/configure_socket.kl b/karel/socket/configure_socket.kl new file mode 100644 index 0000000..eefb3db --- /dev/null +++ b/karel/socket/configure_socket.kl @@ -0,0 +1,57 @@ +PROGRAM CONFIGURE_SOCKET + +%NOLOCKGROUP +%NOBUSYLAMP + +%INCLUDE klevkeys + +VAR + port_string : STRING[5] + port_integer : INTEGER + + server : STRING[32] + ip : STRING[48] + protocol : STRING[8] + status_def : INTEGER + + entry : INTEGER + STATUS : INTEGER + +BEGIN + -- Server IP that robot willl connect to as client + --ip = '10.162.12.207' + ip = '127.0.0.1' + + -- Server Port + port_string = '5000' + port_integer = 5000 + + -- Use C3 Tag for this socket communication + server = '$HOSTC_CFG[3]' + + WRITE TPDISPLAY(CHR(128)) + + -- Set IP + SET_VAR(entry, '*SYSTEM*', server + '.$STRT_REMOTE', ip, STATUS) + + SET_VAR(entry, '*SYSTEM*', server + '.$STATE', 1, STATUS) + SET_VAR(entry, '*SYSTEM*', server + '.$OPER', 3, STATUS) + + -- Set Port + SET_VAR(entry, '*SYSTEM*', server + '.$PORT' , port_string, STATUS) + SET_VAR(entry, '*SYSTEM*', server + '.$SERVER_PORT' , port_integer, STATUS) + + -- Set Protocol -> Socket Messaging + protocol = 'SM' + SET_VAR(entry, '*SYSTEM*', server + '.$PROTOCOL', protocol, STATUS) + + -- Set Server Status -> DEFINED + status_def = 2 + SET_VAR(entry, '*SYSTEM*', server + '.$STATE', status_def, STATUS) + + --To apply changes reboot the robot in COLD start + + + WRITE TPDISPLAY('DONE!',CR) + +END CONFIGURE_SOCKET diff --git a/karel/socket/k_sktMsg_ini.kl b/karel/socket/k_sktMsg_ini.kl deleted file mode 100644 index 40dc37d..0000000 --- a/karel/socket/k_sktMsg_ini.kl +++ /dev/null @@ -1,59 +0,0 @@ -PROGRAM k_sktMsg_ini - -%NOLOCKGROUP -%NOBUSYLAMP - -%INCLUDE klevkeys - -VAR - portString : STRING[5] - portInteger : INTEGER - - server : STRING[32] - ip : STRING[48] - protocol : STRING[8] - statusDef : INTEGER - - entry : INTEGER - STATUS : INTEGER - -BEGIN - -- IP of your SERVER (Robot will be Client) - ip = '192.168.2.120' - --ip = '127.0.0.1' - - -- Port Computer Server is listening to - --portString = '65531' - portString = '8000' - -- Robot Communication Hub uses 65531 - portInteger = 8000 - - -- System variable for Client Settings - server = '$HOSTC_CFG[3]' - - WRITE TPDISPLAY(CHR(128)) - - -- Set IP - SET_VAR(entry, '*SYSTEM*', server + '.$STRT_REMOTE', ip, STATUS) - - SET_VAR(entry, '*SYSTEM*', server + '.$STATE', 1, STATUS) - SET_VAR(entry, '*SYSTEM*', server + '.$OPER', 3, STATUS) - - -- Set Port - SET_VAR(entry, '*SYSTEM*', server + '.$PORT' , portString, STATUS) - SET_VAR(entry, '*SYSTEM*', server + '.$SERVER_PORT' , portInteger, STATUS) - - -- Set Protocol -> Socket Messaging - protocol = 'SM' - SET_VAR(entry, '*SYSTEM*', server + '.$PROTOCOL', protocol, STATUS) - - -- Set Server Status -> DEFINED - statusDef = 2 - SET_VAR(entry, '*SYSTEM*', server + '.$STATE', statusDef, STATUS) - - --Reboot robot in COLD start - - - WRITE TPDISPLAY('DONE!',CR) - -END k_sktMsg_ini diff --git a/karel/socket/socket_utils.kl b/karel/socket/socket_utils.kl deleted file mode 100644 index 6aac99d..0000000 --- a/karel/socket/socket_utils.kl +++ /dev/null @@ -1,70 +0,0 @@ -ROUTINE realToStr(input: REAL) : STRING -VAR - buf : STRING[20] -BEGIN - CNV_REAL_STR(input,8,8, buf) - return (SUB_STR(buf, 2, 254)) -END realToStr - -ROUTINE connectServ(serverTag : STRING) -BEGIN - MSG_DISCO(serverTag, STATUS) - MSG_CONNECT(serverTag, STATUS) - - checkError('Connecting to Server', STATUS) - - -- Open File - OPEN FILE connFile('RW', serverTag) - - STATUS = IO_STATUS(connFile) - - checkError('Opening File.', STATUS) -END connectServ - -ROUTINE requestData(file_path : STRING) : INTEGER -VAR - nBytes : INTEGER - serverMsg : STRING[254] - currentPos: XYZWPREXT -BEGIN - $GROUP[1].$UTOOL = $MNUTOOL[1,1] --uTool 1 - $GROUP[1].$UFRAME = $MNUFRAME[1,1] --uFrame 1 - currentPos = CURPOS(0,0) - WRITE connFile('GET /robot_position/?format=json') - WRITE connFile('&x=', realToStr(currentPos.x)) - WRITE connFile('&y=', realToStr(currentPos.y)) - WRITE connFile('&z=', realToStr(currentPos.z)) - WRITE connFile('&w=', realToStr(currentPos.w)) - WRITE connFile('&p=', realToStr(currentPos.p)) - WRITE connFile('&r=', realToStr(currentPos.r), CR, CR) - REPEAT - BYTES_AHEAD(connFile, nBytes, STATUS) - IF STATUS <> 0 THEN - RETURN (-1) - ENDIF - DELAY 100 - UNTIL nBytes > 0 - - OPEN FILE pipeFile('RW', file_path) - STATUS = IO_STATUS(pipeFile) - checkError('Opening pipeFile', STATUS) - - WHILE nBytes > 0 DO - READ connFile(serverMsg::nBytes) - STATUS = IO_STATUS(connFile) - - checkError('I/O_Status (File)', STATUS) - - --wrapPrint(serverMsg) - WRITE pipeFile(serverMsg) - - BYTES_AHEAD(connFile, nBytes, STATUS) - IF STATUS <> 0 THEN - CLOSE FILE pipeFile - RETURN (STATUS) - ENDIF - - ENDWHILE - CLOSE FILE pipeFile - RETURN (0) -END requestData \ No newline at end of file diff --git a/karel/tp_routines/GRIPPER_CONTROL.ls b/karel/tp_routines/GRIPPER_CONTROL.ls new file mode 100644 index 0000000..ce0a592 --- /dev/null +++ b/karel/tp_routines/GRIPPER_CONTROL.ls @@ -0,0 +1,24 @@ +/PROG GRIPPER_CONTROL +/ATTR +OWNER = MNEDITOR; +COMMENT = "OR Gripper Move"; +PROG_SIZE = 1050; +CREATE = DATE 18-01-21 TIME 12:00:00; +MODIFIED = DATE 18-01-21 TIME 12:00:00; +FILE_NAME = ; +VERSION = 0; +LINE_COUNT = 30; +MEMORY_SIZE = 1310; +PROTECT = READ_WRITE; +TCD: STACK_SIZE = 0, + TASK_PRIORITY = 50, + TIME_SLICE = 0, + BUSY_LAMP_OFF = 0, + ABORT_REQUEST = 0, + PAUSE_REQUEST = 0; +DEFAULT_GROUP = *,*,*,*,*; +CONTROL_CODE = 00000000 00000000; +/MN + : !set wait flag to 0 as calling KAREL routine is not affected by it anyways; + : CALL IPL_OR_RG_MOVE(R[81], R[82], 0); +/END diff --git a/karel/tp_routines/GRIPPER_INFO.ls b/karel/tp_routines/GRIPPER_INFO.ls new file mode 100644 index 0000000..945ada6 --- /dev/null +++ b/karel/tp_routines/GRIPPER_INFO.ls @@ -0,0 +1,29 @@ +/PROG GRIPPER_INFO +/ATTR +OWNER = MNEDITOR; +COMMENT = "OR Gripper Information"; +PROG_SIZE = 1050; +CREATE = DATE 18-01-21 TIME 12:00:00; +MODIFIED = DATE 18-01-21 TIME 12:00:00; +FILE_NAME = ; +VERSION = 0; +LINE_COUNT = 30; +MEMORY_SIZE = 1310; +PROTECT = READ_WRITE; +TCD: STACK_SIZE = 0, + TASK_PRIORITY = 50, + TIME_SLICE = 0, + BUSY_LAMP_OFF = 0, + ABORT_REQUEST = 0, + PAUSE_REQUEST = 0; +DEFAULT_GROUP = *,*,*,*,*; +CONTROL_CODE = 00000000 00000000; +/MN + :CALL IPL_OR_RG_GETDATA(1,71); + :CALL IPL_OR_RG_GETDATA(2,72); + :CALL IPL_OR_RG_GETDATA(3,73); + :CALL IPL_OR_RG_GETDATA(4,74); + :CALL IPL_OR_RG_GETDATA(5,75); + :CALL IPL_OR_RG_GETDATA(6,76); + :CALL IPL_OR_RG_GETDATA(7,77); +/END diff --git a/karel/tp_routines/HS_MOTION_BACKEND.LS b/karel/tp_routines/HS_MOTION_BACKEND.LS new file mode 100644 index 0000000..3f5c591 --- /dev/null +++ b/karel/tp_routines/HS_MOTION_BACKEND.LS @@ -0,0 +1,36 @@ +/PROG HS_MOTION_BACKEND +/ATTR +OWNER = MNEDITOR; +COMMENT = "Motion using reg"; +PROG_SIZE = 2064; +CREATE = DATE 17-10-19 TIME 13:19:58; +MODIFIED = DATE 17-10-20 TIME 11:30:06; +FILE_NAME = ; +VERSION = 0; +LINE_COUNT = 89; +MEMORY_SIZE = 2468; +PROTECT = READ_WRITE; +TCD: STACK_SIZE = 0, + TASK_PRIORITY = 50, + TIME_SLICE = 0, + BUSY_LAMP_OFF = 0, + ABORT_REQUEST = 0, + PAUSE_REQUEST = 0; +DEFAULT_GROUP = 1,*,*,*,*; +CONTROL_CODE = 00000000 00000000; +/MN + : ; + : LBL[1]; + : !cache karel pos in temp preg ; + : PR[2:pos cache]=PR[41:pos] ; + : R[7] = 0; + : !track number of distinct motion instructions; + : R[8] = R[8] + 1; + : !move to point with joint optimized trajectory; + : J PR[2] 100% CNT100 ; + : IF R[7] <> 0, JMP LBL[1]; + : WAIT 0.05sec; + : JMP LBL[1]; + : ABORT ; + : ;/POS +/END diff --git a/karel/tp_routines/MOTION_BACKEND.LS b/karel/tp_routines/MOTION_BACKEND.LS new file mode 100644 index 0000000..6680db0 --- /dev/null +++ b/karel/tp_routines/MOTION_BACKEND.LS @@ -0,0 +1,69 @@ +/PROG MOTION_BACKEND +/ATTR +OWNER = MNEDITOR; +COMMENT = "Motion using reg"; +PROG_SIZE = 2064; +CREATE = DATE 17-10-19 TIME 13:19:58; +MODIFIED = DATE 17-10-20 TIME 11:30:06; +FILE_NAME = ; +VERSION = 0; +LINE_COUNT = 89; +MEMORY_SIZE = 2468; +PROTECT = READ_WRITE; +TCD: STACK_SIZE = 0, + TASK_PRIORITY = 50, + TIME_SLICE = 0, + BUSY_LAMP_OFF = 0, + ABORT_REQUEST = 0, + PAUSE_REQUEST = 0; +DEFAULT_GROUP = 1,*,*,*,*; +CONTROL_CODE = 00000000 00000000; +/MN + : !initialize skip condition; + : SKIP CONDITION R[5:skip]<>0; + : ; + : LBL[1] ; + : ; + : !cache karel pos in temp preg ; + : PR[2:pos cache]=PR[41:pos] ; + : ; + : !check for linear or joint motion; + : IF R[6:linear motion] = 0, JMP LBL[2]; + : ; + : !move to point with linear trajectory; + : !if skip is triggered, execution will continue on the next line; + : !if skip is not triggered during motion, execution will continue on lbl 10; + : L PR[2] 2000mm/sec CNT100 Skip,LBL[1] ; + : ; + : !current traj has been aborted; + : !re enable the skip condition; + : SKIP CONDITION R[5:skip]<>0; + : ; + : !abort if reg is set to anything but 1; + : IF R[5:skip]<>1,JMP LBL[999] ; + : ; + : !skip "signal" has been consumed; + : R[5: skip] = 0 ; + : !lookup next target pos; + : JMP LBL[1] ; + : ; + : LBL[2]; + : !move to point with joint optimized trajectory; + : !if skip is triggered, execution will continue on the next line; + : !if skip is not triggered during motion, execution will continue on lbl 10; + : J PR[2] 100% CNT100 Skip,LBL[1] ; + : ; + : !current traj has been aborted; + : !re enable the skip condition; + : SKIP CONDITION R[5:skip]<>0; + : !abort if reg is set to anything but 1; + : IF R[5:skip]<>1,JMP LBL[999] ; + : !skip "signal" has been consumed; + : R[5: skip] = 0 ; + : !lookup next target pos; + : JMP LBL[1] ; + : ; + : LBL[999:abort]; + : ABORT ; + : ;/POS +/END diff --git a/karel/types.kl b/karel/types.kl deleted file mode 100644 index b370ca1..0000000 --- a/karel/types.kl +++ /dev/null @@ -1,4 +0,0 @@ -TYPE - object_position = STRUCTURE - coordinates: XYZWPR - ENDSTRUCTURE diff --git a/karel/util/file_util.kl b/karel/util/file_util.kl new file mode 100644 index 0000000..b919862 --- /dev/null +++ b/karel/util/file_util.kl @@ -0,0 +1,29 @@ +-- FILE_COPY ... Copy file contents by transmitting chunks of maximum string length (254 characters) +-- +-- Routine Signature description: +-- +-- source: FILE ... File to copy contents from +-- destination: FILE ... File to paste contents into +ROUTINE FILE_COPY(source: FILE; destination: FILE) +VAR +string_buffer: STRING[254] -- maximum string length that can be stored in a variable +n_bytes: INTEGER +STATUS: INTEGER + +BEGIN +BYTES_AHEAD(source, n_bytes, STATUS) -- initialize nBytes + +WHILE n_bytes > 0 DO + READ source (string_buffer::n_bytes) -- read min(nBytes, 254) characters from source file + + STATUS = IO_STATUS(source) + + WRITE destination(string_buffer) + + BYTES_AHEAD(source, n_bytes, STATUS) -- update minBytes + -- interpret scanning error as end of file (as source could be e.g. closed socket) + IF STATUS <> 0 THEN + n_bytes = 0 + ENDIF +ENDWHILE +END FILE_COPY \ No newline at end of file diff --git a/karel/util/generic_util.kl b/karel/util/generic_util.kl new file mode 100644 index 0000000..bad6e10 --- /dev/null +++ b/karel/util/generic_util.kl @@ -0,0 +1,90 @@ +-- BOOL_TO_STR ... Converts a BOOLEAN value to a STRING +-- +-- Routine Signature description: +-- +-- input_bool: BOOLEAN ... Input variable +-- RETURN: STRING ... {"0";"1"} +ROUTINE BOOL_TO_STR(input_bool: BOOLEAN): STRING +BEGIN + -- serialize booleans as c-like integers (can be casted in most languages) + IF(input_bool) THEN + RETURN ('1') + ELSE + RETURN ('0') + ENDIF +END BOOL_TO_STR + + +-- STR_TO_BOOL ... Convert a STRING to a BOOLEAN value +-- +-- Routine Signature description: +-- +-- input_str: STRING ... Input variable +-- RETURN: BOOLEAN ... {0;1} +ROUTINE STR_TO_BOOL(input_str: STRING): BOOLEAN +BEGIN + IF((STR_LEN(input_str) = 1) AND (input_str = '1')) THEN + RETURN (TRUE) + ELSE + -- all invalid input strings are treated as false + RETURN (FALSE) + ENDIF +END STR_TO_BOOL + + +-- REAL_TO_STR ... Convert a REAL value to a STRING, abstracts built in KAREL library function +-- +-- Routine Signature description: +-- +-- input_real: REAL ... Input variable +-- RETURN: STRING ... Float string representation +ROUTINE REAL_TO_STR(input_real: REAL) : STRING +VAR + buffer : STRING[16] +BEGIN + CNV_REAL_STR(input_real,8,8, buffer) + -- remove leading blank + RETURN (SUB_STR(buffer, 2, 254)) +END REAL_TO_STR + + +-- REAL_ALMOST_EQUAL ... Compares two REAL values for equality within a specified error margin +-- +-- Routine Signature description: +-- +-- first_real: REAL ... First operand +-- second_real: REAL ... Second operand +-- margin: REAL ... Error Margin that results in TRUE +-- RETURN: BOOLEAN ... Equality within specified margin +ROUTINE REAL_ALMOST_EQUAL(first_real: REAL; second_real: REAL; margin: REAL): BOOLEAN +BEGIN + IF ((ABS(first_real - second_real)) <= margin) THEN + RETURN (TRUE) + ELSE + RETURN (FALSE) + ENDIF +END REAL_ALMOST_EQUAL + + +-- XYZWPR_ALMOST_EQUAL ... Compares two XYZWPR values for equality within specified component error margins +-- +-- Routine Signature description: +-- +-- first_pos: XYZWPR ... First operand +-- second_pos: XYZWPR ... Second operand +-- dist_margin: REAL ... Error margin for position components (XYZ) +-- rot_margin: REAL ... Error margin for orientation components (WPR) +-- RETURN: BOOLEAN ... Equality within specified margins +ROUTINE XYZWPR_ALMOST_EQUAL(first_pos: XYZWPR; second_pos: XYZWPR; dist_margin: REAL; rot_margin: REAL): BOOLEAN +BEGIN + IF( REAL_ALMOST_EQUAL(first_pos.x, second_pos.x, dist_margin) AND + REAL_ALMOST_EQUAL(first_pos.y, second_pos.y, dist_margin) AND + REAL_ALMOST_EQUAL(first_pos.z, second_pos.z, dist_margin) AND + REAL_ALMOST_EQUAL(first_pos.w, second_pos.w, rot_margin) AND + REAL_ALMOST_EQUAL(first_pos.p, second_pos.p, rot_margin) AND + REAL_ALMOST_EQUAL(first_pos.r, second_pos.r, rot_margin))THEN + RETURN (TRUE) + ELSE + RETURN(FALSE) + ENDIF +END XYZWPR_ALMOST_EQUAL \ No newline at end of file diff --git a/karel/util/http_client_util.kl b/karel/util/http_client_util.kl new file mode 100644 index 0000000..7e7cb69 --- /dev/null +++ b/karel/util/http_client_util.kl @@ -0,0 +1,54 @@ +-- MAKE_HTTP_REQUEST ... Abstracts a HTTP request to a registered server tag +-- +-- Routine Signature description: +-- +-- call_string: STRING ... Full HTTP request as string (use BUILD_PARAM_REQUEST to abstract correct syntax building) +-- server_tag : STRING ... KAREL tag to identify system configuration of the socket over which the request shall be transmitted +-- socket_file: FILE ... File that represents the open socket connection +-- output_file: FILE ... File that stores the HTTP response +-- output_path: STRING ... Path to initialize output file +ROUTINE MAKE_HTTP_REQUEST(call_string: STRING; server_tag : STRING; socket_file: FILE; output_file: FILE; output_path: STRING) +BEGIN + CONNECT_TO_SERVER(server_tag, socket_file) -- establish connection + WRITE socket_file(call_string, CR, CR) -- write and flush + OPEN FILE output_file('RW', output_path) + + AWAIT_SOCKET(socket_file, 1) -- active polling + FILE_COPY(socket_file, output_file) -- socketFile -> outputFile + DISCONNECT_FROM_SERVER(server_tag, socket_file) -- cut connection + CLOSE FILE output_file -- close file so it can be reopened with proper modifiers by consumers +END MAKE_HTTP_REQUEST + + +-- BUILD_PARAM_REQUEST ... Helper to build a valid HTTP request +-- +-- Routine Signature description: +-- +-- request_type: STRING ... GET / PUT / POST / DELETE +-- endpoint: STRING ... Name of the endpoint on the queried server (without "/" prefix) +-- keys: ARRAY[*] OF STRING ... HTTP reuqest parameter keys +-- values: ARRAY[*] OF STRING ... HTTP request parameter values (serialized to strings) +-- RETURN: STRING ... Full HTTP request as string +ROUTINE BUILD_PARAM_REQUEST(request_type: STRING; endpoint: STRING; keys: ARRAY[*] OF STRING; values: ARRAY[*] OF STRING): STRING +VAR + request: STRING[254] + max_index: INTEGER + i: INTEGER + +BEGIN + request = request_type + ' /' + endpoint + '?' + + -- infer highest iteration index from smaller array + IF(ARRAY_LEN(keys) < ARRAY_LEN(values)) THEN + max_index = ARRAY_LEN(keys) + ELSE + max_index = ARRAY_LEN(values) + ENDIF + +FOR i = 1 TO( max_index-1) DO + request = request + keys[i] + '=' + values[i] + '&' +ENDFOR +request = request + keys[max_index] + '=' + values[max_index] + +RETURN (request) +END BUILD_PARAM_REQUEST \ No newline at end of file diff --git a/karel/util/log_util.kl b/karel/util/log_util.kl new file mode 100644 index 0000000..3156d76 --- /dev/null +++ b/karel/util/log_util.kl @@ -0,0 +1,33 @@ +-- log error to file but continue execution +-- REQUIRES variable 'LOG' to be declared & initialized +ROUTINE logError(description : STRING; STATUS : INTEGER) +BEGIN + IF STATUS <> 0 THEN + WRITE LOG ('ERROR: ', description, '; STATUS: ', STATUS, CR) + ENDIF +END logError + +-- terminate programm after logging error +ROUTINE catchError(description : STRING; STATUS : INTEGER) +BEGIN + IF STATUS <> 0 THEN + WRITE LOG ('ERROR: ', description, '; STATUS: ', STATUS, CR) + ABORT --terminate + ENDIF +END catchError + +-- REQUIRES variable 'LOG' to be declared +ROUTINE initLog(filePath: STRING) +BEGIN +OPEN FILE LOG ('RW', filePath) +IF IO_STATUS(LOG) <> 0 THEN + WRITE('ERROR: Could not open LOG file ; STATUS: ', IO_STATUS(LOG), CR) --write to console + ABORT +ENDIF +END initLOG + +-- shorthand to write arbitrary text to logfile +ROUTINE logText(text: STRING) +BEGIN +WRITE LOG(text, CR) +END logText diff --git a/karel/util/socket_util.kl b/karel/util/socket_util.kl new file mode 100644 index 0000000..4103501 --- /dev/null +++ b/karel/util/socket_util.kl @@ -0,0 +1,56 @@ +-- CONNECT_TO_SERVER ... Establish connection to a server, abstracts KAREL library function +-- +-- Routine Signature description: +-- +-- connection_tag : STRING ... KAREL tag to identify system configuration of the socket +-- socket_file: FILE ... File to handle the open server connection +ROUTINE CONNECT_TO_SERVER(connection_tag : STRING; socket_file: FILE) + +VAR + STATUS: INTEGER + +BEGIN + MSG_CONNECT(connection_tag, STATUS) + + OPEN FILE socket_file('RW', connection_tag) + STATUS = IO_STATUS(socket_file) + +END CONNECT_TO_SERVER + + +-- DISCONNECT_FROM_SERVER ... Close connection to a server, abstracts KAREL library function +-- +-- Routine Signature description: +-- +-- connection_tag : STRING ... KAREL tag to identify system configuration of the socket +-- socket_file: FILE ... File to handle the open server connection +ROUTINE DISCONNECT_FROM_SERVER(connection_tag : STRING; socket_file: FILE) + +VAR + STATUS: INTEGER + +BEGIN + CLOSE FILE socket_file --close file first, otherwise msg_disco will abort + MSG_DISCO(connection_tag, STATUS) -- can fail if no current connection, don't catch error + +END DISCONNECT_FROM_SERVER + + +-- AWAIT_SOCKET ... Active wait until data is received on this socket connection +-- +-- Routine Signature description: +-- +-- socket_file: FILE ... File to handle the open server connection +-- pollrate_ms: INTEGER ... Rate in ms at which routine will poll the socket for new data +ROUTINE AWAIT_SOCKET(socket_file: FILE; pollrate_ms: INTEGER) +VAR +n_bytes: INTEGER +STATUS: INTEGER + +BEGIN +REPEAT + BYTES_AHEAD(socket_file, n_bytes, STATUS) + DELAY pollrate_ms -- sleep for x ms +UNTIL n_bytes > 0 + +END AWAIT_SOCKET diff --git a/karel/util/stopwatch_util.kl b/karel/util/stopwatch_util.kl new file mode 100644 index 0000000..13e09d5 --- /dev/null +++ b/karel/util/stopwatch_util.kl @@ -0,0 +1,14 @@ +-- saves the starting time for future use +ROUTINE START_TIME(time_store : INTEGER) +BEGIN + time_store = GET_USEC_TIM +END START_TIME + +-- gets the elapsed delta from timeStore to now, overwrites time_store and returns it +ROUTINE STOP_TIME(time_store : INTEGER) : INTEGER +BEGIN + time_store = GET_USEC_SUB(GET_USEC_TIM, time_store) + RETURN (time_store) +END STOP_TIME + + diff --git a/karel/xml/obj_parser.kl b/karel/xml/obj_parser.kl new file mode 100644 index 0000000..8849ff0 --- /dev/null +++ b/karel/xml/obj_parser.kl @@ -0,0 +1,103 @@ +-- PARSE_OBJ_LIST ... Parse a list of type object_t from XML +-- +-- Routine Signature description: +-- +-- xml_file: FILE ... File Variable +-- xml_path: STRING ... File path, needed to open file +-- entry_buffer: picking_object_t ... Global buffer variable, needed for XML_SCAN subroutine +-- buffer_name: STRING ... Name of global buffer variable in main program, needed for XML_SCAN subroutine +-- prog_name: STRING ... Name of main program, needed for XML_SCAN subroutine +-- entry_array: ARRAY[*] OF picking_object_t ... Target array to parse file content into +-- RETURN: INTEGER ... Number of entries that were filled into target array +ROUTINE PARSE_OBJ_LIST( +xml_file: FILE; + xml_path: STRING; +entry_buffer: picking_object_t; +buffer_name: STRING; +prog_name: STRING; +entry_array: ARRAY[*] OF picking_object_t) : INTEGER + +CONST + -- preceding tag IDs are reserved for system + LIST_TAG = 3 + ENTRY_TAG = 4 + + LIST_KEY = 'obj_list' + ENTRY_KEY = 'obj' + + MAX_TAG_LEN = 16 + +VAR + tag_id: INTEGER -- id of last scanned tag + tag_string: STRING[MAX_TAG_LEN] -- last scanned tagname + func_code: INTEGER -- tag type of last scan + + list_start: BOOLEAN + list_end: BOOLEAN + + next_index: INTEGER + max_index: INTEGER + +BEGIN + + -- close and repoen as read only after setting xml attr +CLOSE FILE xml_file +SET_FILE_ATR (xml_file, ATR_XML) +CLR_IO_STAT(xml_file) +OPEN FILE xml_file('RO', xml_path) +STATUS = IO_STATUS(xml_file) + +-- register tags to be detected by scan +-- Method Signature: +-- XML_ADDTAG(FILE, 'TAG_NAME', TAG_LEN, CASE_SENSITIVE, TAG_ID, STATUS) +XML_ADDTAG(xml_file, LIST_KEY, MAX_TAG_LEN, FALSE, LIST_TAG, STATUS) +XML_ADDTAG(xml_file, ENTRY_KEY, MAX_TAG_LEN, FALSE,ENTRY_TAG, STATUS) + +-- initialize vars +list_start = FALSE +list_end = FALSE +next_index = 1 +max_index = ARRAY_LEN(entry_array) + +-- scan until list start tag is found +WHILE (list_start = FALSE) DO + XML_SCAN(xml_file, tag_string, tag_id, func_code, STATUS) + IF ( STATUS = 0) THEN + CLOSE FILE xml_file + RETURN (0) + ENDIF + IF (STATUS = XML_FUNCTION) THEN -- scanned a tag + IF (tag_id = LIST_TAG) THEN + list_start = TRUE + ELSE + ENDIF + ELSE + ENDIF +ENDWHILE + +-- scan & add list elements until end tag is found +WHILE ((list_end = FALSE) AND (next_index <= max_index)) DO + XML_SCAN(xml_file, tag_string, tag_id, func_code, STATUS) + IF ( STATUS = 0) THEN + CLOSE FILE xml_file + RETURN (0) + ENDIF + IF (STATUS = XML_FUNCTION) THEN -- scanned a tag + SELECT tag_id OF + CASE(LIST_TAG): + list_end = TRUE + CASE(ENTRY_TAG): + -- Method Signature: + -- XML_SETVAR(FILE, 'PROGRAM_NAME', 'VAR_NAME', STATUS) + XML_SETVAR(xml_file, prog_name, buffer_name, STATUS) + entry_array[next_index] = entry_buffer + next_index = next_index + 1 + ENDSELECT + ELSE + ENDIF +ENDWHILE + +CLOSE FILE xml_file +-- return how many entries were actually filled +RETURN (next_index-1) +END PARSE_OBJ_LIST diff --git a/karel/xml/pp_parser.kl b/karel/xml/pp_parser.kl new file mode 100644 index 0000000..d31056f --- /dev/null +++ b/karel/xml/pp_parser.kl @@ -0,0 +1,177 @@ +-- PARSE_PP_LIST ... Parse a list of objects of type picking_pose_t from XML +-- +-- Routine Signature description: +-- +-- xml_file: FILE ... File Variable +-- xml_path: STRING ... File path, needed to open file +-- entry_buffer: picking_pose_t ... Global buffer variable, needed for XML_SCAN subroutine +-- buffer_name: STRING ... Name of global buffer variable in main program, needed for XML_SCAN subroutine +-- prog_name: STRING ... Name of main program, needed for XML_SCAN subroutine +-- entry_array: ARRAY[*] OF picking_pose_t ... Target array to parse file content into +-- RETURN: INTEGER ... Number of entries that were filled into target array +ROUTINE PARSE_PP_LIST( +xml_file: FILE; + xml_path: STRING; +entry_buffer: picking_pose_t; +buffer_name: STRING; +prog_name: STRING; +entry_array: ARRAY[*] OF picking_pose_t) : INTEGER + +CONST + -- preceding tag IDs are reserved for system + LIST_TAG = 3 + ENTRY_TAG = 4 + + LIST_KEY = 'pp_list' + ENTRY_KEY = 'pp' + + MAX_TAG_LEN = 16 + +VAR + tag_id: INTEGER -- id of last scanned tag + tag_string: STRING[MAX_TAG_LEN] -- last scanned tagname + func_code: INTEGER -- tag type of last scan + + list_start: BOOLEAN + list_end: BOOLEAN + + next_index: INTEGER + max_index: INTEGER + +BEGIN + + -- close and repoen as read only after setting xml attr +CLOSE FILE xml_file +SET_FILE_ATR (xml_file, ATR_XML) +CLR_IO_STAT(xml_file) +OPEN FILE xml_file('RO', xml_path) +STATUS = IO_STATUS(xml_file) + +-- register tags to be detected by scan +-- Method Signature: +-- XML_ADDTAG(FILE, 'TAG_NAME', TAG_LEN, CASE_SENSITIVE, TAG_ID, STATUS) +XML_ADDTAG(xml_file, LIST_KEY, MAX_TAG_LEN, FALSE, LIST_TAG, STATUS) +XML_ADDTAG(xml_file, ENTRY_KEY, MAX_TAG_LEN, FALSE,ENTRY_TAG, STATUS) + +-- initialize vars +list_start = FALSE +list_end = FALSE +next_index = 1 +max_index = ARRAY_LEN(entry_array) + +-- scan until list start tag is found +WHILE (list_start = FALSE) DO + XML_SCAN(xml_file, tag_string, tag_id, func_code, STATUS) + IF ( STATUS = 0) THEN + CLOSE FILE xml_file + RETURN (0) + ENDIF + IF (STATUS = XML_FUNCTION) THEN -- scanned a tag + IF (tag_id = LIST_TAG) THEN + list_start = TRUE + ELSE + ENDIF + ELSE + ENDIF +ENDWHILE + +-- scan & add list elements until end tag is found +WHILE ((list_end = FALSE) AND (next_index <= max_index)) DO + XML_SCAN(xml_file, tag_string, tag_id, func_code, STATUS) + IF ( STATUS = 0) THEN + CLOSE FILE xml_file + RETURN (0) + ENDIF + IF (STATUS = XML_FUNCTION) THEN -- scanned a tag + SELECT tag_id OF + CASE(LIST_TAG): + list_end = TRUE + CASE(ENTRY_TAG): + -- Method Signature: + -- XML_SETVAR(FILE, 'PROGRAM_NAME', 'VAR_NAME', STATUS) + XML_SETVAR(xml_file, prog_name, buffer_name, STATUS) + entry_array[next_index] = entry_buffer + next_index = next_index + 1 + ENDSELECT + ELSE + ENDIF +ENDWHILE + +CLOSE FILE xml_file +-- return how many entries were actually filled +RETURN (next_index-1) +END PARSE_PP_LIST + + +-- PARSE_PP_SINGLE ... Parse a single object of type picking_pose_t from XML +-- +-- Routine Signature description: +-- +-- xml_file: FILE ... File Variable +-- xml_path: STRING ... File path, needed to open file +-- entry_buffer: picking_pose_t ... Global buffer variable, needed for XML_SCAN subroutine +-- buffer_name: STRING ... Name of global buffer variable in main program, needed for XML_SCAN subroutine +-- prog_name: STRING ... Name of main program, needed for XML_SCAN subroutine +-- RETURN: BOOLEAN ... Flag that is true if an entry was written into the buffer variable +ROUTINE PARSE_PP_SINGLE( +xml_file: FILE; + xml_path: STRING; +entry_buffer: picking_pose_t; +buffer_name: STRING; +prog_name: STRING): BOOLEAN + +CONST + -- use same tag number as list parser + ENTRY_TAG = 4 + + ENTRY_KEY = 'pp' + + MAX_TAG_LEN = 16 + +VAR + tag_id: INTEGER -- id of last scanned tag + tag_string: STRING[MAX_TAG_LEN] -- last scanned tagname + func_code: INTEGER -- tag type of last scan + scan_end: BOOLEAN + +BEGIN + + -- close and repoen as read only after setting xml attr +CLOSE FILE xml_file +SET_FILE_ATR (xml_file, ATR_XML) +CLR_IO_STAT(xml_file) +OPEN FILE xml_file('RO', xml_path) +STATUS = IO_STATUS(xml_file) + +-- register tags to be detected by scan +-- Method Signature: +-- XML_ADDTAG(FILE, 'TAG_NAME', TAG_LEN, CASE_SENSITIVE, TAG_ID, STATUS) +XML_ADDTAG(xml_file, ENTRY_KEY, MAX_TAG_LEN, FALSE,ENTRY_TAG, STATUS) + +-- initialize vars +scan_end = FALSE + +-- scan & add one entry +WHILE (scan_end = FALSE) DO +XML_SCAN(xml_file, tag_string, tag_id, func_code, STATUS) +IF ( STATUS = 0) THEN + CLOSE FILE xml_file + -- return if entry was actually filled + RETURN (FALSE) +ENDIF +IF (STATUS = XML_FUNCTION) THEN -- scanned a tag + SELECT tag_id OF + CASE(ENTRY_TAG): + -- Method Signature: + -- XML_SETVAR(FILE, 'PROGRAM_NAME', 'VAR_NAME', STATUS) + XML_SETVAR(xml_file, prog_name, buffer_name, STATUS) + scan_end = TRUE + ENDSELECT +ELSE +ENDIF +ENDWHILE + +CLOSE FILE xml_file +-- return if entry was actually filled +RETURN (TRUE) +END PARSE_PP_SINGLE diff --git a/karel/xml/xml_types.kl b/karel/xml/xml_types.kl new file mode 100644 index 0000000..fda9a11 --- /dev/null +++ b/karel/xml/xml_types.kl @@ -0,0 +1,26 @@ +TYPE + + picking_object_t = STRUCTURE + id: STRING[32] + x: REAL + y: REAL + bb_x: REAL + bb_y: REAL + ENDSTRUCTURE + + picking_pose_t = STRUCTURE + id: STRING[32] + x: REAL --mm + y: REAL --mm + z: REAL --mm + w: REAL --deg + p: REAL --deg + r: REAL --deg + rot_diff: REAL + ENDSTRUCTURE + + + + + + \ No newline at end of file diff --git a/main.py b/main.py deleted file mode 100644 index c5530c7..0000000 --- a/main.py +++ /dev/null @@ -1,41 +0,0 @@ -import argparse -import logging -import sys - -from io import StringIO -from driver.djangofrontend import DjangoFrontEnd -from driver.fanucfrontend import FANUCFrontEnd - -def main(): - - # Setup debug logging - logger = logging.getLogger() - logger.propagate = False - logger.level = logging.DEBUG - stream_handler = logging.StreamHandler(sys.stdout) - stream_handler.addFilter(lambda r: r.name == "root") - logger.addHandler(stream_handler) - - # Parse arguments - # parser = argparse.ArgumentParser(description='TUM Chair of Cyber Physical Systems in Production Engineering FANUC Robot Driver') - # parser.add_argument('--robot_endpoint', required=True, type=str, help='http endpoint of the robot') - # parser.add_argument('--django_endpoint', required=True, type=str, help='http endpoint of the django server') - # args = parser.parse_args() - - # Instantiate drivers - fanucFrontEnd = FANUCFrontEnd("http://10.162.12.191") - djangoFrontEnd = DjangoFrontEnd("http://127.0.0.1:8000") - - # Begin motion - commands = djangoFrontEnd.make_request() - fanucFrontEnd.start_motion() - for command in commands: - # Move to command position in absolute cartesian motion with joint interpolation - success = fanucFrontEnd.send_motion_request(command['x'], command['y'], command['z'], command['phi'], command['theta'], command['psi'], 'lca') - djangoFrontEnd.send_response(command['id'], success) - - fanucFrontEnd.stop_motion() - - -if __name__ == "__main__": - main() diff --git a/python/API_SERVER.py b/python/API_SERVER.py new file mode 100644 index 0000000..52458eb --- /dev/null +++ b/python/API_SERVER.py @@ -0,0 +1,263 @@ +# Webserver +import random +import time + +import numpy as np +from flask import Flask, Response +from flask_restx import Resource, Api, reqparse +import xml.etree.ElementTree as ET + +# Instantiate flask app and corresponding REST Api +app = Flask(__name__) + +api = Api(app, + default='FieldView Karel API', + default_label='An API to fetch FieldView Data from Karel programs', + default_mediatype='application/xml', + # prefix API calls + prefix='', + # keep doc on root url + doc='/') + +PO_KEY = "obj" +PP_KEY = "pp" + +PO_LIST_KEY = "obj_list" +PP_LIST_KEY = "pp_list" + + +PO_QUALITY_KEYS = ["bb_x", "bb_y"] +PP_QUALITY_KEYS = ["rot_diff", "dist"] + +error_response = Response(ET.tostring(ET.Element("error"), encoding="ascii", method="xml"), mimetype='text/xml') + +fake_target_points = [np.array([600, -300, 300, -170, -50, 0]), np.array([600, -300, 700, -170, -50, -50])] +current_idx = 0 + + +class PickingObject: + def __init__(self, object_id, position_2d: np.ndarray, quality: np.ndarray): + self.object_id = str(object_id) + self.x = float(position_2d[0]) + self.y = float(position_2d[1]) + self.quality = quality + + def generate_xml_params(self): + params = {'id': self.object_id, + 'x': str(self.x), + 'y': str(self.y) + } + n_quality = min(len(PO_QUALITY_KEYS), len(self.quality)) + for i in range(n_quality): + params[PO_QUALITY_KEYS[i]] = str(self.quality[i]) + return params + + def add_to_xml(self, parent): + ET.SubElement(parent, PO_KEY, self.generate_xml_params()) + + +class PickingPoint: + def __init__(self, picking_point_id, pose_6d: np.ndarray, quality: np.ndarray): + self.picking_point_id = str(picking_point_id) + self.x = float(pose_6d[0]) + self.y = float(pose_6d[1]) + self.z = float(pose_6d[2]) + self.w = float(pose_6d[3]) + self.p = float(pose_6d[4]) + self.r = float(pose_6d[5]) + self.quality = quality + + def generate_xml_params(self): + params = {'id': self.picking_point_id, + 'x': str(self.x), + 'y': str(self.y), + 'z': str(self.z), + 'w': str(self.w), + 'p': str(self.p), + 'r': str(self.r)} + n_quality = min(len(PP_QUALITY_KEYS), len(self.quality)) + for i in range(n_quality): + params[PP_QUALITY_KEYS[i]] = str(self.quality[i]) + return params + + def add_to_xml(self, parent): + ET.SubElement(parent, PP_KEY, self.generate_xml_params()) + + def make_xml(self): + return ET.Element(PP_KEY, self.generate_xml_params()) + + +def serialize_picking_objects_xml(picking_objects: [PickingObject]): + root = ET.Element(PO_LIST_KEY) + for po in picking_objects: + po.add_to_xml(root) + return ET.tostring(root, encoding="ascii", method="xml") + + +def serialize_picking_points_xml(picking_points: [PickingPoint]): + root = ET.Element(PP_LIST_KEY) + for pp in picking_points: + pp.add_to_xml(root) + return ET.tostring(root, encoding="ascii", method="xml") + + +def calc_degree_delta(*, alpha, beta): + delta = abs(alpha - beta) + return delta if delta < 180 else 360 - delta + + +def calc_rotation_delta(*, current, target): + total_delta = 0 + + # check if same dimensions + if len(current) == len(target): + # add all angle differences + for i in range(len(current)): + total_delta += calc_degree_delta(alpha=current[i], beta=target[i]) + + return total_delta + +def calc_distance(*, current, target): + distance = 0 + + # check if same dimensions + if len(current) == 3 and len(target) == 3: + distance = np.linalg.norm(np.array(target)-np.array(current)) + + return distance + + +# -------------------------------------------------------------------------- + +@api.route('/objects') +class ObjectsEndpoint(Resource): + # HTTP GET + def get(self): + + pos_2d = np.array([2, 1]) + bounding_box = np.array([1,1]) + + objects = [PickingObject(str(i), pos_2d, bounding_box) for i in range(5)] + + return Response(serialize_picking_objects_xml(objects), mimetype='application/xml') + +# -------------------------------------------------------------------------- + +@api.route('/picking-poses') +class PickingPointsEndpoint(Resource): + parser = reqparse.RequestParser() + parser.add_argument('obj_id', type=str, required=True, help='ID of object for which picking points should be provided') + parser.add_argument('relative', type=int, required=True, help='{0,1} Flag to decide on relative/absolute position') + parser.add_argument('x', type=float, help='TCP Position (assumed 0 if missing)') + parser.add_argument('y', type=float, help='TCP Position (assumed 0 if missing)') + parser.add_argument('z', type=float, help='TCP Position (assumed 0 if missing)') + parser.add_argument('w', type=float, help='TCP Orientation (assumed 0 if missing)') + parser.add_argument('p', type=float, help='TCP Orientation (assumed 0 if missing)') + parser.add_argument('r', type=float, help='TCP Orientation (assumed 0 if missing)') + + # HTTP GET + @api.expect(parser) + def get(self): + global fake_target_points + global current_idx + + # Parse arguments + parser = reqparse.RequestParser() + parser.add_argument('obj_id', type=str, required=True) + parser.add_argument('relative', type=int, required=True) + parser.add_argument('x', type=float) + parser.add_argument('y', type=float) + parser.add_argument('z', type=float) + parser.add_argument('w', type=float) + parser.add_argument('p', type=float) + parser.add_argument('r', type=float) + args = parser.parse_args() + + # new target each pp call + + if current_idx == 0: + current_idx = 1 + else: + current_idx = 0 + + pose_6d = fake_target_points[current_idx] + + offset_6d = np.array([0, 0, 0, 0, 0, 0]) + + + if args['relative']: + # if parameter relative is set, the 6d parameters are assumed to be present + offset_6d = np.array([ + args['x'], + args['y'], + args['z'], + args['w'], + args['p'], + args['r']]) + + rotation_delta = calc_rotation_delta(current=pose_6d[3:], target=offset_6d[3:]) + distance = calc_distance(current=pose_6d[:3], target=offset_6d[:3]) + corrected_pose_6d = pose_6d - offset_6d + + pps = [PickingPoint(str(i), corrected_pose_6d, [rotation_delta, distance]) for i in range(5)] + + return Response(serialize_picking_points_xml(pps), mimetype='application/xml') + +# -------------------------------------------------------------------------- + +@api.route('/track-picking-pose') +class TrackPickingPointEndpoint(Resource): + parser = reqparse.RequestParser() + parser.add_argument('pp_id', type=str, required=True, help='ID of picking point that should be tracked') + parser.add_argument('relative', type=int, required=True, help='{0,1} Flag to decide on relative/absolute position') + parser.add_argument('x', type=float, help='TCP Position (assumed 0 if missing)') + parser.add_argument('y', type=float, help='TCP Position (assumed 0 if missing)') + parser.add_argument('z', type=float, help='TCP Position (assumed 0 if missing)') + parser.add_argument('w', type=float, help='TCP Orientation (assumed 0 if missing)') + parser.add_argument('p', type=float, help='TCP Orientation (assumed 0 if missing)') + parser.add_argument('r', type=float, help='TCP Orientation (assumed 0 if missing)') + + # HTTP GET + @api.expect(parser) + def get(self): + global fake_target_points + global current_idx + # Parse arguments + args = self.parser.parse_args() + + pose_6d = fake_target_points[current_idx] + + # simulate pose estimation jitter + dist_scaling = 5 + rot_scaling = 5 + pose_6d[0] += (random.random() - 0.5) * dist_scaling + pose_6d[1] += (random.random() - 0.5) * dist_scaling + pose_6d[2] += (random.random() - 0.5) * dist_scaling + pose_6d[3] += (random.random() - 0.5) * rot_scaling + pose_6d[4] += (random.random() - 0.5) * rot_scaling + pose_6d[5] += (random.random() - 0.5) * rot_scaling + + offset_6d = np.array([0, 0, 0, 0, 0, 0]) + + if args['relative']: + # if parameter relative is set, the 6d parameters are assumed to be present + offset_6d = np.array([ + args['x'], + args['y'], + args['z'], + args['w'], + args['p'], + args['r']]) + + rotation_delta = calc_rotation_delta(current=pose_6d[3:], target=offset_6d[3:]) + distance = calc_distance(current=pose_6d[:3], target=offset_6d[:3]) + corrected_pose_6d = pose_6d - offset_6d + + pp = PickingPoint(str(0), corrected_pose_6d, [rotation_delta, distance]) + + return Response(ET.tostring(pp.make_xml(), encoding="ascii", method="xml"), mimetype='application/xml') + #return error_response + +if __name__ == '__main__': + # Host on 0.0.0.0 (all local addresses) + app.run(host='0.0.0.0', port=5000, debug=False) diff --git a/python/HTTP_CLIENT.py b/python/HTTP_CLIENT.py new file mode 100644 index 0000000..f2fc8f9 --- /dev/null +++ b/python/HTTP_CLIENT.py @@ -0,0 +1,17 @@ +import requests +import time + +# api-endpoint +URL = "http://127.0.0.1/KAREL/rsiposition" + +# defining a params dict for the parameters to be sent to the API +PARAMS = {} + +start = time.perf_counter() +# sending get request and saving the response as response object +r = requests.get(url=URL, params=PARAMS) +# extracting data in json format +data = r.json() +print(f"Completed Execution in {(time.perf_counter() - start) * 1000} ms") + +print(data) \ No newline at end of file diff --git a/readme-images/add.png b/readme-images/add.png deleted file mode 100644 index ae6296a..0000000 Binary files a/readme-images/add.png and /dev/null differ diff --git a/readme-images/files.png b/readme-images/files.png deleted file mode 100644 index f675379..0000000 Binary files a/readme-images/files.png and /dev/null differ diff --git a/readme-images/jja.png b/readme-images/jja.png deleted file mode 100644 index 6b61b85..0000000 Binary files a/readme-images/jja.png and /dev/null differ diff --git a/readme-images/lca.png b/readme-images/lca.png deleted file mode 100644 index 89e1423..0000000 Binary files a/readme-images/lca.png and /dev/null differ diff --git a/readme-images/verify.png b/readme-images/verify.png deleted file mode 100644 index 53fc7be..0000000 Binary files a/readme-images/verify.png and /dev/null differ diff --git a/readme-images/wideview.png b/readme-images/wideview.png deleted file mode 100644 index d85af92..0000000 Binary files a/readme-images/wideview.png and /dev/null differ diff --git a/requirements.txt b/requirements.txt deleted file mode 100644 index edf6a7d..0000000 --- a/requirements.txt +++ /dev/null @@ -1,2 +0,0 @@ -requests -json \ No newline at end of file diff --git a/server.py b/server.py deleted file mode 100644 index d2dad5b..0000000 --- a/server.py +++ /dev/null @@ -1,16 +0,0 @@ -# Echo server program -import socket -import time - -HOST = '127.0.0.1' -PORT = 65531 -s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) -s.bind((HOST, PORT)) - -s.listen(1) -conn, addr = s.accept() -print ('Connected by', addr) - -conn.sendall('Hello, AIsmart!'.encode()) -print ('Send message') -conn.close() \ No newline at end of file diff --git a/test/__init__.py b/test/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/test/integration_test.py b/test/integration_test.py deleted file mode 100644 index 2e857d6..0000000 --- a/test/integration_test.py +++ /dev/null @@ -1,43 +0,0 @@ -import driver.fanucfrontend as ffe -import logging -import sys -import time - - -# A collection of stand alone tests to verify robot behavior - -def sandbox(): - frontend = ffe.FANUCFrontEnd("http://10.162.12.191") - frontend.reset() - status = frontend.get_status() - print(status) - response = frontend.start_motion() - print("Success?:", response) - response = frontend.send_motion_request(470, 0, -20, 0, 0, 0, "lca") - print("Success?:", response) - time.sleep(1) - response = frontend.send_motion_request(600, 0, 40, 0, 0, 0, "lca") - print("Success?:", response) - time.sleep(1) - response = frontend.send_motion_request(340, 0, 40, 0, 0, 0, "lca") - print("Success?:", response) - time.sleep(1) - response = frontend.send_motion_request(470, 0, -20, 0, 0, 0, "lca") - print("Success?:", response) - time.sleep(1) - response = frontend.stop_motion() - print("Success?:", response) - response = frontend.reset() - print("Success?:", response) - - -if __name__ == "__main__": - # Setup debug logging - logger = logging.getLogger() - logger.propagate = False - logger.level = logging.DEBUG - stream_handler = logging.StreamHandler(sys.stdout) - stream_handler.addFilter(lambda r: r.name == "root") - logger.addHandler(stream_handler) - - sandbox() diff --git a/test/test_djangofrontend.py b/test/test_djangofrontend.py deleted file mode 100644 index 9db518b..0000000 --- a/test/test_djangofrontend.py +++ /dev/null @@ -1,15 +0,0 @@ -import unittest - -from driver.djangofrontend import DjangoFrontEnd - - -class DjangoFrontEndUnitTest(unittest.TestCase): - djangofrontend = DjangoFrontEnd("http://127.0.0.1:8000") - - def test_dummy(self): - res = self.djangofrontend.make_request() - print(res) - - -if __name__ == '__main__': - unittest.main() diff --git a/test/test_fanucfrontend.py b/test/test_fanucfrontend.py deleted file mode 100644 index e200445..0000000 --- a/test/test_fanucfrontend.py +++ /dev/null @@ -1,45 +0,0 @@ -import logging -import unittest - -from driver.fanucfrontend import FANUCFrontEnd -from io import StringIO - - -# Unit tests for FanucFrontEnd to ensure http requests are properly formed -class FanucFrontEndUnitTest(unittest.TestCase): - log_stream = None - frontend = FANUCFrontEnd("http://hookbin.com") - - def setUp(self): - logger = logging.getLogger() - logger.propagate = False - logger.level = logging.DEBUG - - self.log_stream = StringIO() - string_handler = logging.StreamHandler(self.log_stream) - string_handler.addFilter(lambda r: r.name == "root") - logger.addHandler(string_handler) - - # Uncomment to have http URLs printed to terminal. Import sys to use - # stream_handler = logging.StreamHandler(sys.stdout) - # stream_handler.addFilter(lambda r: r.name == "root") - # logger.addHandler(stream_handler) - - def tearDown(self): - self.log_stream = None - - def test_begin_motion(self): - self.frontend.begin_motion() - self.assertEqual(self.log_stream.getvalue(), "https://hookbin.com/KAREL/appstart?task=motion\n") - - def test_send_motion(self): - self.frontend.send_motion_request(1,2,3,4,5,6,"jjr") - self.assertEqual(self.log_stream.getvalue(), "https://hookbin.com/KAREL/appmove?motion_t=jjr&coord1=1&coord2=2&coord3=3&coord4=4&coord5=5&coord6=6\n") - - def test_abort(self): - self.frontend.stop_motion() - self.assertEqual(self.log_stream.getvalue(), "https://hookbin.com/KAREL/appabort\n") - - -if __name__ == '__main__': - unittest.main() diff --git a/test/test_sim.py b/test/test_sim.py deleted file mode 100644 index a04813e..0000000 --- a/test/test_sim.py +++ /dev/null @@ -1,5 +0,0 @@ -from driver.simcamerafrontend import SimCameraFrontEnd - -if __name__ == '__main__': - frontend = SimCameraFrontEnd() - frontend.take_picture() \ No newline at end of file