├── README.md
├── example_calculate_inverse_dynamics.py
├── example_gravity_compensation.py
├── example_visualize_movement.py
├── illustrations
    ├── create_dataset_plots.py
    ├── fep_dataset_acc_plot.svg
    ├── fep_dataset_pid-corrected-torque_plot.svg
    ├── fep_dataset_pos_plot.svg
    ├── fep_dataset_simulated-torque_plot.svg
    ├── fep_dataset_vel_plot.svg
    └── visualize_movement_illustration.gif
├── movement_datasets
    ├── __init__.py
    ├── fep_state_to_pid-corrected-torque_55s_dataset.csv
    ├── fep_state_to_simulated-torque_55s_dataset.csv
    └── read_write_helper.py
├── panda_robot
    ├── __init__.py
    ├── model_description
    │   ├── meshes
    │   │   ├── collision
    │   │   │   ├── finger.stl
    │   │   │   ├── hand.stl
    │   │   │   ├── link0.stl
    │   │   │   ├── link1.stl
    │   │   │   ├── link2.stl
    │   │   │   ├── link3.stl
    │   │   │   ├── link4.stl
    │   │   │   ├── link5.stl
    │   │   │   ├── link6.stl
    │   │   │   └── link7.stl
    │   │   └── visual
    │   │   │   ├── finger.stl
    │   │   │   ├── hand.stl
    │   │   │   ├── link0.stl
    │   │   │   ├── link1.stl
    │   │   │   ├── link2.stl
    │   │   │   ├── link3.stl
    │   │   │   ├── link4.stl
    │   │   │   ├── link5.stl
    │   │   │   ├── link6.stl
    │   │   │   └── link7.stl
    │   ├── panda.urdf
    │   └── panda_with_gripper.urdf
    └── panda_robot.py
└── requirements.txt


/README.md:
--------------------------------------------------------------------------------
 1 | ## PyBullet Simulation of the Franka Emika Panda 7-DOF Robot ##
 2 | 
 3 | ![Python version req](https://img.shields.io/badge/python-3.5%20%7C%203.6%20%7C%203.7-blue)
 4 | ![PyBullet version req](https://img.shields.io/badge/PyBullet-v3.0.8-brightgreen)
 5 | 
 6 | --------------------------------------------------------------------------------
 7 | 
 8 | <p align="center">
 9 |   <img src="./illustrations/visualize_movement_illustration.gif" width="80%" alt="FEP PyBullet Movement gif"/>
10 |   <em><br>Visualization of the Franka Emika Panda in PyBullet Simulation</em>
11 | </p>
12 | 
13 | --------------------------------------------------------------------------------
14 | 
15 | 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.
16 | 
17 | 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.
18 | 
19 | 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)).
20 | 
21 | 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.
22 | 
23 | 
24 | --------------------------------------------------------------------------------
25 | #### Position Plot ####
26 | 
27 | <p align="center">
28 |   <img src="./illustrations/fep_dataset_pos_plot.svg" width="100%" alt="FEP dataset position plot"/>
29 | </p>
30 | 
31 | --------------------------------------------------------------------------------
32 | #### Velocity Plot ####
33 | 
34 | <p align="center">
35 |   <img src="./illustrations/fep_dataset_vel_plot.svg" width="100%" alt="FEP dataset velocity plot"/>
36 | </p>
37 | 
38 | --------------------------------------------------------------------------------
39 | #### Acceleration Plot ####
40 | 
41 | <p align="center">
42 |   <img src="./illustrations/fep_dataset_acc_plot.svg" width="100%" alt="FEP dataset acceleration plot"/>
43 | </p>
44 | 
45 | --------------------------------------------------------------------------------
46 | #### PID-corrected Torque Plot ####
47 | 
48 | <p align="center">
49 |   <img src="./illustrations/fep_dataset_pid-corrected-torque_plot.svg" width="100%" alt="FEP dataset pid-corrected-torque plot"/>
50 | </p>
51 | 
52 | --------------------------------------------------------------------------------
53 | #### PyBullet Simulated Torque Plot ####
54 | 
55 | <p align="center">
56 |   <img src="./illustrations/fep_dataset_simulated-torque_plot.svg" width="100%" alt="FEP dataset simulated-torque plot"/>
57 | </p>
58 | 


--------------------------------------------------------------------------------
/example_calculate_inverse_dynamics.py:
--------------------------------------------------------------------------------
 1 | import numpy as np
 2 | import pybullet as p
 3 | import pybullet_data
 4 | 
 5 | from panda_robot import PandaRobot
 6 | from movement_datasets import read_fep_dataset, write_fep_dataset
 7 | 
 8 | INCLUDE_GRIPPER = True
 9 | DTYPE = 'float64'
10 | SAMPLING_RATE = 1e-3  # 1000Hz sampling rate
11 | FEP_MOVEMENT_DATASET_PATH = "./movement_datasets/fep_state_to_pid-corrected-torque_55s_dataset.csv"
12 | FEP_SIM_OUTPUT_DATASET_PATH = "./movement_datasets/fep_state_to_simulated-torque_55s_dataset.csv"
13 | 
14 | 
15 | def main():
16 |     """"""
17 | 
18 |     # Basic Setup of environment
19 |     physics_client_id = p.connect(p.DIRECT)
20 |     p.setTimeStep(SAMPLING_RATE)
21 |     p.setGravity(0, 0, -9.81)
22 | 
23 |     # Setup plane
24 |     p.setAdditionalSearchPath(pybullet_data.getDataPath())
25 |     plane_id = p.loadURDF("plane.urdf")
26 | 
27 |     # Setup robot
28 |     panda_robot = PandaRobot(include_gripper=INCLUDE_GRIPPER)
29 | 
30 |     # Read FEP movement dataset, setting in context the current state of the robot (the position and acceleration of
31 |     # all joints) to the acceleration that is desired in order to follow the intended movement. The actual FEP
32 |     # PID-corrected tau values are discared as they are irrelevant since this is solely a simulation and no comparison.
33 |     pos, vel, desired_acc, _ = read_fep_dataset(FEP_MOVEMENT_DATASET_PATH, DTYPE)
34 | 
35 |     # Set up variables for simulation loop
36 |     dataset_length = pos.shape[0]
37 |     period = 1 / SAMPLING_RATE
38 |     counter_seconds = -1
39 | 
40 |     # Set up variable to collect simulated tau values
41 |     simulated_tau = np.ndarray(shape=pos.shape, dtype=DTYPE)
42 | 
43 |     # start simulation loop
44 |     for i in range(dataset_length):
45 |         # Print status update every second of the simulation
46 |         if i % period == 0:
47 |             counter_seconds += 1
48 |             print("Passed time in simulation: {:>4} sec".format(counter_seconds))
49 | 
50 |         # Select position, velocity and desired acceleration of current datapoint and convert to list as PyBullet does
51 |         # not yet support numpy arrays as parameters. Save resulting simulated tau value in previously initialized
52 |         # numpy array.
53 |         current_pos = pos[i].tolist()
54 |         current_vel = vel[i].tolist()
55 |         current_desired_acc = desired_acc[i].tolist()
56 |         simulated_tau[i] = panda_robot.calculate_inverse_dynamics(current_pos, current_vel, current_desired_acc)
57 | 
58 |         # Perform simulation step
59 |         p.stepSimulation()
60 | 
61 |     # Exit Simulation
62 |     p.disconnect()
63 |     print("Simulation end")
64 | 
65 |     # Save simulated torques alongside their according position, velocity and desired acceleration as a full FEP
66 |     # simulated dataset
67 |     write_fep_dataset(FEP_SIM_OUTPUT_DATASET_PATH,
68 |                       datasets=[pos, vel, desired_acc, simulated_tau],
69 |                       dtype=DTYPE)
70 | 
71 | 
72 | if __name__ == '__main__':
73 |     main()
74 | 


