def __init__(self, tau, y_init, y_attr, function_apps, name="Dmp", sigmoid_max_rate=-20,forcing_term_scaling="NO_SCALING"):
super().__init__(1, tau, y_init, y_attr, name)
dim_orig = self.dim_orig_
self.goal_system_ = ExponentialSystem(tau,y_init,y_attr,15,'goal')
self.gating_system_ = SigmoidSystem(tau,np.ones(1),sigmoid_max_rate,0.9*tau,'gating')
self.phase_system_ = TimeSystem(tau,False,'phase')
alpha = 20.0
self.spring_system_ = SpringDamperSystem(tau,y_init,y_attr,alpha)
self.function_approximators_ = function_apps
self.forcing_term_scaling_ = forcing_term_scaling
self.ts_train_ = None
# Make room for the subsystems
self.dim_ = 3*dim_orig+2
self.SPRING = np.arange(0*dim_orig+0, 0*dim_orig+0 +2*dim_orig)
self.SPRING_Y = np.arange(0*dim_orig+0, 0*dim_orig+0 +dim_orig)
self.SPRING_Z = np.arange(1*dim_orig+0, 1*dim_orig+0 +dim_orig)
self.GOAL = np.arange(2*dim_orig+0, 2*dim_orig+0 +dim_orig)
self.PHASE = np.arange(3*dim_orig+0, 3*dim_orig+0 + 1)
self.GATING = np.arange(3*dim_orig+1, 3*dim_orig+1 + 1)
dyn_systems.append(TimeSystem(tau))
# TimeSystem (but counting down instead of up)
count_down = True
dyn_systems.append(TimeSystem(tau, count_down, "TimeSystemCountDown"))
# SigmoidSystem
max_rate = -20
inflection_point = tau * 0.8
dyn_systems.append(
SigmoidSystem(tau, initial_state, max_rate, inflection_point))
# SpringDamperSystem
alpha = 12.0
dyn_systems.append(
SpringDamperSystem(tau, initial_state, attractor_state, alpha))
# INTEGRATE ALL DYNAMICAL SYSTEMS IN THE ARRA
# Loop through all systems, and do numerical integration and compute the analytical solution
figure_number = 1
for dyn_system in dyn_systems:
name = dyn_system.name_
print(name + ": \t")
all_data = []
for demo_label in demo_labels:
print(demo_label)
# RUN THE CURRENT TEST FOR THE CURRENT SYSTEM
(xs, xds, ts) = runDynamicalSystemTest(dyn_system, demo_label)
def analyticalSolution(self,ts=None):
if ts is None:
if self.ts_train_ is None:
print("Neither the argument 'ts' nor the member variable self.ts_train_ was set. Returning None.")
return None
else:
# Set the times to the ones the Dmp was trained on.
ts = self.ts_train_
n_time_steps = ts.size
# INTEGRATE SYSTEMS ANALYTICALLY AS MUCH AS POSSIBLE
# Integrate phase
( xs_phase, xds_phase) = self.phase_system_.analyticalSolution(ts)
# Compute gating term
( xs_gating, xds_gating ) = self.gating_system_.analyticalSolution(ts)
# Compute the output of the function approximator
fa_outputs = self.computeFunctionApproximatorOutput(xs_phase)
# Gate the output to get the forcing term
forcing_terms = fa_outputs*xs_gating
# Scale the forcing term, if necessary
if (self.forcing_term_scaling_=="G_MINUS_Y0_SCALING"):
g_minus_y0 = (self.attractor_state_-self.initial_state_)
g_minus_y0_rep = np.tile(g_minus_y0,(n_time_steps,1))
forcing_terms *= g_minus_y0_rep
elif (self.forcing_term_scaling_=="AMPLITUDE_SCALING"):
trajectory_amplitudes_rep = np.tile(self.trajectory_amplitudes_,(n_time_steps,1))
forcing_terms *= trajectory_amplitudes_rep
# Get current delayed goal
if self.goal_system_ is None:
# If there is no dynamical system for the delayed goal, the goal is
# simply the attractor state
xs_goal = np.tile(self.attractor_state_,(n_time_steps,1))
# with zero change
xds_goal = np.zeros(xs_goal.shape)
else:
# Integrate goal system and get current goal state
(xs_goal,xds_goal) = self.goal_system_.analyticalSolution(ts)
xs = np.zeros([n_time_steps,self.dim_])
xds = np.zeros([n_time_steps,self.dim_])
xs[:,self.GOAL] = xs_goal
xds[:,self.GOAL] = xds_goal
xs[:,self.PHASE] = xs_phase
xds[:,self.PHASE] = xds_phase
xs[:,self.GATING] = xs_gating
xds[:,self.GATING] = xds_gating
# THE REST CANNOT BE DONE ANALYTICALLY
# Reset the dynamical system, and get the first state
damping = self.spring_system_.damping_coefficient_
localspring_system = SpringDamperSystem(self.tau_,self.initial_state_,self.attractor_state_,damping)
# Set first attractor state
localspring_system.set_attractor_state(xs_goal[0,:])
# Start integrating spring damper system
(x_spring, xd_spring) = localspring_system.integrateStart()
# For convenience
SPRING = self.SPRING
SPRING_Y = self.SPRING_Y
SPRING_Z = self.SPRING_Z
t0 = 0
xs[t0,SPRING] = x_spring
xds[t0,SPRING] = xd_spring
# Add forcing term to the acceleration of the spring state
xds[0,SPRING_Z] = xds[0,SPRING_Z] + forcing_terms[t0,:]/self.tau_
for tt in range(1,n_time_steps):
dt = ts[tt]-ts[tt-1]
# Euler integration
xs[tt,SPRING] = xs[tt-1,SPRING] + dt*xds[tt-1,SPRING]
# Set the attractor state of the spring system
localspring_system.set_attractor_state(xs[tt,self.GOAL])
# Integrate spring damper system
xds[tt,SPRING] = localspring_system.differentialEquation(xs[tt,SPRING])
# If necessary add a perturbation. May be useful for some off-line tests.
#RowVectorXd perturbation = RowVectorXd::Constant(dim_orig(),0.0)
#if (analytical_solution_perturber_!=NULL)
# for (int i_dim=0 i_dim<dim_orig() i_dim++)
# // Sample perturbation from a normal Gaussian distribution
# perturbation(i_dim) = (*analytical_solution_perturber_)()
# Add forcing term to the acceleration of the spring state
xds[tt,SPRING_Z] = xds[tt,SPRING_Z] + forcing_terms[tt,:]/self.tau_ #+ perturbation
# Compute y component from z
xds[tt,SPRING_Y] = xs[tt,SPRING_Z]/self.tau_
return ( xs, xds, forcing_terms, fa_outputs)