Skip to content

Commit

Permalink
Optimize script code to use scalar operator
Browse files Browse the repository at this point in the history 
Number of operations matters when a urscript is executed with 500Hz
This commit take advantages of the scalar operator to
reduce the number of operations, thus reduce the CPU load
  • Loading branch information
@urrsk
urrsk committed Jul 1, 2023
1 parent 35a5476 commit 21d38e2
Showing 1 changed file with 15 additions and 15 deletions.
30 changes: 15 additions & 15 deletions resources/external_control.urscript
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,9 +84,9 @@ def set_servo_setpoint(q):
end

def extrapolate():
diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
diff = cmd_servo_q - cmd_servo_q_last
cmd_servo_q_last = cmd_servo_q
cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]
cmd_servo_q = cmd_servo_q + diff

return cmd_servo_q
end
Expand Down Expand Up @@ -222,7 +222,7 @@ thread trajectoryThread():
trajectory_points_left = trajectory_points_left - 1

if raw_point[0] > 0:
local q = [ raw_point[1]/ MULT_jointstate, raw_point[2]/ MULT_jointstate, raw_point[3]/ MULT_jointstate, raw_point[4]/ MULT_jointstate, raw_point[5]/ MULT_jointstate, raw_point[6]/ MULT_jointstate]
local q = [ raw_point[1], raw_point[2], raw_point[3], raw_point[4], raw_point[5], raw_point[6]] / MULT_jointstate
local tmptime = raw_point[INDEX_TIME] / MULT_time
local blend_radius = raw_point[INDEX_BLEND] / MULT_time
if trajectory_points_left > 0:
Expand All @@ -244,16 +244,16 @@ thread trajectoryThread():
elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE:
# Cubic spline
if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC:
qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
qd = [ raw_point[7], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate
cubicSplineRun(q, qd, tmptime)

# reset old acceleration
last_spline_qdd = [0, 0, 0, 0, 0, 0]

# Quintic spline
elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC:
qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
qdd = [ raw_point[13]/ MULT_jointstate, raw_point[14]/ MULT_jointstate, raw_point[15]/ MULT_jointstate, raw_point[16]/ MULT_jointstate, raw_point[17]/ MULT_jointstate, raw_point[18]/ MULT_jointstate]
qd = [ raw_point[7], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate
qdd = [ raw_point[13], raw_point[14], raw_point[15], raw_point[16], raw_point[17], raw_point[18]] / MULT_jointstate
quinticSplineRun(q, qd, qdd, tmptime)
end
end
Expand Down Expand Up @@ -383,17 +383,17 @@ thread script_commands():
zero_ftsensor()
elif command == SET_PAYLOAD:
mass = raw_command[2] / MULT_jointstate
cog = [raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate]
cog = [raw_command[3], raw_command[4], raw_command[5]] / MULT_jointstate
set_payload(mass, cog)
elif command == SET_TOOL_VOLTAGE:
tool_voltage = raw_command[2] / MULT_jointstate
set_tool_voltage(tool_voltage)
elif command == START_FORCE_MODE:
task_frame = p[raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate]
selection_vector = [raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate, raw_command[12] / MULT_jointstate, raw_command[13] / MULT_jointstate]
wrench = [raw_command[14] / MULT_jointstate, raw_command[15] / MULT_jointstate, raw_command[16] / MULT_jointstate, raw_command[17] / MULT_jointstate, raw_command[18] / MULT_jointstate, raw_command[19] / MULT_jointstate]
task_frame = p[raw_command[2], raw_command[3], raw_command[4], raw_command[5], raw_command[6], raw_command[7]] / MULT_jointstate
selection_vector = [raw_command[8], raw_command[9], raw_command[10], raw_command[11], raw_command[12], raw_command[13]] / MULT_jointstate
wrench = [raw_command[14], raw_command[15], raw_command[16], raw_command[17], raw_command[18], raw_command[19]] / MULT_jointstate
force_type = raw_command[20] / MULT_jointstate
force_limits = [raw_command[21] / MULT_jointstate, raw_command[22] / MULT_jointstate, raw_command[23] / MULT_jointstate, raw_command[24] / MULT_jointstate, raw_command[25] / MULT_jointstate, raw_command[26] / MULT_jointstate]
force_limits = [raw_command[21], raw_command[22], raw_command[23], raw_command[24], raw_command[25], raw_command[26]] / MULT_jointstate
force_mode(task_frame, selection_vector, wrench, force_type, force_limits)
elif command == END_FORCE_MODE:
end_force_mode()
Expand Down Expand Up @@ -471,10 +471,10 @@ while keepalive > 0 and control_mode > MODE_STOPPED:

# Update the motion commands with new parameters
if control_mode == MODE_SERVOJ:
q = [params_mult[2]/ MULT_jointstate, params_mult[3]/ MULT_jointstate, params_mult[4]/ MULT_jointstate, params_mult[5]/ MULT_jointstate, params_mult[6]/ MULT_jointstate, params_mult[7]/ MULT_jointstate]
q = [params_mult[2], params_mult[3], params_mult[4], params_mult[5], params_mult[6], params_mult[7]] / MULT_jointstate
set_servo_setpoint(q)
elif control_mode == MODE_SPEEDJ:
qd = [params_mult[2]/ MULT_jointstate, params_mult[3]/ MULT_jointstate, params_mult[4]/ MULT_jointstate, params_mult[5]/ MULT_jointstate, params_mult[6]/ MULT_jointstate, params_mult[7]/ MULT_jointstate]
qd = [params_mult[2], params_mult[3], params_mult[4], params_mult[5], params_mult[6], params_mult[7]] / MULT_jointstate
set_speed(qd)
elif control_mode == MODE_FORWARD:
if params_mult[2] == TRAJECTORY_MODE_RECEIVE:
Expand All @@ -489,10 +489,10 @@ while keepalive > 0 and control_mode > MODE_STOPPED:
socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
end
elif control_mode == MODE_SPEEDL:
twist = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
twist = [params_mult[2], params_mult[3], params_mult[4], params_mult[5], params_mult[6], params_mult[7]] / MULT_jointstate
set_speedl(twist)
elif control_mode == MODE_POSE:
pose = p[params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
pose = p[params_mult[2], params_mult[3], params_mult[4], params_mult[5], params_mult[6], params_mult[7]] / MULT_jointstate
set_servo_pose(pose)
elif control_mode == MODE_FREEDRIVE:
if params_mult[2] == FREEDRIVE_MODE_START:
Expand Down

0 comments on commit 21d38e2

Please sign in to comment.