print tip orientation is different for two arms - hardcoded (#58)

src/gen_2cal_test.py

@@ -27,7 +27,7 @@ def create_plan(points, env, gui=False):
         for p in points[arm]:
 
             # Go to the point
-            jpoint = tu.get_closest(env.ik_pos(p,arm),home, max_diff=100)
+            jpoint = tu.get_closest(env.ik_pos(p,arm), home, max_diff=100)
             go2point = plan.plan_to(jpoint, arm)
             plan.appendTrajectory([go2point], arm)
 
@@ -51,7 +51,9 @@ if __name__ == "__main__":
             [-0.215,-0.1, 0.05],
             ]
     points2 = [
-            [ 0.215,-0.1, 0.05],
+            [ 0.215, 0.1, 0.05],
+            [-0.215, 0.1, 0.05],
+            [ 0.0  , 0.0, 0.01],
             ]
     points = [points1, points2]
 

src/utils/simEnv.py

@@ -57,10 +57,14 @@ urdf_folder = Path(__file__).parent / "../../models/urdf"
 ## not exact vertical as IKFast fails to return solutions in degenerate cases
 ## as there are infinite solutions when converting to euler angles
 ## (gimbal lock in the orientations)
-_vert_or = np.array([[-6.74517010e-04,  8.33315658e-04, -9.99999404e-01, -2.90597873e-05],
+_vert_or = [np.array([[-6.74517010e-04,  8.33315658e-04, -9.99999404e-01, -2.90597873e-05],
                      [ 8.33732192e-04, -9.99999285e-01, -8.33877944e-04, -4.81068309e-05],
                      [-9.99999404e-01, -8.34294187e-04,  6.73821778e-04,  2.74792069e-06],
-                     [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]])
+                     [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]]),
+            np.array([[-6.74517010e-04, -8.33315658e-04,  9.99999404e-01, -2.90597873e-05],
+                     [ 8.33732192e-04,  9.99999285e-01,  8.33877944e-04, -4.81068309e-05],
+                     [-9.99999404e-01,  8.34294187e-04, -6.73821778e-04,  2.74792069e-06],
+                     [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]])]
 
 
 def getTransformMat(quat, pos):
@@ -167,7 +171,7 @@ class SimEnv:
             position = np.array(position)/1000
         mat = np.identity(4)
         mat[:3,3] = position
-        mat = self.world2armT[robot] @ mat @ _vert_or @ self.tip2armT[robot]
+        mat = self.world2armT[robot] @ mat @ _vert_or[robot] @ self.tip2armT[robot]
         js = self.ur5e_kin.inverse(mat[:3,].reshape(-1))
 
         # 6 DOF UR