--------------------------------------------------------------------------------
/example_gravity_compensation.py:
--------------------------------------------------------------------------------
 1 | import time
 2 | import pybullet as p
 3 | import pybullet_data
 4 | 
 5 | from panda_robot import PandaRobot
 6 | 
 7 | INCLUDE_GRIPPER = True
 8 | DTYPE = 'float64'
 9 | SAMPLING_RATE = 1e-3  # 1000Hz sampling rate
10 | SIM_LENGTH_SEC = 60
11 | 
12 | 
13 | def main():
14 |     """
15 |     REMARK:
16 |     Due to the I assume initial reset the simulation starts out with some velocity in the joints. It will therefore
17 |     move in the beginning of the simulation. From there on after the simulation however should be properly gravity
18 |     compensating when you move the joints with the mouse, making them stand still or giving them momentum.
19 |     """
20 | 
21 |     # Basic Setup of environment
22 |     physics_client_id = p.connect(p.GUI)
23 |     p.setTimeStep(SAMPLING_RATE)
24 |     p.setGravity(0, 0, -9.81)
25 | 
26 |     # Setup plane
27 |     p.setAdditionalSearchPath(pybullet_data.getDataPath())
28 |     plane_id = p.loadURDF("plane.urdf")
29 | 
30 |     # Setup robot
31 |     panda_robot = PandaRobot(include_gripper=INCLUDE_GRIPPER)
32 | 
33 |     # Set up variables for simulation loop
34 |     period = 1 / SAMPLING_RATE
35 |     counter_seconds = -1
36 |     sim_datapoints = int(SIM_LENGTH_SEC * period)
37 | 
38 |     # start simulation loop
39 |     for i in range(sim_datapoints):
40 |         # Print status update every second of the simulation
41 |         if i % period == 0:
42 |             counter_seconds += 1
43 |             print("Passed time in simulation: {:>4} sec".format(counter_seconds))
44 | 
45 |         # Determine current state (position and velocity) of robot. Set the desired acceleration to 0 in order to only
46 |         # compensate for gravity but leave all other movement as is.
47 |         pos, vel = panda_robot.get_position_and_velocity()
48 |         desired_acc = [0. for _ in pos]
49 | 
50 |         # Determine proper torque for the desired gravity compensation acceleration and set it in the robot
51 |         torques = panda_robot.calculate_inverse_dynamics(pos=pos, vel=vel, desired_acc=desired_acc)
52 |         panda_robot.set_torques(torques)
53 | 
54 |         # Perform simulation step
55 |         p.stepSimulation()
56 |         time.sleep(SAMPLING_RATE)
57 | 
58 |     # Exit Simulation
59 |     p.disconnect()
60 |     print("Simulation end")
61 | 
62 | 
63 | if __name__ == '__main__':
64 |     main()
65 | 


--------------------------------------------------------------------------------
/example_visualize_movement.py:
--------------------------------------------------------------------------------
 1 | import time
 2 | import pybullet as p
 3 | import pybullet_data
 4 | 
 5 | from panda_robot import PandaRobot
 6 | from movement_datasets import read_fep_dataset
 7 | 
 8 | INCLUDE_GRIPPER = True
 9 | DTYPE = 'float64'
10 | SAMPLING_RATE = 1e-3  # 1000Hz sampling rate
11 | FEP_MOVEMENT_DATASET_PATH = "./movement_datasets/fep_state_to_pid-corrected-torque_55s_dataset.csv"
12 | 
13 | 
14 | def main():
15 |     """"""
16 | 
17 |     # Basic Setup of environment
18 |     physics_client_id = p.connect(p.GUI)
19 |     p.setTimeStep(SAMPLING_RATE)
20 |     p.setGravity(0, 0, -9.81)
21 | 
22 |     # Setup plane
23 |     p.setAdditionalSearchPath(pybullet_data.getDataPath())
24 |     plane_id = p.loadURDF("plane.urdf")
25 | 
26 |     # Setup robot
27 |     panda_robot = PandaRobot(include_gripper=INCLUDE_GRIPPER)
28 | 
29 |     # Read FEP movement dataset, discarding everything except the joint positions for each sampling point as PyBullet
30 |     # can be set to figure joint torques out by itself and only requires desired joint positions.
31 |     pos, _, _, _ = read_fep_dataset(FEP_MOVEMENT_DATASET_PATH, DTYPE)
32 | 
33 |     # Set up variables for simulation loop
34 |     dataset_length = pos.shape[0]
35 |     period = 1 / SAMPLING_RATE
36 |     counter_seconds = -1
37 | 
38 |     # start simulation loop
39 |     for i in range(dataset_length):
40 |         # Print status update every second of the simulation
41 |         if i % period == 0:
42 |             counter_seconds += 1
43 |             print("Passed time in simulation: {:>4} sec".format(counter_seconds))
44 | 
45 |         # Select data of current position, then convert to list as PyBullet does not yet support numpy arrays as
46 |         # parameters
47 |         current_pos = pos[i].tolist()
48 |         panda_robot.set_target_positions(current_pos)
49 | 
50 |         # Perform simulation step
51 |         p.stepSimulation()
52 |         time.sleep(SAMPLING_RATE)
53 | 
54 |     # Exit Simulation
55 |     p.disconnect()
56 |     print("Simulation end")
57 | 
58 | 
59 | if __name__ == '__main__':
60 |     main()
61 | 


