Merge pull request #1 from nebbles/motion

Merge motion into master

franka/caller.py

@@ -1,57 +1,136 @@
 import subprocess
-import time
-
-while True:
-    testing = input("Is this program being tested [N/y]: ")
-    if testing == '' or testing.lower() == 'n':
-        testing = False
-        break
-    elif testing.lower() == 'y':
-        testing = True
-        break
-    else:
-        print("Invalid response.")
-print("Testing mode: ", testing)
+import os
 
-while True:
-    direction = input("Type 0 for forward, 1 for back: ")
-    if direction in ['0', '1']:
-        break
-    else:
-        print("direction was not understood")
 
-print("Direction is: ", direction)
+class Caller:
+    """
+    Class which contains methods to control the Franka Panda using supporting C++ binaries.
+    """
+    def __init__(self, ip='192.168.0.88', debug_flag=False):
+        self.ip_address = ip
+        self.debug = debug_flag
+
+    def move_relative(self, dx: float=0.0, dy: float=0.0, dz: float=0.0):
+        """
+        Executes Franka C++ binary which moves the arm relative
+        to its current position according to the input arguments.
+
+        Returns the return_code of the C++ program.
+        """
+        try:
+            dx, dy, dz = float(dx), float(dy), float(dz)
+        except ValueError:
+            print("Arguments are invalid: must be floats")
+            return
+
+        dx, dy, dz = str(dx), str(dy), str(dz)
+
+        path = os.path.dirname(os.path.realpath(__file__))  # gets working dir of this file
+        program = './franka_move_to_relative'  # set executable to be used
+        command = [program, self.ip_address, dx, dy, dz]
+        command_str = " ".join(command)
+
+        if self.debug:
+            print("Working directory: ", path)
+            print("Program: ", program)
+            print("IP Address of robot: ", self.ip_address)
+            print("dx: ", dx)
+            print("dy: ", dy)
+            print("dz: ", dz)
+            print("Command being called: ", command_str)
+            print("Running FRANKA code...")
+
+        return_code = subprocess.call(command, cwd=path)
+
+        if return_code == 0:
+            if self.debug:
+                print("No problems")
+        else:
+            print("Python has registered a problem with ", program)
+
+        return return_code
 
-dx = '0'
+    def move_absolute(self, coordinates: list):
+        if len(coordinates) > 3:
+            raise ValueError("Invalid coordinates. There can only be three dimensions.")
+        x, y, z = coordinates[0], coordinates[1], coordinates[2]
 
-if direction == '0':
-    dx = '0.05'
-elif direction == '1':
-    dx = '-0.05'
+        # TODO: implement safety check for target coordinates
 
-dy = '0'
-dz = '0'
+        path = os.path.dirname(os.path.realpath(__file__))  # gets working dir of this file
+        program = './franka_move_to_absolute'
+        command = [program, self.ip_address, x, y, z]
+        command_str = " ".join(command)
+
+        if self.debug:
+            print("Program: ", program)
+            print("IP Address of robot: ", self.ip_address)
+            print("Go to x: ", x)
+            print("Go to y: ", y)
+            print("Go to z: ", z)
+            print("Command being called: ", command_str)
+            print("Running FRANKA code...")
+
+        return_code = subprocess.call(command, cwd=path)
+
+        if return_code == 0:
+            if self.debug:
+                print("No problems")
+        else:
+            print("Python has registered a problem with ", program)
+
+
+def main():
+    while True:
+        testing = input("Is this program being tested with the arm? [N/y]: ")
+        if testing == '' or testing.lower() == 'n':
+            testing = False
+            break
+        elif testing.lower() == 'y':
+            testing = True
+            break
+        else:
+            print("Invalid response.")
+    print("Testing mode: ", testing)
+
+    while True:
+        direction = input("Enter 0 to move along x slightly, 1 for backwards: ")
+        if direction in ['0', '1']:
+            break
+        else:
+            print("Invalid input. Must be 0/1.")
+
+    if testing:
+        arm = Caller(debug_flag=True)
+
+        if direction == '0':
+            arm.move_relative(dx=0.05)
+        elif direction == '1':
+            arm.move_relative(dx=-0.05)
+
+    else:
+        dx = '0'
+        dy = '0'
+        dz = '0'
+        if direction == '0':
+            dx = 0.05
+        elif direction == '1':
+            dx = -0.05
+        print("dx: ", dx)
+        print("dy: ", dy)
+        print("dz: ", dz)
 
