[Documentation] [TitleIndex] [WordIndex

dmp

Package Summary

dmp

Overview

This package provides a general implementation of Dynamic Movement Primitives (DMPs). A good reference on DMPs can be found here, but this package implements a more stable reformulation of DMPs also described in the referenced paper. Current capabilities include the learning of multi-dimensional DMPs from example trajectories and generation of full and partial plans for arbitrary starting and goal points. Eventually, a wider selection of function approximators will be added, in addition to native support for reinforcement learning.

A few notes:

  1. This implementation is agnostic toward what is being generated by the DMP, i.e. force, acceleration, or any other quantity.
  2. We implement N-dimensional DMPs as N separate DMPs linked together with a single phase system, as in the paper reference above.
  3. Function approximation is done with a simple local linear interpolation scheme, but code for a global function approximator using the Fourier basis is also provided, along with an additional local approximation scheme using radial basis functions. However, it is recommended to just use linear interpolation unless the robot is learning from a large amount of data that should not be stored locally in full.

Using DMPs

First, the DMP server must be running. Type:

   1 roslaunch dmp dmp.launch

Now, let's look at some sample code to learn a DMP from demonstration, set it as the active DMP on the server, and use it to plan, given a new start and goal:

   1 #!/usr/bin/env python
   2 import roslib; 
   3 roslib.load_manifest('dmp')
   4 import rospy 
   5 import numpy as np
   6 from dmp.srv import *
   7 from dmp.msg import *
   8 
   9 #Learn a DMP from demonstration data
  10 def makeLFDRequest(dims, traj, dt, K_gain, 
  11                    D_gain, num_bases):
  12     demotraj = DMPTraj()
  13         
  14     for i in range(len(traj)):
  15         pt = DMPPoint();
  16         pt.positions = traj[i]
  17         demotraj.points.append(pt)
  18         demotraj.times.append(dt*i)
  19             
  20     k_gains = [K_gain]*dims
  21     d_gains = [D_gain]*dims
  22         
  23     print "Starting LfD..."
  24     rospy.wait_for_service('learn_dmp_from_demo')
  25     try:
  26         lfd = rospy.ServiceProxy('learn_dmp_from_demo', LearnDMPFromDemo)
  27         resp = lfd(demotraj, k_gains, d_gains, num_bases)
  28     except rospy.ServiceException, e:
  29         print "Service call failed: %s"%e
  30     print "LfD done"    
  31             
  32     return resp;
  33 
  34 
  35 #Set a DMP as active for planning
  36 def makeSetActiveRequest(dmp_list):
  37     try:
  38         sad = rospy.ServiceProxy('set_active_dmp', SetActiveDMP)
  39         sad(dmp_list)
  40     except rospy.ServiceException, e:
  41         print "Service call failed: %s"%e
  42 
  43 
  44 #Generate a plan from a DMP
  45 def makePlanRequest(x_0, x_dot_0, t_0, goal, goal_thresh, 
  46                     seg_length, tau, dt, integrate_iter):
  47     print "Starting DMP planning..."
  48     rospy.wait_for_service('get_dmp_plan')
  49     try:
  50         gdp = rospy.ServiceProxy('get_dmp_plan', GetDMPPlan)
  51         resp = gdp(x_0, x_dot_0, t_0, goal, goal_thresh, 
  52                    seg_length, tau, dt, integrate_iter)
  53     except rospy.ServiceException, e:
  54         print "Service call failed: %s"%e
  55     print "DMP planning done"   
  56             
  57     return resp;
  58 
  59 
  60 if __name__ == '__main__':
  61     rospy.init_node('dmp_tutorial_node')
  62 
  63     #Create a DMP from a 2-D trajectory
  64     dims = 2                
  65     dt = 1.0                
  66     K = 100                 
  67     D = 2.0 * np.sqrt(K)      
  68     num_bases = 4          
  69     traj = [[1.0,1.0],[2.0,2.0],[3.0,4.0],[6.0,8.0]]
  70     resp = makeLFDRequest(dims, traj, dt, K, D, num_bases)
  71 
  72     #Set it as the active DMP
  73     makeSetActiveRequest(resp.dmp_list)
  74 
  75     #Now, generate a plan
  76     x_0 = [0.0,0.0]          #Plan starting at a different point than demo 
  77     x_dot_0 = [0.0,0.0]   
  78     t_0 = 0                
  79     goal = [8.0,7.0]         #Plan to a different goal than demo
  80     goal_thresh = [0.2,0.2]
  81     seg_length = -1          #Plan until convergence to goal
  82     tau = 2 * resp.tau       #Desired plan should take twice as long as demo
  83     dt = 1.0
  84     integrate_iter = 5       #dt is rather large, so this is > 1  
  85     plan = makePlanRequest(x_0, x_dot_0, t_0, goal, goal_thresh, 
  86                            seg_length, tau, dt, integrate_iter)
  87 
  88     print plan

When executed, this produces:

Starting LfD...
LfD done
Starting DMP planning...
DMP planning done
plan: 
  points: 
    - 
      positions: [0.0, 0.0]
      velocities: [0.0, 0.0]
    - 
      positions: [0.8100075124021597, 0.7087565733518839]
      velocities: [2.492278234944417, 2.1807434555763616]
    - 
      positions: [-3.2149288213481824, -6.1594619175155705]
      velocities: [-1.0934970518496467, -2.769417766916274]
    - 
      positions: [0.8827255030673307, -1.937261989255104]
      velocities: [5.431303987123971, 6.50827015230193]
    - 
      positions: [6.090630509119033, 4.786163676480743]
      velocities: [2.993882533676429, 3.953989716003782]
    - 
      positions: [6.599551969911872, 5.5741095433201595]
      velocities: [-0.14593240997752455, -0.20901919742813732]
    - 
      positions: [7.843761135045964, 7.064792076582948]
      velocities: [1.077860218553606, 1.237107151074346]
  times: [0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0]
at_goal: 1

Parameters

DMPs have several parameters for both learning and planning that require a bit of explanation.

LearnDMPFromDemo Parameters

GetDMPPlan Parameters

Nodes

dmp_server

Services

learn_dmp_from_demo (pr2_dmp/LearnDMPFromDemo) set_active_dmp (pr2_dmp/SetActiveDMP) get_dmp_plan (pr2_dmp/GetDMPPlan)


2019-06-08 12:39