--------------------------------------------------------------------------------
/illustrations/create_dataset_plots.py:
--------------------------------------------------------------------------------
  1 | import os
  2 | import numpy as np
  3 | import matplotlib.pyplot as plt
  4 | 
  5 | from movement_datasets import read_fep_dataset
  6 | 
  7 | DTYPE = 'float64'
  8 | FEP_REAL_DATASET_PATH = "../movement_datasets/fep_state_to_pid-corrected-torque_55s_dataset.csv"
  9 | FEP_SIM_DATASET_PATH = "../movement_datasets/fep_state_to_simulated-torque_55s_dataset.csv"
 10 | PLOT_OUTPUT_DIR_PATH = "./"
 11 | PLOT_FIGSIZE = (20, 10)
 12 | PLOT_DPI = 128
 13 | 
 14 | 
 15 | def main():
 16 |     """"""
 17 |     # Ensure Plot directory exists
 18 |     os.makedirs(PLOT_OUTPUT_DIR_PATH, exist_ok=True)
 19 | 
 20 |     # Read dataset position, velocity, acceleration and real as well as simulated torque arrays
 21 |     print("Reading FEP real and simulated datasets...")
 22 |     pos, vel, acc, real_tau = read_fep_dataset(FEP_REAL_DATASET_PATH, DTYPE)
 23 |     _, _, _, sim_tau = read_fep_dataset(FEP_SIM_DATASET_PATH, DTYPE)
 24 | 
 25 |     # Create plots
 26 |     print("Creating FEP dataset plots...")
 27 | 
 28 |     # Set timesteps to 55000, as 55000 datapoints
 29 |     timesteps = np.arange(0, 55, 1e-3)
 30 | 
 31 |     plt.figure(figsize=PLOT_FIGSIZE)
 32 |     for i in range(7):
 33 |         plt.plot(timesteps, pos[:, i], label=f'j{i + 1}', zorder=2)
 34 |     plt.axhline(0, color='lightgrey', zorder=1)
 35 |     plt.xlabel('Time [s]', fontsize=20)
 36 |     plt.xticks(range(0, 60, 5))
 37 |     plt.ylabel('q(t)\u1D62 [rad]', fontsize=20)
 38 |     plt.yticks([-2.5, -2, -1.5, -1, -0.5, 0, 0.5, 1, 1.5, 2, 2.5])
 39 |     plt.ylim(-2.75, 2.75)
 40 |     plt.legend(loc=1)
 41 |     plt.savefig(fname=PLOT_OUTPUT_DIR_PATH + 'fep_dataset_pos_plot.svg',
 42 |                 dpi=PLOT_DPI,
 43 |                 bbox_inches='tight',
 44 |                 format='svg')
 45 | 
 46 |     plt.figure(figsize=PLOT_FIGSIZE)
 47 |     for i in range(7):
 48 |         plt.plot(timesteps, vel[:, i], label=f'j{i + 1}', zorder=2)
 49 |     plt.axhline(0, color='lightgrey', zorder=1)
 50 |     plt.xlabel('Time [s]', fontsize=20)
 51 |     plt.xticks(range(0, 60, 5))
 52 |     plt.ylabel('\u0071\u0307(t)\u1D62 [rad/s]', fontsize=20)
 53 |     plt.yticks([-2.5, -2, -1.5, -1, -0.5, 0, 0.5, 1, 1.5, 2, 2.5])
 54 |     plt.ylim(-2.75, 2.75)
 55 |     plt.legend(loc=1)
 56 |     plt.savefig(fname=PLOT_OUTPUT_DIR_PATH + 'fep_dataset_vel_plot.svg',
 57 |                 dpi=PLOT_DPI,
 58 |                 bbox_inches='tight',
 59 |                 format='svg')
 60 | 
 61 |     plt.figure(figsize=PLOT_FIGSIZE)
 62 |     for i in range(7):
 63 |         plt.plot(timesteps, acc[:, i], label=f'j{i + 1}', zorder=2)
 64 |     plt.axhline(0, color='lightgrey', zorder=1)
 65 |     plt.xlabel('Time [s]', fontsize=20)
 66 |     plt.xticks(range(0, 60, 5))
 67 |     plt.ylabel('\u0071\u0308(t)\u1D62 [rad/s\u00b2]', fontsize=20)
 68 |     plt.yticks([-2.5, -2, -1.5, -1, -0.5, 0, 0.5, 1, 1.5, 2, 2.5])
 69 |     plt.ylim(-2.75, 2.75)
 70 |     plt.legend(loc=1)
 71 |     plt.savefig(fname=PLOT_OUTPUT_DIR_PATH + 'fep_dataset_acc_plot.svg',
 72 |                 dpi=PLOT_DPI,
 73 |                 bbox_inches='tight',
 74 |                 format='svg')
 75 | 
 76 |     plt.figure(figsize=PLOT_FIGSIZE)
 77 |     for i in range(7):
 78 |         plt.plot(timesteps, real_tau[:, i], label=f'j{i + 1}', zorder=2)
 79 |     plt.axhline(0, color='lightgrey', zorder=1)
 80 |     plt.xlabel('Time [s]', fontsize=20)
 81 |     plt.xticks(range(0, 60, 5))
 82 |     plt.ylabel('\u03c4(t)\u1D62 [Nm]', fontsize=20)
 83 |     plt.yticks([-70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70])
 84 |     plt.ylim(-78, 78)
 85 |     plt.legend(loc=1)
 86 |     plt.savefig(fname=PLOT_OUTPUT_DIR_PATH + 'fep_dataset_pid-corrected-torque_plot.svg',
 87 |                 dpi=PLOT_DPI,
 88 |                 bbox_inches='tight',
 89 |                 format='svg')
 90 | 
 91 |     plt.figure(figsize=PLOT_FIGSIZE)
 92 |     for i in range(7):
 93 |         plt.plot(timesteps, sim_tau[:, i], label=f'j{i + 1}', zorder=2)
 94 |     plt.axhline(0, color='lightgrey', zorder=1)
 95 |     plt.xlabel('Time [s]', fontsize=20)
 96 |     plt.xticks(range(0, 60, 5))
 97 |     plt.ylabel('RNEA \u03c4(t)\u1D62 [Nm]', fontsize=20)
 98 |     plt.yticks([-70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70])
 99 |     plt.ylim(-78, 78)
