Hello everyone,
I’m currently working on a KUKA robot control system, and I’m looking for some guidance on managing and executing robot trajectories dynamically. Specifically, I want to modularize the handling of waypoints (1 to 100 waypoints per trajectory) and allow the PLC to execute these trajectories when needed.
Current Setup:
- The trajectory planner computes robot movement paths and sends the data to the robot. These paths consist of waypoints (A1 to A6), and each trajectory has a fixed number of waypoints (currently 10 waypoints).
- The robot stores these trajectories in its memory, and the PLC can later trigger the execution by specifying the program number (progNum), which determines which trajectory to execute.
Goal:
- I would like to modularize the number of waypoints in a trajectory so that the number of waypoints can vary dynamically, rather than being fixed at 10 waypoints. Ideally, I want the system to support 1 to 100 waypoints per trajectory.
- The planner will compute and send the waypoints, and the PLC will simply execute the trajectory by calling the progNum without worrying about the number of waypoints.
Receive something like this where wp_list it's a list of joint (A1 to A6), num_of_wp (1 to 100) is the number of wp in the trajectory and traj_number it's the number of the trajectory.
RET = EKI_GetRealArray("planner", "Robot/wp_list", wp_list[])
RET = EKI_GetInt("planner", "Robot/num_of_wp", num_of_wp)
RET = EKI_GetInt("planner", "Robot/traj_number", traj_number)
Code
Display More<Robot> <wp_list>134.2</wp_list> <wp_list>-89.7</wp_list> <wp_list>120.5</wp_list> <wp_list>-45.2</wp_list> <wp_list>80.4</wp_list> <wp_list>190.8</wp_list> <wp_list>-30.9</wp_list> <wp_list>-12.3</wp_list> <wp_list>55.0</wp_list> <wp_list>132.7</wp_list> <wp_list>-24.6</wp_list> <wp_list>-310.5</wp_list> ... </Robot> <Robot> <num_of_wp>2</num_of_wp> </Robot> <Robot> <traj_number>1</traj_number> </Robot>
Key Challenges:
- Dynamic Waypoint Length:
- Currently, the system is hardcoded for 10 waypoints. I want to change this so that the number of waypoints can vary from 1 to 100, without requiring any changes to the program code for each new trajectory length.
- Executing Trajectories by progNum (traj_number) :
- The PLC should not need to manage the waypoints directly. It should only provide the progNum to the robot to execute the corresponding trajectory stored in memory.
- Efficient Access and Execution of Trajectories:
- I need a clean and flexible way to store trajectories and execute them dynamically based on the progNum.
Current Approach:
Here is the current logic for executing the trajectory based on progNum:
;FOLD poses definition
iProgNum_move = iProgNum_move - 1000
WAIT FOR ((program[iProgNum_move].position10.speed) <> 0)
;FOLD position 1
BAS(#VEL_PTP, program[iProgNum_move].position1.speed)
BAS(#ACC_PTP, 100)
pose1.A1 = program[iProgNum_move].position1.A1
pose1.A2 = program[iProgNum_move].position1.A2
pose1.A3 = program[iProgNum_move].position1.A3
pose1.A4 = program[iProgNum_move].position1.A4
pose1.A5 = program[iProgNum_move].position1.A5
pose1.A6 = program[iProgNum_move].position1.A6
;ENDFOLD
... (repeated for positions 2 to 10)
LINCORRECTION:
IF NOT lin_correction THEN
;FOLD PTP (plan trajectory)
SPTP pose1 C_DIS
SPTP pose2 C_DIS
SPTP pose3 C_DIS
SPTP pose4 C_DIS
SPTP pose5 C_DIS
SPTP pose6 C_DIS
SPTP pose7 C_DIS
SPTP pose8 C_DIS
SPTP pose9 C_DIS
SPTP pose10
;ENDFOLD
ELSE
; Handle linear correction if needed
...
ENDIF
Display More
This approach works well for 10 fixed waypoints, but I would like to extend it to handle a variable number of waypoints, from 1 to 100 waypoints.
What I’ve done:
I did this outside of my main code. But I am missing the part where the points are linked to a trajectory number.
; Initialize flags and buffer
FOR i = 1 TO 9999
wp_list[i] = 0
ENDFOR
num_of_points = 0
; Initialize EKI communication
RET = EKI_Init("planner")
RET = EKI_Open("planner")
; Move robot to home position first
PTP XHOME
BAS(#VEL_PTP, 100)
BAS(#ACC_PTP, 100)
$ADVANCE = 5
LOOP
; Wait for new trajectory data from ethernet interface
WAIT FOR $FLAG[199] ; Flag signals trajectory_list data ready
RET = EKI_GetRealArray("planner", "Robot/wp_list", wp_list[])
$FLAG[199] = FALSE
WAIT FOR $FLAG[197] ; Flag signals num_of_traj ready
RET = EKI_GetInt("planner", "Robot/num_of_wp", num_of_wpoints)
$FLAG[197] = FALSE
; Validate number of points
IF (num_of_wpoints < 1) OR (num_of_wpoints > 100) THEN
HALT ; invalid number of points
ENDIF
; Parse trajectory_list[] into traj_buffer E6AXIS array
FOR i = 1 TO num_of_points
traj_buffer[i].A1 = wp_list[(i-1)*6 + 1]
ax1 = traj_buffer[i].A1
traj_buffer[i].A2 = wp_list[(i-1)*6 + 2]
ax2 = traj_buffer[i].A2
traj_buffer[i].A3 = wp_list[(i-1)*6 + 3]
ax3 = traj_buffer[i].A3
traj_buffer[i].A4 = wp_list[(i-1)*6 + 4]
ax4 = traj_buffer[i].A4
traj_buffer[i].A5 = wp_list[(i-1)*6 + 5]
ax5 = traj_buffer[i].A5
traj_buffer[i].A6 = wp_list[(i-1)*6 + 6]
ax6 = traj_buffer[i].A6
ENDFOR
; Move through spline trajectory with blending
$APO.CDIS = 150 ; blending zone [mm] adjust for smoothness
FOR i = 1 TO num_of_points
IF i <> num_of_wpoints THEN
SPTP traj_buffer[i] C_DIS
ELSE
SPTP traj_buffer[i]
ENDIF
ENDFOR
; Ready for next trajectory
ENDLOOP
END
Display More
In the end it would be to have a trajectory list which have inside the number of the trajectory, the number of wp and the list of wp.
Example : traj n°1, 24 waypoints, list of wp [A1 … A6]*24
I might have taken the wrong way to do it, so I am happy to receive any suggestions as it's currently blocking my development !
Thanks!