-print("dx: ", dx)
-print("dy: ", dy)
-print("dz: ", dz)
+        program = './franka_move_to_relative'
+        ip_address = '192.168.0.88'
 
-program = './franka_move_to_relative'
-ip_address = '192.168.0.88'
+        print("Program being run is: ", program)
+        print("IP Address of robot: ", ip_address)
 
-print("Program being run is: ", program)
-print("IP Address of robot: ", ip_address)
+        command = [program, ip_address, dx, dy, dz]
+        command_str = " ".join(command)
 
-command = [program, ip_address, dx, dy, dz]
-command_str = " ".join(command)
+        print("Command being called: ", command_str)
 
-print("Command being called: ", command)
 
-if testing:
-    print("Running the actual FRANKA code in 3 seconds!")
-    time.sleep(3)
-    print("Running it now!")
-    returncode = subprocess.call(command)
-    print("Python reads the return code as: ", returncode)
-    if returncode != 0:
-        print("Python has registered a problem with the subprocess.")
+if __name__ == '__main__':
+    main()

franka/caller_test.py

@@ -0,0 +1,57 @@
+import subprocess
+import time
+
+while True:
+    testing = input("Is this program being tested [N/y]: ")
+    if testing == '' or testing.lower() == 'n':
+        testing = False
+        break
+    elif testing.lower() == 'y':
+        testing = True
+        break
+    else:
+        print("Invalid response.")
+print("Testing mode: ", testing)
+
+while True:
+    direction = input("Type 0 for forward, 1 for back: ")
+    if direction in ['0', '1']:
+        break
+    else:
+        print("direction was not understood")
+
+print("Direction is: ", direction)
+
+dx = '0'
+
+if direction == '0':
+    dx = '0.05'
+elif direction == '1':
+    dx = '-0.05'
+
+dy = '0'
+dz = '0'
+
+print("dx: ", dx)
+print("dy: ", dy)
+print("dz: ", dz)
+
+program = './franka_move_to_relative'
+ip_address = '192.168.0.88'
+
+print("Program being run is: ", program)
+print("IP Address of robot: ", ip_address)
+
+command = [program, ip_address, dx, dy, dz]
+command_str = " ".join(command)
+
+print("Command being called: ", command)
+
+if testing:
+    print("Running the actual FRANKA code in 3 seconds!")
+    time.sleep(3)
+    print("Running it now!")
+    returncode = subprocess.call(command)
+    print("Python reads the return code as: ", returncode)
+    if returncode != 0:
+        print("Python has registered a problem with the subprocess.")

motion/Trajectory.py