100 |     plt.legend(loc=1)
101 |     plt.savefig(fname=PLOT_OUTPUT_DIR_PATH + 'fep_dataset_simulated-torque_plot.svg',
102 |                 dpi=PLOT_DPI,
103 |                 bbox_inches='tight',
104 |                 format='svg')
105 | 
106 |     print("Creation of FEP dataset plots finished")
107 | 
108 | 
109 | if __name__ == '__main__':
110 |     main()
111 | 


--------------------------------------------------------------------------------
/illustrations/visualize_movement_illustration.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/illustrations/visualize_movement_illustration.gif


--------------------------------------------------------------------------------
/movement_datasets/__init__.py:
--------------------------------------------------------------------------------
1 | from movement_datasets.read_write_helper import read_fep_dataset
2 | from movement_datasets.read_write_helper import write_fep_dataset
3 | 


--------------------------------------------------------------------------------
/movement_datasets/read_write_helper.py:
--------------------------------------------------------------------------------
 1 | import numpy as np
 2 | import pandas as pd
 3 | 
 4 | 
 5 | def read_fep_dataset(fep_movement_dataset_path, dtype):
 6 |     """"""
 7 |     pd_dataset = pd.read_csv(fep_movement_dataset_path, sep=' ', header=None, dtype=dtype)
 8 | 
 9 |     print("\nSample of the first 10 lines of the FEP movement dataset:")
10 |     print(pd_dataset.head(10))
11 | 
12 |     pos = pd_dataset.iloc[:, 0:7].to_numpy(dtype=dtype)
13 |     vel = pd_dataset.iloc[:, 7:14].to_numpy(dtype=dtype)
14 |     acc = pd_dataset.iloc[:, 14:21].to_numpy(dtype=dtype)
15 |     tau = pd_dataset.iloc[:, 21:28].to_numpy(dtype=dtype)
16 | 
17 |     return pos, vel, acc, tau
18 | 
19 | 
20 | def write_fep_dataset(fep_sim_output_dataset_path, datasets, dtype):
21 |     """"""
22 |     np_dataset = np.concatenate(datasets, axis=1, dtype=dtype)
23 |     pd_dataset = pd.DataFrame(data=np_dataset, dtype=dtype)
24 | 
25 |     print("\nSample of the first 10 lines of the created dataset:")
26 |     print(pd_dataset.head(10))
27 | 
28 |     pd_dataset.to_csv(fep_sim_output_dataset_path, sep=' ', index=False, header=False)
29 | 


--------------------------------------------------------------------------------
/panda_robot/__init__.py:
--------------------------------------------------------------------------------
1 | from panda_robot.panda_robot import PandaRobot
2 | 


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/collision/finger.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/collision/finger.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/collision/hand.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/collision/hand.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/collision/link0.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/collision/link0.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/collision/link1.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/collision/link1.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/collision/link2.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/collision/link2.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/collision/link3.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/collision/link3.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/collision/link4.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/collision/link4.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/collision/link5.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/collision/link5.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/collision/link6.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/collision/link6.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/collision/link7.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/collision/link7.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/visual/finger.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/visual/finger.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/visual/hand.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/visual/hand.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/visual/link0.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/visual/link0.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/visual/link1.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/visual/link1.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/visual/link2.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/visual/link2.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/visual/link3.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/visual/link3.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/visual/link4.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/visual/link4.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/visual/link5.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/visual/link5.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/visual/link6.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/visual/link6.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/meshes/visual/link7.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PaulPauls/franka_emika_panda_pybullet/7387bcf9bca9730c0a0f2caa6c7b91a39f39fcb1/panda_robot/model_description/meshes/visual/link7.stl


--------------------------------------------------------------------------------
/panda_robot/model_description/panda.urdf:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" ?>
  2 | 
  3 | <robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
  4 |   
  5 |   <link name="panda_link0">
  6 |     <visual>
  7 |       <geometry>
  8 |         <mesh filename="package://model_description/meshes/visual/link0.stl"/>
  9 |       </geometry>
 10 |       <material name="panda_gray"/>
 11 |     </visual>
 12 |     <collision>
 13 |       <geometry>
 14 |         <mesh filename="package://model_description/meshes/collision/link0.stl"/>
 15 |       </geometry>
 16 |       <material name="panda_gray"/>
 17 |     </collision>
 18 |     <inertial>
 19 |         <origin rpy="0 0 0" xyz="0 0 0"/>
 20 |         <mass value="2.8142028896"/>
 21 |         <inertia ixx="0.0129886979" ixy="0.0" ixz="0.0" iyy="0.0165355284" iyz="0" izz="0.0203311636"/>    </inertial>
 22 |   </link>
 23 |   
 24 |   <link name="panda_link1">
 25 |     <visual>
 26 |       <geometry>
 27 |         <mesh filename="package://model_description/meshes/visual/link1.stl"/>
 28 |       </geometry>
 29 |       <material name="panda_white"/>
 30 |     </visual>
 31 |     <collision>
 32 |       <geometry>
 33 |         <mesh filename="package://model_description/meshes/collision/link1.stl"/>
 34 |       </geometry>
 35 |       <material name="panda_white"/>
 36 |     </collision>
 37 |     <inertial>
 38 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 39 |       <mass value="2.3599995791"/>
 40 |       <inertia ixx="0.0186863903" ixy="0.0" ixz="0.0" iyy="0.0143789874" iyz="0" izz="0.00906812"/>
 41 |     </inertial>
 42 |   </link>
 43 |   
 44 |   <joint name="panda_joint1" type="revolute">
 45 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
 46 |     <origin rpy="0 0 0" xyz="0 0 0.333"/>
 47 |     <parent link="panda_link0"/>
 48 |     <child link="panda_link1"/>
 49 |     <axis xyz="0 0 1"/>
 50 |     <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
 51 |     <dynamics damping="1.0"/>
 52 |   </joint>
 53 |   
 54 |   <link name="panda_link2">
 55 |     <visual>
 56 |       <geometry>
 57 |         <mesh filename="package://model_description/meshes/visual/link2.stl"/>
 58 |       </geometry>
 59 |       <material name="panda_white"/>
 60 |     </visual>
 61 |     <collision>
 62 |       <geometry>
 63 |         <mesh filename="package://model_description/meshes/collision/link2.stl"/>
 64 |       </geometry>
 65 |       <material name="panda_white"/>
 66 |     </collision>
 67 |     <inertial>
 68 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 69 |       <mass value="2.379518833"/>
 70 |       <inertia ixx="0.0190388734" ixy="0.0" ixz="0.0" iyy="0.0091429124" iyz="0" izz="0.014697537"/>
 71 |     </inertial>
 72 |   </link>
 73 |   
 74 |   <joint name="panda_joint2" type="revolute">
 75 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
 76 |     <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
 77 |     <parent link="panda_link1"/>
 78 |     <child link="panda_link2"/>
 79 |     <axis xyz="0 0 1"/>
 80 |     <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
 81 |     <dynamics damping="1.0"/>
 82 |   </joint>
 83 |   
 84 |   <link name="panda_link3">
 85 |     <visual>
 86 |       <geometry>
 87 |         <mesh filename="package://model_description/meshes/visual/link3.stl"/>
 88 |       </geometry>
 89 |       <material name="panda_white"/>
 90 |     </visual>
 91 |     <collision>
 92 |       <geometry>
 93 |         <mesh filename="package://model_description/meshes/collision/link3.stl"/>
 94 |       </geometry>
 95 |       <material name="panda_white"/>
 96 |     </collision>
 97 |     <inertial>
 98 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 99 |       <mass value="2.6498823337"/>
