Initial Commit

PaulPauls · Feb 10, 2021 · 7387bcf · 7387bcf
commit 7387bcf
Show file tree

Hide file tree

Showing 40 changed files with 390,787 additions and 0 deletions.
diff --git a/README.md b/README.md
@@ -0,0 +1,57 @@
+## PyBullet Simulation of the Franka Emika Panda 7-DOF Robot ##
+
+![Python version req](https://img.shields.io/badge/python-3.5%20%7C%203.6%20%7C%203.7-blue)
+![PyBullet version req](https://img.shields.io/badge/PyBullet-v3.0.8-brightgreen)
+
+--------------------------------------------------------------------------------
+
+<p align="center">
+  <img src="./illustrations/visualize_movement_illustration.gif" width="80%" alt="FEP PyBullet Movement gif"/>
+  <em><br>Visualization of the Franka Emika Panda in PyBullet Simulation</em>
+</p>
+
+--------------------------------------------------------------------------------
+
+Simple simulation environment for the Franka Emika Panda 7-DOF robot. The project also includes the recording of a 55 seconds movement of the Franka Emika Panda with a mounted gripper that aimed to provide a well explored state space dataset as it also recorded the torque values applied by the robots control loop after having been corrected by its internal PID controller. The dataset can be found in 'movement_datasets/fep_state_to_pid-corrected-torque_55s_dataset.csv' and is recorded in radian at a sampling rate of 1000Hz, resulting in 55,000 data points. The dataset is visualized in the plots below.
+
+Aside from the functionality to visualize a recorded FEP dataset does the project also provide the code to create simulated torque values based on PyBullet's internal inverse dynamics calculation via the Recursive Newton-Euler Algorithm (RNEA) as well as the option to perform gravity compensation in the simulation. PyBullets RNEA algorithm is calculated based on the FEP robot model description that was taken from Franka Emikas official ROS repository (see [here](https://github.com/frankaemika/franka_ros)) and was converted from .xacro to .urdf via ROS. In simulation is it possible to consider the dynamic effects of an end effector (a gripper in this project), though only the torques for the 7 joints of the body are recorded.
+
+Further restrictions and specifications of the Franka Emika Panda that were considered in the simulation were taken from the official Franka Emika Panda Datasheet (see [here](https://s3-eu-central-1.amazonaws.com/franka-de-uploads/uploads/Datasheet-EN.pdf)).
+
+Below you will find plots of the recorded robot state over the 55s movement as well as the PID corrected torque and the PyBullet RNEA simulated torque.
+
+
+--------------------------------------------------------------------------------
+#### Position Plot ####
+
+<p align="center">
+  <img src="./illustrations/fep_dataset_pos_plot.svg" width="100%" alt="FEP dataset position plot"/>
+</p>
+
+--------------------------------------------------------------------------------
+#### Velocity Plot ####
+
+<p align="center">
+  <img src="./illustrations/fep_dataset_vel_plot.svg" width="100%" alt="FEP dataset velocity plot"/>
+</p>
+
+--------------------------------------------------------------------------------
+#### Acceleration Plot ####
+
+<p align="center">
+  <img src="./illustrations/fep_dataset_acc_plot.svg" width="100%" alt="FEP dataset acceleration plot"/>
+</p>
+
+--------------------------------------------------------------------------------
+#### PID-corrected Torque Plot ####
+
+<p align="center">
+  <img src="./illustrations/fep_dataset_pid-corrected-torque_plot.svg" width="100%" alt="FEP dataset pid-corrected-torque plot"/>
+</p>
+
+--------------------------------------------------------------------------------
+#### PyBullet Simulated Torque Plot ####
+
+<p align="center">
+  <img src="./illustrations/fep_dataset_simulated-torque_plot.svg" width="100%" alt="FEP dataset simulated-torque plot"/>
+</p>
diff --git a/example_calculate_inverse_dynamics.py b/example_calculate_inverse_dynamics.py
@@ -0,0 +1,73 @@
+import numpy as np
+import pybullet as p
+import pybullet_data
+
+from panda_robot import PandaRobot
+from movement_datasets import read_fep_dataset, write_fep_dataset
+
+INCLUDE_GRIPPER = True
+DTYPE = 'float64'
+SAMPLING_RATE = 1e-3  # 1000Hz sampling rate
+FEP_MOVEMENT_DATASET_PATH = "./movement_datasets/fep_state_to_pid-corrected-torque_55s_dataset.csv"
+FEP_SIM_OUTPUT_DATASET_PATH = "./movement_datasets/fep_state_to_simulated-torque_55s_dataset.csv"
+
+
+def main():
+    """"""
+
+    # Basic Setup of environment
+    physics_client_id = p.connect(p.DIRECT)
+    p.setTimeStep(SAMPLING_RATE)
+    p.setGravity(0, 0, -9.81)
+
+    # Setup plane
+    p.setAdditionalSearchPath(pybullet_data.getDataPath())
+    plane_id = p.loadURDF("plane.urdf")
+
+    # Setup robot
+    panda_robot = PandaRobot(include_gripper=INCLUDE_GRIPPER)
+
+    # Read FEP movement dataset, setting in context the current state of the robot (the position and acceleration of
+    # all joints) to the acceleration that is desired in order to follow the intended movement. The actual FEP
+    # PID-corrected tau values are discared as they are irrelevant since this is solely a simulation and no comparison.
+    pos, vel, desired_acc, _ = read_fep_dataset(FEP_MOVEMENT_DATASET_PATH, DTYPE)
+
+    # Set up variables for simulation loop
+    dataset_length = pos.shape[0]
+    period = 1 / SAMPLING_RATE
+    counter_seconds = -1
+
+    # Set up variable to collect simulated tau values
+    simulated_tau = np.ndarray(shape=pos.shape, dtype=DTYPE)
+
+    # start simulation loop
+    for i in range(dataset_length):
+        # Print status update every second of the simulation
+        if i % period == 0:
+            counter_seconds += 1
+            print("Passed time in simulation: {:>4} sec".format(counter_seconds))
+
+        # Select position, velocity and desired acceleration of current datapoint and convert to list as PyBullet does
+        # not yet support numpy arrays as parameters. Save resulting simulated tau value in previously initialized
+        # numpy array.
+        current_pos = pos[i].tolist()
+        current_vel = vel[i].tolist()
+        current_desired_acc = desired_acc[i].tolist()
+        simulated_tau[i] = panda_robot.calculate_inverse_dynamics(current_pos, current_vel, current_desired_acc)
+
+        # Perform simulation step
+        p.stepSimulation()
+
+    # Exit Simulation
+    p.disconnect()
+    print("Simulation end")
+
+    # Save simulated torques alongside their according position, velocity and desired acceleration as a full FEP
+    # simulated dataset
+    write_fep_dataset(FEP_SIM_OUTPUT_DATASET_PATH,
+                      datasets=[pos, vel, desired_acc, simulated_tau],
+                      dtype=DTYPE)
+
+
+if __name__ == '__main__':
+    main()
diff --git a/example_gravity_compensation.py b/example_gravity_compensation.py
@@ -0,0 +1,64 @@
+import time
+import pybullet as p
+import pybullet_data
+
+from panda_robot import PandaRobot
+
+INCLUDE_GRIPPER = True
+DTYPE = 'float64'
+SAMPLING_RATE = 1e-3  # 1000Hz sampling rate
+SIM_LENGTH_SEC = 60
+
+
+def main():
+    """
+    REMARK:
+    Due to the I assume initial reset the simulation starts out with some velocity in the joints. It will therefore
+    move in the beginning of the simulation. From there on after the simulation however should be properly gravity
+    compensating when you move the joints with the mouse, making them stand still or giving them momentum.
+    """
+
+    # Basic Setup of environment
+    physics_client_id = p.connect(p.GUI)
+    p.setTimeStep(SAMPLING_RATE)
+    p.setGravity(0, 0, -9.81)
+
+    # Setup plane
+    p.setAdditionalSearchPath(pybullet_data.getDataPath())
+    plane_id = p.loadURDF("plane.urdf")
+
+    # Setup robot
+    panda_robot = PandaRobot(include_gripper=INCLUDE_GRIPPER)
+
+    # Set up variables for simulation loop
+    period = 1 / SAMPLING_RATE
+    counter_seconds = -1
+    sim_datapoints = int(SIM_LENGTH_SEC * period)
+
+    # start simulation loop
+    for i in range(sim_datapoints):
+        # Print status update every second of the simulation
+        if i % period == 0:
+            counter_seconds += 1
+            print("Passed time in simulation: {:>4} sec".format(counter_seconds))
+
+        # Determine current state (position and velocity) of robot. Set the desired acceleration to 0 in order to only
+        # compensate for gravity but leave all other movement as is.
+        pos, vel = panda_robot.get_position_and_velocity()
+        desired_acc = [0. for _ in pos]
+
+        # Determine proper torque for the desired gravity compensation acceleration and set it in the robot
+        torques = panda_robot.calculate_inverse_dynamics(pos=pos, vel=vel, desired_acc=desired_acc)
+        panda_robot.set_torques(torques)
+
+        # Perform simulation step
+        p.stepSimulation()
+        time.sleep(SAMPLING_RATE)
+
+    # Exit Simulation
+    p.disconnect()
+    print("Simulation end")
+
+
+if __name__ == '__main__':
+    main()
diff --git a/example_visualize_movement.py b/example_visualize_movement.py
@@ -0,0 +1,60 @@
+import time
+import pybullet as p
+import pybullet_data
+
+from panda_robot import PandaRobot
+from movement_datasets import read_fep_dataset
+
+INCLUDE_GRIPPER = True
+DTYPE = 'float64'
+SAMPLING_RATE = 1e-3  # 1000Hz sampling rate
+FEP_MOVEMENT_DATASET_PATH = "./movement_datasets/fep_state_to_pid-corrected-torque_55s_dataset.csv"
+
+
+def main():
+    """"""
+
+    # Basic Setup of environment
+    physics_client_id = p.connect(p.GUI)
+    p.setTimeStep(SAMPLING_RATE)
+    p.setGravity(0, 0, -9.81)
+
+    # Setup plane
+    p.setAdditionalSearchPath(pybullet_data.getDataPath())
+    plane_id = p.loadURDF("plane.urdf")
+
+    # Setup robot
+    panda_robot = PandaRobot(include_gripper=INCLUDE_GRIPPER)
+
+    # Read FEP movement dataset, discarding everything except the joint positions for each sampling point as PyBullet
+    # can be set to figure joint torques out by itself and only requires desired joint positions.
+    pos, _, _, _ = read_fep_dataset(FEP_MOVEMENT_DATASET_PATH, DTYPE)
+
+    # Set up variables for simulation loop
+    dataset_length = pos.shape[0]
+    period = 1 / SAMPLING_RATE
+    counter_seconds = -1
+
+    # start simulation loop
+    for i in range(dataset_length):
+        # Print status update every second of the simulation
+        if i % period == 0:
+            counter_seconds += 1
+            print("Passed time in simulation: {:>4} sec".format(counter_seconds))
+
+        # Select data of current position, then convert to list as PyBullet does not yet support numpy arrays as
+        # parameters
+        current_pos = pos[i].tolist()
+        panda_robot.set_target_positions(current_pos)
+
+        # Perform simulation step
+        p.stepSimulation()
+        time.sleep(SAMPLING_RATE)
+
+    # Exit Simulation
+    p.disconnect()
+    print("Simulation end")
+
+
+if __name__ == '__main__':
+    main()
diff --git a/illustrations/create_dataset_plots.py b/illustrations/create_dataset_plots.py
@@ -0,0 +1,110 @@
+import os
+import numpy as np
+import matplotlib.pyplot as plt
+
+from movement_datasets import read_fep_dataset
+
+DTYPE = 'float64'
+FEP_REAL_DATASET_PATH = "../movement_datasets/fep_state_to_pid-corrected-torque_55s_dataset.csv"
+FEP_SIM_DATASET_PATH = "../movement_datasets/fep_state_to_simulated-torque_55s_dataset.csv"
+PLOT_OUTPUT_DIR_PATH = "./"
+PLOT_FIGSIZE = (20, 10)
+PLOT_DPI = 128
+
+
+def main():
+    """"""
+    # Ensure Plot directory exists
+    os.makedirs(PLOT_OUTPUT_DIR_PATH, exist_ok=True)
+
+    # Read dataset position, velocity, acceleration and real as well as simulated torque arrays
+    print("Reading FEP real and simulated datasets...")
+    pos, vel, acc, real_tau = read_fep_dataset(FEP_REAL_DATASET_PATH, DTYPE)
+    _, _, _, sim_tau = read_fep_dataset(FEP_SIM_DATASET_PATH, DTYPE)
+
+    # Create plots
+    print("Creating FEP dataset plots...")
+
+    # Set timesteps to 55000, as 55000 datapoints
+    timesteps = np.arange(0, 55, 1e-3)
+
+    plt.figure(figsize=PLOT_FIGSIZE)
+    for i in range(7):
+        plt.plot(timesteps, pos[:, i], label=f'j{i + 1}', zorder=2)
+    plt.axhline(0, color='lightgrey', zorder=1)
+    plt.xlabel('Time [s]', fontsize=20)
+    plt.xticks(range(0, 60, 5))
+    plt.ylabel('q(t)\u1D62 [rad]', fontsize=20)
+    plt.yticks([-2.5, -2, -1.5, -1, -0.5, 0, 0.5, 1, 1.5, 2, 2.5])
+    plt.ylim(-2.75, 2.75)
+    plt.legend(loc=1)
+    plt.savefig(fname=PLOT_OUTPUT_DIR_PATH + 'fep_dataset_pos_plot.svg',
+                dpi=PLOT_DPI,
+                bbox_inches='tight',
+                format='svg')
+
+    plt.figure(figsize=PLOT_FIGSIZE)
+    for i in range(7):
+        plt.plot(timesteps, vel[:, i], label=f'j{i + 1}', zorder=2)
+    plt.axhline(0, color='lightgrey', zorder=1)
+    plt.xlabel('Time [s]', fontsize=20)
+    plt.xticks(range(0, 60, 5))
+    plt.ylabel('\u0071\u0307(t)\u1D62 [rad/s]', fontsize=20)
+    plt.yticks([-2.5, -2, -1.5, -1, -0.5, 0, 0.5, 1, 1.5, 2, 2.5])
+    plt.ylim(-2.75, 2.75)
+    plt.legend(loc=1)
+    plt.savefig(fname=PLOT_OUTPUT_DIR_PATH + 'fep_dataset_vel_plot.svg',
+                dpi=PLOT_DPI,
+                bbox_inches='tight',
+                format='svg')
+
+    plt.figure(figsize=PLOT_FIGSIZE)
+    for i in range(7):
+        plt.plot(timesteps, acc[:, i], label=f'j{i + 1}', zorder=2)
+    plt.axhline(0, color='lightgrey', zorder=1)
+    plt.xlabel('Time [s]', fontsize=20)
+    plt.xticks(range(0, 60, 5))
+    plt.ylabel('\u0071\u0308(t)\u1D62 [rad/s\u00b2]', fontsize=20)
+    plt.yticks([-2.5, -2, -1.5, -1, -0.5, 0, 0.5, 1, 1.5, 2, 2.5])
+    plt.ylim(-2.75, 2.75)
+    plt.legend(loc=1)
+    plt.savefig(fname=PLOT_OUTPUT_DIR_PATH + 'fep_dataset_acc_plot.svg',
+                dpi=PLOT_DPI,
+                bbox_inches='tight',
+                format='svg')
+
+    plt.figure(figsize=PLOT_FIGSIZE)
+    for i in range(7):
+        plt.plot(timesteps, real_tau[:, i], label=f'j{i + 1}', zorder=2)
+    plt.axhline(0, color='lightgrey', zorder=1)
+    plt.xlabel('Time [s]', fontsize=20)
+    plt.xticks(range(0, 60, 5))
+    plt.ylabel('\u03c4(t)\u1D62 [Nm]', fontsize=20)
+    plt.yticks([-70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70])
+    plt.ylim(-78, 78)
+    plt.legend(loc=1)
+    plt.savefig(fname=PLOT_OUTPUT_DIR_PATH + 'fep_dataset_pid-corrected-torque_plot.svg',
+                dpi=PLOT_DPI,
+                bbox_inches='tight',
+                format='svg')
+
+    plt.figure(figsize=PLOT_FIGSIZE)
+    for i in range(7):
+        plt.plot(timesteps, sim_tau[:, i], label=f'j{i + 1}', zorder=2)
+    plt.axhline(0, color='lightgrey', zorder=1)
+    plt.xlabel('Time [s]', fontsize=20)
+    plt.xticks(range(0, 60, 5))
+    plt.ylabel('RNEA \u03c4(t)\u1D62 [Nm]', fontsize=20)
+    plt.yticks([-70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70])
+    plt.ylim(-78, 78)
+    plt.legend(loc=1)
+    plt.savefig(fname=PLOT_OUTPUT_DIR_PATH + 'fep_dataset_simulated-torque_plot.svg',
+                dpi=PLOT_DPI,
+                bbox_inches='tight',
+                format='svg')
+
+    print("Creation of FEP dataset plots finished")
+
+
+if __name__ == '__main__':
+    main()