Un/pack ergo eval values and interpolate feasibility

src/autopack/ergonomic_evaluation.py

@@ -11,43 +11,51 @@
 
 
 def create_ergonomic_cost_field(
-    ips, problem_setup, max_geometry_dist=0.2, min_point_dist=0.1
+    ips: IPSInstance,
+    problem_setup: ProblemSetup,
+    max_geometry_dist=0.2,
+    min_point_dist=0.1,
+    max_grip_diff=0.1,
 ):
+    ref_cost_field = problem_setup.cost_fields[0]
+    ref_costs = ref_cost_field.costs
+    ref_coords = ref_cost_field.coordinates
+    ref_coords_flat = ref_coords.reshape(-1, 3)
+
     geometries_to_consider = [
         geo.name for geo in problem_setup.harness_setup.geometries if geo.assembly
     ]
-    sparse_points = sparse_cost_field(problem_setup.cost_fields[0], min_point_dist)
+    sparse_points = sparse_cost_field(ref_cost_field, min_point_dist)
     points_close_to_surface = check_distance_of_points(
         ips, problem_setup.harness_setup, sparse_points, max_geometry_dist
     )
-    selected_coords = [
-        num for i, num in enumerate(sparse_points) if points_close_to_surface[i] == 1
-    ]
-    ergo_evaluations = ergonomic_evaluation(
-        ips, geometries_to_consider, selected_coords
-    )
-    array1 = np.array(selected_coords)
-    REBA_vals = np.array(ergo_evaluations)[:, 0].reshape(-1, 1)
-    RULA_vals = np.array(ergo_evaluations)[:, 1].reshape(-1, 1)
-    REBA_res = np.hstack([array1, REBA_vals])
-    RULA_res = np.hstack([array1, RULA_vals])
-    wanted_coords = problem_setup.cost_fields[0].coordinates.reshape(-1, 3)
-    new_reba_values = interpolation(REBA_res, wanted_coords)[:, -1]
-    new_rula_values = interpolation(RULA_res, wanted_coords)[:, -1]
-    cost_field_shape = problem_setup.cost_fields[0].costs.shape
-    rula_costs = new_rula_values.reshape(cost_field_shape)
-    reba_costs = new_reba_values.reshape(cost_field_shape)
-    rula_cost_field = CostField(
-        name="RULA",
-        coordinates=problem_setup.cost_fields[0].coordinates,
-        costs=rula_costs,
+    eval_coords = np.array(
+        [num for i, num in enumerate(sparse_points) if points_close_to_surface[i] == 1]
     )
-    reba_cost_field = CostField(
-        name="REBA",
-        coordinates=problem_setup.cost_fields[0].coordinates,
-        costs=reba_costs,
-    )
-    return rula_cost_field, reba_cost_field
+    ergo_eval = ergonomic_evaluation(ips, geometries_to_consider, eval_coords)
+    ergo_standards = ergo_eval["ergoStandards"]
+    ergo_values = np.array(ergo_eval["ergoValues"])
+    assert ergo_values.shape == (len(eval_coords), len(ergo_standards))
+    grip_distances = np.array(ergo_eval["gripDiffs"])
+    predicted_grip_diffs = interpolation(eval_coords, grip_distances, ref_coords_flat)
+    infeasible_mask = predicted_grip_diffs > max_grip_diff
+
+    cost_fields = []
+    for ergo_std_idx, ergo_std in enumerate(ergo_standards):
+        true_costs = ergo_values[:, ergo_std_idx]
+
+        predicted_costs = interpolation(eval_coords, true_costs, ref_coords_flat)
+        predicted_costs[infeasible_mask] = np.inf
+
+        cost_fields.append(
+            CostField(
+                name=ergo_std,
+                coordinates=ref_coords,
+                costs=predicted_costs.reshape(ref_costs.shape),
+            )
+        )
+
+    return cost_fields
 
 
 def sparse_cost_field(cost_field, min_point_dist):
@@ -62,19 +70,11 @@ def sparse_cost_field(cost_field, min_point_dist):
     return reshaped_array
 
 
-def interpolation(known_costs, new_cost_pos):
-    # known_costs = np array with known cost values size n*4 [[coord_x, coord_y, coord_z, cost],...]
-    # new_cost_pos = np array with new pos coordinates size m*3 [[coord_x, coord_y, coord_z],...]
-    # Splitting known_costs array into positions and costs
-    pos = known_costs[:, :3]  # positions
-    costs = known_costs[:, 3]  # costs
+def interpolation(known_x, known_y, predict_x):
     # Defining Kriging model
     sm = KRG(theta0=[1e-2] * 3, print_global=False)
-    # Training the model
-    sm.set_training_values(pos, costs)
+    sm.set_training_values(known_x, known_y)
     sm.train()
-    # Predicting the cost for the new positions
-    predicted_costs = sm.predict_values(new_cost_pos)
-    # Appending the predicted costs to new_cost_pos
-    new_cost_pos_with_costs = np.hstack((new_cost_pos, predicted_costs))
-    return new_cost_pos_with_costs
+    predict_y = sm.predict_values(predict_x)
+
+    return predict_y

src/autopack/ips_communication/ips_commands.py

@@ -138,12 +138,7 @@ def ergonomic_evaluation(ips_instance, parts, coords):
 
     time.sleep(1)
     command = lua_commands.ergonomic_evaluation(parts, coords)
-    results = ips_instance.call(command)
-    result_array = results.decode("utf-8").strip().replace('"', "").split()
-    output = []
-    for i in range(0, len(result_array) - 1, 2):
-        output.append([float(result_array[i]), float(result_array[i + 1])])
-    return output
+    return ips_instance.call_unpack(command)
 
 
 def cost_field_vis(ips_instance, cost_field):

src/autopack/ips_communication/lua_commands.py

@@ -377,27 +377,12 @@ def get_stl_meshes():
 
 
 def ergonomic_evaluation(parts, coords):
-    stl_paths = ",".join(parts)
-    coords_str = ",".join([" ".join(map(str, sublist)) for sublist in coords])
     command = f"""
-    geos = [[{stl_paths}]]
-    coordinates = [[{coords_str}]]
+    geo_names = {to_inline_lua(parts)}
+    coordinates = {to_inline_lua(coords)}
 
-    function split(source, delimiters)
-            local elements = {{}}
-            local pattern = '([^'..delimiters..']+)'
-            string.gsub(source, pattern, function(value) elements[#elements + 1] =     value;  end);
-            return elements
-    end
-    function tablelength(T)
-        local count = 0
-        for _ in pairs(T) do count = count + 1 end
-        return count
-    end
     function copy_to_static_geometry(part_table)
-        numb_of_parts = tablelength(part_table)
-        for ii = 1,numb_of_parts,1 do
-            part_name = part_table[ii]
+        for _, part_name in pairs(part_table) do
             local localtreeobject = Ips.getActiveObjectsRoot();
             local localobject = localtreeobject:findFirstExactMatch(part_name);
             local localrigidObject = localobject:toRigidBodyObject()
@@ -408,7 +393,6 @@ def ergonomic_evaluation(parts, coords):
                 if i == 1 then
                     localpositionedObject = localrigidObject:getFirstChild()
                     localtoCopy = localpositionedObject:isPositionedTreeObject()
-
                 else
                     localpositionedObject = localpositionedObject:getNextSibling()
                     localtoCopy = localpositionedObject:isPositionedTreeObject()
@@ -417,14 +401,11 @@ def ergonomic_evaluation(parts, coords):
                     Ips.copyTreeObject(localpositionedObject, localgeometryRoot)
                 end
             end
-            --Ips.copyTreeObject
-            --positionedObject = object:toPositionedTreeObject()
             localrigidObject:setLocked(true)
         end
     end
 
-    geos_table = split(geos, ",")
-    copy_to_static_geometry(geos_table)
+    copy_to_static_geometry(geo_names)
 
     local treeobject = Ips.getActiveObjectsRoot();
 
@@ -434,44 +415,37 @@ def ergonomic_evaluation(parts, coords):
     local gp1=gp:toGripPointVisualization();
     local gp2=gp1:getGripPoint();
     local family = treeobject:findFirstExactMatch("Family 1");
-    print(family)
     local f1=family:toManikinFamilyVisualization();
     local f2=f1:getManikinFamily();
     f2:enableCollisionAvoidance();
 
     measureTree = Ips.getMeasuresRoot()
     measure = measureTree:findFirstExactMatch("measure")
     measure_object = measure:toMeasure()
-    print(measure_object)
     local gp_geo = treeobject:findFirstExactMatch("gripGeo");
     gp_geo1 = gp_geo:toPositionedTreeObject()
-    --results = ""
-    local outputTable = {{}}
-    coord_array = split(coordinates, ",")
-    numb_of_coords = tablelength(coord_array)
-    for i = 1,numb_of_coords,1 do
-        coord = split(coord_array[i], " ")
-        local trans = Transf3(r, Vector3d(tonumber(coord[1]), tonumber(coord[2]), tonumber(coord[3])))
+
+    local ergoStandards = autopack.ipsNVecToTable(f2:getErgoStandards())
+    local outputTable = {{
+        ergoStandards = ergoStandards,
+        ergoValues = {{}},
+        gripDiffs = {{}},
+    }}
+    for coordIdx, coord in pairs(coordinates) do
+        local trans = Transf3(r, Vector3d(coord[1], coord[2], coord[3]))
         gp_geo1:setTControl(trans)
         Ips.moveTreeObject(gp, family);
         dist = measure_object:getValue()
-        if dist>0.1 then
-            f6_tostring = "99";
-            f8_tostring = "99";
-        else
-            local f4=f2:getErgoStandards();
-            local f5=f4[0];
-            local f7=f4[1];
-            local f6=f2:evaluateStaticErgo(f5, 0);
-            local f8=f2:evaluateStaticErgo(f7, 0);
-            f6_tostring = tostring(f6);
-            f8_tostring = tostring(f8);
+
+        local coordErgoValues = {{}}
+        for ergoStandardIdx, ergoStandard in pairs(ergoStandards) do
+            local ergoValue = f2:evaluateStaticErgo(ergoStandard, 0)
+            coordErgoValues[ergoStandardIdx] = ergoValue
         end
-        --results = results .. " " .. f6_tostring .. " " .. f8_tostring
-        table.insert(outputTable, f6_tostring .. " " .. f8_tostring)
+        outputTable.ergoValues[coordIdx] = coordErgoValues
+        outputTable.gripDiffs[coordIdx] = dist
     end
-    return table.concat(outputTable, " ")
-    --return results
+    return autopack.pack(outputTable)
     """
     return command