100 |       <inertia ixx="0.0129300178" ixy="0.0" ixz="0.0" iyy="0.0150242121" iyz="0" izz="0.0142734598"/>
101 |     </inertial>
102 |   </link>
103 |   
104 |   <joint name="panda_joint3" type="revolute">
105 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
106 |     <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
107 |     <parent link="panda_link2"/>
108 |     <child link="panda_link3"/>
109 |     <axis xyz="0 0 1"/>
110 |     <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
111 |     <dynamics damping="1.0"/>
112 |   </joint>
113 |   
114 |   <link name="panda_link4">
115 |     <visual>
116 |       <geometry>
117 |         <mesh filename="package://model_description/meshes/visual/link4.stl"/>
118 |       </geometry>
119 |       <material name="panda_white"/>
120 |     </visual>
121 |     <collision>
122 |       <geometry>
123 |         <mesh filename="package://model_description/meshes/collision/link4.stl"/>
124 |       </geometry>
125 |       <material name="panda_white"/>
126 |     </collision>
127 |     <inertial>
128 |       <origin rpy="0 0 0" xyz="0 0 0"/>
129 |       <mass value="2.6948018744"/>
130 |       <inertia ixx="0.0133874611" ixy="0.0" ixz="0.0" iyy="0.014514325" iyz="0" izz="0.0155175551"/>
131 |     </inertial>
132 |   </link>
133 |   
134 |   <joint name="panda_joint4" type="revolute">
135 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
136 |     <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
137 |     <parent link="panda_link3"/>
138 |     <child link="panda_link4"/>
139 |     <axis xyz="0 0 1"/>
140 |     <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
141 |     <dynamics damping="1.0"/>
142 |   </joint>
143 |   
144 |   <link name="panda_link5">
145 |     <visual>
146 |       <geometry>
147 |         <mesh filename="package://model_description/meshes/visual/link5.stl"/>
148 |       </geometry>
149 |       <material name="panda_white"/>
150 |     </visual>
151 |     <collision>
152 |       <geometry>
153 |         <mesh filename="package://model_description/meshes/collision/link5.stl"/>
154 |       </geometry>
155 |       <material name="panda_white"/>
156 |     </collision>
157 |     <inertial>
158 |       <origin rpy="0 0 0" xyz="0 0 0"/>
159 |       <mass value="2.9812816864"/>
160 |       <inertia ixx="0.0325565705" ixy="0.0" ixz="0.0" iyy="0.0270660472" iyz="0" izz="0.0115023375"/>
161 |     </inertial>
162 |   </link>
163 |   
164 |   <joint name="panda_joint5" type="revolute">
165 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
166 |     <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
167 |     <parent link="panda_link4"/>
168 |     <child link="panda_link5"/>
169 |     <axis xyz="0 0 1"/>
170 |     <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
171 |     <dynamics damping="1.0"/>
172 |   </joint>
173 |   
174 |   <link name="panda_link6">
175 |     <visual>
176 |       <geometry>
177 |         <mesh filename="package://model_description/meshes/visual/link6.stl"/>
178 |       </geometry>
179 |       <material name="panda_white"/>
180 |     </visual>
181 |     <collision>
182 |       <geometry>
183 |         <mesh filename="package://model_description/meshes/collision/link6.stl"/>
184 |       </geometry>
185 |       <material name="panda_white"/>
186 |     </collision>
187 |     <inertial>
188 |       <origin rpy="0 0 0" xyz="0 0 0"/>
189 |       <mass value="1.1285806309"/>
190 |       <inertia ixx="0.0026052565" ixy="0.0" ixz="0.0" iyy="0.0039897229" iyz="0" izz="0.0047048591"/>
191 |     </inertial>
192 |   </link>
193 |   
194 |   <joint name="panda_joint6" type="revolute">
195 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
196 |     <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
197 |     <parent link="panda_link5"/>
198 |     <child link="panda_link6"/>
199 |     <axis xyz="0 0 1"/>
200 |     <limit effort="12" lower="-0.0175" upper="3.752" velocity="2.6100"/>
201 |     <dynamics damping="1.0"/>
202 |   </joint>
203 |   
204 |   <link name="panda_link7">
205 |     <visual>
206 |       <geometry>
207 |         <mesh filename="package://model_description/meshes/visual/link7.stl"/>
208 |       </geometry>
209 |       <material name="panda_white"/>
210 |     </visual>
211 |     <collision>
212 |       <geometry>
213 |         <mesh filename="package://model_description/meshes/collision/link7.stl"/>
214 |       </geometry>
215 |       <material name="panda_white"/>
216 |     </collision>
217 |     <inertial>
218 |       <origin rpy="0 0 0" xyz="0 0 0"/>
219 |       <mass value="0.4052912465"/>
220 |       <inertia ixx="0.0006316592" ixy="0.0" ixz="0.0" iyy="0.0006319639" iyz="0" izz="0.0010607721"/>
221 |     </inertial>
222 |   </link>
223 |   
224 |   <joint name="panda_joint7" type="revolute">
225 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
226 |     <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
227 |     <parent link="panda_link6"/>
228 |     <child link="panda_link7"/>
229 |     <axis xyz="0 0 1"/>
230 |     <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
231 |     <dynamics damping="1.0"/>
232 |   </joint>
233 |   
234 |   <!-- FLANGE -->
235 |   <link name="panda_EndEffector"></link>
236 | 
237 |   <joint name="panda_joint8" type="fixed">
238 |     <origin rpy="0 0 0" xyz="0 0 0.107"/>
239 |     <parent link="panda_link7"/>
240 |     <child link="panda_EndEffector"/>
241 |     <axis xyz="0 0 0"/>
242 |   </joint>
243 |     
244 |   
245 | 
246 | 
247 |   <material name="panda_white">
248 |     <color rgba="1. 1. 1. 1."/>
249 |   </material>
250 |   <material name="panda_gray">
251 |     <color rgba=".8 .8 .8 1."/>
252 |   </material>
253 | </robot>
254 | 