@@ -0,0 +1,132 @@
+from numpy import linspace, sqrt, concatenate
+from matplotlib.pyplot import *
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D  # for: fig.gca(projection = '3d')
+
+
+def intermediate_coords(rest, start, goal, hover):
+    """function to generate the intermediate positions of FRANKA"""
+    rest_h = [rest[0], rest[1], hover]
+    start_h = [start[0], start[1], hover]
+    goal_h = [goal[0], goal[1], hover]
+    return rest_h, start_h, goal_h
+
+
+def create_line(a, b, v, vectors, dt):
+    """function to create an array of poses between 2 points in space"""
+    vector = [b[0]-a[0], b[1]-a[1], b[2]-a[2]]  # vector from b to a
+    vectors.append(vector)
+    distance = sqrt(sum(i**2 for i in vector))  # straight line distance between points
+    # time to complete movement
+    time = v/distance
+
+    # number of points
+    n = time/dt
+    n = int(n)
+
+    # pre-allocate space
+    line = np.zeros((n, 3))
+
+    # generate array of poses
+    line_x = linspace(a[0], b[0], n)
+    line_y = linspace(a[1], b[1], n)
+    line_z = linspace(a[2], b[2], n)
+
+    # append coordinates
+    for i in range(n):
+        line[i, 0] = line_x[i]
+        line[i, 1] = line_y[i]
+        line[i, 2] = line_z[i]
+    return line
+
+
+def get_test_trajectory():
+    # start and goal poses
+    rest = [200, 0, 500]
+    start = [400, 0, 10]
+    goal = [550, 200, 10]
+    hover = 200  # hover height
+
+    # selecting variables
+    dt = 0.0001  # resolution in seconds
+    velocity = 5  # speed in mm/s
+
+    # create complete array of poses
+    rest_h, start_h, goal_h = intermediate_coords(rest, start, goal, hover)
+
+    # creating the lines
+    vectors = []
+
+    create_line(rest, rest_h, velocity, vectors, dt)
+    create_line(rest_h, start_h, velocity, vectors, dt)
+    create_line(start_h, start, velocity, vectors, dt)
+    create_line(start, start_h, velocity, vectors, dt)
+    create_line(start_h, goal_h, velocity, vectors, dt)
+    create_line(goal_h, goal, velocity, vectors, dt)
+    create_line(goal, goal_h, velocity, vectors, dt)
+    create_line(goal_h, rest_h, velocity, vectors, dt)
+    create_line(rest_h, rest, velocity, vectors, dt)
+
+    print("BEN'S Vectors: ", vectors)
+    return vectors
+
+
+def main():
+    # start and goal poses
+    rest = [200, 0, 500]
+    start = [400, 0, 10]
+    goal = [550, 200, 10]
+    hover = 200  # hover height
+
+    # selecting variables
+    dt = 0.0001  # resolution in seconds
+    velocity = 5  # speed in mm/s
+
+    # create complete array of poses
+    rest_h, start_h, goal_h = intermediate_coords(rest, start, goal, hover)
+
+    # creating the lines
+    vectors = []
+
+    l1 = create_line(rest, rest_h, velocity, vectors, dt)
+    l2 = create_line(rest_h, start_h, velocity, vectors, dt)
+    l3 = create_line(start_h, start, velocity, vectors, dt)
+    l4 = create_line(start, start_h, velocity, vectors, dt)
+    l5 = create_line(start_h, goal_h, velocity, vectors, dt)
+    l6 = create_line(goal_h, goal, velocity, vectors, dt)
+    l7 = create_line(goal, goal_h, velocity, vectors, dt)
+    l8 = create_line(goal_h, rest_h, velocity, vectors, dt)
+    l9 = create_line(rest_h, rest, velocity, vectors, dt)
+
+    print("BEN'S Vectors: ", vectors)
+
+    # joining the lines into a trjectory
+    line_list = (l1, l2, l3, l4, l5, l6, l7, l8, l9)
+    trajectory = concatenate(line_list, axis=0)
+
+    line_list_1 = (l1, l2, l3)
+    line_list_2 = (l4, l5, l6)
+    line_list_3 = (l7, l8, l9)
+
+    trajectory_1 = concatenate(line_list_1, axis=0)
+    trajectory_2 = concatenate(line_list_2, axis=0)
+    trajectory_3 = concatenate(line_list_3, axis=0)
+
+    # action list
+    print(trajectory_1)
+    print('grip')
+    print(trajectory_2)
+    print('ungrip')
+    print(trajectory_3)
+
+    # plotting
+    fig = matplotlib.pyplot.figure()
+    ax = fig.gca(projection='3d')
+    ax.plot(trajectory[:, 0], trajectory[:, 1], trajectory[:, 2])
+    matplotlib.pyplot.show()
+
+    return vectors
+
+
+if __name__ == '__main__':
+    main()

test_motion.py

@@ -0,0 +1,20 @@
+from motion import Trajectory
+from franka.caller import Caller
+
+vector_list = Trajectory.get_test_trajectory()
+
+new_vectors = []
+for vector in vector_list:
+    dx = float(vector[0])/1000.0
+    dy = float(vector[1])/1000.0
+    dz = float(vector[2])/1000.0
+    new_vectors.append([dx, dy, dz])
+
+print("New vectors (mm converted to m): ", new_vectors)
+
+arm = Caller(debug_flag=True)
+for move in new_vectors:
+    x = move[0]
+    y = move[1]
+    z = move[2]
+    arm.move_relative(dx=x, dy=y, dz=z)