├── .gitignore ├── README.md ├── examples ├── python_promp │ ├── conditioning.py │ ├── marginal_dist.py │ ├── simple_example.py │ ├── toy_example.py │ └── train.py └── strike_mov.npz └── robpy ├── __init__.py ├── full_promp.py ├── kinematics ├── __init__.py └── forward.py └── utils.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | *.pyc 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Probabilistic Movement Primitive Library 2 | ======================================== 3 | 4 | A Probabilistic Movement Primitive (ProMP) is a probabilistic generative model used to model movement, it is typically 5 | used in the robotics community to learn movements from a human demonstrator (or teacher) and replicate those 6 | movements in a robotic platform. 7 | 8 | This repository contains the implementation in Python and C++ of the Probabilistic Movement Primitive framework as 9 | described in [this paper](https://arxiv.org/pdf/1808.10648.pdf). Typically, the operations we want from a ProMP 10 | are: 11 | 12 | 1) Learning a ProMP from several human demonstrations. Typically, we consider we learn from trajectories in joint 13 | space. 14 | 2) Conditioning in joint space. For example, force the movement to start in the current position of the robot. 15 | 3) Conditioning in task space. For example, conditioning a table tennis strike movement to hit the ball in a 16 | certain position in Cartesian coordinates. 17 | 4) Controlling a robot to track a ProMP 18 | 19 | We provide code for the first three operations. We assume that the learning is done in Python, and only implemented 20 | the adaptation operators in C++ (They are also provided in Python). We also provide code for the following operations: 21 | 22 | * Compute the likelihood of a given trajectory for a given ProMP 23 | * Sample trajectories from a ProMP 24 | * Save and load ProMPs 25 | 26 | Code Examples 27 | ------------- 28 | 29 | Please refer to the example folder in the repository. We just show a simple example 30 | that illustrates how to use the API in general. 31 | 32 | ### A very simple example 33 | 34 | The following example loads a dataset of strike trajectories from a file called 35 | "strike_mov.npz" and trains a ProMP with the given trajectories. The example 36 | file is provided as part of the repository. Subsequently, the script draws samples 37 | from the learned ProMP and a ProMP conditioned to start at a particular location. 38 | 39 | ``` 40 | import robpy.full_promp as promp 41 | import robpy.utils as utils 42 | import numpy as np 43 | from matplotlib import pyplot as plt 44 | 45 | #1) Take the first 10 striking movements from a file with recorded demonstrations 46 | with open('strike_mov.npz','r') as f: 47 | data = np.load(f) 48 | time = data['time'][0:10] 49 | Q = data['Q'][0:10] 50 | 51 | #2) Create a ProMP with basis functions: 3 RBFs with scale 0.25 and 52 | # centers 0.25, 0.5 and 0.75. Use also polynomial basis functions of 53 | # degree one (constant and linear term) 54 | full_basis = { 55 | 'conf': [ 56 | {"type": "sqexp", "nparams": 4, "conf": {"dim": 3}}, 57 | {"type": "poly", "nparams": 0, "conf": {"order": 1}} 58 | ], 59 | 'params': [np.log(0.25),0.25,0.5,0.75] 60 | } 61 | robot_promp = promp.FullProMP(basis=full_basis) 62 | 63 | #3) Train ProMP with NIW prior on the covariance matrix (as described in the paper) 64 | 65 | dof = 7 66 | dim_basis_fun = 5 67 | inv_whis_mean = lambda v, Sigma: utils.make_block_diag(Sigma, dof) 68 | prior_Sigma_w = {'v':dim_basis_fun*dof, 'mean_cov_mle': inv_whis_mean} 69 | train_summary = robot_promp.train(time, q=Q, max_iter=10, prior_Sigma_w=prior_Sigma_w, 70 | print_inner_lb=True) 71 | 72 | 73 | #4) Plot some samples from the learned ProMP and conditioned ProMP 74 | 75 | n_samples = 5 # Number of samples to draw 76 | plot_dof = 3 # Degree of freedom to plot 77 | sample_time = [np.linspace(0,1,200) for i in range(n_samples)] 78 | 79 | #4.1) Make some samples from the unconditioned ProMP 80 | promp_samples = robot_promp.sample(sample_time) 81 | 82 | #4.2) Condition the ProMP to start at q_cond_init and draw samples from it 83 | q_cond_init = [1.54, 0.44, 0.15, 1.65, 0.01, -0.09, -1.23] 84 | robot_promp.condition(t=0, T=1, q=q_cond_init, ignore_Sy=False) 85 | cond_samples = robot_promp.sample(sample_time) 86 | ``` 87 | 88 | ### Using task space conditioning 89 | 90 | In order to use task space conditioning you need to implement the forward kinematics of your own robot. 91 | You simply need to implement a class with a method called "position_and_jac(q)" that given a joint space 92 | configuration q produces a tuple (x, jac, ori) representing respectively the cartesian position, the 93 | jacobian and the orientation. We provide an example of the implementation of the kinematics of a Barrett 94 | WAM arm. The following example shows how to condition a ProMP in task space using the Barrett forward 95 | kinematics implementation. 96 | 97 | ``` 98 | import robpy.kinematics.forward as fwd 99 | 100 | # Compute the prior distribution in joint space at the desired time 101 | time_cartesian = 0.9 102 | mean_marg_w, cov_marg_w = robot_promp.marginal_w(np.array([0.0,time_cartesian,1.0]), q=True) 103 | prior_mu_q = mean_marg_w[1] 104 | prior_Sigma_q = cov_marg_w[1] 105 | 106 | # Compute the posterior distribution in joint space after conditioning in task space 107 | fwd_kin = fwd.BarrettKinematics() 108 | prob_inv_kin = promp.ProbInvKinematics(fwd_kin) 109 | 110 | mu_cartesian = np.array([-0.62, -0.44, -0.34]) 111 | Sigma_cartesian = 0.02**2*np.eye(3) 112 | 113 | mu_q, Sigma_q = prob_inv_kin.inv_kin(mu_theta=prior_mu_q, sig_theta=prior_Sigma_q, 114 | mu_x = mu_cartesian, sig_x = Sigma_cartesian) 115 | 116 | # Finally, condition in joint space using the posterior joint space distribution 117 | 118 | robot_promp.condition(t=time_cartesian, T=1.0, q=mu_q, Sigma_q=Sigma_q, ignore_Sy=False) 119 | task_cond_samples = robot_promp.sample(sample_time) 120 | ``` 121 | 122 | Publications 123 | ------------ 124 | 125 | The implementations provided in this repository are based on the following publications: 126 | 127 | 1) [Adaptation and Robust Learning of Probabilistic Movement Primitives](https://arxiv.org/pdf/1808.10648.pdf) 128 | 2) [Using probabilistic movement primitives for striking movements, IEEE RAS International 129 | Conference on Humanoid Robots, 2016](https://ieeexplore.ieee.org/abstract/document/7803322/) 130 | 131 | Please refer to these papers to understand our implementation, get general information about 132 | probabilistic movement primitives and see the evaluation of the implemented methods in real 133 | robotic platforms. We also have a [C++ implementation](https://github.com/sebasutp/promp-cpp) of 134 | the ProMP conditioning in joint and task space and other operators that might require real-time 135 | execution performance. 136 | 137 | -------------------------------------------------------------------------------- /examples/python_promp/conditioning.py: -------------------------------------------------------------------------------- 1 | """ Very simple conditioning example 2 | 3 | Most of the other examples work with a 7 DoF robot. In this example, we show 4 | simply a Polynomial with fixed parameters on a single degree of freedom, and 5 | show how conditioning work in this scenario. 6 | """ 7 | 8 | import robpy.full_promp as promp 9 | import robpy.utils as utils 10 | import numpy as np 11 | import argparse 12 | import os 13 | import matplotlib.pyplot as plt 14 | import json 15 | 16 | print promp.__file__ 17 | 18 | full_basis = { 19 | 'conf': [ 20 | {"type": "poly", "nparams": 0, "conf": {"order": 3}} 21 | ], 22 | 'params': [] 23 | } 24 | dim_basis_fun = promp.dim_comb_basis(**full_basis) 25 | dof = 1 26 | w_dim = dof*dim_basis_fun 27 | 28 | p = promp.FullProMP(num_joints=1, basis=full_basis) 29 | p.mu_w = np.zeros(w_dim) 30 | p.Sigma_w = 1e2*np.eye(w_dim) 31 | p.Sigma_y = 1e-4*np.eye(dof) 32 | 33 | n_samples = 5 # Number of samples to draw 34 | sample_time = [np.linspace(0,1,200) for i in range(n_samples)] 35 | 36 | #1) Condition the ProMP to pass through q=0.5 at time 0, q=-1 at time 0.5 and q=1 at time 1.0 37 | 38 | #1.1) Procedure 1: Use the E-step doing all at the same time 39 | e_step = p.E_step(times=[[0.0,0.5,1.0]], Y = np.array([[[0.5],[-1.0],[1.0]]])) 40 | 41 | #1.2) Procedure 2: Condition the ProMP one by one obs 42 | p.condition(t=0.0, T=1.0, q=np.array([0.5]), ignore_Sy=False) 43 | p.condition(t=0.5, T=1.0, q=np.array([-1]), ignore_Sy=False) 44 | p.condition(t=1.0, T=1.0, q=np.array([1]), ignore_Sy=False) 45 | 46 | # Draw samples of recursive conditioning 47 | recursive_cond_samples = p.sample(sample_time) 48 | 49 | # Draw samples of E-step conditioning 50 | p.mu_w = e_step['w_means'][0] 51 | p.Sigma_w = e_step['w_covs'][0] 52 | e_step_cond_samples = p.sample(sample_time) 53 | 54 | for i in range(n_samples): 55 | plt.plot(sample_time[i], recursive_cond_samples[i][:,0], color='green') 56 | plt.plot(sample_time[i], e_step_cond_samples[i][:,0], color='blue') 57 | plt.title('Samples of the conditioned ProMPs') 58 | plt.show() 59 | 60 | 61 | -------------------------------------------------------------------------------- /examples/python_promp/marginal_dist.py: -------------------------------------------------------------------------------- 1 | """ Computes and plots the marginal joint distribution of a ProMP 2 | 3 | This script computes, plots and optionally saves a distribution of 4 | joint angles given a ProMP. If you wish to see the also plot recorded 5 | distributions along with the marginal please provide the paths to the 6 | data and meta-data as well. 7 | 8 | """ 9 | 10 | import robpy.full_promp as promp 11 | import robpy.utils as utils 12 | import numpy as np 13 | import argparse 14 | import os 15 | import matplotlib.pyplot as plt 16 | import json 17 | 18 | def plot_promp(promp, marginal, **params): 19 | """ Plots independently each of the outputs of the ProMP 20 | """ 21 | params.setdefault("joint", 0) 22 | 23 | d = params['joint'] 24 | dvel = d + promp.num_joints 25 | dist_z = marginal['time'] 26 | means = np.array(marginal['means']) 27 | stdevs = np.array(map(lambda x: np.sqrt(np.diag(x)), marginal['covs'])) 28 | 29 | tot_plots = 2 if params['plot_vel'] else 1 30 | fig = plt.figure() 31 | ax1 = fig.add_subplot(tot_plots,1,1) 32 | ax1.plot(dist_z, means[:,d], 'r') 33 | ax1.fill_between(dist_z, means[:,d]-2*stdevs[:,d], means[:,d]+2*stdevs[:,d], facecolor='red', alpha=0.3) 34 | if params['plot_vel']: 35 | ax2 = fig.add_subplot(2,1,2) 36 | ax2.plot(dist_z, means[:,dvel], 'r') 37 | ax2.fill_between(dist_z, means[:,dvel]-2*stdevs[:,dvel], means[:,dvel]+2*stdevs[:,dvel], facecolor='red', alpha=0.3) 38 | 39 | if 'data' in params and 'time' in params['data']: 40 | for ix,t in enumerate(params['data']['time']): 41 | Tn = len(t) 42 | T = (t[-1] - t[0]) 43 | z = np.linspace(0,1,Tn) 44 | ax1.plot(z, params['data']['q'][ix][:,d], 'g') 45 | if params['plot_vel']: 46 | ax2.plot(z, T*params['data']['qd'][ix][:,d], 'g') 47 | ax1.set_ylabel('$q_{0}$'.format(params['joint'])) 48 | ax1.set_xlabel('time') 49 | if params['plot_vel']: 50 | ax2.set_ylabel('$\dot{q}_{0}$'.format(params['joint'])) 51 | ax2.set_xlabel('time') 52 | if params['tikz']: 53 | from matplotlib2tikz import save as tikz_save 54 | tikz_save(params['tikz'], figureheight = '\\figureheight', figurewidth = '\\figurewidth') 55 | plt.show() 56 | 57 | def main(args): 58 | data = {} 59 | if args.data: 60 | with open(args.data,'r') as f: 61 | ldata = np.load(f) 62 | data['time'] = ldata['time'] 63 | data['q'] = ldata['Q'] 64 | data['qd'] = ldata['Qdot'] 65 | if args.start_ix or args.end_ix: 66 | a = args.start_ix if args.start_ix is not None else 0 67 | b = args.end_ix if args.end_ix is not None else len(data['time']) 68 | for k in data: 69 | data[k] = data[k][a:b] 70 | model = promp.FullProMP.load(args.promp) 71 | mlh = model.log_likelihood(data['time'], q=data['q']) 72 | print("Avg. Log-Likelihood: {}".format(mlh / len(data['time']))) 73 | dist_z = np.linspace(0,1,50) 74 | means,covs = model.marginal_w(dist_z, q=True, qd=True) 75 | marginal = {'time': dist_z, 'means': means, 'covs': covs} 76 | if args.save_dist: 77 | with open(args.save_dist,'w') as f: 78 | json.dump(utils.numpy_serialize(marginal), f) 79 | if args.plot_marg: 80 | for d in xrange(7): 81 | plot_promp(model, marginal, joint=d, tikz=args.tikz, plot_vel=args.plot_vel, data=data) 82 | 83 | 84 | if __name__ == "__main__": 85 | parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) 86 | parser.add_argument('promp', help="Path to the stored JSON ProMP file") 87 | parser.add_argument('--data', help="File with the trajectories used for training " 88 | "as a dictionary with keys {time, Q, Qdot}") 89 | parser.add_argument('--start_ix', type=int, help="From which data index") 90 | parser.add_argument('--end_ix', type=int, help="To which data index") 91 | parser.add_argument('--plot_marg', action="store_true", help="Plot marginal distributions") 92 | parser.add_argument('--plot_vel', action="store_true", help="Plot the velocities and the learned distribution") 93 | parser.add_argument('--save_dist', help="File where the computed marginal distribution is stored") 94 | parser.add_argument('--tikz', help="Name of a latex file to export the produced figure") 95 | args = parser.parse_args() 96 | main(args) 97 | -------------------------------------------------------------------------------- /examples/python_promp/simple_example.py: -------------------------------------------------------------------------------- 1 | import robpy.full_promp as promp 2 | import robpy.utils as utils 3 | import numpy as np 4 | from matplotlib import pyplot as plt 5 | 6 | #1) Take the first 10 striking movements from a file with recorded demonstrations 7 | with open('strike_mov.npz','r') as f: 8 | data = np.load(f) 9 | time = data['time'][0:10] 10 | Q = data['Q'][0:10] 11 | 12 | #2) Create a ProMP with basis functions: 3 RBFs with scale 0.25 and 13 | # centers 0.25, 0.5 and 0.75. Use also polynomial basis functions of 14 | # degree one (constant and linear term) 15 | full_basis = { 16 | 'conf': [ 17 | {"type": "sqexp", "nparams": 4, "conf": {"dim": 3}}, 18 | {"type": "poly", "nparams": 0, "conf": {"order": 1}} 19 | ], 20 | 'params': [np.log(0.25),0.25,0.5,0.75] 21 | } 22 | robot_promp = promp.FullProMP(basis=full_basis) 23 | 24 | #3) Train ProMP with NIW prior on the covariance matrix (as described in the paper) 25 | 26 | dof = 7 27 | dim_basis_fun = 5 28 | inv_whis_mean = lambda v, Sigma: 9e-1*utils.make_block_diag(Sigma, dof) + 1e-1*np.eye(dof*dim_basis_fun) 29 | prior_Sigma_w = {'v':dim_basis_fun*dof, 'mean_cov_mle': inv_whis_mean} 30 | train_summary = robot_promp.train(time, q=Q, max_iter=10, prior_Sigma_w=prior_Sigma_w, 31 | print_inner_lb=True) 32 | 33 | 34 | #4) Plot some samples from the learned ProMP and conditioned ProMP 35 | 36 | n_samples = 5 # Number of samples to draw 37 | plot_dof = 3 # Degree of freedom to plot 38 | sample_time = [np.linspace(0,1,200) for i in range(n_samples)] 39 | 40 | #4.1) Make some samples from the unconditioned ProMP 41 | promp_samples = robot_promp.sample(sample_time) 42 | 43 | #4.2) Condition the ProMP to start at q_cond_init and draw samples from it 44 | q_cond_init = [1.54, 0.44, 0.15, 1.65, 0.01, -0.09, -1.23] 45 | robot_promp.condition(t=0, T=1, q=q_cond_init, ignore_Sy=False) 46 | cond_samples = robot_promp.sample(sample_time) 47 | 48 | #5) An example of conditioning in Task space 49 | 50 | import robpy.kinematics.forward as fwd 51 | 52 | # Compute the prior distribution in joint space at the desired time 53 | time_cartesian = 0.9 54 | mean_marg_w, cov_marg_w = robot_promp.marginal_w(np.array([0.0,time_cartesian,1.0]), q=True) 55 | prior_mu_q = mean_marg_w[1] 56 | prior_Sigma_q = cov_marg_w[1] 57 | 58 | # Compute the posterior distribution in joint space after conditioning in task space 59 | fwd_kin = fwd.BarrettKinematics() 60 | prob_inv_kin = promp.ProbInvKinematics(fwd_kin) 61 | 62 | mu_cartesian = np.array([-0.62, -0.44, -0.34]) 63 | Sigma_cartesian = 0.02**2*np.eye(3) 64 | 65 | mu_q, Sigma_q = prob_inv_kin.inv_kin(mu_theta=prior_mu_q, sig_theta=prior_Sigma_q, 66 | mu_x = mu_cartesian, sig_x = Sigma_cartesian) 67 | 68 | # Finally, condition in joint space using the posterior joint space distribution 69 | 70 | robot_promp.condition(t=time_cartesian, T=1.0, q=mu_q, Sigma_q=Sigma_q, ignore_Sy=False) 71 | task_cond_samples = robot_promp.sample(sample_time) 72 | 73 | #6) Plot all samples 74 | 75 | for i in range(n_samples): 76 | plt.plot(sample_time[i], promp_samples[i][:,plot_dof], color='green') 77 | plt.plot(sample_time[i], cond_samples[i][:,plot_dof], color='blue') 78 | plt.plot(sample_time[i], task_cond_samples[i][:,plot_dof], color='red') 79 | plt.title('Samples on DoF {} for the learned and conditioned ProMPs'.format(plot_dof)) 80 | plt.show() 81 | 82 | -------------------------------------------------------------------------------- /examples/python_promp/toy_example.py: -------------------------------------------------------------------------------- 1 | """ Toy example of a Probabilistic Movement Primitive 2 | 3 | """ 4 | 5 | import robpy.full_promp as promp 6 | import robpy.utils as utils 7 | import numpy as np 8 | import argparse 9 | import os 10 | import matplotlib.pyplot as plt 11 | import json 12 | 13 | 14 | full_basis = { 15 | 'conf': [ 16 | {"type": "sqexp", "nparams": 6, "conf": {"dim": 5}}, 17 | {"type": "poly", "nparams": 0, "conf": {"order": 0}} 18 | ], 19 | 'params': [np.log(0.1),0.0,0.25,0.5,0.75,1.0] 20 | } 21 | dim_basis_fun = promp.dim_comb_basis(**full_basis) 22 | dof = 1 23 | w_dim = dof*dim_basis_fun 24 | test_mu_w = np.array([-10,20,-12,15,-13,-5]) 25 | test_sig_w = 9*np.eye(w_dim) 26 | 27 | inv_whis_mean = lambda v, Sigma: np.diag(np.diag(Sigma)) 28 | params = { 29 | 'new_kernel': full_basis, 30 | #'prior_mu_w': {"m0": np.zeros(5*7), "k0": 1}, 31 | #'prior_Sigma_w': {'v':dim_basis_fun*dof, 'mean_cov_mle': inv_whis_mean}, 32 | 'model_fname': "/tmp/promp.json", 33 | 'diag_sy': True, 34 | 'opt_basis_pars': False, 35 | 'print_inner_lb': True, 36 | 'no_Sw': False, #Approx E-Step with Dirac delta? 37 | 'num_joints': dof, 38 | 'max_iter': 30, 39 | 'init_params': {'mu_w': np.zeros(w_dim), 40 | 'Sigma_w': 1e8*np.eye(w_dim), 41 | 'Sigma_y': np.eye(dof)} 42 | } 43 | 44 | def create_toy_data(n=100, T=30, missing_obs=[40,40]): 45 | p = promp.FullProMP(model={'mu_w': test_mu_w, 46 | 'Sigma_w': test_sig_w, 47 | 'Sigma_y': np.eye(1)}, num_joints=1, basis=full_basis) 48 | times = [np.linspace(0,1,T) for i in xrange(n)] 49 | if missing_obs is not None: 50 | for i in range(missing_obs[0]): times[i] = np.delete(times[i], range(1,T/2)) 51 | for i in range(missing_obs[1]): times[i+missing_obs[0]] = np.delete(times[i+missing_obs[0]], range(T/2,T-1)) 52 | #times = [np.delete(times[i], range(1 + (i % (T/2)),T/2 - 1 + (i % (T/2)))) for i in range(n)] 53 | Phi = [] 54 | X = p.sample(times, Phi=Phi, q=True) 55 | return times, Phi, X 56 | 57 | def trivial_train(times, Phi, X): 58 | W = [] 59 | for i, phi in enumerate(Phi): 60 | y = X[i] 61 | phi_a = np.array(phi) 62 | w = np.dot(np.linalg.pinv(phi_a[:,0,:]),y[:,0]) 63 | W.append(w) 64 | mu_w = np.mean(W,axis=0) 65 | Sigma_w = np.cov(W, rowvar=0) 66 | return mu_w, Sigma_w 67 | 68 | def promp_train(times, X): 69 | p = promp.FullProMP(num_joints=1, basis=full_basis) 70 | p.train(times=times, q=X, **params) 71 | return p.mu_w, p.Sigma_w 72 | 73 | times, Phi, X = create_toy_data() 74 | 75 | for i,t in enumerate(times): 76 | plt.plot(t, X[i]) 77 | plt.show() 78 | 79 | mu_w_t, sig_w_t = trivial_train(times, Phi, X) 80 | mu_w_p, sig_w_p = promp_train(times, X) 81 | 82 | print "mu_w=", test_mu_w 83 | print "mu_w_trivial=", mu_w_t 84 | print "mu_w_em=", mu_w_p 85 | 86 | print "sig_w=", test_sig_w 87 | print "sig_w_trivial=", sig_w_t 88 | print "sig_w_em=", sig_w_p 89 | -------------------------------------------------------------------------------- /examples/python_promp/train.py: -------------------------------------------------------------------------------- 1 | """ Train a Probabilistic Movement Primitive 2 | 3 | This script can load JSON data and meta data to train a ProMP. 4 | """ 5 | 6 | import robpy.full_promp as promp 7 | import robpy.utils as utils 8 | import numpy as np 9 | import argparse 10 | import os 11 | import matplotlib.pyplot as plt 12 | from matplotlib2tikz import save as tikz_save 13 | import json 14 | import logging 15 | 16 | def trainFullPromp(time, Q, Qdot=None, plot_likelihoods=True, **args): 17 | args.setdefault('print_lowerbound', plot_likelihoods) 18 | args.setdefault('print_inner_lb', False) 19 | args.setdefault('max_iter', 10) 20 | if 'init_promp' in args: 21 | robot_promp = promp.FullProMP.load(args['init_promp']) 22 | else: 23 | robot_promp = promp.FullProMP(basis=args['new_kernel']) 24 | if Qdot is not None: 25 | train_summary = robot_promp.train(time, q=Q, qd=Qdot, **args) 26 | else: 27 | train_summary = robot_promp.train(time, q=Q, **args) 28 | np.set_printoptions(precision=4, linewidth=200) 29 | print "Mean Weights:\n", robot_promp.mu_w 30 | print "Stdev Weights:\n", np.sqrt(np.diag(robot_promp.Sigma_w)) 31 | print "Noise stdev:\n", np.sqrt(np.diag(robot_promp.Sigma_y)) 32 | print "Basis function params: ", robot_promp.get_basis_pars() 33 | if plot_likelihoods: 34 | lhoods = train_summary['likelihoods'] 35 | #lhoods -= lhoods[0] #Plot improvement from first iteration 36 | plt.plot(lhoods) 37 | plt.xlabel('Iterations') 38 | plt.ylabel('Log-Likelihood') 39 | if args['save_lh_plot']: 40 | tikz_save(args['save_lh_plot'], 41 | figureheight = '\\figureheight', 42 | figurewidth = '\\figurewidth') 43 | plt.show() 44 | stream = robot_promp.to_stream() 45 | fout = file(args['model_fname'], 'w') 46 | json.dump(stream, fout) 47 | return robot_promp 48 | 49 | def main(args): 50 | logging.basicConfig(filename='/tmp/promp_train.log',level=logging.DEBUG) 51 | with open(args.data,'r') as f: 52 | data = np.load(args.data) 53 | time = data['time'] 54 | Q = data['Q'] 55 | Qdot = data['Qdot'] 56 | durations = [t[-1]-t[0] for t in time] 57 | dm = np.mean(durations) 58 | dstd = np.std(durations) 59 | print("Durations {} +/- {}".format(dm, dstd)) 60 | 61 | if args.trajlim: 62 | time = time[0:args.trajlim] 63 | Q = Q[0:args.trajlim] 64 | Qdot = Qdot[0:args.trajlim] 65 | 66 | full_basis = { 67 | 'conf': [ 68 | {"type": "sqexp", "nparams": 4, "conf": {"dim": 3}}, 69 | {"type": "poly", "nparams": 0, "conf": {"order": 1}} 70 | ], 71 | 'params': [np.log(0.25),0.25,0.5,0.75] 72 | } 73 | dof = np.shape(Q[0])[1] 74 | dim_basis_fun = promp.dim_comb_basis(**full_basis) 75 | w_dim = dof*dim_basis_fun 76 | inv_whis_mean = lambda v, Sigma: utils.make_block_diag(Sigma, dof) + args.pdiag_sw*np.eye(w_dim) 77 | params = { 78 | 'new_kernel': full_basis, 79 | #'prior_mu_w': {"m0": np.zeros(5*7), "k0": 1}, 80 | 'model_fname': "/tmp/promp.json", 81 | 'diag_sy': not args.full_sy, 82 | 'joint_indep': args.joint_indep, 83 | 'use_velocity': args.use_vel, 84 | 'opt_basis_pars': False, 85 | 'print_inner_lb': False, 86 | 'no_Sw': False, #Approx E-Step with Dirac delta? 87 | 'num_joints': dof, 88 | 'max_iter': args.training_iter, 89 | 'save_lh_plot': args.save_lh_plot, 90 | 'init_params': {'mu_w': np.zeros(w_dim), 91 | 'Sigma_w': 1e8*np.eye(w_dim), 92 | 'Sigma_y': np.eye(dof)}, 93 | 'print_inner_lb': False 94 | } 95 | if args.init_promp: 96 | params['init_promp'] = args.init_promp 97 | if not args.max_lh: 98 | params['prior_Sigma_w'] = {'v':dim_basis_fun*dof, 'mean_cov_mle': inv_whis_mean} 99 | if args.use_vel: 100 | model = trainFullPromp(time, Q, Qdot, **params) 101 | else: 102 | model = trainFullPromp(time, Q, **params) 103 | 104 | 105 | if __name__ == "__main__": 106 | parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) 107 | parser.add_argument('data', help="File with the trajectories used for training " 108 | "as a dictionary with keys {time, Q, Qdot}") 109 | parser.add_argument('--full_sy', action="store_true", help="Make the noise matrix full rank instead of diagonal") 110 | parser.add_argument('--joint_indep', action="store_true", help="Make a ProMP with independent joints") 111 | parser.add_argument('--use_vel', action="store_true", help="Should the velocity be used to train the ProMP?") 112 | parser.add_argument('--max_lh', action="store_true", help="Do not use priors") 113 | parser.add_argument('--trajlim', type=int, help="Use for training only the given number of trajectories," 114 | " use it to split training and validation or to test training with fewer data examples") 115 | parser.add_argument('--init_promp', help="Optional starting values for the ProMP parameters") 116 | parser.add_argument('--training_iter', default=10, type=int, help="Number of training iterations") 117 | parser.add_argument('--pdiag_sw', type=float, default=1e-4, help="Prior diagonal element for Sigma_w") 118 | parser.add_argument('--save_lh_plot', help="File where the likelihood w.r.t iterations plot should be saved") 119 | 120 | args = parser.parse_args() 121 | main(args) 122 | -------------------------------------------------------------------------------- /examples/strike_mov.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sebasutp/promp/5c1d2461764dfc1f585ab29d05b5ef12c8a3057e/examples/strike_mov.npz -------------------------------------------------------------------------------- /robpy/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sebasutp/promp/5c1d2461764dfc1f585ab29d05b5ef12c8a3057e/robpy/__init__.py -------------------------------------------------------------------------------- /robpy/full_promp.py: -------------------------------------------------------------------------------- 1 | """ Probabilistic Movement Primitive module 2 | """ 3 | 4 | import scipy.optimize as opt 5 | import scipy.linalg 6 | import autograd.numpy as np 7 | import autograd 8 | import json 9 | import robpy.utils as utils 10 | 11 | def joint_train_data(tt_data): 12 | """ Extracts the joint training data for ProMPs 13 | 14 | This method takes a table tennis training set builder object and 15 | returns the joint angles W and velocities Wdot required by the 16 | ProMP class. 17 | """ 18 | data = tt_data.strike_trajectories() 19 | time = [] 20 | W = [] 21 | Wdot = [] 22 | for instance in data: 23 | time_n = [] 24 | Wn = [] 25 | Wn_dot = [] 26 | for elem in instance: 27 | time_n.append(elem["t"]) 28 | Wn.append(elem["q"]) 29 | Wn_dot.append(elem["qd"]) 30 | time.append(time_n) 31 | W.append(np.array(Wn)) 32 | Wdot.append(np.array(Wn_dot)) 33 | return (time, W, Wdot) 34 | 35 | def sqexp(t, params, **args): 36 | """ A set of radial basis functions in one dimension 37 | """ 38 | sigma_sq = np.exp(params[0])**2 39 | centers = params[1:] 40 | ans = np.exp(-0.5*(t - centers)**2 / sigma_sq) 41 | return ans 42 | 43 | def poly(t, params, **args): 44 | """ Polynomial with order equal to dim-1 45 | """ 46 | order = args['conf']['order'] 47 | basis_f = map(lambda ix: t**ix, range(order+1)) 48 | return np.array(basis_f) 49 | 50 | def comb_basis(t, params, **args): 51 | basis = {"sqexp": sqexp, "poly": poly} 52 | conf = args['conf'] 53 | ans = [] 54 | start = 0 55 | for c in conf: 56 | end = start + c['nparams'] 57 | ans.append(basis[c['type']](t, params[start:start+end], conf=c['conf'])) 58 | start = end 59 | return np.concatenate(ans) 60 | 61 | def dim_comb_basis(**args): 62 | bdim = {'poly': lambda x: x['order'] + 1, 'sqexp': lambda x: x['dim']} 63 | conf = args['conf'] 64 | dim = 0 65 | for c in conf: 66 | dim = dim + bdim[c['type']](c['conf']) 67 | return dim 68 | 69 | def quad(a,X): 70 | """ Computes a quadratic form as a^T X a 71 | """ 72 | return np.dot(a, np.dot(X, a)) 73 | 74 | def cov_mat_precomp(cov_mat): 75 | tmp, log_det = np.linalg.slogdet(cov_mat) 76 | return {'inv': np.linalg.inv(cov_mat), 77 | 'log_det': log_det} 78 | 79 | def lambda_debug(f, x, name): 80 | print "Evaluating {1}({0})".format(x,name) 81 | ans = f(x) 82 | print "Ans=", ans 83 | return ans 84 | 85 | def get_bfun_lambdas(basis_params, basis_fun, q=False, qd=False): 86 | f = lambda z: basis_fun(z, basis_params) 87 | bfun = {} 88 | if q: 89 | bfun['fpos'] = f 90 | if qd: 91 | bfun['fvel'] = autograd.jacobian(f) 92 | return bfun 93 | 94 | def get_Phi_t(t, T, num_joints=1, pos=None, vel=None, acc=None): 95 | """ Builds the matrix Phi_t with the same format as the C++ implementation 96 | """ 97 | assert t>=0 and t<=T 98 | vel_fac = 1.0/T 99 | pos_t = [] 100 | vel_t = [] 101 | acc_t = [] 102 | for d in xrange(num_joints): 103 | if pos is not None: pos_t.append( pos ) 104 | if vel is not None: vel_t.append( vel_fac * vel ) 105 | if acc is not None: acc_t.append( vel_fac**2 * acc ) 106 | ans = [] 107 | if pos is not None: ans.append(scipy.linalg.block_diag(*pos_t)) 108 | if vel is not None: ans.append(scipy.linalg.block_diag(*vel_t)) 109 | if acc is not None: ans.append(scipy.linalg.block_diag(*acc_t)) 110 | return np.concatenate(ans, axis=0) 111 | 112 | def comp_Phi_t(t, T, num_joints=1, fpos=None, fvel=None, facc=None): 113 | """ Builds the matrix Phi_t with the same format as the C++ implementation 114 | """ 115 | vals = {} 116 | if fpos is not None: vals['pos'] = fpos(t/T) 117 | if fvel is not None: vals['vel'] = fvel(t/T) 118 | if facc is not None: vals['acc'] = facc(t/T) 119 | return get_Phi_t(t,T,num_joints,**vals) 120 | 121 | def get_Phi(self, times, bfun): 122 | """ Computes a list with all the matrices Phi_t 123 | """ 124 | Phi = [] 125 | for time in times: 126 | Tn = len(time) 127 | duration = time[-1] - time[0] 128 | Phi_n = [] 129 | for t in xrange(Tn): 130 | curr_time = time[t] - time[0] 131 | phi_nt = self.__comp_Phi_t(curr_time, duration, **bfun) 132 | Phi_n.append(phi_nt) 133 | Phi.append(Phi_n) 134 | return Phi 135 | 136 | def get_y_t(self, q=None, qd=None, qdd=None): 137 | """ Builds the vector y_t to be compatible with the matrix Phi_t 138 | 139 | This method builds a vector y_t with any valid combination of 140 | joint position, velocity and acceleration. 141 | """ 142 | y_t = [] 143 | if 'q' in params: y_t.extend(params['q']) 144 | if 'qd' in params: y_t.extend(params['qd']) 145 | if 'qdd' in params: y_t.extend(params['qdd']) 146 | return np.array(y_t) 147 | 148 | def __get_Y(self, times, **args): 149 | Y = [] 150 | N = len(times) 151 | for n in xrange(N): 152 | y_n = [] 153 | for t in xrange(len(times[n])): 154 | inst = {} 155 | if 'q' in args: 156 | inst['q'] = args['q'][n][t,:] 157 | if 'qd' in args: 158 | inst['qd'] = args['qd'][n][t,:] 159 | y_n.append(self.__get_y_t(**inst)) 160 | Y.append(np.array(y_n)) 161 | return Y 162 | 163 | class FullProMP: 164 | 165 | def __get_bfun_lambdas(self, **args): 166 | args.setdefault('basis_params', self.get_basis_pars()) 167 | args.setdefault('basis_fun', self.basis_fun) 168 | q = False if 'q' in args and isinstance(args['q'],bool) and not args['q'] else True 169 | qd = args['qd'] if 'qd' in args else False 170 | return get_bfun_lambdas(args['basis_params'], args['basis_fun'], q=q, qd=qd) 171 | 172 | def __get_bfun_grad_lambdas(self, **args): 173 | pass 174 | 175 | def __get_Phi_t(self, t, T, **args): 176 | """ Builds the matrix Phi_t with the same format as the C++ implementation 177 | """ 178 | assert t>=0 and t<=T 179 | vel_fac = 1.0/T 180 | pos_t = [] 181 | vel_t = [] 182 | acc_t = [] 183 | for d in xrange(self.num_joints): 184 | if 'pos' in args: pos_t.append( args['pos'] ) 185 | if 'vel' in args: vel_t.append( vel_fac * args['vel'] ) 186 | if 'acc' in args: acc_t.append( vel_fac**2 * args['acc'] ) 187 | ans = [] 188 | if 'pos' in args: ans.append(scipy.linalg.block_diag(*pos_t)) 189 | if 'vel' in args: ans.append(scipy.linalg.block_diag(*vel_t)) 190 | if 'acc' in args: ans.append(scipy.linalg.block_diag(*acc_t)) 191 | return np.concatenate(ans, axis=0) 192 | 193 | def __comp_Phi_t(self, t, T, **args): 194 | """ Builds the matrix Phi_t with the same format as the C++ implementation 195 | """ 196 | vals = {} 197 | if 'fpos' in args: vals['pos'] = args['fpos'](t/T) 198 | if 'fvel' in args: vals['vel'] = args['fvel'](t/T) 199 | if 'facc' in args: vals['acc'] = args['facc'](t/T) 200 | return self.__get_Phi_t(t,T,**vals) 201 | 202 | def __get_Phi(self, times, **args): 203 | """ Builds a list with all the matrices Phi_t already pre-computed 204 | """ 205 | if not 'basis_params' in args and self.__Phi: 206 | return self.__Phi 207 | args.setdefault('bfun', self.__get_bfun_lambdas(**args)) 208 | bfun = args['bfun'] 209 | Phi = [] 210 | for time in times: 211 | Tn = len(time) 212 | duration = time[-1] - time[0] 213 | Phi_n = [] 214 | for t in xrange(Tn): 215 | curr_time = time[t] - time[0] 216 | phi_nt = self.__comp_Phi_t(curr_time, duration, **bfun) 217 | Phi_n.append(phi_nt) 218 | Phi.append(Phi_n) 219 | if not 'basis_params' in args: 220 | self.__Phi = Phi 221 | return Phi 222 | 223 | def get_Phi(self, times, **args): 224 | return self.__get_Phi(times, **args) 225 | 226 | def __get_y_t(self, **params): 227 | """ Builds the vector y_t to be compatible with the matrix Phi_t 228 | 229 | This method builds a vector y_t with any valid combination of 230 | joint position, velocity and acceleration. 231 | """ 232 | y_t = [] 233 | if 'q' in params: y_t.extend(params['q']) 234 | if 'qd' in params: y_t.extend(params['qd']) 235 | if 'qdd' in params: y_t.extend(params['qdd']) 236 | return np.array(y_t) 237 | 238 | def __get_Y(self, times, **args): 239 | Y = [] 240 | N = len(times) 241 | for n in xrange(N): 242 | y_n = [] 243 | for t in xrange(len(times[n])): 244 | inst = {} 245 | if 'q' in args: 246 | inst['q'] = args['q'][n][t,:] 247 | if 'qd' in args: 248 | inst['qd'] = args['qd'][n][t,:] 249 | y_n.append(self.__get_y_t(**inst)) 250 | Y.append(np.array(y_n)) 251 | return Y 252 | 253 | def get_Y(self, times, **args): 254 | return self.__get_Y(times, **args) 255 | 256 | def set_internal_params(self, **args): 257 | if 'mu_w' in args: self.mu_w = args['mu_w'] 258 | if 'Sigma_w' in args: self.Sigma_w = args['Sigma_w'] 259 | if 'Sigma_y' in args: self.Sigma_y = args['Sigma_y'] 260 | 261 | def __init__(self, **args): 262 | args.setdefault("basis", { 263 | "conf": [ 264 | {"type": "sqexp", "nparams": 4, "conf": {"dim": 3}}, 265 | {"type": "poly", "nparams": 0, "conf": {"order": 1}} 266 | ], 267 | "params": np.array([np.log(0.25),0.0,0.5,1.0]) 268 | }) 269 | self.__basis_conf = args['basis']['conf'] 270 | self.set_basis_pars( args['basis']['params'] ) 271 | if 'basis_fun' in args: 272 | self.basis_fun = args['basis_fun'] 273 | else: 274 | self.basis_fun = lambda t,params: comb_basis(t, params, conf=args["basis"]["conf"]) 275 | if 'num_joints' in args: self.num_joints = args['num_joints'] 276 | if 'model' in args: 277 | self.mu_w = np.array(args['model']['mu_w']) 278 | self.Sigma_w = np.array(args['model']['Sigma_w']) 279 | self.Sigma_y = np.array(args['model']['Sigma_y']) 280 | 281 | def get_basis_pars(self): 282 | return self.__basis_pars 283 | 284 | def set_basis_pars(self, basis_pars): 285 | self.__basis_pars = np.array(basis_pars) 286 | self.__Phi = None 287 | 288 | def __em_lb_likelihood(self, times, Y, expectations, **args): 289 | #1) Set default values to some variables 290 | args.setdefault('Sigma_w', self.Sigma_w) 291 | args.setdefault('Sigma_y', self.Sigma_y) 292 | args.setdefault('Sigma_w_val', cov_mat_precomp(args['Sigma_w'])) 293 | args.setdefault('Sigma_y_val', cov_mat_precomp(args['Sigma_y'])) 294 | args.setdefault('mu_w', self.mu_w) 295 | #2) Load values in some variables 296 | inv_sig_w = args['Sigma_w_val']['inv'] 297 | log_det_sig_w = args['Sigma_w_val']['log_det'] 298 | inv_sig_y = args['Sigma_y_val']['inv'] 299 | log_det_sig_y = args['Sigma_y_val']['log_det'] 300 | mu_w = args['mu_w'] 301 | w_means = expectations['w_means'] 302 | w_covs = expectations['w_covs'] 303 | Phi = self.__get_Phi(times, **args) 304 | #3) Actually compute lower bound 305 | ans = 0.0 306 | for n in xrange(len(times)): 307 | Tn = len(times[n]) 308 | lpw = log_det_sig_w + np.trace(np.dot(inv_sig_w,w_covs[n])) + quad(w_means[n]-mu_w, inv_sig_w) 309 | lhood = 0.0 310 | for t in xrange(Tn): 311 | phi_nt = Phi[n][t] 312 | y_nt = Y[n][t] 313 | lhood = lhood + log_det_sig_y + quad(y_nt-np.dot(phi_nt,w_means[n]),inv_sig_y) + \ 314 | np.trace(np.dot(inv_sig_y, np.dot(phi_nt, np.dot(w_covs[n], phi_nt.T)))) 315 | ans = ans + lpw + lhood 316 | return -0.5*ans 317 | 318 | def __em_lb_grad_basis_pars(self, times, Y, expectations, **args): 319 | #1) Set default values to some variables 320 | args.setdefault('Sigma_y', self.Sigma_y) 321 | args.setdefault('Sigma_y_val', cov_mat_precomp(args['Sigma_y'])) 322 | #2) Load values in some variables 323 | inv_sig_y = args['Sigma_y_val']['inv'] 324 | w_means = expectations['w_means'] 325 | w_covs = expectations['w_covs'] 326 | bfun = args['bfun'] 327 | Phi = self.__get_Phi(times, **args) 328 | pars = args['basis_params'] 329 | #3) Actually compute the gradient 330 | for n in xrange(len(times)): 331 | Tn = len(times[n]) 332 | for t in xrange(Tn): 333 | #3.1) Compute the derivative of lower-bound w.r.t phi_nt 334 | A = np.outer( np.dot(Phi[n][t], w_means[n]), w_means[n] ) + \ 335 | np.dot(Phi[n][t], w_covs[n]) - np.outer(Y[n][t], w_means[n]) 336 | d_lb_phi_nt = 2*np.dot(inv_sig_y, A) 337 | #3.1) Compute the derivatives of phi_nt w.r.t each parameter 338 | for d in xrange(len(pars)): 339 | pass 340 | 341 | 342 | 343 | def __EM_lowerbound(self, times, Y, expectations, **args): 344 | """ Computes the EM lowerbound 345 | Receives a list of time vectors from the training set, the expectations computed in the 346 | E-step of the algorithm, and a list of optional arguments. As an optional argument eigther 347 | the angle positions, velocities or accelerations of the training set should be included. 348 | The optional arguments can also specify any of the parameters that are being optimized as 349 | a special value. 350 | """ 351 | #1) Load default values 352 | args.setdefault('Sigma_w', self.Sigma_w) 353 | args.setdefault('Sigma_y', self.Sigma_y) 354 | args.setdefault('Sigma_w_val', cov_mat_precomp(args['Sigma_w'])) 355 | args.setdefault('mu_w', self.mu_w) 356 | #2) Load useful variables (Including likelihood) 357 | inv_sig_w = args['Sigma_w_val']['inv'] 358 | log_det_sig_w = args['Sigma_w_val']['log_det'] 359 | lhood_lb = self.__em_lb_likelihood(times, Y, expectations, **args) 360 | #3) Compute prior log likely-hood 361 | lprior = 0.0 362 | if 'prior_mu_w' in args: 363 | m0 = args['prior_mu_w']['m0'] 364 | inv_V0 = args['prior_mu_w']['k0']*inv_sig_w #Normal-Inverse-Wishart prior 365 | lprior = lprior + quad(args['mu_w']-m0, inv_V0) 366 | if 'prior_Sigma_w' in args: 367 | prior_Sigma_w = args['prior_Sigma_w'] 368 | v0 = prior_Sigma_w['v'] 369 | D = np.shape(self.Sigma_w)[0] 370 | if 'mean_cov_mle' in prior_Sigma_w: 371 | S0 = prior_Sigma_w['mean_cov_mle'](v0, self.__Sigma_w_mle) * (v0 + D + 1) 372 | else: 373 | S0 = prior_Sigma_w['invS0'] 374 | lprior = lprior + (v0 + D + 1)*log_det_sig_w + np.trace(np.dot(S0, inv_sig_w)) 375 | #4) Compute full lower bound 376 | return -0.5*lprior + lhood_lb 377 | 378 | def __E_step(self, times, Y, **args): 379 | #1) Set up default values 380 | args.setdefault('Sigma_w', self.Sigma_w) 381 | args.setdefault('Sigma_y', self.Sigma_y) 382 | args.setdefault('Sigma_w_val', cov_mat_precomp(args['Sigma_w'])) 383 | args.setdefault('Sigma_y_val', cov_mat_precomp(args['Sigma_y'])) 384 | args.setdefault('mu_w', self.mu_w) 385 | #2) Load some variables 386 | inv_sig_w = args['Sigma_w_val']['inv'] 387 | inv_sig_y = args['Sigma_y_val']['inv'] 388 | mu_w = args['mu_w'] 389 | Phi = self.__get_Phi(times, **args) 390 | #3) Compute expectations 391 | w_means = [] 392 | w_covs = [] 393 | for n,time in enumerate(times): 394 | Tn = len(Y[n]) 395 | sum_mean = np.dot(inv_sig_w, mu_w) 396 | sum_cov = inv_sig_w 397 | for t in xrange(Tn): 398 | phi_nt = Phi[n][t] 399 | tmp1 = np.dot(np.transpose(phi_nt),inv_sig_y) 400 | sum_mean = sum_mean + np.dot(tmp1, Y[n][t]) 401 | sum_cov = sum_cov + np.dot(tmp1, phi_nt) 402 | Swn = utils.force_sym(np.linalg.inv(sum_cov)) 403 | wn = np.dot(Swn, sum_mean) 404 | w_means.append(wn) 405 | w_covs.append(Swn) 406 | return {'w_means': w_means, 'w_covs': w_covs} 407 | 408 | def E_step(self, times, Y, **args): 409 | return self.__E_step(times, Y, **args) 410 | 411 | def __M_step(self, times, Y, expectations, **args): 412 | Phi = self.__get_Phi(times, **args) 413 | N = len(times) 414 | w_means = expectations['w_means'] 415 | w_covs = expectations['w_covs'] 416 | n_var = lambda X: sum(map(lambda x: np.outer(x,x), X)) 417 | 418 | #1) Optimize mu_w 419 | wn_sum = sum(w_means) 420 | if 'prior_mu_w' in args: 421 | prior_mu_w = args['prior_mu_w'] 422 | mu_w = (wn_sum + prior_mu_w['k0']*prior_mu_w['m0'])/(N + prior_mu_w['k0']) 423 | else: 424 | mu_w = (wn_sum) / N 425 | 426 | #2) Optimize Sigma_w 427 | diff_w = map(lambda x: x - mu_w, w_means) 428 | if 'no_Sw' in args and args['no_Sw']==True: 429 | sw_sum = utils.force_sym(n_var(diff_w)) 430 | else: 431 | sw_sum = utils.force_sym(sum(w_covs) + n_var(diff_w)) 432 | 433 | self.__Sigma_w_mle = sw_sum / N # Maximum likelyhood estimate for Sigma_w 434 | if 'prior_Sigma_w' in args: 435 | prior_Sigma_w = args['prior_Sigma_w'] 436 | v0 = prior_Sigma_w['v'] 437 | D = np.shape(self.Sigma_w)[0] 438 | if 'mean_cov_mle' in prior_Sigma_w: 439 | S0 = prior_Sigma_w['mean_cov_mle'](v0, self.__Sigma_w_mle) * (v0 + D + 1) 440 | else: 441 | S0 = prior_Sigma_w['invS0'] 442 | Sigma_w = (S0 + sw_sum) / (N + v0 + D + 1) 443 | else: 444 | Sigma_w = self.__Sigma_w_mle 445 | 446 | #3) Optimize Sigma_y 447 | diff_y = [] 448 | uncert_w_y = [] 449 | for n in xrange(N): 450 | for t in xrange(len(times[n])): 451 | diff_y.append(Y[n][t] - np.dot(Phi[n][t], w_means[n])) 452 | uncert_w_y.append(np.dot(np.dot(Phi[n][t],w_covs[n]),Phi[n][t].T)) 453 | if 'no_Sw' in args and args['no_Sw']==True: 454 | Sigma_y = (n_var(diff_y)) / len(diff_y) 455 | else: 456 | Sigma_y = (n_var(diff_y) + sum(uncert_w_y)) / len(diff_y) 457 | 458 | #4) Update 459 | self.mu_w = mu_w 460 | if args['print_inner_lb']: 461 | print 'lb(mu_w)=', self.__EM_lowerbound(times, Y, expectations, **args) 462 | 463 | self.Sigma_w = utils.force_sym(Sigma_w) 464 | if args['joint_indep']: self.Sigma_w = utils.make_block_diag(self.Sigma_w, args['num_joints']) 465 | if args['print_inner_lb']: 466 | print 'lb(Sigma_w)=', self.__EM_lowerbound(times, Y, expectations, **args) 467 | 468 | if args['diag_sy']: 469 | self.Sigma_y = np.diag(np.diag(Sigma_y)) 470 | else: 471 | self.Sigma_y = utils.force_sym(Sigma_y) 472 | if args['print_inner_lb']: 473 | print 'lb(Sigma_y)=', self.__EM_lowerbound(times, Y, expectations, **args) 474 | 475 | #5) Update optional parameters 476 | if args['opt_basis_pars']: 477 | obj = lambda pars: -self.__em_lb_likelihood(times, Y, expectations, mu_w=mu_w, \ 478 | Sigma_w=Sigma_w, Sigma_y=Sigma_y, basis_params=pars, q=True) 479 | obj_debug = lambda x: lambda_debug(obj, x, "cost") 480 | jac = autograd.grad(obj) 481 | #print "Objective at x0: ", obj(self.get_basis_pars()) 482 | #print "Gradient at x0: ", jac(self.get_basis_pars()) 483 | #o_basis_pars = opt.minimize(lambda x: lambda_debug(obj,x,"cost"), self.get_basis_pars(), method="CG", jac=lambda x: lambda_debug(jac,x,"grad")) 484 | o_basis_pars = opt.minimize(obj, self.get_basis_pars(), method="Powell") 485 | #o_basis_pars = opt.minimize(obj, self.get_basis_pars(), method="Nelder-Mead") 486 | if o_basis_pars.success: 487 | self.set_basis_pars(o_basis_pars.x) 488 | else: 489 | print "Warning: The optimization of the basis parameters failed. Message: ", o_basis_pars.message 490 | if args['print_inner_lb']: 491 | print 'lb(basis_params)=', self.__EM_lowerbound(times, Y, expectations, **args) 492 | 493 | def __EM_training(self, times, **args): 494 | #1) Initialize state before training 495 | args.setdefault('opt_basis_pars', False) 496 | args.setdefault('max_iter', 10) 497 | args.setdefault('print_lowerbound', False) 498 | args.setdefault('print_inner_lb', False) 499 | args.setdefault('print_inner_params', False) 500 | args.setdefault('diag_sy', True) 501 | args.setdefault('joint_indep', False) 502 | args.setdefault('bfun', self.__get_bfun_lambdas(**args)) 503 | if args['opt_basis_pars']: 504 | args.setdefault('bfun_grad', self.__get_bfun_grad_lambdas(**args)) 505 | Y = self.__get_Y(times, **args) 506 | bfun = args['bfun'] 507 | if 'q' in args: 508 | self.num_joints = np.shape(args['q'][0])[1] 509 | elif 'qd' in args: 510 | self.num_joints = np.shape(args['qd'][0])[1] 511 | else: 512 | raise AttributeError("The positions q or velocities qd are expected but not found") 513 | y_dim, w_dim = np.shape( self.__comp_Phi_t(0.0,1.0,**bfun) ) 514 | is_initalized = hasattr(self, 'mu_w') and hasattr(self,'Sigma_w') and hasattr(self,'Sigma_y') 515 | if not is_initalized: 516 | #Only set internal parameters if they are missing 517 | args.setdefault('init_params', {'mu_w': np.zeros(w_dim), 518 | 'Sigma_w': np.eye(w_dim), 519 | 'Sigma_y': np.eye(y_dim)}) 520 | self.set_internal_params(**args['init_params']) 521 | self.__Sigma_w_mle = self.Sigma_w 522 | 523 | likelihoods = [] 524 | #2) Train 525 | for it in xrange(args['max_iter']): 526 | expectations = self.__E_step(times, Y, **args) 527 | if ('early_quit' in args and args['early_quit']()): break 528 | if args['print_lowerbound']: 529 | lh = self.__EM_lowerbound(times, Y, expectations, **args) 530 | print 'E-step LB:', lh 531 | likelihoods.append(lh) 532 | self.__M_step(times, Y, expectations, **args) 533 | if ('early_quit' in args and args['early_quit']()): break 534 | if args['print_lowerbound']: 535 | print 'M-step LB:', self.__EM_lowerbound(times, Y, expectations, **args) 536 | return {'likelihoods': likelihoods} 537 | 538 | def log_likelihood(self, times, Sigma_w_mle=None, **args): 539 | self.__Sigma_w_mle = self.Sigma_w if Sigma_w_mle is None else Sigma_w_mle 540 | Y = self.__get_Y(times, **args) 541 | expectations = self.__E_step(times, Y, **args) 542 | return self.__EM_lowerbound(times, Y, expectations, **args) 543 | 544 | def train(self, times, **args): 545 | return self.__EM_training(times, **args) 546 | 547 | def sample(self, times, Phi=None, **args): 548 | N = len(times) 549 | args.setdefault('weights', np.random.multivariate_normal(self.mu_w, self.Sigma_w, N)) 550 | if not 'noise' in args: args['noise'] = np.diag(self.Sigma_y) 551 | W = args['weights'] 552 | noise = args['noise'] 553 | _Phi = self.__get_Phi(times, basis_params=self.get_basis_pars(), **args) 554 | if Phi is None: 555 | Phi = _Phi 556 | elif isinstance(Phi,list) and len(Phi)==0: 557 | Phi.extend(_Phi) 558 | ans = [] 559 | for n in xrange(N): 560 | Tn = len(times[n]) 561 | w = W[n] 562 | curr_sample = [] 563 | for t in xrange(Tn): 564 | y = np.dot(Phi[n][t], w) + np.multiply(np.random.standard_normal(len(noise)), noise) 565 | curr_sample.append(y) 566 | ans.append(np.array(curr_sample)) 567 | return ans 568 | 569 | def marginal_w(self, time, **args): 570 | phi_n = self.__get_Phi([time], basis_params=self.get_basis_pars(), **args)[0] 571 | means = [] 572 | covs = [] 573 | for phi_nt in phi_n: 574 | means.append(np.dot(phi_nt, self.mu_w)) 575 | covs.append(np.dot(np.dot(phi_nt,self.Sigma_w),phi_nt.T)) 576 | return means, covs 577 | 578 | def to_stream(self): 579 | model = {"mu_w": self.mu_w.tolist(), "Sigma_w": self.Sigma_w.tolist(), "Sigma_y": self.Sigma_y.tolist()} 580 | basis = {"conf": self.__basis_conf, "params": self.get_basis_pars().tolist()} 581 | ans = {"model": model, "basis": basis, "num_joints": self.num_joints} 582 | return ans 583 | 584 | def condition(self, t, T, q, Sigma_q=None, ignore_Sy = True): 585 | """ Conditions the ProMP 586 | 587 | Condition the ProMP to pass be at time t with some desired position and velocity. If there is 588 | uncertainty on the conditioned point pass it as the optional matrices Sigma_q, 589 | Sigma_qd. 590 | """ 591 | times = [[0,t,T]] 592 | _Phi = self.__get_Phi(times, basis_params=self.get_basis_pars()) 593 | phi_t = _Phi[0][1] 594 | d,lw = phi_t.shape 595 | mu_q = self.__get_y_t(q=q) 596 | if ignore_Sy: 597 | tmp1 = np.dot(self.Sigma_w, phi_t.T) 598 | tmp2 = np.dot(phi_t, np.dot(self.Sigma_w, phi_t.T)) 599 | tmp2 = np.linalg.inv(tmp2) 600 | tmp3 = np.dot(tmp1,tmp2) 601 | mu_w = self.mu_w + np.dot(tmp3, (mu_q - np.dot(phi_t, self.mu_w))) 602 | tmp4 = np.eye(d) 603 | if Sigma_q is not None: 604 | tmp4 -= np.dot(Sigma_q, tmp2) 605 | Sigma_w = self.Sigma_w - np.dot(tmp3, np.dot(tmp4, tmp1.T)) 606 | else: 607 | inv_Sig_w = np.linalg.inv(self.Sigma_w) 608 | inv_Sig_y = np.linalg.inv(self.Sigma_y) 609 | Sw = np.linalg.inv(inv_Sig_w + np.dot(phi_t.T,np.dot(inv_Sig_y, phi_t))) 610 | A = np.dot(np.dot(Sw, phi_t.T), inv_Sig_y) 611 | b = np.dot(Sw, np.dot(inv_Sig_w, self.mu_w)) 612 | mu_w = np.dot(A, mu_q) + b 613 | if Sigma_q is not None: 614 | Sigma_w = Sw + np.dot(A,np.dot(Sigma_q,A.T)) 615 | else: 616 | Sigma_w = Sw 617 | 618 | self.mu_w = mu_w 619 | self.Sigma_w = Sigma_w 620 | 621 | 622 | @classmethod 623 | def load(cls, f_name): 624 | f = open(f_name, 'r') 625 | args = json.load(f) 626 | f.close() 627 | return cls(**args) 628 | 629 | 630 | class ProbInvKinematics: 631 | #params: 632 | #fwd_k: A forward kinematics object 633 | 634 | def __laplace_cost_and_grad(self, theta, mu_theta, inv_sigma_theta, mu_x, inv_sigma_x): 635 | f_th, jac_th, ori = self.fwd_k.position_and_jac(theta) 636 | jac_th = jac_th[0:3,:] 637 | diff1 = theta - mu_theta 638 | tmp1 = np.dot(inv_sigma_theta, diff1) 639 | diff2 = f_th - mu_x 640 | tmp2 = np.dot(inv_sigma_x, diff2) 641 | nll = 0.5*(np.dot(diff1,tmp1) + np.dot(diff2,tmp2)) 642 | grad_nll = tmp1 + np.dot(jac_th.T,tmp2) 643 | return nll, grad_nll 644 | 645 | def __init__(self, fwd_kinematics): 646 | self.fwd_k = fwd_kinematics 647 | 648 | def inv_kin(self, mu_theta, sig_theta, mu_x, sig_x): 649 | inv_sig_theta = np.linalg.inv(sig_theta) 650 | inv_sig_x = np.linalg.inv(sig_x) 651 | cost_grad = lambda theta: self.__laplace_cost_and_grad(theta, mu_theta, inv_sig_theta, mu_x, inv_sig_x) 652 | cost = lambda theta: cost_grad(theta)[0] 653 | grad = lambda theta: cost_grad(theta)[1] 654 | res = opt.minimize(cost, mu_theta, method='BFGS', jac=grad) 655 | post_mean = res.x 656 | post_cov = res.hess_inv 657 | return post_mean, post_cov 658 | -------------------------------------------------------------------------------- /robpy/kinematics/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sebasutp/promp/5c1d2461764dfc1f585ab29d05b5ef12c8a3057e/robpy/kinematics/__init__.py -------------------------------------------------------------------------------- /robpy/kinematics/forward.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | 5 | class BarrettKinematics: 6 | """ Forward kinematics object for the Barrett Wam 7 | This class implements the forwark kinematics functionality for the 8 | Barrett Wam arm used in the table tennis setup at the MPI. The end 9 | effector position can be changes with the endeff parameter received 10 | in the constructor. 11 | """ 12 | 13 | def __init__(self, endeff = [0.0, 0.0, 0.3, 0.0, 0.0, 0.0]): 14 | self.ZSFE = 0.346 15 | self.ZHR = 0.505 16 | self.YEB = 0.045 17 | self.ZEB = 0.045 18 | self.YWR = -0.045 19 | self.ZWR = 0.045 20 | self.ZWFE = 0.255 21 | self.endeff = endeff 22 | 23 | def _link_matrices(self,q): 24 | cq = np.cos(q) 25 | sq = np.sin(q) 26 | 27 | sa=np.sin(self.endeff[3]) 28 | ca=np.cos(self.endeff[3]) 29 | 30 | sb=np.sin(self.endeff[4]) 31 | cb=np.cos(self.endeff[4]) 32 | 33 | sg=np.sin(self.endeff[5]) 34 | cg=np.cos(self.endeff[5]) 35 | 36 | hi00 = np.array([[1,0,0,0],[0,-1,0,0],[0,0,-1,0],[0,0,0,1]]) 37 | hi01 = np.array([[cq[0],-sq[0],0,0],[sq[0],cq[0],0,0],[0,0,1,self.ZSFE],[0,0,0,1]]) 38 | hi12 = np.array([[0,0,-1,0],[sq[1],cq[1],0,0],[cq[1],-sq[1],0,0],[0,0,0,1]]) 39 | hi23 = np.array([[0,0,1,self.ZHR],[sq[2],cq[2],0,0],[-cq[2],sq[2],0,0],[0,0,0,1]]) 40 | hi34 = np.array([[0,0,-1,0],[sq[3],cq[3],0,self.YEB],[cq[3],-sq[3],0,self.ZEB],[0,0,0,1]]) 41 | hi45 = np.array([[0,0,1,self.ZWR],[sq[4],cq[4],0,self.YWR],[-cq[4],sq[4],0,0],[0,0,0,1]]) 42 | hi56 = np.array([[0,0,-1,0],[sq[5],cq[5],0,0],[cq[5],-sq[5],0,self.ZWFE],[0,0,0,1]]) 43 | hi67 = np.array([[0,0,1,0],[sq[6],cq[6],0,0],[-cq[6],sq[6],0,0],[0,0,0,1]]) 44 | hi78 = np.array([[cb*cg, -(cb*sg), sb, self.endeff[0]], \ 45 | [cg*sa*sb + ca*sg, ca*cg - sa*sb*sg, -(cb*sa), self.endeff[1]], \ 46 | [-(ca*cg*sb) + sa*sg, cg*sa + ca*sb*sg, ca*cb, self.endeff[2]], \ 47 | [0,0,0,1]]) 48 | return [hi00,hi01,hi12,hi23,hi34,hi45,hi56,hi67,hi78] 49 | 50 | def forward_kinematics(self,q): 51 | H = self._link_matrices(q) 52 | A = H[0] 53 | ans = [] 54 | for i in xrange(1,len(H)): 55 | A = np.dot(A,H[i]) 56 | ans.append(A) 57 | return ans 58 | 59 | def __rotMatToEul(self,rotMat): 60 | eul = np.zeros(3) 61 | eul[0] = np.arctan2(-rotMat[2,1],rotMat[2,2]) 62 | eul[1] = np.arctan2(rotMat[2,0],np.sqrt(rotMat[2,1]**2+rotMat[2,2]**2)) 63 | eul[2] = np.arctan2(-rotMat[1,0],rotMat[0,0]) 64 | return eul 65 | 66 | def __end_eff(self, q, As): 67 | end_eff = As[-1] 68 | pos = end_eff[0:3,3] 69 | orientation = self.__rotMatToEul(end_eff[0:3,0:3].transpose()) 70 | return pos, orientation 71 | 72 | def end_effector(self,q): 73 | As = self.forward_kinematics(q) 74 | return self.__end_eff(q, As) 75 | 76 | def __num_jac(self,q,eps): 77 | ans = np.zeros((3,len(q))) 78 | fx,ori = self.end_effector(q) 79 | for i in xrange(len(q)): 80 | q[i] += eps 81 | fxh,ori = self.end_effector(q) 82 | ans[:,i] = (fxh - fx) / eps 83 | q[i] -= eps 84 | return ans 85 | 86 | def __analytic_jac(self, q, As): 87 | ans = np.zeros((6,len(q))) 88 | pe = As[-1][0:3,3] 89 | for i in xrange(len(q)): 90 | zprev = As[i][0:3,2] 91 | pprev = As[i][0:3,3] 92 | ans[0:3,i] = np.cross(zprev, pe - pprev) 93 | ans[3:6,i] = zprev 94 | return ans 95 | 96 | def jacobian(self, q): 97 | As = self.forward_kinematics(q) 98 | #j1 = self.__num_jac(q,1e-6) 99 | j2 = self.__analytic_jac(q, As) 100 | #print "diff = ", np.linalg.norm(j1 - j2[0:3,:]) 101 | return j2 102 | 103 | def position_and_jac(self, q): 104 | As = self.forward_kinematics(q) 105 | jac = self.__analytic_jac(q, As) 106 | pos, ori = self.__end_eff(q, As) 107 | return pos, jac, ori 108 | 109 | def end_eff_trajectory(self, Q): 110 | pos = [] 111 | orientation = [] 112 | for t in xrange(len(Q)): 113 | post, ort = self.end_effector(Q[t]) 114 | pos.append(post) 115 | orientation.append(ort) 116 | return np.array(pos), np.array(orientation) 117 | -------------------------------------------------------------------------------- /robpy/utils.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import json 4 | 5 | def force_sym(A): 6 | """ Returns a symmetric matrix from the received matrix 7 | """ 8 | return (A + A.T) / 2.0 9 | 10 | def make_block_diag(A, num_blocks): 11 | n,m = A.shape 12 | assert(n == m and (n % num_blocks)==0) 13 | block_len = n // num_blocks 14 | B = np.zeros((n,m)) 15 | for i in xrange(n): 16 | for j in xrange(n): 17 | if (i // block_len) == (j // block_len): 18 | B[i,j] = A[i,j] 19 | return B 20 | 21 | def numpy_serialize(obj): 22 | if isinstance(obj, list): 23 | return map(numpy_serialize, obj) 24 | elif isinstance(obj, dict): 25 | return {k: numpy_serialize(v) for k,v in obj.items()} 26 | elif isinstance(obj, np.ndarray): 27 | return obj.tolist() 28 | else: 29 | return obj 30 | 31 | def lod2dol(lod): 32 | """ Converts a list of dictionaries to a dictionary of lists 33 | """ 34 | dol = {} 35 | for elem in lod: 36 | for k,v in elem.items(): 37 | if k not in dol: 38 | dol[k] = [] 39 | dol[k].append(v) 40 | return dol 41 | --------------------------------------------------------------------------------