--------------------------------------------------------------------------------
/panda_robot/model_description/panda_with_gripper.urdf:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" ?>
  2 | 
  3 | <robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
  4 |   
  5 |   <link name="panda_link0">
  6 |     <visual>
  7 |       <geometry>
  8 |         <mesh filename="package://model_description/meshes/visual/link0.stl"/>
  9 |       </geometry>
 10 |       <material name="panda_gray"/>
 11 |     </visual>
 12 |     <collision>
 13 |       <geometry>
 14 |         <mesh filename="package://model_description/meshes/collision/link0.stl"/>
 15 |       </geometry>
 16 |       <material name="panda_gray"/>
 17 |     </collision>
 18 |     <inertial>
 19 |       <origin xyz="0 0 0" rpy="0 0 0"/>
 20 |       <mass value="3.06"/>
 21 |       <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
 22 |     </inertial>
 23 |   </link>
 24 |   
 25 |   <link name="panda_link1">
 26 |     <visual>
 27 |       <geometry>
 28 |         <mesh filename="package://model_description/meshes/visual/link1.stl"/>
 29 |       </geometry>
 30 |       <material name="panda_white"/>
 31 |     </visual>
 32 |     <collision>
 33 |       <geometry>
 34 |         <mesh filename="package://model_description/meshes/collision/link1.stl"/>
 35 |       </geometry>
 36 |       <material name="panda_white"/>
 37 |     </collision>
 38 |     <inertial>
 39 |       <origin xyz="0 0 0" rpy="0 0 0"/>
 40 |       <mass value="2.34"/>
 41 |       <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
 42 |     </inertial>
 43 |   </link>
 44 |   
 45 |   <joint name="panda_joint1" type="revolute">
 46 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
 47 |     <origin rpy="0 0 0" xyz="0 0 0.333"/>
 48 |     <parent link="panda_link0"/>
 49 |     <child link="panda_link1"/>
 50 |     <axis xyz="0 0 1"/>
 51 |     <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
 52 |     <dynamics damping="0.5"/>
 53 |   </joint>
 54 |   
 55 |   <link name="panda_link2">
 56 |     <visual>
 57 |       <geometry>
 58 |         <mesh filename="package://model_description/meshes/visual/link2.stl"/>
 59 |       </geometry>
 60 |       <material name="panda_white"/>
 61 |     </visual>
 62 |     <collision>
 63 |       <geometry>
 64 |         <mesh filename="package://model_description/meshes/collision/link2.stl"/>
 65 |       </geometry>
 66 |       <material name="panda_white"/>
 67 |     </collision>
 68 |     <inertial>
 69 |       <origin xyz="0 0 0" rpy="0 0 0"/>
 70 |       <mass value="2.36"/>
 71 |       <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
 72 |     </inertial>
 73 |   </link>
 74 |   
 75 |   <joint name="panda_joint2" type="revolute">
 76 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
 77 |     <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
 78 |     <parent link="panda_link1"/>
 79 |     <child link="panda_link2"/>
 80 |     <axis xyz="0 0 1"/>
 81 |     <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
 82 |     <dynamics damping="0.5"/>
 83 |   </joint>
 84 |   
 85 |   <link name="panda_link3">
 86 |     <visual>
 87 |       <geometry>
 88 |         <mesh filename="package://model_description/meshes/visual/link3.stl"/>
 89 |       </geometry>
 90 |       <material name="panda_white"/>
 91 |     </visual>
 92 |     <collision>
 93 |       <geometry>
 94 |         <mesh filename="package://model_description/meshes/collision/link3.stl"/>
 95 |       </geometry>
 96 |       <material name="panda_white"/>
 97 |     </collision>
 98 |     <inertial>
 99 |       <origin xyz="0 0 0" rpy="0 0 0"/>
