├── .github └── FUNDING.yml ├── CM6_BOM_v2.xlsx ├── CM6_V2_code ├── CM6_DH_model.py ├── CM6_SIMULATOR.py ├── CM6_SIM_mimick.py ├── CM6_full_test.py ├── CM6_keyboard_control.py ├── com_test_joint.py └── coms_test2.py ├── LICENSE ├── README.md ├── STEPS_CM6 ├── CM6_V2_PARTS.zip ├── SSG_GRIPPER_CM6.zip ├── STEPS.zip ├── STEPS_2.zip ├── STEPS_3.zip ├── STEPS_4.zip ├── STEPS_J5.zip └── STEPS_J6.zip ├── assembly_instructions_v2.pdf ├── images └── CM6_img.png └── simply.zip /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | # These are supported funding model platforms 2 | 3 | github: PCrnjak 4 | patreon: PCrnjak 5 | open_collective: # Replace with a single Open Collective username 6 | ko_fi: # Replace with a single Ko-fi username 7 | tidelift: # Replace with a single Tidelift platform-name/package-name e.g., npm/babel 8 | community_bridge: # Replace with a single Community Bridge project-name e.g., cloud-foundry 9 | liberapay: # Replace with a single Liberapay username 10 | issuehunt: # Replace with a single IssueHunt username 11 | otechie: # Replace with a single Otechie username 12 | lfx_crowdfunding: # Replace with a single LFX Crowdfunding project-name e.g., cloud-foundry 13 | custom: # Replace with up to 4 custom sponsorship URLs e.g., ['link1', 'link2'] 14 | -------------------------------------------------------------------------------- /CM6_BOM_v2.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_COBOT_ROBOT/883c2df6d364e15ab69c7a79f047ea2b6439e828/CM6_BOM_v2.xlsx -------------------------------------------------------------------------------- /CM6_V2_code/CM6_DH_model.py: -------------------------------------------------------------------------------- 1 | from roboticstoolbox import DHRobot, RevoluteDH, ERobot, ELink, ETS 2 | from math import pi, sin, cos 3 | import numpy as np 4 | #np.set_printoptions(linewidth=100, suppress=True) 5 | 6 | 7 | # robot length values (metres) 8 | l1 = 162.65 / 1000 9 | l2 = 280 / 1000 10 | l3 = 250 / 1000 11 | l4 = 37.2 / 1000 12 | 13 | alpha_DH = [pi/2, 0, pi/2, -pi/2, pi/2, 0] 14 | 15 | robot = DHRobot( 16 | [ 17 | RevoluteDH(d=l1, a=0, alpha=alpha_DH[0]), 18 | RevoluteDH(d=0, a=l2, alpha=alpha_DH[1]), 19 | RevoluteDH(d=0, a=0, alpha=alpha_DH[2]), 20 | RevoluteDH(d=l3, a=0, alpha=alpha_DH[3]), 21 | RevoluteDH(d=0, a=0, alpha=alpha_DH[4]), 22 | RevoluteDH(d=l4, a=0, alpha=alpha_DH[5]), 23 | ], 24 | name="CM6", 25 | ) 26 | 27 | #print(robot.isspherical()) 28 | 29 | print(robot) 30 | robot_plot = robot.plot([0,pi/2,0,0,0,0]) 31 | robot_plot.hold() -------------------------------------------------------------------------------- /CM6_V2_code/CM6_SIMULATOR.py: -------------------------------------------------------------------------------- 1 | import customtkinter 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | import platform 5 | import os 6 | import logging 7 | import random 8 | from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg 9 | import matplotlib.animation as animation 10 | from s_visual_kinematics.RobotSerial import * 11 | import numpy as np 12 | from math import pi 13 | import socket 14 | import select 15 | import time 16 | import re 17 | from roboticstoolbox import DHRobot, RevoluteDH, ERobot, ELink, ETS 18 | import roboticstoolbox as rp 19 | from spatialmath import * 20 | 21 | 22 | #Setup IP address and Simulator port 23 | ip = "127.0.0.1" #Loopback address 24 | port = 5001 25 | 26 | # Create a UDP socket 27 | sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 28 | sock.bind((ip, port)) 29 | print(f'Start listening to {ip}:{port}') 30 | 31 | # Debug config 32 | """ 33 | logging.basicConfig(level = logging.DEBUG, 34 | format='%(asctime)s.%(msecs)03d %(levelname)s:\t%(message)s', 35 | datefmt='%H:%M:%S' 36 | ) 37 | """ 38 | #logging.disable(logging.DEBUG) 39 | 40 | # Finds out where the program and images are stored 41 | my_os = platform.system() 42 | if my_os == "Windows": 43 | Image_path = os.path.join(os.path.dirname(os.path.realpath(__file__))) 44 | logging.debug("Os is Windows") 45 | else: 46 | Image_path = os.path.join(os.path.dirname(os.path.realpath(__file__))) 47 | logging.debug("Os is Linux") 48 | 49 | logging.debug(Image_path) 50 | 51 | # graphical scale factor 52 | scale_factor = 1 53 | # robot length values (metres) 54 | l1 = 162.65 / 1000 * scale_factor 55 | l2 = 280 / 1000 * scale_factor 56 | l3 = 250 / 1000 * scale_factor 57 | l4 = 37.2 / 1000 * scale_factor 58 | 59 | alpha_DH = [pi/2, 0, pi/2, -pi/2, pi/2, 0] 60 | 61 | robot_tb = DHRobot( 62 | [ 63 | RevoluteDH(d=l1, a=0, alpha=alpha_DH[0]), 64 | RevoluteDH(d=0, a=l2, alpha=alpha_DH[1]), 65 | RevoluteDH(d=0, a=0, alpha=alpha_DH[2],qlim = [-1.9, 3.2]), 66 | RevoluteDH(d=l3, a=0, alpha=alpha_DH[3]), 67 | RevoluteDH(d=0, a=0, alpha=alpha_DH[4]), 68 | RevoluteDH(d=l4, a=0, alpha=alpha_DH[5]), 69 | ], 70 | name="CM6", 71 | ) 72 | 73 | 74 | # Elbow up config 75 | q_t1 = np.array([-0.05177185, 0.92230595, -0.09833656, -0.01817177, -0.80872745, -0.22856314]) 76 | 77 | # Elbow down config 78 | #q_t1 = np.array([-0.00929976, -0.51556136, 3.43765095, 0.20160048, -1.22769596, -0.14588157]) 79 | 80 | text_size = 14 81 | customtkinter.set_appearance_mode("Dark") # Modes: "System" (standard), "Dark", "Light" 82 | customtkinter.set_default_color_theme("blue") # Themes: "blue" (standard), "green", "dark-blue" 83 | 84 | position_in_data = np.array([0, pi/2, 0, 0, 0, 0]) 85 | cart_data = np.array([0, 0, 0, 0, 0, 0]) 86 | prev_position_in_data = None 87 | max_position_change_threshold = 0.5 # Adjust this threshold as needed 88 | 89 | 90 | 91 | def GUI(Position_in): 92 | 93 | # This functions runs every interval of func.animation 94 | def show_robot(var): 95 | global position_in_data 96 | global cart_data 97 | global prev_position_in_data 98 | global q_t1 99 | received_data = None 100 | data = None 101 | time1 = time.perf_counter() 102 | 103 | """ 104 | # Check if the elbow is up or down 105 | def check_elbow_configuration(robot, q): 106 | # Forward kinematics to get the pose of the third link 107 | T3 = robot.links[0].A(q[0]) * robot.links[1].A(q[1]) * robot.links[2].A(q[2]) 108 | 109 | # Extract the z-axis of the orientation matrix of the third link 110 | z_axis = T3.R[:, 2] 111 | 112 | # Check the sign of the z component 113 | if z_axis[2] > 0: 114 | return "Elbow Down" 115 | else: 116 | return "Elbow Up" 117 | """ 118 | def check_elbow_configuration(q): 119 | 120 | straight_pose_j3 = pi/2 121 | if q[2] > straight_pose_j3: 122 | return "Elbow Down" 123 | 124 | elif q[2] < straight_pose_j3: 125 | return "Elbow Up" 126 | 127 | 128 | while True: 129 | try: 130 | # Check if the socket is ready to read 131 | ready_to_read, _, _ = select.select([sock], [], [], 0) # Timeout of 0 second, no blocking read instantly 132 | #print(ready_to_read) 133 | # Check if there's data available to read 134 | if sock in ready_to_read: 135 | # Receive data from the socket 136 | data, addr = sock.recvfrom(1024) # data needs to be decoded 137 | else: 138 | #print(f"No sock in ready") 139 | break 140 | except KeyboardInterrupt: 141 | # Handle keyboard interrupt 142 | break 143 | except Exception as e: 144 | # Handle other exceptions 145 | print(f"Error: {e}") 146 | break 147 | 148 | # Process the last received packet after exiting the loop 149 | if data: 150 | #print(data) 151 | received_data = data.decode() 152 | #print(received_data) 153 | # Check if the received data is in the format "pos,x,y,z,rx,ry,rz" 154 | if received_data.startswith("pos,"): 155 | pattern = re.compile(r'^pos,([-+]?\d*\.?\d+(?:[eE][-+]?\d+)?),([-+]?\d*\.?\d+(?:[eE][-+]?\d+)?),([-+]?\d*\.?\d+(?:[eE][-+]?\d+)?),([-+]?\d*\.?\d+(?:[eE][-+]?\d+)?),([-+]?\d*\.?\d+(?:[eE][-+]?\d+)?),([-+]?\d*\.?\d+(?:[eE][-+]?\d+)?)$') 156 | match = pattern.match(received_data) 157 | 158 | if match: 159 | # Extract position values from the matched groups 160 | position_in_data = np.array([float(match.group(i)) for i in range(1, 7)]) 161 | # Print the updated position_in array 162 | print("Received position data:", position_in_data) 163 | else: 164 | print("Error: Invalid message format") 165 | # Check if the received data is in the format "cart,x,y,z" 166 | elif received_data.startswith("cart,"): 167 | # Split the message by comma and extract x, y, z values 168 | _, x, y, z, roll, pitch, jaw = received_data.split(",") 169 | cart_data = np.array([float(x), float(y), float(z), float(roll), float(pitch), float(jaw)]) #0, pi/2, pi kako mora biti; 0, 0, pi is good 170 | cart_data2 = np.array([float(x) *1000, float(y)*1000, float(z)*1000, float(roll), float(pitch), float(jaw)]) # Assuming rx, ry, rz are 0 171 | #print("Received cartesian position data:", cart_data2) 172 | 173 | # Construct a matrix from given arguments, this will be needed pose 174 | Needed_pose = SE3.RPY([cart_data[3], cart_data[4], cart_data[5]], unit='rad',order='xyz') 175 | Needed_pose.t[0] = cart_data[0] 176 | Needed_pose.t[1] = cart_data[1] 177 | Needed_pose.t[2] = cart_data[2] 178 | 179 | # Calculate joint angles using ik 180 | prev_position_in_data = position_in_data 181 | q1 = robot_tb.ik_LM(Needed_pose,method = "chan",q0 = q_t1, ilimit = 25, slimit = 25) 182 | position_in_data = q1[0] 183 | elbow_config = check_elbow_configuration(position_in_data) 184 | #print(elbow_config) 185 | #print(position_in_data) 186 | #print(position_in_data) 187 | 188 | 189 | # If you want elbow down config comment this out 190 | if elbow_config == "Elbow Down": 191 | q1 = robot_tb.ik_LM(Needed_pose,method = "chan",q0 = prev_position_in_data, ilimit = 25, slimit = 25) 192 | position_in_data = q1[0] 193 | print("trying again") 194 | 195 | if prev_position_in_data is not None: 196 | position_change = np.abs(position_in_data - prev_position_in_data) 197 | large_position_change_indices = np.where(position_change > max_position_change_threshold)[0] 198 | if len(large_position_change_indices) > 0: 199 | print("Error: Position change is too quick for joints:", large_position_change_indices) 200 | print("Position change values:", position_change[large_position_change_indices]) 201 | 202 | 203 | 204 | else: 205 | print("Error: Unknown message format") 206 | 207 | #time2 = time.perf_counter() 208 | #print(f"time {time2-time1} ") 209 | theta = np.array([0, pi/2, 0, 0, 0, 0]) 210 | f = robot.forward(position_in_data) 211 | robot.draw() 212 | 213 | dh_params = np.array([[l1, 0, alpha_DH[0], 0.], 214 | [0., l2, alpha_DH[1], 0], 215 | [0., 0, alpha_DH[2], 0], 216 | [l3, 0., alpha_DH[3], 0], 217 | [0., 0.,alpha_DH[4], 0.], 218 | [l4, 0, alpha_DH[5], 0]]) 219 | 220 | robot = RobotSerial(dh_params) 221 | 222 | fig = plt.figure(figsize=(7,7)) 223 | #ax = fig.add_subplot(111, projection="3d") 224 | #print(plt.margins()) 225 | #fig.margins(3,4) 226 | 227 | robot.init_custum_frame(fig) 228 | random.seed(5) 229 | np.set_printoptions(precision=3, suppress=True) 230 | theta = np.array([0., 0., -0.25 * pi, 0., 0., 0.]) 231 | f = robot.forward(theta) 232 | 233 | app = customtkinter.CTk() 234 | app.lift() 235 | app.attributes('-topmost',True) 236 | logging.debug("Simulator running!") 237 | # configure window 238 | app.title("Simulator.py") 239 | app.geometry(f"{750}x{680}") 240 | app.wm_attributes('-topmost',False) 241 | 242 | # configure grid layout (4x4) 243 | app.grid_columnconfigure((1,2), weight=1) 244 | app.grid_columnconfigure((0), weight=1) 245 | app.grid_rowconfigure((0), weight=0) 246 | app.grid_rowconfigure((1), weight=0) 247 | app.grid_rowconfigure((2), weight=1) 248 | app.grid_rowconfigure((3), weight=0) 249 | 250 | Top_frame = customtkinter.CTkFrame(app ,height = 0,width=150, corner_radius=0, ) 251 | Top_frame.grid(row=0, column=0, columnspan=4, padx=(5,5), pady=(5,5),sticky="new") 252 | Top_frame.grid_columnconfigure(0, weight=0) 253 | Top_frame.grid_rowconfigure(0, weight=0) 254 | 255 | def Top_frame(): 256 | Top_frame = customtkinter.CTkFrame(app ,height = 0,width=150, corner_radius=0, ) 257 | Top_frame.grid(row=0, column=0, columnspan=4, padx=(5,5), pady=(5,5),sticky="new") 258 | Top_frame.grid_columnconfigure(0, weight=0) 259 | Top_frame.grid_rowconfigure(0, weight=0) 260 | 261 | 262 | Control_button = customtkinter.CTkButton( Top_frame,text="Pause", width= 50,fg_color ="#313838", font = customtkinter.CTkFont(size=15, family='TkDefaultFont'),command=Pause) 263 | Control_button.grid(row=0, column=0, padx=20,pady = (5,5),sticky="news") 264 | 265 | Config_button = customtkinter.CTkButton( Top_frame,text="Run", width= 50,fg_color ="#313838", font = customtkinter.CTkFont(size=15, family='TkDefaultFont'),command=Run) 266 | Config_button.grid(row=0, column=1, padx=20,pady = (5,5),sticky="news") 267 | 268 | Setup_button = customtkinter.CTkButton( Top_frame,text="Sync", width= 50,fg_color ="#313838", font = customtkinter.CTkFont(size=15, family='TkDefaultFont')) 269 | Setup_button.grid(row=0, column=2, padx=20,pady = (5,5),sticky="news") 270 | 271 | def Pause(): 272 | ani.event_source.stop() 273 | 274 | def Run(): 275 | ani.event_source.start() 276 | 277 | Top_frame() 278 | 279 | canvas = FigureCanvasTkAgg(fig, master=app) 280 | canvas.draw() 281 | canvas.get_tk_widget().grid(row=1, column=1, padx=20,pady = (5,5),sticky="news") 282 | 283 | ani = animation.FuncAnimation(fig, show_robot, interval=85,frames=30) 284 | 285 | app.mainloop() 286 | 287 | 288 | if __name__ == "__main__": 289 | 290 | Position_in = [0,0,0,0,0,0] 291 | GUI(Position_in) 292 | -------------------------------------------------------------------------------- /CM6_V2_code/CM6_SIM_mimick.py: -------------------------------------------------------------------------------- 1 | import Spectral_BLDC as Spectral 2 | import SourceRoboticsToolbox 3 | import time 4 | import socket 5 | import numpy as np 6 | import keyboard 7 | 8 | # Sender configuration 9 | ip = "127.0.0.1" #Loopback address 10 | port = 5001 11 | # Create a UDP socket 12 | sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 13 | 14 | 15 | Master_position = [11800,6910,770,3255,9477,5487] 16 | Joint_reduction_ratio = [8, 8, 9.142857143, 6.5, 6, 5] # Reduction ratio we have on our joints 17 | 18 | Communication1 = Spectral.CanCommunication(bustype='slcan', channel='COM41', bitrate=1000000) 19 | 20 | Motor = [] 21 | 22 | Motor.append(Spectral.SpectralCAN(node_id=0, communication=Communication1)) 23 | Motor.append(Spectral.SpectralCAN(node_id=1, communication=Communication1)) 24 | Motor.append(Spectral.SpectralCAN(node_id=2, communication=Communication1)) 25 | Motor.append(Spectral.SpectralCAN(node_id=3, communication=Communication1)) 26 | Motor.append(Spectral.SpectralCAN(node_id=4, communication=Communication1)) 27 | Motor.append(Spectral.SpectralCAN(node_id=5, communication=Communication1)) 28 | 29 | Joint = [] 30 | 31 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[0], gear_ratio = Joint_reduction_ratio[0], offset = 0, dir = 0)) 32 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[1], gear_ratio = Joint_reduction_ratio[1], offset = 3.512, dir = 0)) 33 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[2], gear_ratio = Joint_reduction_ratio[2], offset = -1.265, dir = 1)) 34 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[3], gear_ratio = Joint_reduction_ratio[3], offset = 0, dir = 0)) 35 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[4], gear_ratio = Joint_reduction_ratio[4], offset = 0, dir = 1)) 36 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[5], gear_ratio = Joint_reduction_ratio[5], offset = 0, dir = 0)) 37 | 38 | timeout_setting = 0.001 39 | 40 | initial = 0 41 | initial_setup = [0,0,0,0,0,0] 42 | 43 | testing = 5 44 | 45 | # Initialize position values 46 | position_values = np.array([0, np.pi/2, 0, 0, 0, 0]) 47 | received_ids = [0,0,0,0,0,0] 48 | 49 | while True: 50 | 51 | # Check for keyboard input 52 | if keyboard.is_pressed('a'): 53 | print("You pressed 'a'") 54 | if keyboard.is_pressed('b'): 55 | print("You pressed 'b'") 56 | 57 | 58 | Motor[0].Send_Respond_Encoder_data() 59 | Motor[1].Send_Respond_Encoder_data() 60 | Motor[2].Send_Respond_Encoder_data() 61 | Motor[3].Send_Respond_Encoder_data() 62 | Motor[4].Send_Respond_Encoder_data() 63 | Motor[5].Send_Respond_Encoder_data() 64 | 65 | for i in range(1, 9): # Loop 9-1=8 to check for received data 66 | message, UnpackedMessageID = Communication1.receive_can_messages(timeout=timeout_setting) 67 | #print(f"unpack{i} is: {UnpackedMessageID}") 68 | 69 | # Check if UnpackedMessageID is not None 70 | if UnpackedMessageID is not None: 71 | 72 | # Update received id index; meaning that we received response from that CAN ID 73 | received_ids[UnpackedMessageID[0]] = 1 74 | Motor[UnpackedMessageID[0]].UnpackData(message,UnpackedMessageID) 75 | #print(f"Motor {UnpackedMessageID[0]}, position is: {Motor[UnpackedMessageID[0]].position}") 76 | unwrapped_position_raw = Joint[UnpackedMessageID[0]].unwrap_position(Motor[UnpackedMessageID[0]].position) 77 | position_values[UnpackedMessageID[0]] = Joint[UnpackedMessageID[0]].get_joint_position(Motor[UnpackedMessageID[0]].position) 78 | 79 | 80 | if initial_setup[UnpackedMessageID[0]] == 0: 81 | initial_setup[UnpackedMessageID[0]] = 1 82 | Joint[UnpackedMessageID[0]].determine_sector(Motor[UnpackedMessageID[0]].position) 83 | 84 | print(position_values) 85 | 86 | 87 | 88 | position_values_str = ','.join(map(str, position_values)) 89 | msg = f"pos,{position_values_str}" # Include "pos" at the beginning 90 | msg_bytes = msg.encode() 91 | sock.sendto(msg_bytes, (ip, port)) 92 | 93 | time.sleep(0.05) 94 | -------------------------------------------------------------------------------- /CM6_V2_code/CM6_full_test.py: -------------------------------------------------------------------------------- 1 | import Spectral_BLDC as Spectral 2 | import SourceRoboticsToolbox 3 | import time 4 | import socket 5 | 6 | 7 | # Sender configuration 8 | ip = "127.0.0.1" #Loopback address 9 | port = 5001 10 | # Create a UDP socket 11 | sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 12 | 13 | 14 | Master_position = [11800,6910,770,3255,9477,5487] 15 | Joint_reduction_ratio = [8, 8, 9.142857143, 6.5, 6, 5] # Reduction ratio we have on our joints 16 | 17 | Communication1 = Spectral.CanCommunication(bustype='slcan', channel='COM41', bitrate=1000000) 18 | 19 | Motor = [] 20 | 21 | Motor.append(Spectral.SpectralCAN(node_id=0, communication=Communication1)) 22 | Motor.append(Spectral.SpectralCAN(node_id=1, communication=Communication1)) 23 | Motor.append(Spectral.SpectralCAN(node_id=2, communication=Communication1)) 24 | Motor.append(Spectral.SpectralCAN(node_id=3, communication=Communication1)) 25 | Motor.append(Spectral.SpectralCAN(node_id=4, communication=Communication1)) 26 | Motor.append(Spectral.SpectralCAN(node_id=5, communication=Communication1)) 27 | 28 | Joint = [] 29 | 30 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[0], gear_ratio = Joint_reduction_ratio[0], offset = 0, dir = 0)) 31 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[1], gear_ratio = Joint_reduction_ratio[1], offset = 3.512, dir = 0)) 32 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[2], gear_ratio = Joint_reduction_ratio[2], offset = -1.265, dir = 1)) 33 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[3], gear_ratio = Joint_reduction_ratio[3], offset = 0, dir = 0)) 34 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[4], gear_ratio = Joint_reduction_ratio[4], offset = 0, dir = 1)) 35 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[5], gear_ratio = Joint_reduction_ratio[5], offset = 0, dir = 0)) 36 | 37 | timeout_setting = 0.001 38 | 39 | initial = 0 40 | initial_setup = [0,0,0,0,0,0] 41 | 42 | testing = 5 43 | 44 | Joint_positions = [0,0,0,0,0,0] 45 | 46 | while True: 47 | 48 | Motor[testing].Send_Respond_Encoder_data() 49 | message, UnpackedMessageID = Communication1.receive_can_messages(timeout=timeout_setting) 50 | if message is not None: 51 | 52 | Motor[testing].UnpackData(message,UnpackedMessageID) 53 | unwrapped_position_raw = Joint[testing].unwrap_position(Motor[testing].position) 54 | print(f"Motor6 position is: {Motor[testing].position} motor unwrapped {unwrapped_position_raw}") 55 | True_position = Joint[testing].get_joint_position(Motor[testing].position) 56 | print(True_position) 57 | #tick = Joint[testing].get_encoder_position(True_position) 58 | #print(tick ) 59 | 60 | if initial == 0: 61 | initial = 1 62 | Joint[testing].determine_sector(Motor[testing].position) 63 | 64 | else: 65 | print(f"No message after timeout period mot1! number: ") 66 | 67 | # Convert position value and send time to a string separated by a comma 68 | msg = f"{100}".encode() 69 | 70 | sock.sendto(msg, (ip, port)) 71 | 72 | time.sleep(1) 73 | -------------------------------------------------------------------------------- /CM6_V2_code/CM6_keyboard_control.py: -------------------------------------------------------------------------------- 1 | import Spectral_BLDC as Spectral 2 | import SourceRoboticsToolbox 3 | import time 4 | import socket 5 | import numpy as np 6 | import keyboard 7 | 8 | # Sender configuration 9 | ip = "127.0.0.1" #Loopback address 10 | port = 5001 11 | # Create a UDP socket 12 | sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 13 | 14 | 15 | Master_position = [11800,6910,770,3255,9477,5487] 16 | Joint_reduction_ratio = [8, 8, 9.142857143, 6.5, 6, 5] # Reduction ratio we have on our joints 17 | 18 | Communication1 = Spectral.CanCommunication(bustype='slcan', channel='COM41', bitrate=1000000) 19 | 20 | Motor = [] 21 | 22 | Motor.append(Spectral.SpectralCAN(node_id=0, communication=Communication1)) 23 | Motor.append(Spectral.SpectralCAN(node_id=1, communication=Communication1)) 24 | Motor.append(Spectral.SpectralCAN(node_id=2, communication=Communication1)) 25 | Motor.append(Spectral.SpectralCAN(node_id=3, communication=Communication1)) 26 | Motor.append(Spectral.SpectralCAN(node_id=4, communication=Communication1)) 27 | Motor.append(Spectral.SpectralCAN(node_id=5, communication=Communication1)) 28 | 29 | Joint = [] 30 | 31 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[0], gear_ratio = Joint_reduction_ratio[0], offset = 0, dir = 0)) 32 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[1], gear_ratio = Joint_reduction_ratio[1], offset = 3.512, dir = 0)) 33 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[2], gear_ratio = Joint_reduction_ratio[2], offset = -1.265, dir = 1)) 34 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[3], gear_ratio = Joint_reduction_ratio[3], offset = 0, dir = 0)) 35 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[4], gear_ratio = Joint_reduction_ratio[4], offset = 0, dir = 1)) 36 | Joint.append(SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=Master_position[5], gear_ratio = Joint_reduction_ratio[5], offset = 0, dir = 0)) 37 | 38 | timeout_setting = 0.001 39 | 40 | initial = 0 41 | initial_setup = [0,0,0,0,0,0] 42 | 43 | testing = 5 44 | 45 | # Initialize position values 46 | position_values = np.array([0, np.pi/2, 0, 0, 0, 0]) 47 | received_ids = [0,0,0,0,0,0] 48 | active_command = 'i' 49 | active_hold = np.array([0, np.pi/2, 0, 0, 0, 0]) 50 | 51 | while True: 52 | 53 | # Check for keyboard input 54 | if keyboard.is_pressed('i'): 55 | print("You pressed 'i'") 56 | active_command = 'i' 57 | elif keyboard.is_pressed('c'): 58 | print("You pressed 'c'") 59 | active_command = 'c' 60 | elif keyboard.is_pressed('p'): 61 | print("You pressed 'p'") 62 | active_command = 'p' 63 | active_hold = np.copy(position_values) 64 | elif keyboard.is_pressed('d'): 65 | print("You pressed 'd'") 66 | active_command = 'd' 67 | elif keyboard.is_pressed('e'): 68 | print("You pressed 'e'") 69 | active_command = 'e' 70 | elif keyboard.is_pressed('f'): 71 | print("You pressed 'f'") 72 | active_command = 'f' 73 | 74 | if active_command == 'i': 75 | for motor in Motor: 76 | motor.Send_Respond_Encoder_data() 77 | 78 | elif active_command == 'c': 79 | for motor in Motor: 80 | motor.Send_Clear_Error() 81 | 82 | elif active_command == 'd': 83 | Motor[0].Send_PD_Gains(0.49,0.01) 84 | Motor[1].Send_PD_Gains(0.49,0.01) 85 | Motor[2].Send_PD_Gains(0.49,0.01) 86 | Motor[3].Send_PD_Gains(0.49,0.01) 87 | Motor[4].Send_PD_Gains(0.49,0.01) 88 | #Motor[5].Send_Respond_Encoder_data() 89 | Motor[5].Send_PD_Gains(0.49,0.01) 90 | 91 | elif active_command == 'e': 92 | Motor[0].Send_PD_Gains(0.14,0.0028) 93 | Motor[1].Send_PD_Gains(0.14,0.0028) 94 | Motor[2].Send_PD_Gains(0.14,0.0028) 95 | Motor[3].Send_PD_Gains(0.14,0.0028) 96 | Motor[4].Send_PD_Gains(0.14,0.0028) 97 | #Motor[5].Send_Respond_Encoder_data() 98 | Motor[5].Send_PD_Gains(0.14,0.028) 99 | 100 | elif active_command == 'f': 101 | Motor[0].Send_PD_Gains(0.49,0.028) 102 | Motor[1].Send_PD_Gains(0.49,0.028) 103 | Motor[2].Send_PD_Gains(0.49,0.028) 104 | Motor[3].Send_PD_Gains(0.49,0.028) 105 | Motor[4].Send_PD_Gains(0.49,0.028) 106 | #Motor[5].Send_Respond_Encoder_data() 107 | Motor[5].Send_PD_Gains(0.49,0.028) 108 | 109 | elif active_command == 'p': 110 | 111 | #Motor[0].Send_Respond_Encoder_data() 112 | #Motor[1].Send_Respond_Encoder_data() 113 | #Motor[2].Send_Respond_Encoder_data() 114 | #Motor[3].Send_Respond_Encoder_data() 115 | #Motor[4].Send_Respond_Encoder_data() 116 | #Motor[5].Send_Respond_Encoder_data() 117 | 118 | tick = Joint[0].get_encoder_position(active_hold[0]) 119 | Motor[0].Send_data_pack_PD(tick,0,0) 120 | tick = Joint[1].get_encoder_position(active_hold[1]) 121 | Motor[1].Send_data_pack_PD(tick,0,0) 122 | tick = Joint[2].get_encoder_position(active_hold[2]) 123 | Motor[2].Send_data_pack_PD(tick,0,0) 124 | tick = Joint[3].get_encoder_position(active_hold[3]) 125 | Motor[3].Send_data_pack_PD(tick,0,0) 126 | tick = Joint[4].get_encoder_position(active_hold[4]) 127 | Motor[4].Send_data_pack_PD(tick,0,0) 128 | tick = Joint[5].get_encoder_position(active_hold[5]) 129 | Motor[5].Send_data_pack_PD(tick,0,0) 130 | 131 | for i in range(1, 9): # Loop 9-1=8 to check for received data 132 | message, UnpackedMessageID = Communication1.receive_can_messages(timeout=timeout_setting) 133 | print(f"unpack{i} is: {UnpackedMessageID}") 134 | 135 | # Check if UnpackedMessageID is not None 136 | if UnpackedMessageID is not None: 137 | 138 | # Update received id index; meaning that we received response from that CAN ID 139 | received_ids[UnpackedMessageID[0]] = 1 140 | Motor[UnpackedMessageID[0]].UnpackData(message,UnpackedMessageID) 141 | print(f"Motor {UnpackedMessageID[0]}, position is: {Motor[UnpackedMessageID[0]].position}") 142 | unwrapped_position_raw = Joint[UnpackedMessageID[0]].unwrap_position(Motor[UnpackedMessageID[0]].position) 143 | position_values[UnpackedMessageID[0]] = Joint[UnpackedMessageID[0]].get_joint_position(Motor[UnpackedMessageID[0]].position) 144 | 145 | 146 | if initial_setup[UnpackedMessageID[0]] == 0: 147 | initial_setup[UnpackedMessageID[0]] = 1 148 | Joint[UnpackedMessageID[0]].determine_sector(Motor[UnpackedMessageID[0]].position) 149 | 150 | print(position_values) 151 | print(f"active commadn is : {active_command}") 152 | print(f"active hold is: {active_hold}") 153 | position_values_str = ','.join(map(str, position_values)) 154 | msg = f"pos,{position_values_str}" # Include "pos" at the beginning 155 | msg_bytes = msg.encode() 156 | sock.sendto(msg_bytes, (ip, port)) 157 | 158 | time.sleep(0.05) 159 | -------------------------------------------------------------------------------- /CM6_V2_code/com_test_joint.py: -------------------------------------------------------------------------------- 1 | import Spectral_BLDC as Spectral 2 | import SourceRoboticsToolbox 3 | import time 4 | import psutil 5 | import os 6 | 7 | 8 | Communication1 = Spectral.CanCommunication(bustype='slcan', channel='COM41', bitrate=1000000) 9 | Motor6 = Spectral.SpectralCAN(node_id=5, communication=Communication1) 10 | Joint = SourceRoboticsToolbox.Joint(encoder_resolution = 14, master_position=6000, gear_ratio = 5, offset = 0, dir = 0) 11 | 12 | 13 | timeout_setting = 0.001 14 | 15 | initial = 0 16 | True_position = 0 17 | 18 | 19 | while True: 20 | 21 | Motor6.Send_Respond_Encoder_data() 22 | message, UnpackedMessageID = Communication1.receive_can_messages(timeout=timeout_setting) 23 | if message is not None: 24 | 25 | Motor6.UnpackData(message,UnpackedMessageID) 26 | unwrapped_position_raw = Joint.unwrap_position(Motor6.position) 27 | print(f"Motor6 position is: {Motor6.position} motor unwrapped {unwrapped_position_raw}") 28 | 29 | True_position = Joint.get_joint_position(Motor6.position) 30 | print(True_position) 31 | #Joint.print_offset_ticks() 32 | tick = Joint.get_encoder_position(True_position) 33 | print(tick ) 34 | 35 | else: 36 | print(f"No message after timeout period mot1! number: ") 37 | 38 | if initial == 0: 39 | initial = 1 40 | Joint.determine_sector(Motor6.position) 41 | 42 | time.sleep(1) 43 | -------------------------------------------------------------------------------- /CM6_V2_code/coms_test2.py: -------------------------------------------------------------------------------- 1 | import Spectral_BLDC as Spectral 2 | import time 3 | import numpy as np 4 | 5 | # Set the CPU affinity to CPU core 0 6 | 7 | 8 | Communication1 = Spectral.CanCommunication(bustype='slcan', channel='COM41', bitrate=1000000) 9 | Motor = [] 10 | 11 | Motor.append(Spectral.SpectralCAN(node_id=0, communication=Communication1)) 12 | Motor.append(Spectral.SpectralCAN(node_id=1, communication=Communication1)) 13 | Motor.append(Spectral.SpectralCAN(node_id=2, communication=Communication1)) 14 | Motor.append(Spectral.SpectralCAN(node_id=3, communication=Communication1)) 15 | Motor.append(Spectral.SpectralCAN(node_id=4, communication=Communication1)) 16 | Motor.append(Spectral.SpectralCAN(node_id=5, communication=Communication1)) 17 | 18 | timeout_setting = 0.00005 19 | received_ids = [0,0,0,0,0,0] 20 | 21 | # Initialize position values 22 | position_values = np.array([0, 0, 0, 0, 0, 0]) 23 | 24 | while True: 25 | 26 | time1 = time.perf_counter() 27 | # Send data to all motors 28 | Motor[0].Send_Respond_Encoder_data() 29 | Motor[1].Send_Respond_Encoder_data() 30 | Motor[2].Send_Respond_Encoder_data() 31 | Motor[3].Send_Respond_Encoder_data() 32 | Motor[4].Send_Respond_Encoder_data() 33 | Motor[5].Send_Respond_Encoder_data() 34 | 35 | 36 | for i in range(1, 9): # Loop 9-1=8 to check for received data 37 | message, UnpackedMessageID = Communication1.receive_can_messages(timeout=timeout_setting) 38 | print(f"unpack{i} is: {UnpackedMessageID}") 39 | 40 | # Check if UnpackedMessageID is not None 41 | if UnpackedMessageID is not None: 42 | 43 | # Update received id index; meaning that we received response from that CAN ID 44 | received_ids[UnpackedMessageID[0]] = 1 45 | Motor[UnpackedMessageID[0]].UnpackData(message,UnpackedMessageID) 46 | print(f"Motor {UnpackedMessageID[0]}, position is: {Motor[UnpackedMessageID[0]].position}") 47 | position_values[UnpackedMessageID[0]] = Motor[UnpackedMessageID[0]].position 48 | 49 | """ 50 | print("Received IDs:", received_ids) 51 | if all(count == 1 for count in received_ids): 52 | print("All indexes are equal to 1") 53 | else: 54 | # Find indices not equal to 1 55 | indices_not_equal_to_1 = [i for i, count in enumerate(received_ids) if count != 1] 56 | if indices_not_equal_to_1: 57 | print("Indexes not equal to 1:", indices_not_equal_to_1) 58 | # Send data packs for indices not equal to 1 and try to receive data 59 | for index in indices_not_equal_to_1: 60 | Motor[index].Send_Respond_Encoder_data() 61 | # Try to receive data after sending data 62 | message, UnpackedMessageID = Communication1.receive_can_messages(timeout=timeout_setting) 63 | if UnpackedMessageID is not None: 64 | print(f"Received data after sending to Motor {index + 1}: {UnpackedMessageID}") 65 | Motor[UnpackedMessageID[0]].UnpackData(message,UnpackedMessageID) 66 | print(f"Motor {UnpackedMessageID[0]}, position is: {Motor[UnpackedMessageID[0]].position}") 67 | """ 68 | print(position_values) 69 | received_ids = [0,0,0,0,0,0] 70 | 71 | time2 = time.perf_counter() 72 | print(f"Total time is: {time2-time1}") 73 | 74 | time.sleep(1) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Petar Crnjak 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | [![License: MIT](https://img.shields.io/badge/License-MIT-green.svg)](https://opensource.org/licenses/MIT) ![Issues](https://img.shields.io/github/issues/PCrnjak/CM6_COBOT_ROBOT) ![release](https://img.shields.io/github/v/release/PCrnjak/CM6_COBOT_ROBOT) 3 | # CM6 V2 Robotic arm 4 | 5 | Our official website: https://source-robotics.com/
6 | Join the discord community: https://discord.com/invite/prjUvjmGpZ 7 | 8 | drawing 9 | 10 | 11 | **If you want to build old version of CM6 check branch CM6_V1**
12 | 13 | **Main difference between V1 and V2 are in the drivers used. V1 used S-drive that was hard to source and had poor software support and were expensive while V2 uses [Spectral drives](https://github.com/PCrnjak/Spectral-Micro-BLDC-controller) that are much cheeper and have good software support and connectivity.** 14 | 15 | 16 | CM6 uses 6 gimbal BLDC motors paired with small gear ratio gearboxes ( from 5:1 to 9:1), by doing that it is passively compliant and safe. Each Joint uses an [Spectral driver](https://github.com/PCrnjak/Spectral-Micro-BLDC-controller) that is mounted on modular actuator designs for specific gimbal motors. Design can be changed easily by using different-sized aluminum extrusions or changing the gear ratio of modular gearboxes. The robots main goal is to be research platform for developement of compliant and safe robotics. 17 | 18 | ***Note* that this robot is not as refined as our other robots like PAROL6; some parts are hard to source and might be much more expensive. Building instructions are also not as good and you will need to tinker a lot!** 19 | 20 | 21 | # Contents: 22 | 23 | - [Robot arm build instructions](https://github.com/PCrnjak/CM6_COBOT_ROBOT/blob/main/assembly_instructions_v2.pdf) 24 | - [Robot python software and GUI *Coming soon]() 25 | 26 | # Where to see more CM6 robotic arm? 27 | - [Youtube](https://www.youtube.com/channel/UCp3sDRwVkbm7b2M-2qwf5aQ) 28 | - [Hackaday](https://hackaday.io/project/180588-cm6-compliant-3d-printed-robotic-arm) 29 | - [Instagram](https://www.instagram.com/source_robotics/) 30 | 31 | 32 | # Support the project 33 | 34 | The majority of this project is open source and freely available to everyone. Your assistance, whether through donations or advice, is highly valued. Thank you! 35 | 36 | [![General badge](https://img.shields.io/badge/PayPal-00457C?style=for-the-badge&logo=paypal&logoColor=white)](https://paypal.me/PCrnjak?locale.x=en_US) 37 | [![General badge](https://img.shields.io/badge/Patreon-F96854?style=for-the-badge&logo=patreon&logoColor=white)](https://www.patreon.com/PCrnjak) 38 | 39 | # Project is under MIT Licence 40 | -------------------------------------------------------------------------------- /STEPS_CM6/CM6_V2_PARTS.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_COBOT_ROBOT/883c2df6d364e15ab69c7a79f047ea2b6439e828/STEPS_CM6/CM6_V2_PARTS.zip -------------------------------------------------------------------------------- /STEPS_CM6/SSG_GRIPPER_CM6.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_COBOT_ROBOT/883c2df6d364e15ab69c7a79f047ea2b6439e828/STEPS_CM6/SSG_GRIPPER_CM6.zip -------------------------------------------------------------------------------- /STEPS_CM6/STEPS.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_COBOT_ROBOT/883c2df6d364e15ab69c7a79f047ea2b6439e828/STEPS_CM6/STEPS.zip -------------------------------------------------------------------------------- /STEPS_CM6/STEPS_2.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_COBOT_ROBOT/883c2df6d364e15ab69c7a79f047ea2b6439e828/STEPS_CM6/STEPS_2.zip -------------------------------------------------------------------------------- /STEPS_CM6/STEPS_3.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_COBOT_ROBOT/883c2df6d364e15ab69c7a79f047ea2b6439e828/STEPS_CM6/STEPS_3.zip -------------------------------------------------------------------------------- /STEPS_CM6/STEPS_4.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_COBOT_ROBOT/883c2df6d364e15ab69c7a79f047ea2b6439e828/STEPS_CM6/STEPS_4.zip -------------------------------------------------------------------------------- /STEPS_CM6/STEPS_J5.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_COBOT_ROBOT/883c2df6d364e15ab69c7a79f047ea2b6439e828/STEPS_CM6/STEPS_J5.zip -------------------------------------------------------------------------------- /STEPS_CM6/STEPS_J6.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_COBOT_ROBOT/883c2df6d364e15ab69c7a79f047ea2b6439e828/STEPS_CM6/STEPS_J6.zip -------------------------------------------------------------------------------- /assembly_instructions_v2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_COBOT_ROBOT/883c2df6d364e15ab69c7a79f047ea2b6439e828/assembly_instructions_v2.pdf -------------------------------------------------------------------------------- /images/CM6_img.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_COBOT_ROBOT/883c2df6d364e15ab69c7a79f047ea2b6439e828/images/CM6_img.png -------------------------------------------------------------------------------- /simply.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_COBOT_ROBOT/883c2df6d364e15ab69c7a79f047ea2b6439e828/simply.zip --------------------------------------------------------------------------------