├── Arctos pick and place.rdk ├── Arctos.py ├── Arctos.rdk ├── Arctos_RoboDK.jpg └── README.md /Arctos pick and place.rdk: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Arctos-Robotics/RoboDK/0df0e8fca603b150f3706e450852c2aef720ce09/Arctos pick and place.rdk -------------------------------------------------------------------------------- /Arctos.py: -------------------------------------------------------------------------------- 1 | # Import RoboDK tools 2 | from robodk import * 3 | 4 | def pose_2_str(pose): 5 | """Prints a pose target""" 6 | [x, y, z, r, p, w] = pose_2_xyzrpw(pose) 7 | return ('X%.3f Y%.3f Z%.3f R%.3f P%.3f W%.3f' % (x, y, z, r, p, w)) 8 | 9 | def joints_2_str(joints): 10 | """Prints a joint target""" 11 | str = '' 12 | data = ['X ', ' Y', ' Z ', ' A ', ' B ', ' C ', 'F', 'H', 'I', 'J', 'K', 'L'] 13 | for i in range(len(joints)): 14 | if i == 4: 15 | str = str + ('%s%.2f' % (data[i], -1 * joints[i])) # flip joint 4 rotation 16 | else: 17 | str = str + ('%s%.2f' % (data[i], joints[i])) 18 | return str 19 | 20 | # Object class that handles the robot instructions/syntax 21 | class RobotPost(object): 22 | """Robot post object""" 23 | PROG_EXT = 'gcode' # set the program extension 24 | 25 | # other variables 26 | ROBOT_POST = '' 27 | ROBOT_NAME = '' 28 | PROG_FILES = [] 29 | 30 | PROG = '' 31 | LOG = '' 32 | nAxes = 6 33 | 34 | def __init__(self, robotpost=None, robotname=None, robot_axes=6, **kwargs): 35 | self.ROBOT_POST = robotpost 36 | self.ROBOT_NAME = robotname 37 | self.PROG = '' 38 | self.LOG = '' 39 | self.nAxes = robot_axes 40 | 41 | def ProgStart(self, progname): 42 | self.addline('F800 (Feedrate)') 43 | 44 | def ProgFinish(self, progname): 45 | self.addline('G90 X 0 Y 0 Z 0 A 0 B 0 C 0') 46 | 47 | def ProgSave(self, folder, progname, ask_user=False, show_result=False): 48 | progname = progname + '.' + self.PROG_EXT 49 | if ask_user or not DirExists(folder): 50 | filesave = getSaveFile(folder, progname, 'Save program as...') 51 | if filesave is not None: 52 | filesave = filesave.name 53 | else: 54 | return 55 | else: 56 | filesave = folder + '/' + progname 57 | fid = open(filesave, "w") 58 | fid.write(self.PROG) 59 | fid.close() 60 | print('SAVED: %s\n' % filesave) 61 | # ---------------------- show result 62 | if show_result: 63 | if type(show_result) is str: 64 | # Open file with provided application 65 | import subprocess 66 | p = subprocess.Popen([show_result, filesave]) 67 | else: 68 | # open file with default application 69 | import os 70 | os.startfile(filesave) 71 | 72 | if len(self.LOG) > 0: 73 | mbox('Program generation LOG:\n\n' + self.LOG) 74 | 75 | def ProgSendRobot(self, robot_ip, remote_path, ftp_user, ftp_pass): 76 | ser = serial.Serial(COM10, baudrate=115200, timeout=10) 77 | 78 | def MoveJ(self, pose, joints, conf_RLF=None): 79 | """Add a joint movement""" 80 | self.addline('G90 ' + joints_2_str(joints)) 81 | 82 | def MoveL(self, pose, joints, conf_RLF=None): 83 | """Add a linear movement""" 84 | self.addline('G90 ' + joints_2_str(joints)) 85 | 86 | def MoveC(self, pose1, joints1, pose2, joints2, conf_RLF_1=None, conf_RLF_2=None): 87 | """Add a circular movement""" 88 | self.addline('G90 ' + joints_2_str(joints1)) 89 | self.addline('G90 ' + joints_2_str(joints2)) 90 | 91 | def setFrame(self, pose, frame_id=None, frame_name=None): 92 | """Change the robot reference frame""" 93 | 94 | def setTool(self, pose, tool_id=None, tool_name=None): 95 | """Change the robot TCP""" 96 | 97 | def Pause(self, time_ms): 98 | """Pause the robot program""" 99 | if time_ms < 0: 100 | self.addline('PAUSE') 101 | else: 102 | self.addline('M30T%s' % (time_ms)) 103 | 104 | def setSpeed(self, speed_mms): 105 | """Changes the robot speed (in mm/s)""" 106 | 107 | def setAcceleration(self, accel_mmss): 108 | """Changes the robot acceleration (in mm/s2)""" 109 | 110 | def setSpeedJoints(self, speed_degs): 111 | """Changes the robot joint speed (in deg/s)""" 112 | self.SPEED_RADS = speed_degs * pi / 180 113 | self.addline('M105 %.3f' % self.SPEED_RADS) 114 | 115 | def setAccelerationJoints(self, accel_degss): 116 | """Changes the robot joint acceleration (in deg/s2)""" 117 | self.ACCEL_RADSS = accel_degss * pi / 180 118 | self.addline('M110 %i' % accel_degss) 119 | 120 | def setZoneData(self, zone_mm): 121 | """Changes the rounding radius (aka CNT, APO or zone data) to make the movement smoother""" 122 | 123 | def setDO(self, io_var, io_value): 124 | """Sets a variable (digital output) to a given value""" 125 | if type(io_var) != str: 126 | io_var = 'OUT[%s]' % str(io_var) 127 | if type(io_value) != str: 128 | if io_value > 0: 129 | io_value = 'TRUE' 130 | else: 131 | io_value = 'FALSE' 132 | 133 | self.addline('%s=%s' % (io_var, io_value)) 134 | 135 | def waitDI(self, io_var, io_value, timeout_ms=-1): 136 | """Waits for a variable (digital input) io_var to attain a given value io_value. Optionally, a timeout can be provided.""" 137 | if type(io_var) != str: 138 | io_var = 'IN[%s]' % str(io_var) 139 | if type(io_value) != str: 140 | if io_value > 0: 141 | io_value = 'TRUE' 142 | else: 143 | io_value = 'FALSE' 144 | 145 | if timeout_ms < 0: 146 | self.addline('WAIT FOR %s==%s' % (io_var, io_value)) 147 | else: 148 | self.addline('WAIT FOR %s==%s TIMEOUT=%.1f' % (io_var, io_value, timeout_ms)) 149 | 150 | def RunCode(self, code, is_function_call=False): 151 | """Adds code or a function call""" 152 | if is_function_call: 153 | code = code.replace(' ', '_') 154 | if not code.endswith('()'): 155 | code = code + '()' 156 | if code == 'Open()': 157 | self.addline('M97 G4P1 B40 T1') # Add the G-code instructions for the Open subprogram 158 | elif code == 'Close()': 159 | self.addline('M97 G4P1 B0 T1') # Add the G-code instructions for the Close subprogram 160 | else: 161 | self.addline(code) 162 | else: 163 | self.addline(code) 164 | 165 | def RunMessage(self, message, iscomment=False): 166 | """Display a message in the robot controller screen (teach pendant)""" 167 | if iscomment: 168 | if message == "Attach to Gripper V2": 169 | self.addline("M21") 170 | elif message == "Detach from Gripper V2": 171 | self.addline("M20") 172 | else: 173 | self.addlog('Show message on teach pendant not implemented (%s)' % message) 174 | 175 | def addlineEnd(self, newline): 176 | """Add a program line""" 177 | self.PROG = self.PROG + newline 178 | 179 | def addline(self, newline): 180 | """Add a program line""" 181 | self.PROG = self.PROG + newline + '\n' 182 | 183 | def addlog(self, newline): 184 | """Add a log message""" 185 | self.LOG = self.LOG + newline + '\n' 186 | -------------------------------------------------------------------------------- /Arctos.rdk: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Arctos-Robotics/RoboDK/0df0e8fca603b150f3706e450852c2aef720ce09/Arctos.rdk -------------------------------------------------------------------------------- /Arctos_RoboDK.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Arctos-Robotics/RoboDK/0df0e8fca603b150f3706e450852c2aef720ce09/Arctos_RoboDK.jpg -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RoboDK postprocessor for Arctos 0.16.7 2 | 3 | This repository contains RoboDK packages for the Arctos robotic arm, enabling motion planning, execution, and simulation in both virtual and real environments. 4 | 5 | ## How to Use: 6 | 7 | ### Requirements 8 | 9 | 1. [RoboDK](https://robodk.com/download) 10 | 2. [UGS](https://winder.github.io/ugs_website/download/) 11 | 12 | ### Import Arctos.rdk in RoboDK 13 | 14 | 1. File > Open > Arctos.rdk 15 | 2. Double click on Prog1 to see the test move 16 | 3. Import Arctos.py post processor to installation folder. 17 | For example "C:\RoboDK\Posts" 18 | 4. Right click on Prog1 > Select Post Processor > there should be "Arctos" in the list. 19 | 20 | ![Arctos_RoboDK.jpg](/Arctos_RoboDK.jpg) 21 | 22 | ### Generate robot program 23 | 24 | 1. Right click on Prog1 > Generate robot program > select Arctos > OK 25 | 26 | If you want to make your custom program follow the RoboDK [tutorials](https://www.youtube.com/watch?v=xZ2_JEbS_E0&list=PLjiA6TvRACQd8pL0EnE9Djc_SCH7wxxXl) 27 | 28 | In short: 29 | - double click on Arctos in design tree below the My Mechanism Base opens the panel 30 | - In panel it is possible to jog and move the robot to desired position 31 | More fun way is to click anywhere in space and hit ALT key, it allows to move the robot 32 | by pivoting tools on each target. 33 | - Add target to desired position 34 | - Create New program 35 | - Select target 36 | - Add movement to a program (joint, linear, circular) 37 | - Double click on Prog2 to see the movement 38 | - Step 1. 39 | 40 | 41 | ### Open robot program in UGS 42 | 43 | 1. Connect the robot with USB cable 44 | 2. Refresh the Port and select COM port, Select Baud 115200 45 | 3. Connect the robot 46 | 4. Make sure it is in the same position as target "Home" in RoboDK 47 | 5. File > Open > Prog1.gcode 48 | 6. Toolbox > Reset Zero 49 | 7. Play the program on robot 50 | 51 | 52 | ## To do: 53 | 54 | 1. Add gripper in RoboDK 55 | 2. Custom macro in UGS for manual opening and closing the gripper 56 | 57 | 58 | --------------------------------------------------------------------------------