diff --git a/.gitmodules b/.gitmodules index 65d5ef54e..6bdc5afb7 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1253,6 +1253,9 @@ [submodule "vendor/grammars/typst-grammar"] path = vendor/grammars/typst-grammar url = https://github.com/michidk/typst-grammar.git +[submodule "vendor/grammars/urscript-extension"] + path = vendor/grammars/urscript-extension + url = https://github.com/ahernguo/urscript-extension.git [submodule "vendor/grammars/verilog.tmbundle"] path = vendor/grammars/verilog.tmbundle url = https://github.com/textmate/verilog.tmbundle diff --git a/grammars.yml b/grammars.yml index 219734dcf..5d695c502 100644 --- a/grammars.yml +++ b/grammars.yml @@ -1118,6 +1118,8 @@ vendor/grammars/typespec: - source.tsp vendor/grammars/typst-grammar: - source.typst +vendor/grammars/urscript-extension: +- source.urscript vendor/grammars/verilog.tmbundle: - source.verilog vendor/grammars/vhdl: diff --git a/lib/linguist/heuristics.yml b/lib/linguist/heuristics.yml index 923303e6d..48fb380d3 100644 --- a/lib/linguist/heuristics.yml +++ b/lib/linguist/heuristics.yml @@ -709,6 +709,10 @@ disambiguations: - language: Markdown # Markdown syntax for scdoc pattern: '^#+\s+(NAME|SYNOPSIS|DESCRIPTION)' +- extensions: ['.script'] + rules: + - language: URScript + pattern: '^\s*def\s+[a-zA-Z_]\w*\s*\([^)]*\)\s*:[\s\S]*?\send($|\s)' - extensions: ['.sol'] rules: - language: Solidity diff --git a/lib/linguist/languages.yml b/lib/linguist/languages.yml index b5eebfe28..e7ffcb59a 100644 --- a/lib/linguist/languages.yml +++ b/lib/linguist/languages.yml @@ -7547,6 +7547,14 @@ Typst: tm_scope: source.typst ace_mode: text language_id: 704730682 +URScript: + type: programming + color: "#56A2D6" + extensions: + - ".script" + tm_scope: source.urscript + ace_mode: text + language_id: 431829457 Unified Parallel C: type: programming color: "#4e3617" diff --git a/samples/URScript/admittance_control.script b/samples/URScript/admittance_control.script new file mode 100644 index 000000000..9c305fae8 --- /dev/null +++ b/samples/URScript/admittance_control.script @@ -0,0 +1,251 @@ + +### +# Helper function to make a diagnol matrix +# @param values array input list or pose +# @returns matrix diagnol matrix +### +def adm_diag_6_6(values): + return [[values[0], 0, 0, 0, 0, 0], [0, values[1], 0, 0, 0, 0], [0, 0, values[2], 0, 0, 0], [0, 0, 0, values[3], 0, 0], [0, 0, 0, 0, values[4], 0], [0, 0, 0, 0, 0, values[5]]] +end + + +### +# Helper function to extract the position part from a pose +# @param p_in pose input pose +# @returns array position from pose +### +def adm_get_pos_from_pose(p_in): + return [p_in[0], p_in[1], p_in[2]] +end + +### +# Helper function to extract the rotation part from a pose +# @param p_in pose input pose +# @returns array rotation from pose +### +def adm_get_rot_from_pose(p_in): + return [p_in[3], p_in[4], p_in[5]] +end + +### +# Get the measured wrench at the tool flange +# @returns array wrench at the tool flange +### +def adm_get_flange_wrench(): + local ft = get_tcp_force() #Tcp force returns the force and torques at the tool flange with base orientation + local t_end_base = pose_trans(get_target_tcp_pose(), pose_inv(get_tcp_offset())) + local current_rot_in_tool = pose_inv(p[0, 0, 0, t_end_base[3], t_end_base[4], t_end_base[5]]) + local f = pose_trans(current_rot_in_tool, p[ft[0], ft[1], ft[2], 0, 0, 0]) + local t = pose_trans(current_rot_in_tool, p[ft[3], ft[4], ft[5], 0, 0, 0]) + return [f[0], f[1], f[2], t[0], t[1], t[2]] +end + +### +# Apply a dead band for force and torques. For make the robot stand still when not driven external forces +# @param wrench_in array input wrench that might be zeroed +# @param bandwidth_force number force bandwith +# @param bandwidth_torque number torque bandwith +# @returns array wrench that might be zeroed +### +def adm_apply_dead_band(wrench_in, bandwidth_force, bandwidth_torque): + local F = adm_get_pos_from_pose(wrench_in) + local F_norm = norm(F) + local T = adm_get_rot_from_pose(wrench_in) + local T_norm = norm(T) + + if ((F_norm < bandwidth_force) and (T_norm < bandwidth_torque)): + wrench_in = wrench_in * 0 + end + + return wrench_in +end + +### +# Get a smooth dead band scale. For use in the appplying dead band functions +# @param value number input value +# @param deadBand number deadBand +# @param smoothBand number smoothBand +# @returns number scale in the range from 0-1 +### +def adm_smooth_dead_single_dim_scale(value, deadBand, smoothBand): + local normVal = norm(value) + if normVal <= deadBand: + #In deadband + return 0 + + elif normVal > (deadBand + smoothBand): + #Pass deadband + return ((normVal - deadBand - smoothBand * 0.5) / normVal) + end + + #Smooth trancision + local s = normVal - deadBand + return (0.5 * s * s / smoothBand) / normVal +end + +### TEST_START +# # Test function for continuity in adm_smooth_dead_single_dim_scale +# def adm_test_smooth_dead_band(): +# def test_smooth_config(dead_band, smooth_band): +# #zero in-out test +# assert(adm_smooth_dead_single_dim_scale(0, dead_band, smooth_band) == 0) + +# #continuity test +# def continuity(dead_band, smooth_band): +# local x = 0 +# local dx = 0.1 +# local y = 0 +# while x < (dead_band + smooth_band + 50): +# local dy = norm(adm_smooth_dead_single_dim_scale(x, dead_band, smooth_band) - y) +# #Test that the change in y does not gets higher than the change in x +# assert(dy <= dx) +# x = x + dx +# y = y + dy +# end +# end +# continuity(dead_band, smooth_band) + +# end +# test_smooth_config(1, 2) +# test_smooth_config(10, 20) +# test_smooth_config(0, 20) +# test_smooth_config(20, 0) + +# end +# adm_test_smooth_dead_band() #run test +### TEST_STOP + +### +# Apply a smooth dead band to make to robot stand still when not driven external forces +# @param wrench_in array wrench_in that will be scaled +# @param bandwidth_force number force bandwidth +# @param bandwidth_torque number torque bandwidth +# @param smooth_force number force smooth +# @param smooth_torque number torque smooth +# @returns array wrench where the smooth dead band is applied +### +def adm_apply_dead_band_smooth(wrench_in, bandwidth_force, bandwidth_torque, smooth_force, smooth_torque): + local wrench_gain = [adm_smooth_dead_single_dim_scale(wrench_in[0], bandwidth_force, smooth_force), adm_smooth_dead_single_dim_scale(wrench_in[1], bandwidth_force, smooth_force), adm_smooth_dead_single_dim_scale(wrench_in[2], bandwidth_force, smooth_force), adm_smooth_dead_single_dim_scale(wrench_in[3], bandwidth_torque, smooth_torque), adm_smooth_dead_single_dim_scale(wrench_in[4], bandwidth_torque, smooth_torque), adm_smooth_dead_single_dim_scale(wrench_in[5], bandwidth_torque, smooth_torque)] + return wrench_gain * wrench_in +end + +### +# Transform a velocity vector +# @param t pose transformation from the input velocity to the output velocity +# @param v array inout velocity vector +# @returns array tranformed output velocity vector +### +def adm_vel_trans(t, v): + local vw = wrench_trans(t, [v[3], v[4], v[5], v[0], v[1], v[2]]) + return [vw[3], vw[4], vw[5], vw[0], vw[1], vw[2]] +end + +### +# rotates a velocity into new reference frame +# @param frame pose current velocity reference frame +# @param velocity array input velocity vector +# @returns array velocity with new reference frame +### +def adm_rotate_velocity_in_frame(frame, velocity): + local p_tcp = p[0, 0, 0, frame[3], frame[4], frame[5]] + + local velocity_pos_p = p[velocity[0], velocity[1], velocity[2], 0, 0, 0] + local velocity_rot_p = p[velocity[3], velocity[4], velocity[5], 0, 0, 0] + + local trans = pose_trans(p_tcp, velocity_pos_p) + local rot = pose_trans(p_tcp, velocity_rot_p) + + return [trans[0], trans[1], trans[2], rot[0], rot[1], rot[2]] +end + +### +# rotates a wrench into new reference frame +# @param frame pose current wrench reference frame +# @param wrench array input wrench vector +# @returns array wrench with new reference frame +### +def adm_rotate_wrench_in_frame(frame, wrench): + return adm_rotate_velocity_in_frame(frame, wrench) +end + +# Please note that parameters needs to be tuned for each use case and robot model. +# The default parameters is tune for a light behavior on a UR5e without payload. +# Watch out for singular positions! +### +# summary +# @param mass_scaling number scale of mases +# @param damping_scaling number scale of the damping +# @param mass_list array list of masses for the 6 DoF +# @param damping_list array list of damping for the 6 DoF +# @param base_to_compliance_frame pose compliance frame +# @param tool_flange_to_compliance_center pose transformation from tool flange to center of compliance +# @param dead_band array list of the deadband [Force dead band, Torque dead band, Force smooth band, Torque smooth band] +# @param compliance_vector array A 6d vector of 0s and 1s. 1 means that the robot will apply admittance in the corresponding axis of the compliance frame. +# @param stiffness_params array list of stiffness for the 6 DoF +# @param target_wrench array the target wrench the robot shall apply +### +def admittance_control(mass_scaling = 0.5, damping_scaling = 0.5, mass_list = [22.5, 22.5, 22.5, 1, 1, 1], damping_list = [25, 25, 25, 2, 2, 2], base_to_compliance_frame = p[0, 0, 0, 0, 0, 0], tool_flange_to_compliance_center = p[0, 0, 0, 0, 0, 0], dead_band = [2, 0.15, 2, 0.15], compliance_vector = [1, 1, 1, 1, 1, 1], stiffness_params = [0, 0, 0, 0, 0, 0], target_wrench = [0, 0, 0, 0, 0, 0]): + + ## Admittance control parameters ## + local M = adm_diag_6_6(mass_list) #Mass + local M = M * mass_scaling + local D = adm_diag_6_6(damping_list) # Damping + local D = D * damping_scaling + local K = adm_diag_6_6(stiffness_params) # Stiffness + + ## Initialize Error terms ## + local vel_target = [0, 0, 0, 0, 0, 0] + local x_e = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # pos error term + local dx_e = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # vel error term + local ddx_e = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # acc error term + local last_ddx_e = ddx_e + local last_dx_e = dx_e + local last_x_e = x_e + + ## Zero F/T sensor before starting ## + zero_ftsensor() + + ## Admittance control loop ## + while True: + #Dead band to make to robot stand still when not driven external forces + local wrench_at_tool = adm_apply_dead_band_smooth(adm_get_flange_wrench(), dead_band[0], dead_band[1], dead_band[2], dead_band[3]) + + # Make it possible to define the compliance center relative to the tool flange by "tool_flange_to_compliance_center" argument + local wrench_at_compliance_center = wrench_trans(tool_flange_to_compliance_center, wrench_at_tool) + + # Rotate the wrench to align with the compliance frame give by "base_to_compliance_frame" argument + local T_compliance_center_to_tcp = pose_trans(pose_inv(tool_flange_to_compliance_center), get_tcp_offset()) + local T_compliance_center_to_base = pose_trans(T_compliance_center_to_tcp, pose_inv(get_target_tcp_pose())) + local T_compliance_frame_to_compliance_center = pose_inv(pose_trans(T_compliance_center_to_base, base_to_compliance_frame)) + local force_torque_error = adm_rotate_wrench_in_frame(T_compliance_frame_to_compliance_center, wrench_at_compliance_center) + + # Apply compliance selection in "base_to_compliance_frame" + force_torque_error = force_torque_error * compliance_vector + + # Do the admittance integration + ddx_e = inv(M) * ((force_torque_error - target_wrench) - (K * x_e) - (D * dx_e)) + dx_e = (get_steptime() * 0.5) * (ddx_e + last_ddx_e) + last_dx_e + x_e = (get_steptime() * 0.5) * (dx_e + last_dx_e) + last_x_e + + last_ddx_e = ddx_e + last_dx_e = dx_e + last_x_e = x_e + + # Rotate back from the compliance frame to the compliance tool center frame + local vel_target_flange = adm_rotate_wrench_in_frame(pose_inv(T_compliance_frame_to_compliance_center), dx_e) + + # Transform the velocity to be based at TCP + local vel_target_tcp = adm_vel_trans(pose_inv(T_compliance_center_to_tcp), vel_target_flange) + + # Rotation the velocity to be based at TCP with base orientation to fit the format for speedl + local vel_target_base_tcp = adm_rotate_velocity_in_frame(get_target_tcp_pose(), vel_target_tcp) + + #Make the robot move + speedl(vel_target_base_tcp, a = 5, t = get_steptime(), aRot = 45) + end +end + +# Run the admittance control function +ALONG_AND_AROUND_Z_AXIS = [0, 0, 1, 0, 0, 1] +ALL_AXIS = [1, 1, 1, 1, 1, 1] +admittance_control(compliance_vector = ALL_AXIS) \ No newline at end of file diff --git a/samples/URScript/math.script b/samples/URScript/math.script new file mode 100644 index 000000000..52ac9cff7 --- /dev/null +++ b/samples/URScript/math.script @@ -0,0 +1,77 @@ +### +# Convert a axis angle rotation vector into a rotation matrix +# @param rotvec array rotation vector in axis angle representation (rx, ry, rx) +# @returns floatmatrix rotation matrix +### +def rotvec2rotmat(rotvec): + + local rx = rotvec[0] + local ry = rotvec[1] + local rz = rotvec[2] + + # rotation vector to angle and unit vector + local theta = sqrt(rx*rx + ry*ry + rz*rz) + ux = 0 + uy = 0 + uz = 0 + if (theta != 0): + ux = rx/theta + uy = ry/theta + uz = rz/theta + end + cth = cos(theta) + sth = sin(theta) + vth = 1-cos(theta) + + # column 1 + local r11 = ux*ux*vth + cth + local r21 = ux*uy*vth + uz*sth + local r31 = ux*uz*vth - uy*sth + # column 2 + local r12 = ux*uy*vth - uz*sth + local r22 = uy*uy*vth + cth + local r32 = uy*uz*vth + ux*sth + # column 3 + local r13 = ux*uz*vth + uy*sth + local r23 = uy*uz*vth - ux*sth + local r33 = uz*uz*vth + cth + + # elements are represented as an array + #local rotmat = [[r11, r21, r31],[ r12, r22, r32],[ r13, r23, r33] ] + local rotmat = [[r11, r12, r13],[ r21, r22, r23],[ r31, r32, r33] ] + + return rotmat +end + +### +# Convert a inertia vector(Ixx, Iyy, Izz, Ixy, Ixz, Iyz) into a symetric Inertia matrix +# @param i array input inertia vector(Ixx, Iyy, Izz, Ixy, Ixz, Iyz) +# @returns floatmatrix symetric Inertia matrix +### +def ivec2imat(i): + + # column 1 + local r11 = i[0] + local r21 = i[3] + local r31 = i[4] + # column 2 + local r12 = i[3] + local r22 = i[1] + local r32 = i[5] + # column 3 + local r13 = i[4] + local r23 = i[5] + local r33 = i[2] + local imat = [[r11, r12, r13],[ r21, r22, r23],[ r31, r32, r33] ] + + return imat +end + +### +# Convert a symetric Inertia matrix into a inertia vector(Ixx, Iyy, Izz, Ixy, Ixz, Iyz) +# @param i floatmatrix symetric Inertia matrix +# @returns array input inertia vector(Ixx, Iyy, Izz, Ixy, Ixz, Iyz) +### +def imat2ivec(i): + return [i[0,0], i[1,1], i[2,2], i[0,1], i[0,2], i[1,2]] +end \ No newline at end of file diff --git a/samples/URScript/payload_get_struct.script b/samples/URScript/payload_get_struct.script new file mode 100644 index 000000000..6df5940d5 --- /dev/null +++ b/samples/URScript/payload_get_struct.script @@ -0,0 +1,83 @@ +### +# Get a payload struct of a rectangular cuboid +# @param mass float cuboid mass[kg] +# @param x float x lenght of cuboid[meter] +# @param y float y lenght of cuboid[meter] +# @param z float z lenght of cuboid[meter] +# @returns struct payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +### +def payload_get_rectangular_cuboid(mass, x, y, z): + local s=1/12 + local m=mass + return payload_get_struct(mass, [0,0,0], [s*m*(y*y+z*z), s*m*(x*x+z*z), s*m*(x*x+y*y), 0,0,0]) +end + + +### +# Get a payload struct of a solid sphere +# @param mass float sphere mass[kg] +# @param r float sphere radius[meter] +# @returns struct payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +### +def payload_get_sphere_solid(mass, r): + local s=2/5 + local m=mass + return payload_get_struct(mass, [0,0,0], [s*m*r*r, s*m*r*r, s*m*r*r, 0,0,0]) +end + +### +# Get a payload struct of a hollow sphere +# @param mass float sphere mass[kg] +# @param r float sphere radius[meter] +# @returns struct payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +### +def payload_get_sphere_hollow(mass, r): + local s=2/3 + local m=mass + return payload_get_struct(mass, [0,0,0], [s*m*r*r, s*m*r*r, s*m*r*r, 0,0,0]) +end + + + +### +# Get a payload struct of a cylindrical tube, where the height is along the z-axis +# @param mass float tube mass[kg] +# @param height float height of tube[meter] +# @param radius1 float outer radius of tube[meter] +# @param radius2 float inner radius of tube[meter] +# @returns struct payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +### +def payload_get_cylindrical_tube(mass, height, radius1, radius2): + local s=1/12 + local m=mass + local h=height + local r1 = radius1 + local r2 = radius2 + return payload_get_struct(mass, [0,0,0], [s*m*(3*(r2*r2+r1*r1)+h*h), s*m*(3*(r2*r2+r1*r1)+h*h), 0.5*m*(r2*r2+r1*r1), 0,0,0]) +end + +### +# Get a payload struct of a cylinder , where the height is along the z-axis +# @param mass float tube mass[kg] +# @param height float height of cylinder[meter] +# @param radius float outer radius of cylinder[meter] +# @returns struct payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +### +def payload_get_cylinder(mass, height, radius): + return payload_get_cylindrical_tube(mass, height, 0, radius) +end + +### +# Get a payload struct of a cylinder , where the height is along the z-axis +# @param mass float mass[kg] +# @param cog array center of gravity(x,y,x)[meter] +# @param inertia array inertia matrix(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*meter^2] +# @returns struct payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +### +def payload_get_struct(mass,cog,inertia): + return_value = struct(mass = mass, cog = cog, inertia = inertia) + if not is_payload_struct(return_value): + return_value = struct(mass = 0, cog = [0,0,0], inertia = [0,0,0,0,0,0]) + end + return return_value +end \ No newline at end of file diff --git a/samples/URScript/payload_manipulators.script b/samples/URScript/payload_manipulators.script new file mode 100644 index 000000000..fe5228ba0 --- /dev/null +++ b/samples/URScript/payload_manipulators.script @@ -0,0 +1,75 @@ +### +# Get a payload struct that is the resulting payload of the two arguments +# @param a struct first payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +# @param b struct second payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +# @returns struct reulting payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +### +def payload_add(a,b): + + #cog index + local X = 0 + local Y = 1 + local Z = 2 + + local IXX = 0 + local IYY = 1 + local IZZ = 2 + local IXY = 3 + local IXZ = 4 + local IYZ = 5 + + + local mass = a.mass + b.mass + local cog = (a.mass*a.cog + b.mass*b.cog)/mass + + local disp_a = a.cog-cog + local disp_b = b.cog-cog + + + #point mass contribution + local disp_inertia_a = [0,0,0,0,0,0] + disp_inertia_a[IXX] = (disp_a[Y]*disp_a[Y]+disp_a[Z]*disp_a[Z])*a.mass + disp_inertia_a[IYY] = (disp_a[X]*disp_a[X]+disp_a[Z]*disp_a[Z])*a.mass + disp_inertia_a[IZZ] = (disp_a[X]*disp_a[X]+disp_a[Y]*disp_a[Y])*a.mass + disp_inertia_a[IXY] = -(disp_a[X]*disp_a[Y])*a.mass + disp_inertia_a[IXZ] = -(disp_a[X]*disp_a[Z])*a.mass + disp_inertia_a[IYZ] = -(disp_a[Y]*disp_a[Z])*a.mass + #disp_inertia_a = abs_of_list(disp_inertia_a) + + local disp_inertia_b = [0,0,0,0,0,0] + disp_inertia_b[IXX] = (disp_b[Y]*disp_b[Y]+disp_b[Z]*disp_b[Z])*b.mass + disp_inertia_b[IYY] = (disp_b[X]*disp_b[X]+disp_b[Z]*disp_b[Z])*b.mass + disp_inertia_b[IZZ] = (disp_b[X]*disp_b[X]+disp_b[Y]*disp_b[Y])*b.mass + disp_inertia_b[IXY] = -(disp_b[X]*disp_b[Y])*b.mass + disp_inertia_b[IXZ] = -(disp_b[X]*disp_b[Z])*b.mass + disp_inertia_b[IYZ] = -(disp_b[Y]*disp_b[Z])*b.mass + + local inertia = a.inertia + b.inertia + disp_inertia_a + disp_inertia_b + + return payload_get_struct(mass, cog, inertia) +end + +### +# Get a payload struct that is the resulting payload of the two arguments +# @param payload struct payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +# @param trans pose pose to translate the payload struct +# @returns struct reulting payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +### +def payload_trans(payload, trans): + + local translation = [trans[0], trans[1], trans[2]] + local rotation = [trans[3], trans[4], trans[5]] + #translate cog + payload.cog = payload.cog + translation + + r_matrix = rotvec2rotmat(rotation) + r_matrix_t = transpose(r_matrix) + i_matrix = ivec2imat(payload.inertia) + + i_matrix = r_matrix*i_matrix + i_matrix = i_matrix*r_matrix_t + + payload.inertia = imat2ivec(i_matrix) + + return payload +end \ No newline at end of file diff --git a/samples/URScript/payload_set.script b/samples/URScript/payload_set.script new file mode 100644 index 000000000..9f54c16b2 --- /dev/null +++ b/samples/URScript/payload_set.script @@ -0,0 +1,7 @@ +### +# Set the target(active) payload for the UR controller +# @param payload struct payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +### +def payload_set_target(payload): + set_target_payload(payload.mass, payload.cog, payload.inertia) +end \ No newline at end of file diff --git a/samples/URScript/payload_tests.script b/samples/URScript/payload_tests.script new file mode 100644 index 000000000..687c68898 --- /dev/null +++ b/samples/URScript/payload_tests.script @@ -0,0 +1,211 @@ +### +# Report if the argument is a payload strct +# @param payload struct payload struct +# @returns bool True if the argument is a payload struct +### +def is_payload_struct(payload): + local return_value = True + str = to_str(payload) + return_value = return_value and ("struct{"==str_sub(str, 0, 7)) + return_value = return_value and (str_find(str, "mass", 0)) + return_value = return_value and (str_find(str, "cog", 0)) + return_value = return_value and (str_find(str, "inertia", 0)) + if return_value: + return_value = return_value and (3 == length(payload.cog)) + return_value = return_value and (6 == length(payload.inertia)) + end + return return_value +end + +### +# Payload struct compare +# @param a struct first payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +# @param b struct second payload struct containing mass[kg], cog(x,y,x)[m], inertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz)[kg*m^2] +# @param epsilon float epsilon +# @returns bool True if the two payload struct are equal +### +def payload_cmp(a, b, epsilon=0.000001): + return_value = True + return_value = return_value and (norm(a.mass-b.mass)b.inertia[i]+epsilon: + return_value = False + end + i=i+1 + end + return return_value +end + +### +# Find max value of list +# @param l array list of floats or integers +# @returns float the maximum value in the list +### +def max_value_of_list(l): + local max = -1000000000 + local i = 0 + while i < get_list_length(l): + if l[i]>max: + max = l[i] + end + i=i+1 + end + return max +end + +### +# Get a list where every entry is the abs of the input +# @param l array list of floats or integers +# @returns array the list where every entry is positive +### +def abs_of_list(l): + local i = 0 + while i < get_list_length(l): + l[i] = norm(l[i]) + i=i+1 + end + return l +end + +### +# all test functions +### +def tests(): + #1 + assert(is_payload_struct(payload_get_struct(0,[0,0,0],[0,0,0,0,0,0]))) + + #2 + assert(False == is_payload_struct(0)) + + #3 + p1 = payload_get_struct(1,[1,2,3], [0,0,0,0,0,0]) + p2 = payload_get_struct(2,[1,-4,1], [0,0,0,0,0,0]) + p3 = payload_add(p1, p2) + assert(p3.mass==3) + + #4 + p1 = payload_get_struct(2,[2,2,2], [0,0,0,0,0,0]) + p2 = payload_get_struct(2,[2,2,2], [0,0,0,0,0,0]) + assert(payload_cmp(p1,p2)) + + #5 + p1 = payload_get_struct(1,[2,2,2], [0,0,0,0,0,0]) + p2 = payload_get_struct(2,[2,2,2], [0,0,0,0,0,0]) + assert(not payload_cmp(p1,p2)) + + #6 + p1 = payload_get_struct(2,[2,1,2], [0,0,0,0,0,0]) + p2 = payload_get_struct(2,[2,2,2], [0,0,0,0,0,0]) + assert(not payload_cmp(p1,p2)) + + #7 + p1 = payload_get_struct(2,[2,2,2], [0,0,0,0,0,0]) + p2 = payload_get_struct(2,[2,2,2], [1,1,1,0,0,0]) + assert(not payload_cmp(p1,p2)) + + #8 + p1 = payload_get_rectangular_cuboid(1, 0.1, 0.1, 0.1) + p2 = payload_trans(p1, p[0.1,0,0,0,0,0]) + p3 = payload_trans(p1, p[-0.1,0,0,0,0,0]) + p4 = payload_add(p1, p2) + p4 = payload_add(p4, p3) + pt = payload_get_rectangular_cuboid(3, 0.3, 0.1, 0.1) + assert(payload_cmp(p4,pt)) + + #9 + p1 = payload_get_rectangular_cuboid(1, 0.1, 0.3, 0.1) + p2 = payload_get_rectangular_cuboid(1, 0.3, 0.1, 0.1) + pt = payload_trans(p2, p[0,0,0,0,0,d2r(90)]) + assert(payload_cmp(p1,pt)) + + #10 + p1 = payload_get_rectangular_cuboid(1, 1, 1, 3) + p2 = p1 + p1 = payload_trans(p1, p[0,0,0,-d2r(45),0,0]) + p1 = payload_trans(p1, p[0,0,0,0,d2r(45),0]) + p2 = payload_trans(p2, pose_trans(p[0,0,0,0,d2r(45),0], p[0,0,0,-d2r(45),0,0])) + assert(payload_cmp(p1,p2)) + + + + #11 + p1 = payload_get_rectangular_cuboid(1, 1, 1, 1) + p2 = payload_trans(p1, p[sqrt(3)/3,sqrt(3)/3,sqrt(3)/3,0,0,0]) + p3 = payload_trans(p1, p[-sqrt(3)/3,-sqrt(3)/3,-sqrt(3)/3,0,0,0]) + p4 = payload_add(p1, p2) + p4 = payload_add(p4, p3) + pt = payload_get_rectangular_cuboid(3, 1, 3, 1) + pt = payload_trans(pt, p[0,0,0,0.58312,-0.24154,-0.75994]) + assert(payload_cmp(p4,pt, epsilon=0.0001)) + + #12 sphere < Surrounding box + r = 0.1 + m = 1 + p1 = payload_get_sphere_solid(m, r) + p2 = payload_get_rectangular_cuboid(m, 2*r, 2*r, 2*r) + assert(payload_inertia_lt(p1, p2)) + + #13 sphere > box inside box + r = 0.1 + m = 1 + p1 = payload_get_sphere_solid(m, r) + l = 2*r*sqrt(3)/3 + p2 = payload_get_rectangular_cuboid(m, l, l, l) + assert(payload_inertia_lt(p2, p1)) + + #14 cylinder < Surrounding box + r = 0.1 + l = 2*r + m = 1 + + p1 = payload_get_cylinder(m, l,r) + p2 = payload_get_rectangular_cuboid(m, l, l, l) + assert(payload_inertia_lt(p1, p2)) + + #15 cylinder > box inside box + r = 0.1 + l = 2*r*sqrt(2)/2 + m = 1 + + p1 = payload_get_cylinder(m, l,r) + p2 = payload_get_rectangular_cuboid(m, l, l, l) + assert(payload_inertia_lt(p2, p1)) + + #16 payload_get_cylindrical_tube(0 inner diameter) == cylinder + r = 0.1 + l = 2*r + m = 1 + p1 = payload_get_cylinder(m, l,r) + p2 = payload_get_cylindrical_tube(m, l,r, 0) + assert(payload_cmp(p1,p2)) + + #17 hollow sphere > sphere + r = 0.1 + m = 1 + p1 = payload_get_sphere_solid(m, r) + p2 = payload_get_sphere_hollow(m, r) + assert(payload_inertia_lt(p1, p2)) + assert(not payload_inertia_lt(p2, p1)) + + popup("tests passed") + + +end + +tests() \ No newline at end of file diff --git a/test/test_heuristics.rb b/test/test_heuristics.rb index ed2800210..12890600e 100755 --- a/test/test_heuristics.rb +++ b/test/test_heuristics.rb @@ -941,6 +941,12 @@ def test_scd_by_heuristics }, alt_name="test.scd") end + def test_script_by_heuristics + assert_heuristics({ + "URScript" => all_fixtures("URScript", "*.script") + }) + end + def test_sol_by_heuristics assert_heuristics({ "Gerber Image" => Dir.glob("#{fixtures_path}/Generic/sol/Gerber Image/*"), diff --git a/vendor/README.md b/vendor/README.md index f2b1a53f6..3c2572edd 100644 --- a/vendor/README.md +++ b/vendor/README.md @@ -595,6 +595,7 @@ This is a list of grammars that Linguist selects to provide syntax highlighting - **TypeScript:** [tree-sitter/tree-sitter-typescript](https://github.com/tree-sitter/tree-sitter-typescript) 🐌 - **TypeSpec:** [microsoft/typespec](https://github.com/microsoft/typespec) - **Typst:** [michidk/typst-grammar](https://github.com/michidk/typst-grammar) +- **URScript:** [ahernguo/urscript-extension](https://github.com/ahernguo/urscript-extension) - **Unified Parallel C:** [textmate/c.tmbundle](https://github.com/textmate/c.tmbundle) - **Unity3D Asset:** [atom/language-yaml](https://github.com/atom/language-yaml) - **Unix Assembly:** [calculuswhiz/Assembly-Syntax-Definition](https://github.com/calculuswhiz/Assembly-Syntax-Definition) diff --git a/vendor/grammars/urscript-extension b/vendor/grammars/urscript-extension new file mode 160000 index 000000000..51d5c4021 --- /dev/null +++ b/vendor/grammars/urscript-extension @@ -0,0 +1 @@ +Subproject commit 51d5c4021fccc1fd9549c01f80156d3e001ee5ed diff --git a/vendor/licenses/git_submodule/urscript-extension.dep.yml b/vendor/licenses/git_submodule/urscript-extension.dep.yml new file mode 100644 index 000000000..23d2f94f0 --- /dev/null +++ b/vendor/licenses/git_submodule/urscript-extension.dep.yml @@ -0,0 +1,31 @@ +--- +name: urscript-extension +version: 51d5c4021fccc1fd9549c01f80156d3e001ee5ed +type: git_submodule +homepage: https://github.com/ahernguo/urscript-extension.git +license: mit +licenses: +- sources: LICENSE.md + text: | + MIT License + + Copyright (c) 2023 Ahern Guo + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +notices: []