Merge pull request #8 from nebbles/motion

Motion

.gitignore

@@ -1,4 +1,7 @@
 .idea
+*.pyc
+*/__pycache__
 
 # Ignore build directory in docs dir
 /docs/build
+/ignore*

README.md

@@ -4,7 +4,7 @@
 
 # DE3 Group Project for Robotics 1
 
-Project CHESS:
+Project CHESS Team:
 
 - Anna Bernbaum ([@AnnaBernbaum](https://github.com/AnnaBernbaum))
 - Ben Greenberg ([@nebbles](https://github.com/nebbles))
@@ -14,14 +14,40 @@ Project CHESS:
 - Paolo Ruegg ([@pa17](https://github.com/pa17))
 - Sylvia Zhang
 
-Guide:
+## Popular source code
 
-- See the ``game-engine`` branch for developments on the chess logic and decision libraries.
-- See the ``perception`` branch for developments on converting rgb image feed and tracking of chess board.
-- See the ``motion`` branch for development of motion planning and motion controllers.
+* Source code for controlling Franka without ROS
 
-Remember to document your work as you go, *including your docstrings into classes and functions/methods*. If you aren't sure what you need to do for it, ask Ben first so he can help you add them to do the project.
+```bash
+svn export https://github.com/nebbles/DE3-ROB1-CHESS/trunk/franka
+```
 
-Finally, remember to **push your changes and updates to your relevant GitHub branch**. GitHub tracks all contributions of code to this group project.
+* Source code for controlling Franka with ROS
+
+todo
+
+* Source code for converting between reference frames
+
+```bash
+svn export https://github.com/nebbles/DE3-ROB1-CHESS/trunk/calibration.py
+```
+
+## Downloading the repository
+
+```bash
+git clone http://github.com/nebbles/DE3-ROB1-CHESS
+```
+
+## Installation
+
+todo
+
+## Compiling the documentation offline
+
+```bash
+cd docs/
+make html
+open build/html/index.html
+```
 
 ![LICENSE](CC4.0-BY.jpg)

calibration.py

@@ -0,0 +1,64 @@
+#
+# Benedict Greenberg, March 2018
+import numpy as np
+
+
+class Calibration:
+    """Stores the transformation relationship between original and target reference frames. The
+    class should be instantiated with a minimum of 4 frame->frame calibration points."""
+    def __init__(self, frame_a_points, frame_b_points, debug=False):
+        # Check data passed into class object
+        if type(frame_a_points) is not np.ndarray or type(frame_b_points) is not np.ndarray:
+            raise ValueError("Calibration requires data as (n x 3) numpy array.")
+        if np.shape(frame_a_points)[1] != 3 or np.shape(frame_b_points)[1] != 3:
+            raise ValueError("Array should have only three dimensions.")
+        if np.shape(frame_a_points)[0] < 4 or np.shape(frame_b_points)[0] < 4:
+            raise ValueError("Array should have at least 4 calibration points.")
+        if np.shape(frame_a_points) != np.shape(frame_b_points):
+            raise ValueError("Array sizes should match.")
+
+        self.points_from = frame_a_points
+        self.points_to = frame_b_points
+
+        number_pts = np.shape(frame_a_points)[0]
+        ones = np.ones((number_pts, 1))
+        mat_a = np.column_stack([frame_a_points, ones])
+        mat_b = np.column_stack([frame_b_points, ones])
+
+        self.transformation = np.linalg.lstsq(mat_a, mat_b, rcond=None)[0]
+        self.transformation_reversed = np.linalg.lstsq(mat_b, mat_a, rcond=None)[0]
+        if debug:
+            print(self.transformation)
+
+    def transform(self, coordinate):
+        """Transforms an x,y,z coordinate to the target reference frame."""
+        if np.shape(coordinate) != (3,):
+            raise ValueError("Point must be a (3,) vector.")
+        point = np.append(coordinate, 1)
+        return np.dot(point, self.transformation)
+
+    def transform_reversed(self, coordinate):
+        """Transforms an x,y,z coordinate from the target reference frame back to the original."""
+        if np.shape(coordinate) != (3,):
+            raise ValueError("Point must be a (3,) vector.")
+        point = np.append(coordinate, 1)
+        return np.dot(point, self.transformation_reversed)
+
+
+if __name__ == '__main__':
+    from numpy import genfromtxt
+    my_data = genfromtxt('cal_data_example.csv', delimiter=',')
+
+    num_pts = 4
+
+    A = my_data[0:num_pts, 0:3]
+    B = my_data[0:num_pts, 3:]
+
+    calibrate = Calibration(A, B)
+
+    sample_in = my_data[0, 0:3]
+    sample_out = calibrate.transform(sample_in)
+    actual_out = my_data[0, 3:]
+    print("Pass in coordinates: ", sample_in)
+    print("Convert coordinates: ", sample_out)
+    print("Correct coordinates: ", actual_out)

docs/source/calibration.rst

@@ -0,0 +1,107 @@
+***********
+Calibration
+***********
+
+To use our reference frame conversion code, use the following command in the terminal when in the directory you want it copied to::
+
+  svn export https://github.com/nebbles/DE3-ROB1-CHESS/trunk/calibration.py
+
+Introduction
+============
+
+.. todo:: Intro to our calibration procedure...
+
+Procedure
+=========
+
+.. todo:: The procedure as follows... move in contained area, detect markers
+
+Reference Frames
+================
+
+Overview
+--------
+
+To be able to convert camera coordinates, provided by opencv tracking tools and other methods, we need to maintain a relationship between multiple reference frames. The key relationship is that which relates the camera reference frame and the robot base reference frame used by the frankalib controller. This relationship is maintained in a 4-by-4 transformation matrix, and is constructed using the following general formula:
+
+.. math::
+
+  aX = b
+
+This is modelled on the idea that we can take a coordinate in our main frame (e.g. RGB-D camera provides ``u, v, w`` coordinates) and convert it to the equivalent, corresponding coordinate in the robots reference frame (e.g. ``x, y, z``) so that the robot can move to that point on the camera's view. ``a`` represents our camera coordinate, and ``b`` represents the output of our function, that mulitplies ``a`` with our transformation matrix ``X``, which represents the same point but on the robots reference frame.
+
+.. todo:: add image of reference frames of both robot, camera, board
+
+Creating the transformation matrix
+----------------------------------
+
+To create the transformation matrix, we construct a set of linear equations that we want to solve using a simple least squares algorithm, commonly used in linear regression. This algorithm tries to minimise the sum of squares for each solution to the set of linear equations.
+
+This set of linear equations is constructed using *calibration points*. These points (usually a minimum of 4) are a set of known, corresponding coordinates in both the cameras reference frame and the robots. These can be automatically sourced with a setup program, or manually. To manually get these points, the robots end effector would be moved to a point in the field of view of the camera, and the robot would report its position (``x, y, z``). The camera would then detect the robot end effector in the field of view and report the location according to its own reference frame (``u, v, w``) and so these two points are the same point (they correspond) but are in different reference frames. We collect a minimum of 4 calibration points, ideally up to 8 or 10 because this will increase the accuracy of our transformation matrix, as there may have been a small error in the values reported by the camera or robot.
+
+.. todo:: add image of linear regression with caption
+
+We now have our calibration equation (of *n* calibration points), and we want to solve for the unknowns in the transformation matrix, *X*.
+
+.. math::
+
+  \begin{bmatrix}
+    u_i&v_i&w_i\\
+    &\vdots&\\
+    u_n&v_n&w_n
+  \end{bmatrix} X =
+  \begin{bmatrix}
+    x_i&y_i&z_i\\
+    &\vdots&\\
+    x_n&y_n&z_n
+  \end{bmatrix}
+
+Where :math:`m_{ij}` is the unknown in *X*,
+
+  .. math::
+
+    X =\begin{bmatrix}
+      m_{11}&m_{12}&m_{13}&m_{14}\\
+      m_{21}&m_{22}&m_{23}&m_{24}\\
+      m_{31}&m_{32}&m_{33}&m_{34}\\
+      m_{41}&m_{42}&m_{43}&m_{44}
+    \end{bmatrix}
+
+In MATLab, the function for solving this equation is simply ``X = a\b``, or less commonly written as ``X = mldivide(a,b)``. `The mldivide() function`_ in MATLab is a complex one, and utilises many different possible algorithms depending on its inputs. To get the similar behaviour in Python, we use `numpy's lstsq function`_ which has similarites and differences which have been discussed `{1}`_ `{2}`_, but ultimately provides us the same functionality of returning a least square solution to the equation. We use the function as in our example below::
+
+  import numpy as np
+  from numpy import random
+
+  num_pts = 4
+
+  A = random.rand(num_pts, 3)
+  one = np.ones((num_pts, 1))
+  A = np.column_stack([A, one])
+  print("A", A)
+  print("\n")
+
+  T = random.rand(3, 4)
+  xrow = np.array([0,0,0,1])
+  T = np.vstack([T, xrow])
+  print("T", T)
+  print("\n")
+
+  B = np.dot(A, T)
+  print("B", B)
+  print("\n")
+
+  x = np.linalg.lstsq(A, B, rcond=None)[0]
+  print("x", x)
+
+.. _`The mldivide() function`: http://uk.mathworks.com/help/matlab/ref/mldivide.html
+.. _`numpy's lstsq function`: https://docs.scipy.org/doc/numpy/reference/generated/numpy.linalg.lstsq.html
+
+.. _`{1}`: https://stackoverflow.com/questions/33559946/numpy-vs-mldivide-matlab-operator
+.. _`{2}`: https://stackoverflow.com/questions/33614378/how-can-i-obtain-the-same-special-solutions-to-underdetermined-linear-systems?noredirect=1&lq=1
+
+Implementation
+--------------
+
+.. automodule:: calibration
+  :members:
+  :undoc-members:

docs/source/controller.rst

@@ -2,6 +2,8 @@
 Controller API
 **************
 
+.. todo:: redo and add system diagram
+
 Below is the specification for the modules of the Chess Project Python Program. This is to ensure that each module fulfills its function and that the overall goal of the project is achieved. Each module can then seperately be improved and updated, assuming compliance with this specification.
 
 **Clock** module:

docs/source/franka.rst

@@ -5,20 +5,22 @@ FRANKA Panda
 Starting up the Panda
 =====================
 
-The Panda arm should be connected to the workshop floor controller and powered up. When booting, the arm LEDs will flash yellow. Once the arm is booted (solid yellow when completed) you should release the brakes by connecting a computer via ethernet to the base of the arm (see image).
+The Panda arm should be connected to the workshop floor controller with the thick black cable and the controller box should be powered up. When booting, the arm LEDs will flash yellow. Once the arm is booted (solid yellow when completed) you can release the brakes via the web interface.
 
-.. figure:: _static/franka_wiring_guide_robot_arm.png
+.. figure:: _static/franka_wiring_guide_general.png
     :align: center
     :figclass: align-center
 
-    Configuration of wiring for connecting to the FRANKA web interface.
+    Wiring configuration for using the Franka.
 
-Log into the controller web interface (http://robot.franka.de) with:
+Log into the controller web interface (http://192.168.0.88) with:
 
 * Username: ``robin``
 * Password: ``panda``
 
-To release the brakes:
+.. note:: Your browser may say the connection is not secure, or some other warning. If this is the case, click on the 'advanced' button, and then click 'proceed anyway'.
+
+To release the brakes on the web interface:
 
 #. Make sure the external activation device (EAD) is pressed down.
 #. On the web interface, bottom right, *open* the brakes.
@@ -28,33 +30,29 @@ To release the brakes:
 .. attention::
   The white light means the robot is in movement mode. The external activation device (EAD) / button is a software stop. It 'detaches' the arm from the controller which causes the arm to lock (yellow light on). It is **not** an emergency stop, as it does not cut power to the arm/controller.
 
-With the Arm in movement mode, it can be manually manipulated by squeezing the buttons at the end effector. The Arm will automatically go into gravity compensation mode when manually moving it.
+With the Arm in movement mode, it can be manually manipulated by squeezing the buttons at the end effector. The buttons have a two stage press, and if you press too hard on the buttons, the arm will lock again. The Arm will automatically go into gravity compensation mode when manually moving it.
+
+.. tip:: If the end-effector has recently been removed or readded, the gravity compensation may not be performing well. This is because the web interface settings have not been updated to account for the decrease/increase to expected weight. See the section on `Removing or adding the Franka's end-effector (hand)`_.
 
 .. note::
   When designing motion controllers, we are recommended to use impedance control, not high gain control. This will mean we can reduce the stiffness of the arm when moving making it safer for collaborative environments.
 
-Networking with Panda
-=====================
-
-If you now want to use a workstation computer to control the Arm via the FRANKA Control Interface (FCI) libraries, **first ensure you have completed the above steps to unlock the Arm brakes**. Then move the ethernet cable from the base of the Arm, and connect it to the shop floor controller (as seen in image).
-
-.. figure:: _static/franka_wiring_guide_shop_floor.png
-    :align: center
-    :figclass: align-center
+Networking information
+======================
 
-    Configuration of wiring for connecting to the FRANKA Control Interface (ROS libraries).
+If you now want to use a workstation computer to control the Arm via the FRANKA Control Interface (FCI) libraries, **first ensure you have completed the above steps to unlock the Arm brakes**. Also check the ethernet cable is attached either to a network switch or directly to the shop floor controller.
 
 .. attention::
   According to `FRANKA documentation <https://frankaemika.github.io/docs/getting_started.html#operating-the-robot>`_: "the workstation PC which commands your robot using the FCI must always be connected to the LAN port of Control (shop floor network) and **not** to the LAN port of the Arm (robot network)."
 
-With the workstation computer connected to the shop floor controller, you must set a static IPv4 address for the computer in the Ubuntu network settings. The recommended values are seen below:
+With the main workstation computer *should* have a static IPv4 address for the computer in the Ubuntu network settings. The recommended values are seen below:
 
 =======================  ============  ==============================
 Device                   IP Address    Notes
 =======================  ============  ==============================
 FRANKA Arm               192.168.1.0   This does not change
 Shop floor (controller)  192.168.0.88  This does not change
-Workstation              192.168.0.77  Should be static (in settings)
+Workstation (main)       192.168.0.77  Should be static (in settings)
 =======================  ============  ==============================
 
 .. important::
@@ -64,17 +62,54 @@ You can confirm that the workstation computer is able to communicate with the wo
 
   $ ping 192.168.0.88
 
-.. note:: Communicating with the Panda does not currently work over a network switch. The ethernet cable should be direct from shop floor controller to workstation computer. Unfortunately this means you computer will not be able to connect to the internet.
+.. note:: Communicating with the Franka over the switch with a static IP does allow you to have internet access, just note that the gateway and DNS settings should be provided in the Ubuntu settings accordingly to make it work.
+
+Removing or adding the Franka's end-effector (hand)
+===================================================
+
+There are a number of steps that need to be done when removing or readding hte frnka hand ot the arm.
+
+.. todo:: add full guidance on how to add/remove hand and change settings for gravity compensation.
+
+.. _franka-emika-software:
+
+Franka Emika Software
+=====================
+
+Software updates can be found at: http://support.franka.de/
 
-Panda Software
-==============
+The software versions currently used in the robotics lab are:
 
-The FRANKA Panda currently has ``v1.0.9`` which supports versions ``libfranka < 0.2.0``. Updates for the Panda can be found at: http://support.franka.de/
+=======================  ============  ==============================
+Software                 Version       Notes
+=======================  ============  ==============================
+Franka Firmware          1.0.9         Supports ``libfranka < 0.2.0``
+ros-kinetic-libfranka    0.1.0         Current ROS version is 0.2.0
+franka_ros               ??            Currently unused
+=======================  ============  ==============================
+
+.. warning:: The lab only supports libfranka 0.1.0 which is currently unavailable from ``apt install``. **Do NOT uninstall ROS or libfranka on workstations which already have it installed**.
 
 Shutting down the Panda
 =======================
 
-Enter the web interface for the Panda. In the lower right menu, lock the brakes. Then in the top right menu, select shutdown.
+Enter the web interface for the Panda. In the lower right menu, lock the brakes. Then in the top right menu, select shutdown, and confirm.
 
 .. important::
   Remember to shutdown the controller from the web interface. This device is a computer, and should not be switched off from mains.
+
+Appendix
+========
+
+In rare cases, you may need to access the Franka arm directly by connecting the ethernet cable as seen in the image below:
+
+.. figure:: _static/franka_wiring_guide_robot_arm.png
+    :align: center
+    :figclass: align-center
+
+    Wiring configuration fo accessing the Arm directly (through the web interface).
+
+Log into the controller web interface (http://robot.franka.de) with:
+
+* Username: ``robin``
+* Password: ``panda``