Merge branch 'micah-dev' into sam-dev

Samuel-Drew · Samuel-Drew · commit 7e1fbf4d7825 · 2020-06-15T14:57:06.000+10:00
diff --git a/graphics/graphics_robot.py b/graphics/graphics_robot.py
@@ -1,6 +1,9 @@
 from graphics.graphics_canvas import *
 from graphics.common_functions import *
 
+# TODO
+#  update non-stl obj creation (position wrong, ref wrong)
+
 
 class DefaultJoint:
     """
@@ -50,8 +53,8 @@ def __init__(self,
         self.__length = max(self.__graphic_obj.length, self.__graphic_obj.width, self.__graphic_obj.height)
 
         # Set the other reference frame vectors
-        self.__graphic_ref = draw_reference_frame_axes(self.__connect_to, self.__x_vector, self.__x_rotation)
         self.__update_reference_frame()
+        self.__graphic_ref = draw_reference_frame_axes(self.__connect_to, self.__x_vector, self.__x_rotation)
 
     def update_position(self, new_pos):
         """
@@ -241,6 +244,12 @@ def __set_graphic(self, given_obj):
                               up=z_axis_vector)
             # Set the boxes new origin
             graphic_obj = compound([graphic_obj], origin=vector(0, 0, 0), axis=x_axis_vector)
+            # Set the boxes x and y vector first before applying the Z axis
+            # Otherwise rotates to the new vector from the original point (unintentionally rotating around it's z axis)
+            graphic_obj.axis = vector((self.__connect_to.x - self.__connect_from.x),
+                                      (self.__connect_to.y - self.__connect_from.y),
+                                      0)
+            graphic_obj.axis.z = (self.__connect_to.z - self.__connect_from.z)
             return graphic_obj
         else:
             # TODO When texture application available, put it here
diff --git a/graphics/graphics_test_features.py b/graphics/graphics_test_features.py
@@ -120,7 +120,12 @@ def test_rotational_link():
 
 def test_graphical_robot():
     canvas_grid = init_canvas()
-    x = GraphicalRobot([create_link_0()])
+
+    x = GraphicalRobot([
+        RotationalJoint(vector(0, 0, 0), vector(1, 1, 1))
+    ])
+
+    x.set_joint_angle(0, radians(20))
 
 
 def test_place_joint():
