diff --git a/TODOS b/TODOS
new file mode 100644
index 0000000000000000000000000000000000000000..2efde79ddc07f8349329f836aa510f3dc59362c2
--- /dev/null
+++ b/TODOS
@@ -0,0 +1,23 @@
+IMPORTANT
+-------------
+1. urdf
+- understand how it works (read the docs)
+- use the calibration data from the robot and put it in the urdf
+- ensure that gripper urdf makes sense
+- do test to see if the calibration is ok
+- recalibrate if necessary
+
+NICE TO HAVE
+---------------
+1. other inverse kinematics algorithms
+- standard list of them, for fun and to practise eigen and pinocchio a bit
+
+2. (ur) simulation
+- put the gripper on the robot in ursim
+- possibly fiddle to get communication with it in the sim to work
+
+FUTURE PROSPECTS, CURRENT URGENCY ZERO
+--------------------------------------
+3. hooking up with a real simulator, ex. mujoco
+
+
diff --git a/clik/.robotiq_gripper.py.swp b/clik/.robotiq_gripper.py.swp
deleted file mode 100644
index 6f387ee01160141069385d952c68d35d5a67715d..0000000000000000000000000000000000000000
Binary files a/clik/.robotiq_gripper.py.swp and /dev/null differ
diff --git a/dmp/DMP.m b/dmp/DMP.m
new file mode 100644
index 0000000000000000000000000000000000000000..176c3fdf31a3a17817d2f6b4d7b82d87eafb9e16
--- /dev/null
+++ b/dmp/DMP.m
@@ -0,0 +1,128 @@
+classdef DMP
+ properties
+ % State
+ x % [z; y; s]
+ tau
+ tauDot
+ % Settings
+ K
+ D
+ alpha_s
+ n_kernel
+ n
+ g
+ y0
+ nominal_tau
+ x0
+ mu
+ std_dev
+ w
+ demo_traj
+ end
+
+ methods
+ function obj = DMP(dmp_params,demo_traj)
+ obj.K = dmp_params.K;
+ obj.D = dmp_params.D;
+ obj.alpha_s = dmp_params.alpha_s;
+ obj.n_kernel = dmp_params.n_kernel;
+ obj.demo_traj = demo_traj;
+ obj = obj.fit();
+ end
+
+ function obj = init(obj)
+ obj.x = obj.x0;
+ obj.tau = obj.nominal_tau;
+ obj.tauDot = 0;
+ end
+
+ function obj = step(obj,tauDot,dt)
+ xDot = [obj.h();obj.z();-obj.s()]/obj.tau;
+ obj.x = obj.x + xDot*dt;
+ obj.tau = obj.tau + tauDot*dt;
+ obj.tauDot = tauDot;
+ end
+
+ function ref_pos_val = ref_pos(obj)
+ ref_pos_val = obj.y();
+ end
+
+ function ref_pos_val = ref_vel(obj)
+ ref_pos_val = obj.z()/obj.tau;
+ end
+
+ function ref_pos_val = ref_acc(obj)
+ ref_pos_val = (obj.h() - obj.tauDot*obj.z()) / obj.tau^2;
+ end
+
+ function yVal = y(obj)
+ yVal = obj.x(obj.n+1:2*obj.n);
+ end
+
+ function zVal = z(obj)
+ zVal = obj.x(1:obj.n);
+ end
+
+ function sVal = s(obj)
+ sVal = obj.x(end);
+ end
+
+ function hVal = h(obj)
+ psi = exp(-1./(2*obj.std_dev.^2).*(obj.s()-obj.mu).^2);
+ hVal = obj.K*(obj.g-obj.y()) - obj.D*obj.z() + obj.w*psi' ./ max(sum(psi),1e-8) .* (obj.g-obj.y0)*obj.s();
+ end
+
+ function traj = rollout(obj,dt)
+ dmp = obj.init();
+ traj.t = 0;
+ traj.pos = dmp.y();
+ traj.vel = dmp.z()/dmp.nominal_tau;
+ traj.acc = dmp.h()/dmp.nominal_tau^2;
+ traj.s = dmp.s();
+ while norm(abs(dmp.y()-dmp.g)) > 1e-2
+ dmp = dmp.step(0,dt);
+ traj.t = [traj.t traj.t(end)+dt];
+ traj.pos = [traj.pos dmp.ref_pos()];
+ traj.vel = [traj.vel dmp.ref_vel()];
+ traj.acc = [traj.acc dmp.ref_acc()];
+ traj.s = [traj.s dmp.s()];
+ end
+ end
+
+ function obj = fit(obj)
+
+ psi = @(s,c,d) exp(-1./(2*d.^2).*(s-c).^2); % Basis function
+ obj.n = size(obj.demo_traj.pos,1);
+ obj.y0 = obj.demo_traj.pos(:,1);
+ obj.g = obj.demo_traj.pos(:,end);
+ obj.x0 = [zeros(obj.n,1); obj.y0; 1];
+ obj.nominal_tau = obj.demo_traj.t(end);
+
+ lastKernel = 1*obj.nominal_tau; % Time of last kernel center
+ ct = (0:lastKernel/(obj.n_kernel-1):lastKernel); % Time means of basis functions
+ obj.mu = exp(-obj.alpha_s/obj.nominal_tau*ct); % Phase means of basis functions
+ obj.std_dev = abs((diff(obj.mu)));
+ obj.std_dev = [obj.std_dev obj.std_dev(end)]; % Standard deviation of basis functions
+
+ svec = exp(-obj.alpha_s/obj.nominal_tau*obj.demo_traj.t);
+ obj.w = zeros(obj.n,obj.n_kernel);
+ for varNr = 1:obj.n
+ if abs(obj.g(varNr)-obj.y0(varNr)) < 0.001
+ continue;
+ end
+ fGain = svec*(obj.g(varNr)-obj.y0(varNr));
+ f_target = obj.nominal_tau^2*obj.demo_traj.acc(varNr,:) - obj.K*(obj.g(varNr)-obj.demo_traj.pos(varNr,:)) + obj.D*obj.nominal_tau*obj.demo_traj.vel(varNr,:);
+ for i = 1:obj.n_kernel
+ Gamma_i = diag(psi(svec,obj.mu(i),obj.std_dev(i)));
+ if fGain*Gamma_i*fGain' < 1e-8
+ obj.w(varNr,i) = 0;
+ else
+ obj.w(varNr,i) = fGain*Gamma_i*f_target'/(fGain*Gamma_i*fGain');
+ end
+ end
+ end
+ end
+
+ end
+end
+
diff --git a/dmp/dmp_node b/dmp/dmp_node
new file mode 100755
index 0000000000000000000000000000000000000000..9a72514662eb72ffbbcc665108375321c8a1f256
--- /dev/null
+++ b/dmp/dmp_node
@@ -0,0 +1,140 @@
+#!/usr/bin/env python
+
+from motion_planning.dmp import DMP, TCVelAccConstrained, NoTC
+import numpy as np
+import rospy
+from robot_ctrl.msg import CtrlTarget
+from std_msgs.msg import Float64, String
+from motion_planning.msg import DMPState
+
+
+class DMPNode:
+ def __init__(self):
+ self.dmp_active = False
+ self.rate = None
+ self.dt = None
+ self.dmp = DMP()
+ self.tc = None
+
+ # Load target trajectory and fit DMP
+ self.load_trajectory()
+
+ # Load parameters from server
+ self.load_params()
+
+ # Setup ros topics
+ self.target_pub = rospy.Publisher("robot_ctrl/command", CtrlTarget, queue_size=1)
+ self.dmp_state = rospy.Publisher("motion_planning/dmp_state", DMPState, queue_size=1)
+ rospy.Subscriber("motion_planning/command", String, self.command_parser)
+
+ # Go into spin
+ self.spin()
+
+ def load_trajectory(self):
+ if rospy.has_param('motion_planning/trajectory_loadpath'):
+ # Set demo trajectory
+ trajectory_loadpath = rospy.get_param('motion_planning/trajectory_loadpath')
+ data = np.genfromtxt(trajectory_loadpath, delimiter=',')
+ t_demo = data[:, 0]
+ path_demo = np.array(data[:, 1:]).T
+ self.dmp.fit(path_demo, t_demo)
+ rospy.set_param("motion_planning/init_pos", np.squeeze(self.dmp.y0).tolist())
+ else:
+ rospy.logerr("Could not load trajectory. Parameter " + rospy.get_namespace() +
+ "motion_planning/trajectory_loadpath not found")
+
+ def load_params(self):
+ # Load parameters from server
+ update_rate = rospy.get_param('motion_planning/update_rate', 125)
+ tc_type = rospy.get_param('motion_planning/tc/type', 0)
+ if rospy.has_param('motion_planning/tau0'):
+ self.dmp.tau0 = rospy.get_param('motion_planning/tau0')
+
+ # Set temporal coupling
+ if tc_type == 0:
+ self.tc = NoTC()
+ elif tc_type == 1:
+ gamma_nominal = rospy.get_param('motion_planning/tc/gamma_nominal', 1)
+ gamma_a = rospy.get_param('motion_planning/tc/gamma_a', 0.5)
+ v_max = np.array(rospy.get_param('motion_planning/tc/v_max', np.inf))
+ a_max = np.array(rospy.get_param('motion_planning/tc/a_max', np.inf))
+ eps = np.array(rospy.get_param('motion_planning/tc/eps', 0.001))
+ self.tc = TCVelAccConstrained(gamma_nominal, gamma_a, v_max, a_max, eps)
+ # Set rate for ros spin
+ self.rate = rospy.Rate(update_rate)
+ self.dt = 1.0 / update_rate
+
+ def command_parser(self, msg):
+ if msg.data == 'start':
+ self.dmp_active = True
+ elif msg.data == 'stop':
+ self.dmp_active = False
+ elif msg.data == 'reset':
+ self.dmp.reset()
+ self.dmp_active = False
+ rospy.loginfo("Reset motion_planner.")
+ elif msg.data == 'step':
+ self.dmp.step(self.dt)
+ elif msg.data == 'load_params':
+ self.load_params()
+ rospy.loginfo("Reloaded motion_planner parameters.")
+ elif msg.data == 'load_trajectory':
+ self.load_trajectory()
+ rospy.loginfo("Reloaded motion_planner trajectory.")
+ else:
+ rospy.loginfo("Invalid command string for motion planner.")
+
+ # spin -----------------------------------------------------
+ # -----------------------------------------------------------
+ def spin(self):
+ print_flag = True
+ while (not rospy.is_shutdown()):
+
+ if self.dmp_active:
+ # Step DMP
+ self.dmp.step(self.dt)
+ # Step temporal coupling
+ tau = self.dmp.tau + self.tc.update(self.dmp, self.dt) * self.dt
+ self.dmp.set_tau(tau)
+ # Send target trajectory state command
+ pub_msg = CtrlTarget()
+ pub_msg.header.stamp = rospy.Time.now()
+ pub_msg.acceleration = self.dmp.acc
+ pub_msg.velocity = self.dmp.vel
+ pub_msg.position = self.dmp.pos
+ self.target_pub.publish(pub_msg)
+ else:
+ print_flag = True
+
+ # Publish DMP state command
+ pub_msg = DMPState()
+ pub_msg.header.stamp = rospy.Time.now()
+ pub_msg.acceleration = self.dmp.acc
+ pub_msg.velocity = self.dmp.vel
+ pub_msg.position = self.dmp.pos
+ pub_msg.theta = self.dmp.theta
+ pub_msg.s = self.dmp.s()
+ pub_msg.tau = self.dmp.tau
+ self.dmp_state.publish(pub_msg)
+
+ # Notify when DMP trajectory is finished
+ if print_flag and self.dmp.theta > 1:
+ rospy.loginfo("DMP trajectory finished.")
+ print_flag = False
+
+ # Sleep
+ self.rate.sleep()
+
+
+# --------------------------------------------------------------------
+# Here is the main entry point
+if __name__ == '__main__':
+ try:
+ # Init the node:
+ rospy.init_node('DMPNode')
+
+ # Initializing and continue running the main class:
+ DMPNode()
+
+ except rospy.ROSInterruptException:
+ pass
diff --git a/dmp/matlab/DMP.m b/dmp/matlab/DMP.m
new file mode 100644
index 0000000000000000000000000000000000000000..176c3fdf31a3a17817d2f6b4d7b82d87eafb9e16
--- /dev/null
+++ b/dmp/matlab/DMP.m
@@ -0,0 +1,128 @@
+classdef DMP
+ properties
+ % State
+ x % [z; y; s]
+ tau
+ tauDot
+ % Settings
+ K
+ D
+ alpha_s
+ n_kernel
+ n
+ g
+ y0
+ nominal_tau
+ x0
+ mu
+ std_dev
+ w
+ demo_traj
+ end
+
+ methods
+ function obj = DMP(dmp_params,demo_traj)
+ obj.K = dmp_params.K;
+ obj.D = dmp_params.D;
+ obj.alpha_s = dmp_params.alpha_s;
+ obj.n_kernel = dmp_params.n_kernel;
+ obj.demo_traj = demo_traj;
+ obj = obj.fit();
+ end
+
+ function obj = init(obj)
+ obj.x = obj.x0;
+ obj.tau = obj.nominal_tau;
+ obj.tauDot = 0;
+ end
+
+ function obj = step(obj,tauDot,dt)
+ xDot = [obj.h();obj.z();-obj.s()]/obj.tau;
+ obj.x = obj.x + xDot*dt;
+ obj.tau = obj.tau + tauDot*dt;
+ obj.tauDot = tauDot;
+ end
+
+ function ref_pos_val = ref_pos(obj)
+ ref_pos_val = obj.y();
+ end
+
+ function ref_pos_val = ref_vel(obj)
+ ref_pos_val = obj.z()/obj.tau;
+ end
+
+ function ref_pos_val = ref_acc(obj)
+ ref_pos_val = (obj.h() - obj.tauDot*obj.z()) / obj.tau^2;
+ end
+
+ function yVal = y(obj)
+ yVal = obj.x(obj.n+1:2*obj.n);
+ end
+
+ function zVal = z(obj)
+ zVal = obj.x(1:obj.n);
+ end
+
+ function sVal = s(obj)
+ sVal = obj.x(end);
+ end
+
+ function hVal = h(obj)
+ psi = exp(-1./(2*obj.std_dev.^2).*(obj.s()-obj.mu).^2);
+ hVal = obj.K*(obj.g-obj.y()) - obj.D*obj.z() + obj.w*psi' ./ max(sum(psi),1e-8) .* (obj.g-obj.y0)*obj.s();
+ end
+
+ function traj = rollout(obj,dt)
+ dmp = obj.init();
+ traj.t = 0;
+ traj.pos = dmp.y();
+ traj.vel = dmp.z()/dmp.nominal_tau;
+ traj.acc = dmp.h()/dmp.nominal_tau^2;
+ traj.s = dmp.s();
+ while norm(abs(dmp.y()-dmp.g)) > 1e-2
+ dmp = dmp.step(0,dt);
+ traj.t = [traj.t traj.t(end)+dt];
+ traj.pos = [traj.pos dmp.ref_pos()];
+ traj.vel = [traj.vel dmp.ref_vel()];
+ traj.acc = [traj.acc dmp.ref_acc()];
+ traj.s = [traj.s dmp.s()];
+ end
+ end
+
+ function obj = fit(obj)
+
+ psi = @(s,c,d) exp(-1./(2*d.^2).*(s-c).^2); % Basis function
+ obj.n = size(obj.demo_traj.pos,1);
+ obj.y0 = obj.demo_traj.pos(:,1);
+ obj.g = obj.demo_traj.pos(:,end);
+ obj.x0 = [zeros(obj.n,1); obj.y0; 1];
+ obj.nominal_tau = obj.demo_traj.t(end);
+
+ lastKernel = 1*obj.nominal_tau; % Time of last kernel center
+ ct = (0:lastKernel/(obj.n_kernel-1):lastKernel); % Time means of basis functions
+ obj.mu = exp(-obj.alpha_s/obj.nominal_tau*ct); % Phase means of basis functions
+ obj.std_dev = abs((diff(obj.mu)));
+ obj.std_dev = [obj.std_dev obj.std_dev(end)]; % Standard deviation of basis functions
+
+ svec = exp(-obj.alpha_s/obj.nominal_tau*obj.demo_traj.t);
+ obj.w = zeros(obj.n,obj.n_kernel);
+ for varNr = 1:obj.n
+ if abs(obj.g(varNr)-obj.y0(varNr)) < 0.001
+ continue;
+ end
+ fGain = svec*(obj.g(varNr)-obj.y0(varNr));
+ f_target = obj.nominal_tau^2*obj.demo_traj.acc(varNr,:) - obj.K*(obj.g(varNr)-obj.demo_traj.pos(varNr,:)) + obj.D*obj.nominal_tau*obj.demo_traj.vel(varNr,:);
+ for i = 1:obj.n_kernel
+ Gamma_i = diag(psi(svec,obj.mu(i),obj.std_dev(i)));
+ if fGain*Gamma_i*fGain' < 1e-8
+ obj.w(varNr,i) = 0;
+ else
+ obj.w(varNr,i) = fGain*Gamma_i*f_target'/(fGain*Gamma_i*fGain');
+ end
+ end
+ end
+ end
+
+ end
+end
+
diff --git a/dmp/matlab/NoTemporalCoupling.m b/dmp/matlab/NoTemporalCoupling.m
new file mode 100644
index 0000000000000000000000000000000000000000..bec7d3781a42d3f5d516cd0834a7c5135ba6c64e
--- /dev/null
+++ b/dmp/matlab/NoTemporalCoupling.m
@@ -0,0 +1,19 @@
+classdef NoTemporalCoupling
+ properties
+ v_max
+ a_max
+ end
+
+ methods
+
+ function obj = NoTemporalCoupling(params)
+ obj.v_max = params.v_max;
+ obj.a_max = params.a_max;
+ end
+
+ function tau_dot_val = tau_dot(obj,dmp,dt)
+ tau_dot_val = 0;
+ end
+ end
+end
+
diff --git a/dmp/matlab/TemporalCoupling.m b/dmp/matlab/TemporalCoupling.m
new file mode 100644
index 0000000000000000000000000000000000000000..68caf4490d56dcf9971b60130d4e318611790013
--- /dev/null
+++ b/dmp/matlab/TemporalCoupling.m
@@ -0,0 +1,109 @@
+classdef TemporalCoupling
+ properties
+ % Settings
+ v_max
+ a_max
+ nominal_tau
+ gamma_nominal
+ gamma_a
+ eps
+ end
+
+ methods
+ function obj = TemporalCoupling(params)
+ obj.nominal_tau = params.nominal_tau;
+ obj.v_max = params.v_max;
+ obj.a_max = params.a_max;
+ obj.gamma_nominal = params.gamma_nominal;
+ obj.gamma_a = params.gamma_a;
+ obj.eps = params.eps;
+ end
+
+ function tau_dot_val = tau_dot(obj,dmp,dt)
+ % DMP prediction step
+ dmp_next = dmp.step(0,dt);
+
+ % Formulate matrices
+ [A,B,C,D] = obj.tc_mtrcs(dmp);
+ [A_next,~,C_next,~] = obj.tc_mtrcs(dmp_next);
+
+ % Compute bounds
+ tau_dot_min_a = obj.tau_dot_min_a(A,B,C,dmp.tau);
+ tau_dot_max_a = obj.tau_dot_max_a(A,B,C,dmp.tau);
+ tau_dot_min_v = obj.tau_dot_min_v(A_next,D,dmp.tau,dt);
+ tau_dot_min_f = obj.tau_dot_min_f(A_next,B,C_next,dmp.tau,dt);
+ tau_dot_min_nominal = obj.tau_dot_min_nominal(dmp.tau,dt);
+
+ % Base update law
+ tau_dot_val = obj.gamma_nominal*(obj.nominal_tau-dmp.tau) + dmp.tau*obj.sigma(dmp);
+
+ % Saturate
+ tau_dot_min = max([tau_dot_min_a,tau_dot_min_v,tau_dot_min_f,tau_dot_min_nominal]);
+ if tau_dot_val > tau_dot_max_a
+ tau_dot_val = tau_dot_max_a;
+ end
+ if tau_dot_val < tau_dot_min
+ tau_dot_val = tau_dot_min;
+ end
+ end
+
+ function [A,B,C,D] = tc_mtrcs(obj,dmp)
+ A = [-dmp.z();
+ dmp.z()];
+ B = [-obj.a_max;
+ -obj.a_max];
+ C = [dmp.h();
+ -dmp.h()];
+ D = [-obj.v_max;
+ -obj.v_max];
+ end
+
+ function minVal = tau_dot_min_a(obj,A,B,C,tau)
+ i = A < 0;
+ minVal = max(-(B(i)*tau^2+C(i))./A(i));
+ end
+
+ function maxVal = tau_dot_max_a(obj,A,B,C,tau)
+ i = A > 0;
+ maxVal = min(-(B(i)*tau^2+C(i))./A(i));
+ end
+
+ function minVal = tau_dot_min_f(obj,A,B,C,tau,dt)
+ Ais = A(A<0);
+ Ajs = A(A>0);
+ Bis = B(A<0);
+ Bjs = B(A>0);
+ Cis = C(A<0);
+ Cjs = C(A>0);
+ tauNextMin = -inf;
+ for i=1:length(Ais)
+ for j = 1:length(Ajs)
+ num = Cis(i)*abs(Ajs(j))+Cjs(j)*abs(Ais(i));
+ den = abs(Bis(i)*Ajs(j))+abs(Bjs(j)*Ais(i));
+ if num > 0
+ if sqrt(num/den) > tauNextMin
+ tauNextMin = sqrt(num/den);
+ end
+ end
+ end
+ end
+ minVal = (tauNextMin-tau)/dt;
+ end
+
+ function minVal = tau_dot_min_v(obj,A,D,tau,dt)
+ tauNextMax = max(-A./D);
+ minVal = (tauNextMax-tau)/dt;
+ end
+
+ function minVal = tau_dot_min_nominal(obj,tau,dt)
+ minVal = (obj.nominal_tau-tau)/dt;
+ end
+
+ function sigmaVal = sigma(obj,dmp)
+ a_normalized = dmp.h()./(dmp.tau^2*obj.a_max);
+ sigmaVal = obj.gamma_a*sum(a_normalized.^2./max(1-a_normalized.^2,obj.gamma_a*obj.eps));
+ end
+
+ end
+end
+
diff --git a/dmp/matlab/TemporalCouplingNoPotentials.m b/dmp/matlab/TemporalCouplingNoPotentials.m
new file mode 100644
index 0000000000000000000000000000000000000000..de4002e41b9af40e0a5ae456f72192501ec2ed5b
--- /dev/null
+++ b/dmp/matlab/TemporalCouplingNoPotentials.m
@@ -0,0 +1,100 @@
+classdef TemporalCouplingNoPotentials
+ properties
+ % Settings
+ v_max
+ a_max
+ nominal_tau
+ gamma_nominal
+ end
+
+ methods
+ function obj = TemporalCouplingNoPotentials(params)
+ obj.nominal_tau = params.nominal_tau;
+ obj.v_max = params.v_max;
+ obj.a_max = params.a_max;
+ obj.gamma_nominal = params.gamma_nominal;
+ end
+
+ function tau_dot_val = tau_dot(obj,dmp,dt)
+ % DMP prediction step
+ dmp_next = dmp.step(0,dt);
+
+ % Formulate matrices
+ [A,B,C,D] = obj.tc_mtrcs(dmp);
+ [A_next,~,C_next,~] = obj.tc_mtrcs(dmp_next);
+
+ % Compute bounds
+ tau_dot_min_a = obj.tau_dot_min_a(A,B,C,dmp.tau);
+ tau_dot_max_a = obj.tau_dot_max_a(A,B,C,dmp.tau);
+ tau_dot_min_v = obj.tau_dot_min_v(A_next,D,dmp.tau,dt);
+ tau_dot_min_f = obj.tau_dot_min_f(A_next,B,C_next,dmp.tau,dt);
+ tau_dot_min_nominal = obj.tau_dot_min_nominal(dmp.tau,dt);
+
+ % Base update law
+ tau_dot_val = obj.gamma_nominal*(obj.nominal_tau-dmp.tau);
+
+ % Saturate
+ tau_dot_min = max([tau_dot_min_a,tau_dot_min_v,tau_dot_min_f,tau_dot_min_nominal]);
+ if tau_dot_val > tau_dot_max_a
+ tau_dot_val = tau_dot_max_a;
+ end
+ if tau_dot_val < tau_dot_min
+ tau_dot_val = tau_dot_min;
+ end
+ end
+
+ function [A,B,C,D] = tc_mtrcs(obj,dmp)
+ A = [-dmp.z();
+ dmp.z()];
+ B = [-obj.a_max;
+ -obj.a_max];
+ C = [dmp.h();
+ -dmp.h()];
+ D = [-obj.v_max;
+ -obj.v_max];
+ end
+
+ function minVal = tau_dot_min_a(obj,A,B,C,tau)
+ i = A < 0;
+ minVal = max(-(B(i)*tau^2+C(i))./A(i));
+ end
+
+ function maxVal = tau_dot_max_a(obj,A,B,C,tau)
+ i = A > 0;
+ maxVal = min(-(B(i)*tau^2+C(i))./A(i));
+ end
+
+ function minVal = tau_dot_min_f(obj,A,B,C,tau,dt)
+ Ais = A(A<0);
+ Ajs = A(A>0);
+ Bis = B(A<0);
+ Bjs = B(A>0);
+ Cis = C(A<0);
+ Cjs = C(A>0);
+ tauNextMin = -inf;
+ for i=1:length(Ais)
+ for j = 1:length(Ajs)
+ num = Cis(i)*abs(Ajs(j))+Cjs(j)*abs(Ais(i));
+ den = abs(Bis(i)*Ajs(j))+abs(Bjs(j)*Ais(i));
+ if num > 0
+ if sqrt(num/den) > tauNextMin
+ tauNextMin = sqrt(num/den);
+ end
+ end
+ end
+ end
+ minVal = (tauNextMin-tau)/dt;
+ end
+
+ function minVal = tau_dot_min_v(obj,A,D,tau,dt)
+ tauNextMax = max(-A./D);
+ minVal = (tauNextMax-tau)/dt;
+ end
+
+ function minVal = tau_dot_min_nominal(obj,tau,dt)
+ minVal = (obj.nominal_tau-tau)/dt;
+ end
+
+ end
+end
+
diff --git a/dmp/matlab/main.m b/dmp/matlab/main.m
new file mode 100644
index 0000000000000000000000000000000000000000..56e64005f7aca3133ae5da4ab1ab475155a1512f
--- /dev/null
+++ b/dmp/matlab/main.m
@@ -0,0 +1,55 @@
+clearvars
+clc
+close all
+
+% Create demonstration trajectory
+load('trajectories/g')
+% load('trajectories/omega')
+% load('trajectories/z')
+% load('trajectories/s')
+% load('trajectories/psi')
+
+% Nominal trajectory functions
+dmp_params.D = 20;
+dmp_params.K = dmp_params.D^2/4;
+dmp_params.n_kernel = 100;
+dmp_params.alpha_s = 1;
+dmp = DMP(dmp_params,demo_traj);
+
+% Simulation setup
+sim_params.dt = 1/1000;
+sim_params.T_max = 100;
+sim_params.kv = 10;
+sim_params.kp = sim_params.kv^2/4;
+
+nominalTraj = dmp.rollout(sim_params.dt);
+sim_params.a_max = 0.5*[max(abs(nominalTraj.acc(1,:))); max(abs(nominalTraj.acc(2,:)))];
+sim_params.v_max = inf*[1 1]';
+sim_params.s_v_max_online = 0.65;
+sim_params.v_max_online = [inf;0.15];
+% sim_params.v_max_online = inf*[1 1]';
+
+% Scaling parameters
+tc_params.nominal_tau = dmp.nominal_tau;
+tc_params.eps = 1e-3;
+tc_params.gamma_nominal = 1;
+tc_params.gamma_a = 0.5;
+tc_params.a_max = sim_params.a_max;
+tc_params.v_max = sim_params.v_max;
+
+% Create temporal coupling objects
+tc = {};
+tc{1} = TemporalCoupling(tc_params);
+tc{2} = NoTemporalCoupling(tc_params);
+tc{3} = TemporalCouplingNoPotentials(tc_params);
+
+% Simulate
+nSim = length(tc);
+res = cell(length(tc),1);
+for d = 1:nSim
+ disp(['Running simulation ' num2str(d) '/' num2str(length(tc)) '...']);
+ res{d} = run_simulation(dmp,tc{d},sim_params);
+end
+
+% Plot result
+plot_result
diff --git a/dmp/matlab/plot_result.m b/dmp/matlab/plot_result.m
new file mode 100644
index 0000000000000000000000000000000000000000..50571b3a4c01af9ad8eaf0b0360eddfb3ba15d90
--- /dev/null
+++ b/dmp/matlab/plot_result.m
@@ -0,0 +1,110 @@
+close all
+
+
+figure('Name','Velocity')
+
+subplot(2,1,1)
+plot(res{1}.s,res{1}.ref_vel(1,:),'b-','LineWidth',1.5), hold on
+plot(res{2}.s,res{2}.ref_vel(1,:),'k--','LineWidth',1.5)
+plot(res{3}.s,res{3}.ref_vel(1,:),'m-.','LineWidth',1.5)
+plot([0 sim_params.s_v_max_online],[sim_params.v_max_online(1) sim_params.v_max_online(1)],'r:','LineWidth',1)
+plot([0 sim_params.s_v_max_online],-[sim_params.v_max_online(1) sim_params.v_max_online(1)],'r:','LineWidth',1)
+yticks([])
+xticks([round(min([res{1}.s res{2}.s res{3}.s]),2) sim_params.s_v_max_online 0.9 1])
+xlim([min([res{1}.s res{2}.s res{3}.s]) 1])
+xlabel('$s$', 'Interpreter','latex','Fontsize',30)
+ylim([min([res{2}.ref_vel(1,:) res{2}.ref_vel(2,:)]) max([res{2}.ref_vel(1,:) res{2}.ref_vel(2,:)])])
+ylabel('$\dot{y}_1$', 'Interpreter','latex','Fontsize',30)
+set(gca,'XDir','reverse')
+set(gca,'FontSize',16)
+
+subplot(2,1,2)
+plot(res{1}.s,res{1}.ref_vel(2,:),'b-','LineWidth',1.5), hold on
+plot(res{2}.s,res{2}.ref_vel(2,:),'k--','LineWidth',1.5)
+plot(res{3}.s,res{3}.ref_vel(2,:),'m-.','LineWidth',1.5)
+plot([0 sim_params.s_v_max_online],[sim_params.v_max_online(2) sim_params.v_max_online(2)],'r:','LineWidth',1)
+plot([0 sim_params.s_v_max_online],-[sim_params.v_max_online(2) sim_params.v_max_online(2)],'r:','LineWidth',1)
+yticks([-sim_params.v_max_online(2) 0 sim_params.v_max_online(2)])
+set(gca, 'YTickLabel', {'-v_{max}','0','v_{max}'},'FontSize',16);
+xticks([round(min([res{1}.s res{2}.s res{3}.s]),2) sim_params.s_v_max_online 0.9 1])
+xlim([min([res{1}.s res{2}.s res{3}.s]) 1])
+xlabel('$s$', 'Interpreter','latex','Fontsize',30)
+ylim([min([res{2}.ref_vel(1,:) res{2}.ref_vel(2,:)]) max([res{2}.ref_vel(1,:) res{2}.ref_vel(2,:)])])
+ylabel('$\dot{y}_2$', 'Interpreter','latex','Fontsize',30)
+set(gca,'XDir','reverse')
+
+figure('Name','Acceleration')
+subplot(2,1,1)
+plot(res{1}.s,res{1}.ref_acc(1,:),'b-','LineWidth',1.5), hold on
+plot(res{2}.s,res{2}.ref_acc(1,:),'k--','LineWidth',1.5)
+plot(res{3}.s,res{3}.ref_acc(1,:),'m-.','LineWidth',1.5)
+plot([res{3}.s(1) res{3}.s(end)],[sim_params.a_max(1) sim_params.a_max(1)],'r:','LineWidth',1)
+plot([res{3}.s(1) res{3}.s(end)],-[sim_params.a_max(1) sim_params.a_max(1)],'r:','LineWidth',1)
+plot([res{1}.s(1) res{1}.s(end)],[sim_params.a_max(1) sim_params.a_max(1)],'r:','LineWidth',1)
+plot([res{1}.s(1) res{1}.s(end)],-[sim_params.a_max(1) sim_params.a_max(1)],'r:','LineWidth',1)
+xticks([round(min([res{1}.s res{2}.s res{3}.s]),2) sim_params.s_v_max_online 0.9 1])
+yticks([-sim_params.a_max(1) 0 sim_params.a_max(1)])
+set(gca, 'YTickLabel', {'-a_{max}','0','a_{max}'},'FontSize',16);
+xlim([min([res{1}.s res{2}.s res{3}.s]) 1])
+xlabel('$s$', 'Interpreter','latex','Fontsize',30)
+ylim([min([res{2}.ref_acc(1,:) res{2}.ref_acc(2,:)]) max([res{2}.ref_acc(1,:) res{2}.ref_acc(2,:)])])
+ylabel('$\ddot{y}_1$', 'Interpreter','latex','Fontsize',30)
+set(gca,'XDir','reverse')
+
+subplot(2,1,2)
+plot(res{1}.s,res{1}.ref_acc(2,:),'b-','LineWidth',1.5), hold on
+plot(res{2}.s,res{2}.ref_acc(2,:),'k--','LineWidth',1.5)
+plot(res{3}.s,res{3}.ref_acc(2,:),'m-.','LineWidth',1.5)
+plot([res{3}.s(1) res{3}.s(end)],[sim_params.a_max(2) sim_params.a_max(2)],'r:','LineWidth',1)
+plot([res{3}.s(1) res{3}.s(end)],-[sim_params.a_max(2) sim_params.a_max(2)],'r:','LineWidth',1)
+plot([res{1}.s(1) res{1}.s(end)],[sim_params.a_max(2) sim_params.a_max(2)],'r:','LineWidth',1)
+plot([res{1}.s(1) res{1}.s(end)],-[sim_params.a_max(2) sim_params.a_max(2)],'r:','LineWidth',1)
+xticks([round(min([res{1}.s res{2}.s res{3}.s]),2) sim_params.s_v_max_online 0.9 1])
+yticks([-sim_params.a_max(2) 0 sim_params.a_max(2)])
+set(gca, 'YTickLabel', {'-a_{max}','0','a_{max}'},'FontSize',16);
+xlim([min([res{1}.s res{2}.s res{3}.s]) 1])
+xlabel('$s$', 'Interpreter','latex','Fontsize',30)
+ylim([min([res{2}.ref_acc(1,:) res{2}.ref_acc(2,:)]) max([res{2}.ref_acc(1,:) res{2}.ref_acc(2,:)])])
+ylabel('$\ddot{y}_2$', 'Interpreter','latex','Fontsize',30)
+set(gca,'XDir','reverse')
+
+% Path
+figure('Name','Path')
+clf
+tmp = 200;
+plot(res{1}.ref_pos(1,1:tmp:end),res{1}.ref_pos(2,1:tmp:end),'g*','LineWidth',8), hold on
+plot(res{1}.sys_pos(1,:),res{1}.sys_pos(2,:),'b-','LineWidth',2.5)
+plot(res{2}.sys_pos(1,:),res{2}.sys_pos(2,:),'k--','LineWidth',2.5)
+plot(res{3}.sys_pos(1,:),res{3}.sys_pos(2,:),'m-.','LineWidth',2.5)
+xlabel('$\xi_{1}$', 'Interpreter','latex','Fontsize',30)
+ylabel('$\xi_{2}$', 'Interpreter','latex','Fontsize',30)
+xticks([])
+yticks([])
+
+% Tau
+figure('Name','Tau')
+plot(res{1}.s,res{1}.tau,'b','LineWidth',2.5), hold on
+plot(res{2}.s,res{2}.tau,'k--','LineWidth',2.5)
+plot(res{3}.s,res{3}.tau,'m-.','LineWidth',2.5)
+xticks([round(min([res{1}.s res{2}.s res{3}.s]),2) sim_params.s_v_max_online 0.9 1])
+yticks([])
+set(gca,'FontSize',16)
+xlim([min([res{1}.s res{2}.s res{3}.s]) 1])
+ylim([0.9*dmp.nominal_tau 1.05*max(res{1}.tau)])
+xlabel('$s$', 'Interpreter','latex','Fontsize',30)
+ylabel('$\tau$', 'Interpreter','latex','Fontsize',30)
+set(gca,'XDir','reverse')
+
+
+% s
+figure('Name','s')
+plot(res{1}.t,res{1}.s,'b','LineWidth',2.5), hold on
+plot(res{2}.t,res{2}.s,'k--','LineWidth',2.5)
+plot(res{3}.t,res{3}.s,'m-.','LineWidth',2.5)
+yticks([round(min([res{1}.s res{2}.s res{3}.s]),2) sim_params.s_v_max_online 0.9 1])
+set(gca,'FontSize',20)
+xlim([0 1.05*max([res{1}.t(end) res{2}.t(end) res{3}.t(end)])])
+ylim([0.9*res{1}.s(end) 1.05])
+xlabel('$t$', 'Interpreter','latex','Fontsize',40)
+ylabel('$s$', 'Interpreter','latex','Fontsize',40)
+
diff --git a/dmp/matlab/run_simulation.m b/dmp/matlab/run_simulation.m
new file mode 100644
index 0000000000000000000000000000000000000000..1ce873af8f74df9e810f47bae4d78018667bc008
--- /dev/null
+++ b/dmp/matlab/run_simulation.m
@@ -0,0 +1,65 @@
+function res = run_simulation(dmp,tmp_couple,params)
+
+% Parameters
+dt = params.dt;
+T_max = params.T_max;
+kp = params.kp;
+kv = params.kv;
+a_max = params.a_max;
+v_max = params.v_max;
+s_v_max_online = params.s_v_max_online;
+v_max_online = params.v_max_online;
+
+% Initialize
+dmp = dmp.init();
+t = 0;
+k = 1;
+sys.pos = dmp.ref_pos();
+sys.vel = 0*sys.pos;
+sys.acc = 0*sys.pos;
+
+while t < T_max
+ % Store values
+ res.t(k) = t;
+ res.s(k) = dmp.s();
+ res.ref_pos(:,k) = dmp.ref_pos();
+ res.ref_vel(:,k) = dmp.ref_vel();
+ res.ref_acc(:,k) = dmp.ref_acc();
+ res.sys_pos(:,k) = sys.pos;
+ res.sys_vel(:,k) = sys.vel;
+ res.sys_acc(:,k) = sys.acc;
+ res.tau(k) = dmp.tau;
+
+ % Final position convergence condition
+ if norm(dmp.ref_pos()-dmp.g) < 1e-2
+ break;
+ end
+
+ % Perform one time step
+ k = k + 1;
+ t = t + dt;
+
+ % Add velocity limit online
+ if dmp.s() < s_v_max_online
+ v_max = v_max_online;
+ tmp_couple.v_max= v_max_online;
+ end
+
+ % Step trajectory generator
+ tau_dot = tmp_couple.tau_dot(dmp,dt);
+ dmp = dmp.step(tau_dot,dt);
+
+ % Controller
+ sys_acc_ctrl = dmp.ref_acc() + kv*(dmp.ref_vel()-sys.vel) + kp*(dmp.ref_pos()-sys.pos);
+ % Saturate acceleration
+ sys_acc_ctrl = min(max(sys_acc_ctrl,-a_max),a_max);
+ % Velocity integration
+ vel_prev = sys.vel;
+ sys.vel = sys.vel + sys_acc_ctrl*dt;
+ % Saturate velocity
+ sys.vel = min(max(sys.vel,-v_max),v_max);
+ sys.acc = (sys.vel-vel_prev)/dt;
+ % Position integration
+ sys.pos = sys.pos + sys.vel*dt;
+
+end
\ No newline at end of file
diff --git a/dmp/matlab/trajectories/g.mat b/dmp/matlab/trajectories/g.mat
new file mode 100644
index 0000000000000000000000000000000000000000..e03f71a1b5d2900da911de5751cec75588386db4
Binary files /dev/null and b/dmp/matlab/trajectories/g.mat differ
diff --git a/dmp/matlab/trajectories/omega.mat b/dmp/matlab/trajectories/omega.mat
new file mode 100644
index 0000000000000000000000000000000000000000..8cd3b2b1561142fbc2ee4b9bd768e1773333c6bd
Binary files /dev/null and b/dmp/matlab/trajectories/omega.mat differ
diff --git a/dmp/matlab/trajectories/psi.mat b/dmp/matlab/trajectories/psi.mat
new file mode 100644
index 0000000000000000000000000000000000000000..c1b6372952d1262d5ad2bc6ac09f37ceafd74776
Binary files /dev/null and b/dmp/matlab/trajectories/psi.mat differ
diff --git a/dmp/matlab/trajectories/s.mat b/dmp/matlab/trajectories/s.mat
new file mode 100644
index 0000000000000000000000000000000000000000..6102e05dd231ec6bbc5abd5191e89a5242db05b5
Binary files /dev/null and b/dmp/matlab/trajectories/s.mat differ
diff --git a/dmp/matlab/trajectories/z.mat b/dmp/matlab/trajectories/z.mat
new file mode 100644
index 0000000000000000000000000000000000000000..09243cdc8e4d974bfd3bc1abade837cd672076b5
Binary files /dev/null and b/dmp/matlab/trajectories/z.mat differ
diff --git a/dmp/motion_planning/__init__.py b/dmp/motion_planning/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/dmp/motion_planning/dmp/__init__.py b/dmp/motion_planning/dmp/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..40b5f425576ed7037e884202289aa719420a699b
--- /dev/null
+++ b/dmp/motion_planning/dmp/__init__.py
@@ -0,0 +1,2 @@
+from dmp import DMP
+from temporal_coupling import TCVelAccConstrained, NoTC
\ No newline at end of file
diff --git a/dmp/motion_planning/dmp/__init__.pyc b/dmp/motion_planning/dmp/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..e776f2856a3618cc12f701c89327b4429c25b01a
Binary files /dev/null and b/dmp/motion_planning/dmp/__init__.pyc differ
diff --git a/dmp/motion_planning/dmp/dmp.py b/dmp/motion_planning/dmp/dmp.py
new file mode 100644
index 0000000000000000000000000000000000000000..958441d0a9fc01045e880ce39a92679d66a032bc
--- /dev/null
+++ b/dmp/motion_planning/dmp/dmp.py
@@ -0,0 +1,166 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+
+class DMP:
+ def __init__(self, k=100, d=20, a_s=1, n_bfs=100):
+ # parameters
+ self.k = k
+ self.d = d
+ self.a_s = a_s
+ self.n_bfs = n_bfs
+
+ # for obstacle avoidance
+ self.g_obs = 1000.
+ self.b_obs = 20. / np.pi
+ self.a_obs = 0.2
+
+ # trajectory parameters
+ self.n = 0
+ self.y0 = None
+ self.tau0 = None
+ self.g = None
+ self.tau = None
+
+ # initialize basis functions for LWR
+ self.w = None
+ self.centers = None
+ self.widths = None
+
+ # state
+ self.x = None
+ self.theta = None
+ self.pos = None
+ self.vel = None
+ self.acc = None
+
+ # desired path
+ self.path = None
+
+ def reset(self):
+ self.x = np.vstack((np.zeros((self.n, 1)), self.y0, 1.))
+ self.tau = self.tau0
+ self.theta = 0
+ self.pos = self.y0
+ self.vel = 0 * self.y0
+ self.acc = 0 * self.y0
+
+ def z(self, x=None):
+ if x is None:
+ x = self.x
+ return x[0:self.n]
+
+ def y(self, x=None):
+ if x is None:
+ x = self.x
+ return x[self.n:2 * self.n]
+
+ def s(self, x=None):
+ if x is None:
+ x = self.x
+ return x[2 * self.n]
+
+ def set_goal(self, g):
+ self.g = g
+
+ def set_tau(self, tau):
+ self.tau = tau
+
+ def psi(self, s, i=None):
+ if i is not None and len(s) == 1:
+ return np.exp(-1 / (2 * self.widths[i] ** 2) * (s - self.centers[i]) ** 2)
+ if i is None:
+ return np.exp(-1 / (2 * self.widths ** 2) * (s - self.centers) ** 2)
+
+ def h(self, x=None):
+ if x is None:
+ x = self.x
+ psi = self.psi(self.s(x)).reshape((self.n_bfs, 1))
+ v = (self.w.dot(psi)) / np.maximum(np.sum(psi), 1e-8) * (self.g - self.y0) * self.s(x)
+ h = self.k * (self.g - self.y(x)) - self.d * self.z(x) + v
+ return h
+
+ def f(self, x=None):
+ if x is None:
+ x = self.x
+ return np.vstack((self.h(x), self.z(x), -self.a_s * self.s(x)))
+
+ def obstacle_avoidance(self):
+ p = np.zeros(self.pos.shape)
+
+ for obstacle in self.obstacles:
+ # calculate vector to object relative to body
+ o = np.squeeze(obstacle)
+ x = np.squeeze(self.pos)
+ v = np.squeeze(self.vel)
+ # if we're moving
+ if np.linalg.norm(self.vel) > 1e-8 and np.linalg.norm(o - x) < self.a_obs:
+ # print("Dist: " + str(np.linalg.norm(o - x)) + "at position: " + str(x))
+ phi = np.arccos( np.dot(o - x, v) / (np.linalg.norm(o - x) * np.linalg.norm(v)) )
+ dphi = self.g_obs * phi * np.exp(-self.b_obs * abs(phi))
+
+ r = np.cross(o - x, v)
+ r = r / np.linalg.norm(r)
+ R = np.array([[0,-r[2],r[1]], [r[2],0,r[0]], [-r[1],r[0],0]]) + np.outer(r,r)
+ pval = np.nan_to_num(np.dot(R, v) * dphi)
+
+ # check to see if the distance to the obstacle is further than
+ # the distance to the target, if it is, ignore the obstacle
+ if np.linalg.norm(o - x) > np.linalg.norm(self.g - self.pos):
+ pval = p * 0.
+
+ p += pval.reshape(self.pos.shape)
+ return p
+
+ def step(self, dt):
+ # Update state
+ self.x = self.x + self.f() / self.tau * dt
+ self.theta = self.theta + 1 / self.tau * dt
+
+ # Extract trajectory state
+ vel_prev = self.vel
+ self.pos = self.y()
+ self.vel = self.z() / self.tau
+ self.acc = (self.vel - vel_prev) / dt
+
+ def fit(self, y_demo, t_demo):
+ # Set target trajectory parameters
+ t_demo = t_demo.reshape(1, len(t_demo))
+ self.n = y_demo.shape[0]
+ self.path = y_demo
+ self.y0 = y_demo[:, 0].reshape((self.n, 1))
+ self.g = y_demo[:, -1].reshape((self.n, 1))
+ self.tau0 = t_demo[0, -1]
+
+ # Set basis functions
+ t_centers = np.linspace(0, self.tau0, self.n_bfs, endpoint=True)
+ self.centers = np.exp(-self.a_s / self.tau0 * t_centers)
+ widths = np.abs((np.diff(self.centers)))
+ self.widths = np.concatenate((widths, [widths[-1]]))
+
+ # Calculate derivatives
+ yd_demo = (y_demo[:, 1:] - y_demo[:, :-1]) / (t_demo[0, 1:] - t_demo[0, :-1])
+ yd_demo = np.concatenate((yd_demo, np.zeros((self.n, 1))), axis=1)
+ ydd_demo = (yd_demo[:, 1:] - yd_demo[:, :-1]) / (t_demo[0, 1:] - t_demo[0, :-1])
+ ydd_demo = np.concatenate((ydd_demo, np.zeros((self.n, 1))), axis=1)
+
+ # Compute weights
+ s_seq = np.exp(-self.a_s / self.tau0 * t_demo)
+ self.w = np.zeros((self.n, self.n_bfs))
+ for i in range(self.n):
+ if abs(self.g[i] - self.y0[i]) < 1e-5:
+ continue
+ f_gain = s_seq * (self.g[i] - self.y0[i])
+ f_target = self.tau0 ** 2 * ydd_demo[i, :] - self.k * (
+ self.g[i] - y_demo[i, :]) + self.d * self.tau0 * yd_demo[i, :]
+ for j in range(self.n_bfs):
+ psi_j = self.psi(s_seq, j)
+ num = f_gain.dot((psi_j * f_target).T)
+ den = f_gain.dot((psi_j * f_gain).T)
+ if abs(den) < 1e-6:
+ continue
+ self.w[i, j] = num / den
+
+ # Reset state
+ self.reset()
diff --git a/dmp/motion_planning/dmp/dmp.pyc b/dmp/motion_planning/dmp/dmp.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..1bedeaf35b653fa33fa47689814842914482a17d
Binary files /dev/null and b/dmp/motion_planning/dmp/dmp.pyc differ
diff --git a/dmp/motion_planning/dmp/temporal_coupling.py b/dmp/motion_planning/dmp/temporal_coupling.py
new file mode 100644
index 0000000000000000000000000000000000000000..5d9107a02500449d00e5644d64f6291c46a9290b
--- /dev/null
+++ b/dmp/motion_planning/dmp/temporal_coupling.py
@@ -0,0 +1,80 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+
+class NoTC:
+ def update(self, dmp, dt):
+ return 0
+
+
+class TCVelAccConstrained:
+
+ def __init__(self, gamma_nominal, gamma_a, v_max, a_max, eps=0.001):
+ self.gamma_nominal = gamma_nominal
+ self.gamma_a = gamma_a
+ self.eps = eps
+ self.v_max = v_max.reshape((len(v_max), 1))
+ self.a_max = a_max.reshape((len(a_max), 1))
+
+ def generate_matrices(self, dmp, dt):
+ A = np.vstack((-dmp.z(), dmp.z()))
+ B = np.vstack((-self.a_max, -self.a_max))
+ C = np.vstack((dmp.h(), -dmp.h()))
+ D = np.vstack((-self.v_max, -self.v_max))
+ x_next = dmp.x + dmp.f(dmp.x) / dmp.tau * dt
+ A_next = np.vstack((-dmp.z(x_next), dmp.z(x_next)))
+ C_next = np.vstack((dmp.h(x_next), -dmp.h(x_next)))
+ return A, B, C, D, A_next, C_next
+
+ def update(self, dmp, dt):
+
+ A, B, C, D, A_next, C_next = self.generate_matrices(dmp, dt)
+
+ # Acceleration bounds
+ i = np.squeeze(A < 0)
+ if i.any():
+ taud_min_a = np.max(- (B[i] * dmp.tau ** 2 + C[i]) / A[i])
+ else:
+ taud_min_a = -np.inf
+ i = np.squeeze(A > 0)
+ if i.any():
+ taud_max_a = np.min(- (B[i] * dmp.tau ** 2 + C[i]) / A[i])
+ else:
+ taud_max_a = np.inf
+ # Velocity bounds
+ i = range(len(A_next))
+ tau_min_v = np.max(-A_next[i] / D[i])
+ taud_min_v = (tau_min_v - dmp.tau) / dt
+ # Feasibility bounds
+ ii = np.arange(len(A_next))[np.squeeze(A_next < 0)]
+ jj = np.arange(len(A_next))[np.squeeze(A_next > 0)]
+ tau_min_f = -np.inf
+ for i in ii:
+ for j in jj:
+ num = C_next[i] * abs(A_next[j]) + C_next[j] * abs(A_next[i])
+ if num > 0:
+ den = abs(B[i] * A_next[j]) + abs(B[j] * A_next[i])
+ tmp = np.sqrt(num / den)
+ if tmp > tau_min_f:
+ tau_min_f = tmp
+ taud_min_f = (tau_min_f - dmp.tau) / dt
+ # Nominal bound
+ taud_min_nominal = (dmp.tau0 - dmp.tau) / dt
+
+ taud_min = np.max((taud_min_a, taud_min_v, taud_min_f, taud_min_nominal))
+
+ # Base update law
+ ydd_bar = dmp.h() / (dmp.tau**2 * self.a_max)
+ if self.gamma_a > 0:
+ pot_a = self.gamma_a * np.sum(ydd_bar ** 2 / np.maximum(1 - ydd_bar ** 2, self.gamma_a * self.eps * np.ones((len(ydd_bar), 1))))
+ else:
+ pot_a = 0
+ #pot_a = self.gamma_a * np.amax(ydd_bar ** 2 / np.maximum(1 - ydd_bar ** 2, self.gamma_a * self.eps * np.ones((len(ydd_bar), 1))))
+ taud = self.gamma_nominal * (dmp.tau0 - dmp.tau) + dmp.tau * pot_a
+
+ # Saturate
+ taud = np.min((taud, taud_max_a))
+ taud = np.max((taud, taud_min))
+
+ return taud
diff --git a/dmp/motion_planning/dmp/temporal_coupling.pyc b/dmp/motion_planning/dmp/temporal_coupling.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..ad105c2dfe090b880329d78c3dd0cb0fe173a9ed
Binary files /dev/null and b/dmp/motion_planning/dmp/temporal_coupling.pyc differ
diff --git a/dmp/motion_planning/online_traj_scaling/__init__.py b/dmp/motion_planning/online_traj_scaling/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..1d156231bbd0cb1a221023cdddc65337b30630c1
--- /dev/null
+++ b/dmp/motion_planning/online_traj_scaling/__init__.py
@@ -0,0 +1,2 @@
+from gmr_oa import GMRwOA
+from trajectory_generator import TrajectoryGenerator
\ No newline at end of file
diff --git a/dmp/motion_planning/online_traj_scaling/__init__.pyc b/dmp/motion_planning/online_traj_scaling/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..12e3377806349a16333dbb0e3992de92b9d20efc
Binary files /dev/null and b/dmp/motion_planning/online_traj_scaling/__init__.pyc differ
diff --git a/dmp/motion_planning/online_traj_scaling/__pycache__/gmr_oa.cpython-36.pyc b/dmp/motion_planning/online_traj_scaling/__pycache__/gmr_oa.cpython-36.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..1833879cf8567e520fb4534cbd3be33e2a81c347
Binary files /dev/null and b/dmp/motion_planning/online_traj_scaling/__pycache__/gmr_oa.cpython-36.pyc differ
diff --git a/dmp/motion_planning/online_traj_scaling/example.yaml b/dmp/motion_planning/online_traj_scaling/example.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..94f5fb3ed096e87b1e50054613968d9c736e02fe
--- /dev/null
+++ b/dmp/motion_planning/online_traj_scaling/example.yaml
@@ -0,0 +1,38 @@
+update_rate: 125
+x0: [-29.1, 114.9]
+s_end: 3.5
+priors: [0.2599, 0.1352, 0.1344, 0.1377, 0.0550, 0.1664, 0.1113]
+mu: [[-112.6856, 54.1569, -88.7499, -154.9200], [74.9928, -98.9324, 122.6443, 182.4869],
+ [22.6435, 1.4977, -102.7541, 2.7533], [-22.5173, -123.9775, 299.0837, -48.8174],
+ [-64.5564, 98.1626, -160.4187, -170.3178], [78.6112, -17.0895, -101.3953, 115.8752],
+ [-123.0283, -63.0123, 174.2271, -279.1635]]
+sigma: [[[0.6009, 0.0300, -0.0491, 1.0150],
+ [0.0300, 1.9033, -3.1190, 1.1773],
+ [-0.0491, -3.1190, 5.9745, -2.1668],
+ [1.0150, 1.1773, -2.1668, 3.1809]],
+ [[0.6290, 0.1268, -1.2774, 1.5296],
+ [0.1268, 0.9079, -2.3844, 0.3074],
+ [-1.2774, -2.3844, 8.2566, -3.0188],
+ [1.5296, 0.3074, -3.0188, 3.7767]],
+ [[0.2605, -0.0328, -0.6769, 0.3306],
+ [-0.0328, 0.0282, -0.1585, -0.1858],
+ [-0.6769, -0.1585, 7.9287, 1.6320],
+ [0.3306, -0.1858, 1.6320, 1.7752]],
+ [[1.7232, -0.7104, 0.3933, 3.7694],
+ [-0.7104, 0.7734, -1.2591, -1.5555],
+ [0.3933, -1.2591, 3.6391, 0.7968],
+ [3.7694, -1.5555, 0.7968, 8.5589]],
+ [[1.094, -0.079, -0.568, 1.331],
+ [-0.079, 2.376, -4.876, -2.423],
+ [-0.568, -4.876, 15.627, 9.517],
+ [1.331, -2.423, 9.517, 44.624]],
+ [[0.4217, -0.3304, 0.2178, 1.2149],
+ [-0.3304, 0.4037, -0.6332, -1.3119],
+ [0.2178, -0.6332, 2.2057, 2.3334],
+ [1.2149, -1.3119, 2.3334, 4.9441]],
+ [[0.5734, -0.6797, 1.3006, 1.320],
+ [-0.6797, 1.4485, -2.9368, -1.5774],
+ [1.3006, -2.9368, 6.0104, 2.9347],
+ [1.3200, -1.5774, 2.9347, 4.7359]]]
+o_c: [-100, -70]
+o_r: 30
\ No newline at end of file
diff --git a/dmp/motion_planning/online_traj_scaling/gmr_oa.py b/dmp/motion_planning/online_traj_scaling/gmr_oa.py
new file mode 100644
index 0000000000000000000000000000000000000000..cc49b5a1cb4da2f07d6c385defd0c55b688587a0
--- /dev/null
+++ b/dmp/motion_planning/online_traj_scaling/gmr_oa.py
@@ -0,0 +1,363 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+
+class GMRwOA:
+ def __init__(self, params):
+ self.x0 = np.array(params['x0'])
+ self.s_end = params['s_end']
+ self.n = self.x0.size
+ self.o_c = np.array(params['o_c'])
+ self.o_r = params['o_r']
+ self.priors = np.array(params['priors'])
+ self.mu = np.array(params['mu'])
+ self.sigma = np.array(params['sigma'])
+ self.nKernels = self.priors.size
+ self.A = np.zeros((self.nKernels, self.n, self.n))
+ self.b = np.zeros((self.nKernels, self.n))
+ for w in range(self.nKernels):
+ self.A[w, :, :] = self.sigma[w, self.n:, :self.n].dot(np.linalg.inv(
+ self.sigma[w, :self.n, :self.n]))
+ self.b[w, :] = self.mu[w, self.n:] - self.A[w, :, :].dot(self.mu[w, :self.n])
+
+ def f(self, x):
+ weights = self.weights(x)
+ f = np.zeros(self.n)
+ gmr_f = sum(weights[w] * (self.A[w, :, :].dot(x) + self.b[w, :]) for w in range(self.nKernels))
+ return self.modulation_matrix(x).dot(gmr_f)
+
+ def g(self, x):
+ return x
+
+ def gx(self, x):
+ return np.eye(self.n)
+
+ def dx_gxf(self, x):
+ x1 = x[0]
+ x2 = x[1]
+ t2 = x2 + 7.0e1
+ t3 = x1 + 1.0e2
+ t4 = t2 ** 2
+ t5 = t3 ** 2
+ t6 = x2 - 9.816261394778176e1
+ t7 = x1 * 1.530596366426007e-5
+ t8 = x2 * 2.109513339111852e-4
+ t9 = t7 + t8 - 1.971943673320373e-2
+ t10 = x1 + 6.455638083061622e1
+ t11 = x1 * 4.582396934010209e-4
+ t12 = x2 * 1.530596366426007e-5
+ t13 = t11 + t12 + 2.807982275662759e-2
+ t79 = t6 * t9
+ t80 = t10 * t13
+ t14 = -t79 - t80
+ t15 = np.exp(t14)
+ t16 = x2 - 5.415693532998247e1
+ t17 = x1 * 1.312704257151209e-5
+ t82 = x2 * 2.629035561972274e-4
+ t18 = t17 - t82 + 1.571727929602906e-2
+ t19 = t16 * t18
+ t20 = x1 + 1.126855799373714e2
+ t21 = x1 * 8.327024557111857e-4
+ t83 = x2 * 1.312704257151209e-5
+ t22 = t21 - t83 + 9.454447953270757e-2
+ t84 = t20 * t22
+ t23 = t19 - t84
+ t24 = np.exp(t23)
+ t25 = x1 + 2.251733280467149e1
+ t26 = x1 * 4.670158568124115e-4
+ t27 = x2 * 4.289836618923725e-4
+ t28 = t26 + t27 + 6.370025953155209e-2
+ t29 = x2 + 1.239774676360322e2
+ t30 = x1 * 4.289836618923725e-4
+ t31 = x2 * 1.040569631086783e-3
+ t32 = t30 + t31 + 1.386667556436968e-1
+ t88 = t25 * t28
+ t89 = t29 * t32
+ t33 = -t88 - t89
+ t34 = np.exp(t33)
+ t35 = t34 * 2.408872289627689e-5
+ t36 = x1 - 7.499276410192803e1
+ t37 = x2 * 1.142541742241694e-4
+ t90 = x1 * 8.178935231312593e-4
+ t38 = t37 - t90 + 7.263953274615121e-2
+ t39 = t36 * t38
+ t40 = x2 + 9.893237408811206e1
+ t41 = x2 * 5.666798935371871e-4
+ t91 = x1 * 1.142541742241694e-4
+ t42 = t41 - t91 + 6.463122355088623e-2
+ t92 = t40 * t42
+ t43 = t39 - t92
+ t44 = np.exp(t43)
+ t45 = t44 * 2.888145048134885e-5
+ t46 = x1 - 2.264354107101735e1
+ t47 = x1 * 2.247790700044474e-3
+ t48 = x2 * 2.608028701302809e-3
+ t49 = t47 + t48 - 5.480387720544926e-2
+ t50 = x2 - 1.497658429905395
+ t51 = x1 * 2.608028701302809e-3
+ t52 = x2 * 2.072560326248954e-2
+ t53 = t51 + t52 - 9.00948794532844e-2
+ t93 = t46 * t49
+ t94 = t50 * t53
+ t54 = -t93 - t94
+ t55 = np.exp(t54)
+ t56 = t55 * 2.698593356722665e-4
+ t57 = x2 + 6.301228183938932e1
+ t58 = x1 * 9.222652594971037e-4
+ t59 = x2 * 7.779669674794524e-4
+ t60 = t58 + t59 + 1.624861712161579e-1
+ t61 = x1 + 1.230282678775932e2
+ t62 = x1 * 1.965364578835002e-3
+ t63 = x2 * 9.222652594971037e-4
+ t64 = t62 + t63 + 2.999094383441548e-1
+ t95 = t57 * t60
+ t96 = t61 * t64
+ t65 = -t95 - t96
+ t66 = np.exp(t65)
+ t67 = t66 * 2.919147494282987e-5
+ t68 = x1 - 7.861117292542697e1
+ t69 = x1 * 3.302595736312175e-3
+ t70 = x2 * 2.702662948139369e-3
+ t71 = t69 + t70 - 2.134336703875113e-1
+ t72 = x2 + 1.708953540592262e1
+ t73 = x1 * 2.702662948139369e-3
+ t74 = x2 * 3.450285534628542e-3
+ t75 = t73 + t74 - 1.534957275707511e-1
+ t97 = t68 * t71
+ t98 = t72 * t75
+ t76 = -t97 - t98
+ t77 = np.exp(t76)
+ t78 = t77 * 1.071251714048381e-4
+ t81 = t15 * 5.43860953025742e-6
+ t85 = t24 * 3.869452814178776e-5
+ t86 = t35 + t45 + t56 + t67 + t78 + t81 + t85
+ t87 = 1.0 / t86
+ t99 = x1 * 6.605191472624351e-3
+ t100 = x2 * 5.405325896278738e-3
+ t101 = t99 + t100 - 4.268673407750226e-1
+ t102 = x1 * 3.869851041803628
+ t103 = x2 * 1.010037993543203e1
+ t104 = t102 + t103
+ t105 = x1 * 4.495581400088948e-3
+ t106 = x2 * 5.216057402605618e-3
+ t107 = t105 + t106 - 1.096077544108985e-1
+ t108 = x2 * 2.285083484483388e-4
+ t117 = x1 * 1.635787046262519e-3
+ t109 = t108 - t117 + 1.452790654923024e-1
+ t110 = x1 * 1.665404911422371e-3
+ t115 = x2 * 2.625408514302419e-5
+ t111 = t110 - t115 + 1.890889590654151e-1
+ t112 = x1 * 1.983731639577469
+ t113 = x2 * 3.191903357349043
+ t114 = t112 + t113 - 1.4210854715202e-14
+ t116 = t24 * t111 * 3.869452814178776e-5
+ t118 = t77 * t101 * 1.071251714048381e-4
+ t119 = x1 * 9.164793868020419e-4
+ t120 = x2 * 3.061192732852015e-5
+ t121 = t119 + t120 + 5.615964551325517e-2
+ t122 = t15 * t121 * 5.43860953025742e-6
+ t123 = x1 * 9.34031713624823e-4
+ t124 = x2 * 8.57967323784745e-4
+ t125 = t123 + t124 + 1.274005190631042e-1
+ t126 = t34 * t125 * 2.408872289627689e-5
+ t127 = t55 * t107 * 2.698593356722665e-4
+ t128 = x1 * 3.930729157670005e-3
+ t129 = x2 * 1.844530518994207e-3
+ t130 = t128 + t129 + 5.998188766883095e-1
+ t131 = t66 * t130 * 2.919147494282987e-5
+ t136 = t44 * t109 * 2.888145048134885e-5
+ t132 = t116 + t118 + t122 + t126 + t127 + t131 - t136
+ t133 = 1.0 / t86 ** 2
+ t134 = x1 * 3.231732335122654e-5
+ t154 = x2 * 1.638686742732473
+ t135 = t134 - t154
+ t137 = x1 * 1.54461351435013
+ t138 = x2 * 2.410526413045497
+ t139 = t137 + t138
+ t140 = x1 * 7.128956258032981e-1
+ t141 = x2 * 2.282924141766298
+ t142 = t140 + t141
+ t143 = x1 * 3.04539679230562e-1
+ t144 = x2 * 2.17037228002446
+ t145 = t143 + t144 - 2.842170943040401e-14
+ t146 = x1 * 6.694733262807462e-1
+ t147 = x2 * 2.074491355757708
+ t148 = t146 + t147
+ t149 = x1 * 2.0
+ t150 = t149 + 2.0e2
+ t151 = t4 + t5
+ t152 = 1.0 / t151 ** 2
+ t183 = t3 * t150
+ t153 = t4 + t5 - t183
+ t155 = x1 * 1.145820509435398
+ t157 = x2 * 9.81512008356454e-1
+ t156 = t155 - t157
+ t158 = x1 * 5.169583816001215e-1
+ t166 = x2 * 5.977666165428746
+ t159 = t158 - t166 + 4.440892098500626e-16
+ t160 = x1 * 1.659508447689607
+ t161 = x2 * 5.924016783798807e-1
+ t162 = t160 + t161
+ t163 = x1 * 2.279092938141525
+ t165 = x2 * 1.950918964167032e-2
+ t164 = t163 - t165
+ t167 = x2 * 2.48561120484202
+ t175 = x1 * 9.336740956565702e-1
+ t168 = t167 - t175 + 2.842170943040401e-14
+ t169 = x1 * 2.43188361814004
+ t174 = x2 * 1.144763805707237e-3
+ t170 = t169 - t174
+ t171 = x2 * 3.289247523618103e-3
+ t173 = x1 * 2.186101706496526
+ t172 = t171 - t173 + 7.105427357601002e-14
+ t176 = 1.0 / t151 ** 3
+ t177 = t15 * t87 * t156 * 5.43860953025742e-6
+ t178 = t55 * t87 * t159 * 2.698593356722665e-4
+ t179 = t24 * t87 * t162 * 3.869452814178776e-5
+ t180 = t66 * t87 * t164 * 2.919147494282987e-5
+ t181 = t44 * t87 * t170 * 2.888145048134885e-5
+ t224 = t77 * t87 * t168 * 1.071251714048381e-4
+ t225 = t34 * t87 * t172 * 2.408872289627689e-5
+ t182 = t177 + t178 + t179 + t180 + t181 - t224 - t225
+ t184 = t152 * t153 * 9.0e2
+ t185 = t184 + 1.0
+ t186 = x1 * 1.844530518994207e-3
+ t187 = x2 * 1.555933934958905e-3
+ t188 = t186 + t187 + 3.249723424323159e-1
+ t189 = x1 * 5.216057402605618e-3
+ t190 = x2 * 4.145120652497908e-2
+ t191 = t189 + t190 - 1.801897589065688e-1
+ t192 = t66 * t188 * 2.919147494282987e-5
+ t193 = x1 * 5.405325896278738e-3
+ t194 = x2 * 6.900571069257084e-3
+ t195 = t193 + t194 - 3.069914551415021e-1
+ t196 = t77 * t195 * 1.071251714048381e-4
+ t197 = x1 * 2.625408514302419e-5
+ t212 = x2 * 5.258071123944548e-4
+ t198 = t197 - t212 + 3.143455859205812e-2
+ t199 = x2 * 1.133359787074374e-3
+ t214 = x1 * 2.285083484483388e-4
+ t200 = t199 - t214 + 1.292624471017725e-1
+ t201 = t44 * t200 * 2.888145048134885e-5
+ t202 = x1 * 8.57967323784745e-4
+ t203 = x2 * 2.081139262173565e-3
+ t204 = t202 + t203 + 2.773335112873937e-1
+ t205 = t34 * t204 * 2.408872289627689e-5
+ t206 = t55 * t191 * 2.698593356722665e-4
+ t207 = x1 * 3.061192732852015e-5
+ t208 = x2 * 4.219026678223704e-4
+ t209 = t207 + t208 - 3.943887346640746e-2
+ t210 = t15 * t209 * 5.43860953025742e-6
+ t213 = t24 * t198 * 3.869452814178776e-5
+ t211 = t192 + t196 + t201 + t205 + t206 + t210 - t213
+ t215 = x2 * 2.0
+ t216 = t215 + 1.4e2
+ t217 = t55 * t87 * t104 * 2.698593356722665e-4
+ t218 = t77 * t87 * t114 * 1.071251714048381e-4
+ t219 = t44 * t87 * t139 * 2.888145048134885e-5
+ t220 = t34 * t87 * t142 * 2.408872289627689e-5
+ t221 = t66 * t87 * t145 * 2.919147494282987e-5
+ t222 = t15 * t87 * t148 * 5.43860953025742e-6
+ t244 = t24 * t87 * t135 * 3.869452814178776e-5
+ t223 = t217 + t218 + t219 + t220 + t221 + t222 - t244
+ t226 = t150 * t152 * 9.0e2
+ t260 = t2 * t216
+ t227 = t4 + t5 - t260
+ t228 = t15 * t87 * 6.231670342579768e-6
+ t229 = t24 * t87 * 6.421389633066001e-5
+ t230 = t34 * t87 * 5.266039823087284e-5
+ t231 = t44 * t87 * 7.023632629371505e-5
+ t232 = t55 * t87 * 1.395060454288188e-4
+ t233 = t66 * t87 * 6.653008439613883e-5
+ t234 = t77 * t87 * 1.000199975334673e-4
+ t235 = t15 * t132 * t133 * t156 * 5.43860953025742e-6
+ t236 = t55 * t132 * t133 * t159 * 2.698593356722665e-4
+ t237 = t24 * t132 * t133 * t162 * 3.869452814178776e-5
+ t238 = t66 * t132 * t133 * t164 * 2.919147494282987e-5
+ t239 = t44 * t87 * t109 * t170 * 2.888145048134885e-5
+ t240 = t34 * t87 * t125 * t172 * 2.408872289627689e-5
+ t241 = t44 * t132 * t133 * t170 * 2.888145048134885e-5
+ t242 = t77 * t87 * t101 * t168 * 1.071251714048381e-4
+ t243 = t228 + t229 + t230 + t231 + t232 + t233 + t234 + t235 + t236 + t237 + t238 + t239 + t240 + t241 + t242 - t15 * t87 * t121 * t156 * 5.43860953025742e-6 - t24 * t87 * t111 * t162 * 3.869452814178776e-5 - t55 * t87 * t107 * t159 * 2.698593356722665e-4 - t66 * t87 * t130 * t164 * 2.919147494282987e-5 - t34 * t132 * t133 * t172 * 2.408872289627689e-5 - t77 * t132 * t133 * t168 * 1.071251714048381e-4
+ t245 = t15 * t87 * 3.641004012563601e-6
+ t246 = t34 * t87 * 1.717274518394355e-5
+ t247 = t44 * t87 * 4.46106787275255e-5
+ t248 = t55 * t87 * 1.044315431291755e-3
+ t249 = t66 * t87 * 8.889962415356397e-6
+ t250 = t77 * t87 * 2.125075919109369e-4
+ t251 = t55 * t104 * t132 * t133 * 2.698593356722665e-4
+ t252 = t44 * t87 * t109 * t139 * 2.888145048134885e-5
+ t253 = t24 * t87 * t111 * t135 * 3.869452814178776e-5
+ t254 = t77 * t114 * t132 * t133 * 1.071251714048381e-4
+ t255 = t44 * t132 * t133 * t139 * 2.888145048134885e-5
+ t256 = t34 * t132 * t133 * t142 * 2.408872289627689e-5
+ t257 = t66 * t132 * t133 * t145 * 2.919147494282987e-5
+ t258 = t15 * t132 * t133 * t148 * 5.43860953025742e-6
+ t259 = t245 + t246 + t247 + t248 + t249 + t250 + t251 + t252 + t253 + t254 + t255 + t256 + t257 + t258 - t24 * t87 * 1.25050357788129e-9 - t55 * t87 * t104 * t107 * 2.698593356722665e-4 - t15 * t87 * t121 * t148 * 5.43860953025742e-6 - t77 * t87 * t101 * t114 * 1.071251714048381e-4 - t34 * t87 * t125 * t142 * 2.408872289627689e-5 - t24 * t132 * t133 * t135 * 3.869452814178776e-5 - t66 * t87 * t130 * t145 * 2.919147494282987e-5
+ t261 = t152 * t227 * 9.0e2
+ t262 = t261 + 1.0
+ t263 = t15 * t87 * 5.338060562709511e-6
+ t264 = t34 * t87 * 7.923377213370146e-8
+ t265 = t44 * t87 * 3.306243916737403e-8
+ t266 = t55 * t87 * 1.613129020273186e-3
+ t267 = t66 * t87 * 5.695020205797351e-7
+ t268 = t77 * t87 * 2.662715263644876e-4
+ t269 = t55 * t87 * t159 * t191 * 2.698593356722665e-4
+ t270 = t34 * t133 * t172 * t211 * 2.408872289627689e-5
+ t271 = t66 * t87 * t164 * t188 * 2.919147494282987e-5
+ t272 = t15 * t87 * t156 * t209 * 5.43860953025742e-6
+ t273 = t44 * t87 * t170 * t200 * 2.888145048134885e-5
+ t274 = t77 * t133 * t168 * t211 * 1.071251714048381e-4
+ t275 = t263 + t264 + t265 + t266 + t267 + t268 + t269 + t270 + t271 + t272 + t273 + t274 - t24 * t87 * 2.292270341531259e-5 - t24 * t87 * t162 * t198 * 3.869452814178776e-5 - t34 * t87 * t172 * t204 * 2.408872289627689e-5 - t15 * t133 * t156 * t211 * 5.43860953025742e-6 - t77 * t87 * t168 * t195 * 1.071251714048381e-4 - t24 * t133 * t162 * t211 * 3.869452814178776e-5 - t44 * t133 * t170 * t211 * 2.888145048134885e-5 - t55 * t133 * t159 * t211 * 2.698593356722665e-4 - t66 * t133 * t164 * t211 * 2.919147494282987e-5
+ t276 = t152 * t216 * 9.0e2
+ t277 = t15 * t87 * 1.128234845786051e-5
+ t278 = t24 * t87 * 6.340821028223617e-5
+ t279 = t34 * t87 * 5.49927270442291e-5
+ t280 = t44 * t87 * 6.9619499232357e-5
+ t281 = t55 * t87 * 2.725681819413176e-3
+ t282 = t66 * t87 * 6.335636802894656e-5
+ t283 = t77 * t87 * 3.419331942636945e-4
+ t284 = t77 * t114 * t133 * t211 * 1.071251714048381e-4
+ t285 = t44 * t133 * t139 * t211 * 2.888145048134885e-5
+ t286 = t34 * t133 * t142 * t211 * 2.408872289627689e-5
+ t287 = t66 * t133 * t145 * t211 * 2.919147494282987e-5
+ t288 = t15 * t133 * t148 * t211 * 5.43860953025742e-6
+ t289 = t55 * t104 * t133 * t211 * 2.698593356722665e-4
+ t290 = t277 + t278 + t279 + t280 + t281 + t282 + t283 + t284 + t285 + t286 + t287 + t288 + t289 - t55 * t87 * t104 * t191 * 2.698593356722665e-4 - t24 * t87 * t135 * t198 * 3.869452814178776e-5 - t15 * t87 * t148 * t209 * 5.43860953025742e-6 - t34 * t87 * t142 * t204 * 2.408872289627689e-5 - t44 * t87 * t139 * t200 * 2.888145048134885e-5 - t77 * t87 * t114 * t195 * 1.071251714048381e-4 - t66 * t87 * t145 * t188 * 2.919147494282987e-5 - t24 * t133 * t135 * t211 * 3.869452814178776e-5
+ return np.reshape([-t185 * t259 + t223 * (
+ t226 + t150 * t153 * t176 * 1.8e3) - t2 * t152 * t182 * 1.8e3 - t2 * t150 * t152 * t243 * 9.0e2 + t2 * t150 ** 2 * t176 * t182 * 1.8e3,
+ t243 * t262 + t182 * (
+ t226 - t150 * t176 * t227 * 1.8e3) + t152 * t216 * t223 * 9.0e2 + t3 * t152 * t216 * t259 * 9.0e2 - t3 * t150 * t176 * t216 * t223 * 1.8e3,
+ -t185 * t290 - t223 * (
+ t276 - t153 * t176 * t216 * 1.8e3) - t150 * t152 * t182 * 9.0e2 + t2 * t150 * t152 * t275 * 9.0e2 + t2 * t150 * t176 * t182 * t216 * 1.8e3,
+ -t262 * t275 - t182 * (
+ t276 + t176 * t216 * t227 * 1.8e3) + t3 * t152 * t223 * 1.8e3 + t3 * t152 * t216 * t290 * 9.0e2 - t3 * t176 * t216 ** 2 * t223 * 1.8e3],
+ [2, 2], 'F')
+
+ def weights(self, x):
+ num = np.zeros(self.nKernels)
+ for w in range(self.nKernels):
+ num[w] = self.priors[w] * self.mvn_pdf(x, self.mu[w, :self.n], self.sigma[w, :self.n, :self.n])
+ den = num.sum()
+ return num / den
+
+ @staticmethod
+ def mvn_pdf(x, mu, sigma):
+ n = x.size
+ return 1 / np.sqrt((2 * np.pi) ** n * np.linalg.det(sigma)) * np.exp(
+ -0.5 * (x - mu).transpose().dot(np.linalg.inv(sigma).dot(x - mu)))
+
+ def modulation_matrix(self, x):
+ xd = x - self.o_c
+ return np.eye(self.n) + (self.o_r / xd.transpose().dot(xd))**2 * (xd.transpose().dot(xd) * np.eye(self.n) -
+ 2 * np.outer(xd, xd))
+
+ def gamma(self, x):
+ return sum((np.linalg.norm(x[i] - self.o_c[i]) / self.o_r) ** 2 for i in range(self.n))
+
+ def lambda_r(self, x):
+ return 1 - 1 / self.gamma(x)
+
+ def lambda_e(self, x):
+ return 1 + 1 / self.gamma(x)
diff --git a/dmp/motion_planning/online_traj_scaling/gmr_oa.pyc b/dmp/motion_planning/online_traj_scaling/gmr_oa.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..248b7e889920da02a0f108bf4358457600deb468
Binary files /dev/null and b/dmp/motion_planning/online_traj_scaling/gmr_oa.pyc differ
diff --git a/dmp/motion_planning/online_traj_scaling/trajectory_generator.py b/dmp/motion_planning/online_traj_scaling/trajectory_generator.py
new file mode 100755
index 0000000000000000000000000000000000000000..899b8af111b59b9f2ed1b4e117a0b3adf47a77c2
--- /dev/null
+++ b/dmp/motion_planning/online_traj_scaling/trajectory_generator.py
@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+
+import matplotlib.pyplot as plt
+import yaml
+from gmr_oa import GMRwOA
+import numpy as np
+
+class TrajectoryGenerator:
+ def __init__(self, dyn_sys):
+ # parameters
+ self.dyn_sys = dyn_sys
+ # state
+ self.x = None
+ self.s = None
+ self.ds = None
+ self.sat_flag = None
+ # Output state
+ self.pos = None
+ self.vel = None
+ self.acc = None
+ # desired path
+ self.path = None
+
+ self.reset()
+
+ def reset(self):
+ self.x = self.dyn_sys.x0
+ self.s = 0
+ self.ds = 1
+ self.sat_flag = 0
+ self.pos = self.dyn_sys.g(self.x)
+ self.vel = self.dyn_sys.gx(self.x).dot(self.dyn_sys.f(self.x))
+ self.acc = self.dyn_sys.dx_gxf(self.x).dot(self.dyn_sys.f(self.x))
+
+ def step(self, dt):
+ # Derivative
+ dx = self.dyn_sys.f(self.x) * self.ds
+ dds, self.sat_flag = self.traj_scaling()
+
+ # Integrate
+ self.x += dx * dt
+ self.s += self.ds * dt
+ self.ds += dds * dt
+
+ # Update reference
+ self.pos = self.dyn_sys.g(self.x)
+ self.vel = self.dyn_sys.gx(self.x).dot(self.dyn_sys.f(self.x))
+ self.acc = self.dyn_sys.gx(self.x).dot(self.dyn_sys.f(self.x)) * dds + self.dyn_sys.dx_gxf(
+ self.x).dot(self.dyn_sys.f(self.x)) * self.ds ** 2
+
+ def roll_out(self, dt, T = None):
+ self.reset()
+ pos = np.array([self.pos])
+ vel = np.array([self.vel])
+ acc = np.array([self.acc])
+ t = np.array([0])
+ s = np.array([0])
+ # for k in range(1, round(T/dt)+1):
+ k = 0
+ while True:
+ k += 1
+ self.step(dt)
+ t = np.append(t, k*dt)
+ s = np.append(s, self.s)
+ pos = np.append(pos, np.array([self.pos]), axis=0)
+ vel = np.append(vel, np.array([self.vel]), axis=0)
+ acc = np.append(acc, np.array([self.acc]), axis=0)
+ if T and k*dt > T:
+ break
+ if not T and self.s > self.dyn_sys.s_end:
+ break
+ return t, s, pos, vel, acc
+
+ def traj_scaling(self):
+ dds = 0
+ sat_flag = 0
+
+ return dds, sat_flag
+
+if __name__ == "__main__":
+ with open("example.yaml", 'r') as stream:
+ try:
+ params = yaml.safe_load(stream)
+ params['sigma'] = [[[el * 1e3 for el in row] for row in col] for col in params['sigma']]
+ gmr = GMRwOA(params)
+ tg = TrajectoryGenerator(gmr)
+ t, s, pos, vel, acc = tg.roll_out(0.01)
+ plt.plot(0.0032*pos[:, 0], 0.0026*pos[:, 1] + 0.7)
+ # plt.plot(s, pos[:, 0])
+ # plt.plot(s, pos[:, 1])
+ plt.show()
+ except yaml.YAMLError as exc:
+ print(exc)
diff --git a/dmp/motion_planning/online_traj_scaling/trajectory_generator.pyc b/dmp/motion_planning/online_traj_scaling/trajectory_generator.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..c1df99f5686f4e652cb2f5d7683dfb0f5585c66d
Binary files /dev/null and b/dmp/motion_planning/online_traj_scaling/trajectory_generator.pyc differ
diff --git a/dmp/msg/DMPState.msg b/dmp/msg/DMPState.msg
new file mode 100644
index 0000000000000000000000000000000000000000..aec661340a80f5bc98105bb4e8a50638a52729e2
--- /dev/null
+++ b/dmp/msg/DMPState.msg
@@ -0,0 +1,8 @@
+Header header
+float64[] position
+float64[] velocity
+float64[] acceleration
+float64 theta
+float64 s
+float64 tau
+
diff --git a/dmp/msg/TrajGenState.msg b/dmp/msg/TrajGenState.msg
new file mode 100644
index 0000000000000000000000000000000000000000..936c4e96b4b84285eea5f00aecc71e992f24b4d0
--- /dev/null
+++ b/dmp/msg/TrajGenState.msg
@@ -0,0 +1,6 @@
+Header header
+float64[] position
+float64[] velocity
+float64[] acceleration
+float64 s
+float64 s_dot
diff --git a/dmp/todos.md b/dmp/todos.md
index ec74f5a845c41a79b87614cf508e8e7400ab7a78..6ac3bb831992623388fb33de998b165b22caa598 100644
--- a/dmp/todos.md
+++ b/dmp/todos.md
@@ -4,7 +4,7 @@ and it's matlab
# calculating the dmp from the trajectory
is in
-TC_DMP_constrainedVelAcc/something/DMP.m
+TC_DMP_constrainedVelAcc/matlab/DMP.m
# running the dmp node
TC_DMP_constrainedVelAcc/catkin_ws/src/motion_planning/nodes/dmp_node
diff --git a/dmp/traj_scaling_node b/dmp/traj_scaling_node
new file mode 100755
index 0000000000000000000000000000000000000000..e4e6acad8e99984466b41df2e84efdcd15a7fdea
--- /dev/null
+++ b/dmp/traj_scaling_node
@@ -0,0 +1,113 @@
+#!/usr/bin/env python
+
+from motion_planning.online_traj_scaling import GMRwOA, TrajectoryGenerator
+import numpy as np
+import rospy
+from robot_ctrl.msg import CtrlTarget
+from std_msgs.msg import Float64, String
+from motion_planning.msg import TrajGenState
+
+
+class TrajGenNode:
+ def __init__(self):
+ self.active = False
+ self.rate = None
+ self.dt = None
+ self.traj_gen = None
+
+ # Load parameters from server
+ if not self.load_params():
+ rospy.logwarn("Trajectory Generator not initialized")
+
+ # Setup ros topics
+ self.target_pub = rospy.Publisher("robot_ctrl/command", CtrlTarget, queue_size=1)
+ self.state_pub = rospy.Publisher("motion_planning/state", TrajGenState, queue_size=1)
+ rospy.Subscriber("motion_planning/command", String, self.command_parser)
+
+ # Go into spin
+ self.spin()
+
+ def load_params(self):
+ parameters = ['x0', 's_end', 'priors', 'mu', 'sigma', 'o_c', 'o_r']
+ if not rospy.has_param('motion_planning/update_rate'):
+ rospy.logerr("Parameter " + rospy.get_namespace() + "motion_planning/update_rate not on parameter server.")
+ return False
+ update_rate = rospy.get_param('motion_planning/update_rate')
+ self.rate = rospy.Rate(update_rate)
+ params = {}
+ for i in range(len(parameters)):
+ if not rospy.has_param('motion_planning/' + parameters[i]):
+ rospy.logerr("Parameter " + rospy.get_namespace() + "motion_planning/" + parameters[i] + " not on parameter server.")
+ return False
+ params[parameters[i]] = rospy.get_param('motion_planning/' + parameters[i])
+ gmr = GMRwOA(params)
+ self.traj_gen = TrajectoryGenerator(gmr)
+ return True
+
+ def command_parser(self, msg):
+ if msg.data == 'start':
+ self.active = True
+ elif msg.data == 'stop':
+ self.active = False
+ elif msg.data == 'reset':
+ self.traj_gen.reset()
+ self.active = False
+ rospy.loginfo("Reset motion_planner.")
+ elif msg.data == 'step':
+ self.traj_gen.step(self.dt)
+ elif msg.data == 'load_params':
+ self.load_params()
+ rospy.loginfo("Reloaded motion_planner parameters.")
+ else:
+ rospy.loginfo("Invalid command string for motion planner.")
+
+ # spin -----------------------------------------------------
+ # -----------------------------------------------------------
+ def spin(self):
+ print_flag = True
+ while not rospy.is_shutdown():
+
+ if self.active:
+ # Step DMP
+ self.traj_gen.step(self.dt)
+ # Send target trajectory state command
+ pub_msg = CtrlTarget()
+ pub_msg.header.stamp = rospy.Time.now()
+ pub_msg.acceleration = self.traj_gen.acc
+ pub_msg.velocity = self.traj_gen.vel
+ pub_msg.position = self.traj_gen.pos
+ self.target_pub.publish(pub_msg)
+ else:
+ print_flag = True
+
+ # Publish DMP state command
+ pub_msg = TrajGenState()
+ pub_msg.header.stamp = rospy.Time.now()
+ pub_msg.acceleration = [0.0032*self.traj_gen.acc[0], 0, 0.0026*self.traj_gen.acc[1]]
+ pub_msg.velocity = [0.0032*self.traj_gen.vel[0], 0, 0.0026*self.traj_gen.vel[1]]
+ pub_msg.position = [0.0032*self.traj_gen.pos[0], 0, 0.0026*self.traj_gen.pos[1]+0.7]
+ pub_msg.s = self.traj_gen.s
+ pub_msg.s_dot = self.traj_gen.ds
+ self.state_pub.publish(pub_msg)
+
+ # Notify when DMP trajectory is finished
+ if print_flag and self.traj_gen.s > self.traj_gen.dyn_sys.s_end:
+ rospy.loginfo("Trajectory finished.")
+ print_flag = False
+
+ # Sleep
+ self.rate.sleep()
+
+
+# --------------------------------------------------------------------
+# Here is the main entry point
+if __name__ == '__main__':
+ try:
+ # Init the node:
+ rospy.init_node('TrajGenNode')
+
+ # Initializing and continue running the main class:
+ TrajGenNode()
+
+ except rospy.ROSInterruptException:
+ pass