Merge branch 'feature/hardware-plans' into feature/decmcts

src/calibrations/r1_tforms.yaml

 calibration:
   pos_ET: [0.14982225501989826, -0.048118796065005576, -0.03482924194973972]
-  pos_WR: [0.033410320051684865, -0.5779185680332759, -0.129]
+  pos_WR: [0.033410320051684865, -0.5779185680332759, -0.130]
   quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
   quat_WR: [-0.0035406624036226648, 0.00401943913741378, -0.9999807593358486, 0.003128702105328147]
 measurements:

src/gen_cal_test.py

+"""
+Generate Plan fo rtesting calibration
+"""
+import gcode2contour as gc
+import utils as tu
+import pybullet as p
+import time
+
+from math import pi
+import numpy as np
+import warnings
+import pickle
+import copy
+
+
+home = [-pi/2, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
+
+
+def plan(points, env, gui=False):
+    """
+    Create plan which simply goines to certain points
+    """
+    plan = tu.Plan(env)
+    n_arms = 1
+    arm = 0
+
+    # Got to each point
+    for p in points:
+
+        # Go to 5cm above the point
+        p_above = copy.deepcopy(p)
+        p_above[2] = p[2] + 0.05
+
+        jabove = tu.get_closest(env.ik_pos(p_above,0),home, max_diff=100)
+        go2above = plan.plan_to(jabove, arm)
+        plan.appendTrajectory([go2above], arm)
+
+        # Go to the point
+        jpoint = tu.get_closest(env.ik_pos(p,0),home, max_diff=100)
+        go2point = plan.plan_to(jpoint, arm)
+        plan.appendTrajectory([go2point], arm)
+
+        # go to 5cm above point again
+        back2above = plan.plan_to(jabove, arm)
+        plan.appendTrajectory([back2above], arm)
+
+
+    # try going to home pos
+    gohome = plan.plan_to(env.home, arm)
+    if gohome is not None and plan.appendTrajectory([gohome], arm)==0:
+        print("done")
+    else:
+        raise Exception("cannot plan back to home position")
+    return plan
+
+
+if __name__ == "__main__":
+
+    # 1arm real calibration
+    env_desc1 = tu.read_env('calibrations/r1_tforms.yaml')
+    env = tu.SimEnv(env_desc=env_desc1, gui=True, home=home)
+
+    # Greedily plan first n contours
+    points = [
+            [ 0    ,  0    , 0],
+            [-0.215,  0.215, 0],
+            [ 0    ,  0.215, 0],
+            [ 0.215,  0.215, 0],
+            [ 0.215,  0    , 0],
+            [ 0.215, -0.215, 0],
+            [ 0    , -0.215, 0],
+            [-0.215, -0.215, 0],
+            [-0.215,  0    , 0],
+            ]
+    plan1 = plan(points, env, gui=True)
+    plan1.save("cal_test.plan")

src/test_plan_gen.py

@@ -125,6 +125,14 @@ def plan(contour_tracker, env, n_contours=None, gui=False):
     return plan
 
 
+def remove_shorts(contours, min_extrude=5):
+    """
+    removes contours which i dont like
+    """
+
+    return [c for c in contours if c.Ext[-1] >= min_extrude]
+
+
 if __name__ == "__main__":
 
     # Read gcode file
@@ -134,7 +142,8 @@ if __name__ == "__main__":
         with open(pkl, 'rb') as f:
             tracker = pickle.load(f)
     except:
-        contours = gc.decode_gcode("gcode/" + model + ".gcode")
+        contours = gc.decode_gcode("gcode/" + model + ".gcode", max_wp_dist=20)
+        contours = remove_shorts(contours)
 
         # 5mm safety, max 5 layers ahead
         print("\nCalculating dependency graph - this could take some time...")
@@ -160,8 +169,11 @@ if __name__ == "__main__":
 
     # Greedily plan first n contours
     plan1 = plan(tracker, env, n_contours=0, gui=True)
-    plan1.save("1.plan")
+    plan1.save("origin.plan")
     plan2 = plan(tracker, env, n_contours=1, gui=True)
-    plan2.save("2.plan")
+    plan2.save("flexirex1.plan")
+    plan3 = plan(tracker, env, n_contours=200, gui=True)
+    plan3.save("flexirex200.plan")
+
 
     #plan3, env = plan(tracker, env_desc1, n_contours=25, gui=True)

src/utils/planning_tools.py

@@ -49,7 +49,7 @@ joint_max   = np.array([ pi]*6)
 joint_range = joint_max - joint_min
 
 
-def _get_closest(sols, js, max_diff=1, joint_range=joint_range):
+def get_closest(sols, js, max_diff=1, joint_range=joint_range):
     """
     Get the closest solution
     with wrapping around joint range (distance between pi and -pi is 0)
@@ -116,7 +116,7 @@ def contour_trajs(c, sim_env, robot):
                 continue
 
             # closest ik solution to last joint position
-            closest_jpos = _get_closest(sols, traj.positions[-1])
+            closest_jpos = get_closest(sols, traj.positions[-1])
             if closest_jpos == None or \
                     sim_env.colCheckSingle(closest_jpos, robot) > 0:
                 # if no close solutions found, remove this trajectory