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)
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)
print(
'If you call with two tests, the results of the two are compared in one plot.\n'
)
sys.exit()
demo_labels = []
for arg in sys.argv[1:]:
demo_labels.append(str(arg))
# ExponentialSystem
tau = 0.6 # Time constant
initial_state = np.array([0.5, 1.0])
attractor_state = np.array([0.8, 0.1])
alpha = 6.0 # Decay factor
dyn_systems = [
ExponentialSystem(tau, initial_state, attractor_state, alpha)
]
# TimeSystem
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))
lib_path = os.path.abspath('../../python/')
sys.path.append(lib_path)
from dynamicalsystems.dynamicalsystems_plotting import *
from dynamicalsystems.ExponentialSystem import ExponentialSystem
if __name__=='__main__':
# Settings for the exponential system
tau = 0.6 # Time constant
initial_state = np.array([0.5, 1.0])
attractor_state = np.array([0.8, 0.1])
alpha = 6.0 # Decay rate
# Construct the system
system = ExponentialSystem(tau, initial_state, attractor_state, alpha)
# Settings for the integration of the system
dt = 0.004 # Integration step duration
integration_duration = 1.5*tau # Integrate for longer than the time constant
n_time_steps = int(np.ceil(integration_duration/dt))+1 # Number of time steps for the integration
# Generate a vector of times, i.e. 0.0, dt, 2*dt, 3*dt .... n_time_steps*dt=integration_duration
ts = np.linspace(0.0,integration_duration,n_time_steps)
# NUMERICAL INTEGRATION
n_dims = system.dim_ # Dimensionality of the system
xs_num = np.empty([n_dims,n_time_steps])
xds_num = np.empty([n_dims,n_time_steps])
# Use DynamicalSystemSystem::integrateStart to get the initial x and xd
class Dmp(DynamicalSystem,Parameterizable):
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)
#print(self.SPRING)
#print(self.SPRING_Y)
#print(self.SPRING_Z)
#print(self.GOAL)
#print(self.PHASE)
#print(self.GATING)
def set_tau(self,tau):
self.tau_ = tau
# Set value in all relevant subsystems also
self.spring_system_.set_tau(tau)
if self.goal_system_:
self.goal_system_.set_tau(tau)
self.phase_system_ .set_tau(tau)
self.gating_system_.set_tau(tau)
def integrateStart(self):
x = np.zeros(self.dim_)
xd = np.zeros(self.dim_)
# Start integrating goal system if it exists
if self.goal_system_ is None:
# No goal system, simply set goal state to attractor state
x[self.GOAL] = self.attractor_state_
xd[self.GOAL] = 0.0
else:
# Goal system exists. Start integrating it.
(x[self.GOAL],xd[self.GOAL]) = self.goal_system_.integrateStart()
# Set the attractor state of the spring system
self.spring_system_.set_attractor_state(x[self.GOAL])
# Start integrating all futher subsystems
(x[self.SPRING],xd[self.SPRING]) = self.spring_system_.integrateStart()
(x[self.PHASE ],xd[self.PHASE ]) = self.phase_system_.integrateStart()
(x[self.GATING],xd[self.GATING]) = self.gating_system_.integrateStart()
# Add rates of change
xd = self.differentialEquation(x)
return (x,xd)
def differentialEquation(self,x):
n_dims = self.dim_
xd = np.zeros(x.shape)
if self.goal_system_ is None:
# If there is no dynamical system for the delayed goal, the goal is
# simply the attractor state
self.spring_system_.set_attractor_state(self.attractor_state_)
# with zero change
xd_goal = np.zeros(n_dims)
else:
# Integrate goal system and get current goal state
self.goal_system_.set_attractor_state(self.attractor_state_)
x_goal = x[self.GOAL]
xd[self.GOAL] = self.goal_system_.differentialEquation(x_goal)
# The goal state is the attractor state of the spring-damper system
self.spring_system_.set_attractor_state(x_goal)
# Integrate spring damper system
#Forcing term is added to spring_state later
xd[self.SPRING] = self.spring_system_.differentialEquation(x[self.SPRING])
# Non-linear forcing term phase and gating systems
xd[self.PHASE] = self.phase_system_.differentialEquation(x[self.PHASE])
xd[self.GATING] = self.gating_system_.differentialEquation(x[self.GATING])
fa_output = self.computeFunctionApproximatorOutput(x[self.PHASE])
# Gate the output of the function approximators
gating = x[self.GATING]
forcing_term = gating*fa_output
# Scale the forcing term, if necessary
if (self.forcing_term_scaling_=="G_MINUS_Y0_SCALING"):
g_minus_y0 = (self.attractor_state_-self.initial_state_)
forcing_term = forcing_term*g_minus_y0
elif (self.forcing_term_scaling_=="AMPLITUDE_SCALING"):
forcing_term = forcing_term*self.trajectory_amplitudes_
# Add forcing term to the ZD component of the spring state
xd[self.SPRING_Z] += np.squeeze(forcing_term)/self.tau_
return xd
def computeFunctionApproximatorOutput(self,phase_state):
n_time_steps = phase_state.size
n_dims = self.dim_orig_
fa_output = np.zeros([n_time_steps,n_dims])
for i_fa in range(n_dims):
if self.function_approximators_[i_fa]:
if self.function_approximators_[i_fa].isTrained():
fa_output[:,i_fa] = self.function_approximators_[i_fa].predict(phase_state)
return fa_output
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)
def train(self,trajectory):
# Set tau, initial_state and attractor_state from the trajectory
self.set_tau(trajectory.ts_[-1])
self.set_initial_state(trajectory.ys_[0,:])
self.set_attractor_state(trajectory.ys_[-1,:])
# This needs to be computed for (optional) scaling of the forcing term.
# Needs to be done BEFORE computeFunctionApproximatorInputsAndTargets
self.trajectory_amplitudes_ = trajectory.getRangePerDim()
(fa_input_phase, f_target) = self.computeFunctionApproximatorInputsAndTargets(trajectory)
for dd in range(self.dim_orig_):
fa_target = f_target[:,dd]
self.function_approximators_[dd].train(fa_input_phase,fa_target)
# Save the times steps on which the Dmp was trained.
# This is just a convenience function to be able to call
# analyticalSolution without the "ts" argument.
self.ts_train_ = trajectory.ts_
def computeFunctionApproximatorInputsAndTargets(self,trajectory):
n_time_steps = trajectory.ts_.size
dim_data = trajectory.dim_
assert(self.dim_orig_==dim_data)
(xs_ana,xds_ana,forcing_terms, fa_outputs) = self.analyticalSolution(trajectory.ts_)
xs_goal = xs_ana[:,self.GOAL]
xs_gating = xs_ana[:,self.GATING]
xs_phase = xs_ana[:,self.PHASE]
fa_inputs_phase = xs_phase
# Get parameters from the spring-dampers system to compute inverse
damping_coefficient = self.spring_system_.damping_coefficient_
spring_constant = self.spring_system_.spring_constant_
mass = self.spring_system_.mass_
# Usually, spring-damper system of the DMP should have mass==1
assert(mass==1.0)
#Compute inverse
tau = self.tau_
f_target = tau*tau*trajectory.ydds_ + (spring_constant*(trajectory.ys_-xs_goal) + damping_coefficient*tau*trajectory.yds_)/mass
# Factor out gating term
for dd in range(self.dim_orig_):
f_target[:,dd] = f_target[:,dd]/np.squeeze(xs_gating)
# Factor out scaling
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))
f_target /= g_minus_y0_rep
elif (self.forcing_term_scaling_=="AMPLITUDE_SCALING"):
trajectory_amplitudes_rep = np.tile(self.trajectory_amplitudes_,(n_time_steps,1))
f_target /= trajectory_amplitudes_rep
return (fa_inputs_phase, f_target)
def statesAsTrajectory(self,ts, x_in, xd_in):
# Left column is time
return Trajectory(ts,x_in[:,self.SPRING_Y], xd_in[:,self.SPRING_Y], xd_in[:,self.SPRING_Z]/self.tau_)
def getParameterVectorSelected(self):
values = np.empty(0)
for fa in self.function_approximators_:
if fa.isTrained():
values = np.append(values,fa.getParameterVectorSelected())
return values
def setParameterVectorSelected(self,values):
size = self.getParameterVectorSelectedSize()
assert(len(values)==size)
offset = 0
for fa in self.function_approximators_:
if fa.isTrained():
cur_size = fa.getParameterVectorSelectedSize()
cur_values = values[offset:offset+cur_size]
fa.setParameterVectorSelected(cur_values)
offset += cur_size
def getParameterVectorSelectedSize(self):
size = 0
for fa in self.function_approximators_:
if fa.isTrained():
size += fa.getParameterVectorSelectedSize()
return size
class Dmp(DynamicalSystem,Parameterizable):
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)
#print(self.SPRING)
#print(self.SPRING_Y)
#print(self.SPRING_Z)
#print(self.GOAL)
#print(self.PHASE)
#print(self.GATING)
def set_tau(self,tau):
self.tau_ = tau
# Set value in all relevant subsystems also
self.spring_system_.set_tau(tau)
if self.goal_system_:
self.goal_system_.set_tau(tau)
self.phase_system_ .set_tau(tau)
self.gating_system_.set_tau(tau)
def integrateStart(self):
x = np.zeros(self.dim_)
xd = np.zeros(self.dim_)
# Start integrating goal system if it exists
if self.goal_system_ is None:
# No goal system, simply set goal state to attractor state
x[self.GOAL] = self.attractor_state_
xd[self.GOAL] = 0.0
else:
# Goal system exists. Start integrating it.
(x[self.GOAL],xd[self.GOAL]) = self.goal_system_.integrateStart()
# Set the attractor state of the spring system
self.spring_system_.set_attractor_state(x[self.GOAL])
# Start integrating all futher subsystems
(x[self.SPRING],xd[self.SPRING]) = self.spring_system_.integrateStart()
(x[self.PHASE ],xd[self.PHASE ]) = self.phase_system_.integrateStart()
(x[self.GATING],xd[self.GATING]) = self.gating_system_.integrateStart()
# Add rates of change
xd = self.differentialEquation(x)
return (x,xd)
def differentialEquation(self,x):
n_dims = self.dim_
xd = np.zeros(x.shape)
if self.goal_system_ is None:
# If there is no dynamical system for the delayed goal, the goal is
# simply the attractor state
self.spring_system_.set_attractor_state(self.attractor_state_)
# with zero change
xd_goal = np.zeros(n_dims)
else:
# Integrate goal system and get current goal state
self.goal_system_.set_attractor_state(self.attractor_state_)
x_goal = x[self.GOAL]
xd[self.GOAL] = self.goal_system_.differentialEquation(x_goal)
# The goal state is the attractor state of the spring-damper system
self.spring_system_.set_attractor_state(x_goal)
# Integrate spring damper system
#Forcing term is added to spring_state later
xd[self.SPRING] = self.spring_system_.differentialEquation(x[self.SPRING])
# Non-linear forcing term phase and gating systems
xd[self.PHASE] = self.phase_system_.differentialEquation(x[self.PHASE])
xd[self.GATING] = self.gating_system_.differentialEquation(x[self.GATING])
fa_output = self.computeFunctionApproximatorOutput(x[self.PHASE])
# Gate the output of the function approximators
gating = x[self.GATING]
forcing_term = gating*fa_output
# Scale the forcing term, if necessary
if (self.forcing_term_scaling_=="G_MINUS_Y0_SCALING"):
g_minus_y0 = (self.attractor_state_-self.initial_state_)
forcing_term = forcing_term*g_minus_y0
elif (self.forcing_term_scaling_=="AMPLITUDE_SCALING"):
forcing_term = forcing_term*self.trajectory_amplitudes_
# Add forcing term to the ZD component of the spring state
xd[self.SPRING_Z] += np.squeeze(forcing_term)/self.tau_
return xd
def computeFunctionApproximatorOutput(self,phase_state):
n_time_steps = phase_state.size
n_dims = self.dim_orig_
fa_output = np.zeros([n_time_steps,n_dims])
for i_fa in range(n_dims):
if self.function_approximators_[i_fa]:
if self.function_approximators_[i_fa].isTrained():
fa_output[:,i_fa] = self.function_approximators_[i_fa].predict(phase_state)
return fa_output
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)
def train(self,trajectory):
# Set tau, initial_state and attractor_state from the trajectory
self.set_tau(trajectory.ts_[-1])
self.set_initial_state(trajectory.ys_[0,:])
self.set_attractor_state(trajectory.ys_[-1,:])
# This needs to be computed for (optional) scaling of the forcing term.
# Needs to be done BEFORE computeFunctionApproximatorInputsAndTargets
self.trajectory_amplitudes_ = trajectory.getRangePerDim()
(fa_input_phase, f_target) = self.computeFunctionApproximatorInputsAndTargets(trajectory)
for dd in range(self.dim_orig_):
fa_target = f_target[:,dd]
self.function_approximators_[dd].train(fa_input_phase,fa_target)
# Save the times steps on which the Dmp was trained.
# This is just a convenience function to be able to call
# analyticalSolution without the "ts" argument.
self.ts_train_ = trajectory.ts_
def computeFunctionApproximatorInputsAndTargets(self,trajectory):
n_time_steps = trajectory.ts_.size
dim_data = trajectory.dim_
assert(self.dim_orig_==dim_data)
(xs_ana,xds_ana,forcing_terms, fa_outputs) = self.analyticalSolution(trajectory.ts_)
xs_goal = xs_ana[:,self.GOAL]
xs_gating = xs_ana[:,self.GATING]
xs_phase = xs_ana[:,self.PHASE]
fa_inputs_phase = xs_phase
# Get parameters from the spring-dampers system to compute inverse
damping_coefficient = self.spring_system_.damping_coefficient_
spring_constant = self.spring_system_.spring_constant_
mass = self.spring_system_.mass_
# Usually, spring-damper system of the DMP should have mass==1
assert(mass==1.0)
#Compute inverse
tau = self.tau_
f_target = tau*tau*trajectory.ydds_ + (spring_constant*(trajectory.ys_-xs_goal) + damping_coefficient*tau*trajectory.yds_)/mass
# Factor out gating term
for dd in range(self.dim_orig_):
f_target[:,dd] = f_target[:,dd]/np.squeeze(xs_gating)
# Factor out scaling
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))
f_target /= g_minus_y0_rep
elif (self.forcing_term_scaling_=="AMPLITUDE_SCALING"):
trajectory_amplitudes_rep = np.tile(self.trajectory_amplitudes_,(n_time_steps,1))
f_target /= trajectory_amplitudes_rep
return (fa_inputs_phase, f_target)
def statesAsTrajectory(self,ts, x_in, xd_in):
# Left column is time
return Trajectory(ts,x_in[:,self.SPRING_Y], xd_in[:,self.SPRING_Y], xd_in[:,self.SPRING_Z]/self.tau_)
def getParameterVectorSelected(self):
values = np.empty(0)
for fa in self.function_approximators_:
if fa.isTrained():
values = np.append(values,fa.getParameterVectorSelected())
return values
def setParameterVectorSelected(self,values):
size = self.getParameterVectorSelectedSize()
assert(len(values)==size)
offset = 0
for fa in self.function_approximators_:
if fa.isTrained():
cur_size = fa.getParameterVectorSelectedSize()
cur_values = values[offset:offset+cur_size]
fa.setParameterVectorSelected(cur_values)
offset += cur_size
def getParameterVectorSelectedSize(self):
size = 0
for fa in self.function_approximators_:
if fa.isTrained():
size += fa.getParameterVectorSelectedSize()
return size
def set_initial_state(self,initial_state):
assert(initial_state.size==self.dim_orig_)
super(Dmp,self).set_initial_state(initial_state);
# Set value in all relevant subsystems also
self.spring_system_.set_initial_state(initial_state);
if self.goal_system_:
self.goal_system_.set_initial_state(initial_state);
def set_attractor_state(self,attractor_state):
assert(attractor_state.size==self.dim_orig_)
super(Dmp,self).set_attractor_state(attractor_state);
# Set value in all relevant subsystems also
if self.goal_system_:
self.goal_system_.set_attractor_state(attractor_state);