100 |       <mass value="2.38"/>
101 |       <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
102 |     </inertial>
103 |   </link>
104 |   
105 |   <joint name="panda_joint3" type="revolute">
106 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
107 |     <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
108 |     <parent link="panda_link2"/>
109 |     <child link="panda_link3"/>
110 |     <axis xyz="0 0 1"/>
111 |     <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
112 |     <dynamics damping="0.5"/>
113 |   </joint>
114 |   
115 |   <link name="panda_link4">
116 |     <visual>
117 |       <geometry>
118 |         <mesh filename="package://model_description/meshes/visual/link4.stl"/>
119 |       </geometry>
120 |       <material name="panda_white"/>
121 |     </visual>
122 |     <collision>
123 |       <geometry>
124 |         <mesh filename="package://model_description/meshes/collision/link4.stl"/>
125 |       </geometry>
126 |       <material name="panda_white"/>
127 |     </collision>
128 |     <inertial>
129 |       <origin xyz="0 0 0" rpy="0 0 0"/>
130 |       <mass value="2.43"/>
131 |       <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
132 |     </inertial>
133 |   </link>
134 |   
135 |   <joint name="panda_joint4" type="revolute">
136 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
137 |     <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
138 |     <parent link="panda_link3"/>
139 |     <child link="panda_link4"/>
140 |     <axis xyz="0 0 1"/>
141 |     <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
142 |     <dynamics damping="0.5"/>
143 |   </joint>
144 |   
145 |   <link name="panda_link5">
146 |     <visual>
147 |       <geometry>
148 |         <mesh filename="package://model_description/meshes/visual/link5.stl"/>
149 |       </geometry>
150 |       <material name="panda_white"/>
151 |     </visual>
152 |     <collision>
153 |       <geometry>
154 |         <mesh filename="package://model_description/meshes/collision/link5.stl"/>
155 |       </geometry>
156 |       <material name="panda_white"/>
157 |     </collision>
158 |     <inertial>
159 |       <origin xyz="0 0 0" rpy="0 0 0"/>
160 |       <mass value="3.5"/>
161 |       <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
162 |     </inertial>
163 |   </link>
164 |   
165 |   <joint name="panda_joint5" type="revolute">
166 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
167 |     <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
168 |     <parent link="panda_link4"/>
169 |     <child link="panda_link5"/>
170 |     <axis xyz="0 0 1"/>
171 |     <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
172 |     <dynamics damping="0.5"/>
173 |   </joint>
174 |   
175 |   <link name="panda_link6">
176 |     <visual>
177 |       <geometry>
178 |         <mesh filename="package://model_description/meshes/visual/link6.stl"/>
179 |       </geometry>
180 |       <material name="panda_white"/>
181 |     </visual>
182 |     <collision>
183 |       <geometry>
184 |         <mesh filename="package://model_description/meshes/collision/link6.stl"/>
185 |       </geometry>
186 |       <material name="panda_white"/>
187 |     </collision>
188 |     <inertial>
189 |       <origin xyz="0 0 0" rpy="0 0 0"/>
190 |       <mass value="1.47"/>
191 |       <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
192 |     </inertial>
193 |   </link>
194 |   
195 |   <joint name="panda_joint6" type="revolute">
196 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
197 |     <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
198 |     <parent link="panda_link5"/>
199 |     <child link="panda_link6"/>
200 |     <axis xyz="0 0 1"/>
201 |     <limit effort="12" lower="-0.0175" upper="3.752" velocity="2.6100"/>
202 |     <dynamics damping="0.5"/>
203 |   </joint>
204 |   
205 |   <link name="panda_link7">
206 |     <visual>
207 |       <geometry>
208 |         <mesh filename="package://model_description/meshes/visual/link7.stl"/>
209 |       </geometry>
210 |       <material name="panda_white"/>
211 |     </visual>
212 |     <collision>
213 |       <geometry>
214 |         <mesh filename="package://model_description/meshes/collision/link7.stl"/>
215 |       </geometry>
216 |       <material name="panda_white"/>
217 |     </collision>
218 |     <inertial>
219 |       <origin xyz="0 0 0" rpy="0 0 0"/>
220 |       <mass value="0.45"/>
221 |       <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3"/>
222 |     </inertial>
223 |   </link>
224 |   
225 |   <joint name="panda_joint7" type="revolute">
226 |     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
227 |     <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
228 |     <parent link="panda_link6"/>
229 |     <child link="panda_link7"/>
230 |     <axis xyz="0 0 1"/>
231 |     <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
232 |     <dynamics damping="0.5"/>
233 |   </joint>
234 |   
235 |   <joint name="panda_hand_joint" type="fixed">
236 |     <parent link="panda_link7"/>
237 |     <child link="panda_hand"/>
238 |     <origin rpy="0 0 -0.785398163397" xyz="0 0 0.107"/>
239 |   </joint>
240 |   
241 |   <link name="panda_hand">
242 |     <visual>
243 |       <geometry>
244 |         <mesh filename="package://model_description/meshes/visual/hand.stl"/>
245 |       </geometry>
246 |       <material name="panda_gray"/>
247 |     </visual>
248 |     <collision>
249 |       <geometry>
250 |         <mesh filename="package://model_description/meshes/collision/hand.stl"/>
251 |       </geometry>
252 |       <material name="panda_gray"/>
253 |     </collision>
254 |     <inertial>
255 |       <origin xyz="0 0 0" rpy="0 0 0"/>
256 |       <mass value="0.68"/>
257 |       <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
258 |     </inertial>
259 |   </link>
260 |   
261 |   <link name="panda_leftfinger">
262 |     <visual>
263 |       <geometry>
264 |         <mesh filename="package://model_description/meshes/visual/finger.stl"/>
265 |       </geometry>
266 |       <material name="panda_gray"/>
267 |     </visual>
268 |     <collision>
269 |       <geometry>
270 |         <mesh filename="package://model_description/meshes/collision/finger.stl"/>
271 |       </geometry>
272 |       <material name="panda_gray"/>
273 |     </collision>
274 |     <inertial>
275 |       <origin xyz="0 0 0" rpy="0 0 0"/>
276 |       <mass value="0.01"/>
277 |       <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
278 |     </inertial>
279 |   </link>
280 |   
281 |   <link name="panda_rightfinger">
282 |     <visual>
283 |       <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
284 |       <geometry>
285 |         <mesh filename="package://model_description/meshes/visual/finger.stl"/>
286 |       </geometry>
287 |       <material name="panda_gray"/>
288 |     </visual>
289 |     <collision>
290 |       <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
291 |       <geometry>
292 |         <mesh filename="package://model_description/meshes/collision/finger.stl"/>
293 |       </geometry>
294 |       <material name="panda_gray"/>
295 |     </collision>
296 |     <inertial>
297 |       <origin xyz="0 0 0" rpy="0 0 0"/>
298 |       <mass value="0.01"/>
299 |       <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
300 |     </inertial>
301 |   </link>
302 |   
303 |   <joint name="panda_finger_joint1" type="prismatic">
304 |     <parent link="panda_hand"/>
305 |     <child link="panda_leftfinger"/>
306 |     <origin rpy="0 0 0" xyz="0 0 0.0584"/>
307 |     <axis xyz="0 1 0"/>
308 |     <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
309 |   </joint>
310 |   
311 |   <joint name="panda_finger_joint2" type="prismatic">
312 |     <parent link="panda_hand"/>
313 |     <child link="panda_rightfinger"/>
314 |     <origin rpy="0 0 0" xyz="0 0 0.0584"/>
315 |     <axis xyz="0 -1 0"/>
316 |     <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
317 |     <mimic joint="panda_finger_joint1"/>
318 |   </joint>
319 |   
320 |   
321 |   <material name="panda_white">
322 |     <color rgba="1. 1. 1. 1."/>
323 |   </material>
324 |   <material name="panda_gray">
325 |     <color rgba=".8 .8 .8 1."/>
326 |   </material>
327 | </robot>
328 | 


