DMP_simulation

A simulation project on Dynamic Movement Primitive (DMP) , containing 1D numerical simulation, 2D learning from demonstration and 3D simulation for robotic arm.

• DMP_simulation
  • 1. About DMP
  • 2. About this project
  • 3. Development environment
  • 4. Simulations and How to use
    • A. Numerical simulation 1-D
    • B. Two-link robotic arm learning from demonstration 2-D
    • C. DMP-based learning from demonstration of Emika Panda 3D (pybullet)

Table of contents generated with markdown-toc

1. About DMP

Supported by several experimental findings, researchers find that biological systems are able to combine and adapt basic units of motion into complex tasks, which finally lead to the formulation of the motor primitives theory. In this respect, Dynamic Movement Primitives (DMPs) represent an elegant mathematical formulation of the motor primitives as stable dynamical systems, and are well suited to generate motor commands for artificial systems like robots. DMP was initially proposed by Schaal et al. in 2002, as the most classical theories for implementing robot imitation learning. Its various extended forms and wide-ranging applications have been prevalent in research over the past two decades. DMP is based on a set of nonlinear differential equations to describe motion trajectories and utilizes a group of Radial Basis Functions (RBFs) for approximation and generation of movements. The DMP model can be applied to various motions, including robotic arm movements, robot path planning, human motion imitation, handwriting and speech generation, among others.

From my personal perspective, DMP is a forced dynamic system: (1) The dynamic system ensures that it can generate smooth and stable outputs, which distinguishes DMP from other purely data-driven primitives (such as ProMP and GP), making it more suitable for guiding the motion of actuators in other dynamic systems. (2) The forcing terms are the core of DMP, typically composed of multiple radial basis functions (RBFs). By calculated the weights of RBFs based on the expert data, DMP can use the forcing term to guide the dynamic system’s output to match the dynamic features of the expert demonstration during generation. (3) It is worth noting that DMP also includes an automatically generated regularization term that gradually decays to zero, its main function is to gradually reduce the influence of the forcing term on the dynamic system, allowing the output of the dynamic system to converge to the target state.

The usage of the DMP model includes two stages: training and reproduction. In the training stage, the parameters of the basis functions (RBFs) are learned from the demonstrated data, resulting in a suitable DMP model. In the reproduction stage, the initial conditions, motion goals, spatial scaling, and temporal scaling can be flexibly set according to the requirements. Finally, by computing the output of the DMP model using the above parameters, it is possible to achieve the reproduction or generalization of the demonstrated motions.

For specific theoretical details and the current researchs of DMP, you can refer to the survey:

Saveriano, Matteo, et al. "Dynamic movement primitives in robotics: A tutorial survey." The International Journal of Robotics Research 42.13 (2023): 1133-1184.

2. About this project

After the theoretical learning, Cheems_JH built the following three demos to further deepen the understanding and application of DMP:

• Verify the DMP model through 1D-trajectory numerical simulation
• Understanding how DMP guides a simple system to perform 2D-trajectory demonstration learning;
• Obtaining data of virtual robotic arm through 3D simulation, and exploring its implementation of demonstration learning based on DMP.

3. Development environment

Windows10(no GPU) + PyCharm(2023.2) + python3.9

4. Simulations and How to use

A. Numerical simulation 1-D

This is a numerical simulation for 1-dimension data based on discrete DMP:

• Code path: /python/discrete_dmp_1D.py.
• Packages: numpy, scipy, matplotlib.

You can simply run this code like:

# Training dmp for the demonstration
dmp = DiscreteDMP(data_set=demo_y)
# Reproduction by DMP
y_re, dy_re, ddy_re = dmp.reproduction()

You can also change the start, target, and time scale during reproduction like this:

y_re, dy_re, ddy_re = dmp.reproduction(start=1.0, target=0.5, tau=2.0)

Show optional parameter configuration