--------------------------------------------------------------------------------
/panda_robot/panda_robot.py:
--------------------------------------------------------------------------------
 1 | import os
 2 | import math
 3 | import pybullet as p
 4 | 
 5 | 
 6 | class PandaRobot:
 7 |     """"""
 8 | 
 9 |     def __init__(self, include_gripper):
10 |         """"""
11 |         p.setAdditionalSearchPath(os.path.dirname(__file__) + '/model_description')
12 |         panda_model = "panda_with_gripper.urdf" if include_gripper else "panda.urdf"
13 |         self.robot_id = p.loadURDF(panda_model, useFixedBase=True, flags=p.URDF_USE_SELF_COLLISION)
14 | 
15 |         # Set maximum joint velocity. Maximum joint velocity taken from:
16 |         # https://s3-eu-central-1.amazonaws.com/franka-de-uploads/uploads/Datasheet-EN.pdf
17 |         p.changeDynamics(bodyUniqueId=self.robot_id, linkIndex=0, maxJointVelocity=150 * (math.pi / 180))
18 |         p.changeDynamics(bodyUniqueId=self.robot_id, linkIndex=1, maxJointVelocity=150 * (math.pi / 180))
19 |         p.changeDynamics(bodyUniqueId=self.robot_id, linkIndex=2, maxJointVelocity=150 * (math.pi / 180))
20 |         p.changeDynamics(bodyUniqueId=self.robot_id, linkIndex=3, maxJointVelocity=150 * (math.pi / 180))
21 |         p.changeDynamics(bodyUniqueId=self.robot_id, linkIndex=4, maxJointVelocity=180 * (math.pi / 180))
22 |         p.changeDynamics(bodyUniqueId=self.robot_id, linkIndex=5, maxJointVelocity=180 * (math.pi / 180))
23 |         p.changeDynamics(bodyUniqueId=self.robot_id, linkIndex=6, maxJointVelocity=180 * (math.pi / 180))
24 | 
25 |         # Set DOF according to the fact that either gripper is supplied or not and create often used joint list
26 |         self.dof = p.getNumJoints(self.robot_id) - 1
27 |         self.joints = range(self.dof)
28 | 
29 |         # Reset Robot
30 |         self.reset_state()
31 | 
32 |     def reset_state(self):
33 |         """"""
34 |         for j in range(self.dof):
35 |             p.resetJointState(self.robot_id, j, targetValue=0)
36 |         p.setJointMotorControlArray(bodyUniqueId=self.robot_id,
37 |                                     jointIndices=self.joints,
38 |                                     controlMode=p.VELOCITY_CONTROL,
39 |                                     forces=[0. for _ in self.joints])
40 | 
41 |     def get_dof(self):
42 |         """"""
43 |         return self.dof
44 | 
45 |     def get_joint_info(self, j):
46 |         """"""
47 |         return p.getJointInfo(self.robot_id, j)
48 | 
49 |     def get_base_position_and_orientation(self):
50 |         """"""
51 |         return p.getBasePositionAndOrientation(self.robot_id)
52 | 
53 |     def get_position_and_velocity(self):
54 |         """"""
55 |         joint_states = p.getJointStates(self.robot_id, self.joints)
56 |         joint_pos = [state[0] for state in joint_states]
57 |         joint_vel = [state[1] for state in joint_states]
58 |         return joint_pos, joint_vel
59 | 
60 |     def calculate_inverse_kinematics(self, position, orientation):
61 |         """"""
62 |         return p.calculateInverseKinematics(self.robot_id, self.dof, position, orientation)
63 | 
64 |     def calculate_inverse_dynamics(self, pos, vel, desired_acc):
65 |         """"""
66 |         assert len(pos) == len(vel) and len(vel) == len(desired_acc)
67 |         vector_length = len(pos)
68 | 
69 |         # If robot set up with gripper, set those positions, velocities and desired accelerations to 0
70 |         if self.dof == 9 and vector_length != 9:
71 |             pos = pos + [0., 0.]
72 |             vel = vel + [0., 0.]
73 |             desired_acc = desired_acc + [0., 0.]
74 | 
75 |         simulated_torque = list(p.calculateInverseDynamics(self.robot_id, pos, vel, desired_acc))
76 | 
77 |         # Remove unnecessary simulated torques for gripper if robot set up with gripper
78 |         if self.dof == 9 and vector_length != 9:
79 |             simulated_torque = simulated_torque[:7]
80 |         return simulated_torque
81 | 
82 |     def set_target_positions(self, desired_pos):
83 |         """"""
84 |         # If robot set up with gripper, set those positions to 0
85 |         if self.dof == 9:
86 |             desired_pos = desired_pos + [0., 0.]
87 |         p.setJointMotorControlArray(bodyUniqueId=self.robot_id,
88 |                                     jointIndices=self.joints,
89 |                                     controlMode=p.POSITION_CONTROL,
90 |                                     targetPositions=desired_pos)
91 | 
92 |     def set_torques(self, desired_torque):
93 |         """"""
94 |         p.setJointMotorControlArray(bodyUniqueId=self.robot_id,
95 |                                     jointIndices=self.joints,
96 |                                     controlMode=p.TORQUE_CONTROL,
97 |                                     forces=desired_torque)
98 | 


--------------------------------------------------------------------------------
/requirements.txt:
--------------------------------------------------------------------------------
 1 | cycler==0.10.0
 2 | kiwisolver==1.3.1
 3 | matplotlib==3.3.4
 4 | numpy==1.20.1
 5 | pandas==1.2.2
 6 | Pillow==8.1.0
 7 | pybullet==3.0.8
 8 | pyparsing==2.4.7
 9 | python-dateutil==2.8.1
10 | pytz==2021.1
11 | six==1.15.0
12 | 


--------------------------------------------------------------------------------