1. Trainning DMP:

   dmp = DiscreteDMP(data_set, n_rbf, alpha_y, beta_y, alpha_x, cs_runtime)

   • Usually set beta_y=alpha_y/4, alpha_x=1.0 and cs_runtime=1.0 by default.
   • The remaining parameters are freely configurable, n_rbf and alpha_y affect how well the DMP fits the demonstration.

2. Reproducing based on DMP:

   dmp.reproduction(self, start, target, tau):

   • The default start and target points for reproduction are the start and target points for the demonstration.
   • You can also specify new start and end points via start and target.
   • tau is the time scaling parameter: if 0<tau<1, slow down; if tau>1, speed up.

3. Basis functions:

   # In line 48 of the code
   self.psi_h = np.ones(self.n_rbf) * self.n_rbf ** 1.5 / self.psi_centers / self.alpha_x

   • Both the centers self.psi_centers and weights self.w of the basis functions in DMP are obtained by data training.
   • However, the width of the basis functions is based on their centers, and this mapping is set manually.
   • Usually, researchers set psi_h=n_rbf/psi_centers, you can also use your experience to change the mapping code on line 48.

B. Two-link robotic arm learning from demonstration 2-D

This is a numerical simulation for 2-dimension data (position) based on discrete DMP:

• Code path: /python/twolink_roboticarm_dmp_2D.py. Before run this code, you also need to download /python/discrete_dmp_1D.py.
• Packages: numpy, scipy, matplotlib, sys, pygame, math, discrete_dmp_1D.

You can see how the simulation works in the following GIF: the trajectory of the mouse drawn on the UI is used for DMP training, and the trained DMP model is further used to guide the two-link robotic arm to reproduce this skill.

And when the UI is turned off, the program will also pop up the training effect of DMP for the demonstration:

You can also control the rate of the roboitc arm k_interpolation, the accuracy of the fit n_rbf, and the target position and size of the generalization start_bias_x, start_bias_y, target_bias_x, target_bias_y by setting the parameters on lines 19-25 in source code.

As you can see in the GIF below, I drew the number 2 and set a parameter to adjust the position and enlarge the skill when generalizing:

C. DMP-based learning from demonstration of Emika Panda 3D (pybullet)

This is a 3D simulation for DMP based learning from demonstration of robotic arm (Emika Panda) based on Pybullet:

• Code path: /python/panda_get_demo.py and /python/panda_DMP_reproduction.py. Before run the above two codes, you also need to download /python/discrete_dmp_1D.py and /python/demonstrations/demo2023-08-21_13-21-12.xlsx.
• Packages: pybullet, numpy, scipy, discrete_dmp_1D, openpyxl.

STEP 1: Teaching robotic arm and get data of demonstration

You can run /python/panda_get_demo.py to get demonstrations: as GIF below, dragging the slider with the mouse can control the movement of each joint of the manipulator and the gripper. When the demonstration is over, press the keyboard 'q' key to exit, and the program will save the.xlsx file named "demo+ current time" in the current directory to store data of this demonstration.

A demonstration is already recorded in file /python/demonstrations/demo2023-08-21_13-21-12.xlsx on this page, where I control the robotic arm grabs the middle blue object and then places it into the red area.

STEP 2: Reproduction or generalization

After demonstration, you can run /python/panda_DMP_reproduction.py to reproduction or generalizations the demonstration. On line 36 in code, you can read different demos by changing the value of the variable data_path. And you can use the parameters on lines 28 through 32 to do generalization or time scaling, as described above.

This gif is a reproduction result of a Pick-and-Place task:

And can also generalize the task to place the object in the red area by modifying the target point:

NOTE:

• Since there is no collision detection and feasible detection for the generalized path in the code, the generalization may collide through the mold or the trajectory is unreachable.
• Since DMP is a training demonstration and is essentially data fitting rather than truly "understanding" the task. Therefore, even if the generalization trajectories are possible and do not collide, it is possible that the generalization will not be successful.