def __init__(self,
n_dmps,
n_bfs,
dt=0.001,
run_time=6,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
self.run_time = run_time
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps) * y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps) * goal
self.goal = goal
if w is None:
# default is f = 0 so all the weights are null
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.ay = np.ones(
n_dmps) * 25.0 if ay is None else ay # Schaal 2012 (*25)
self.by = self.ay / 4.0 if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, run_time=self.run_time, **kwargs)
self.timesteps = self.cs.timesteps
# set up the DMP system
self.reset_state(
) # this is a different reset with respect to CS (it's defined below)
self.gen_centers()
# set variance of Gaussian basis functions
self.h = np.ones(
self.n_bfs) * self.n_bfs**1.5 / self.c**1.8 / self.cs.ax
self.check_offset()
def __init__(self, dims, bfs, ts=None, dt=.01,
y0=0, goal=1, w=None,
ay=None, by=None, **kwargs):
'''
dims int: number of dynamic motor primitives
bfs int: number of basis functions per DMP
dt float: timestep for simulation
y0 list: initial state of DMPs
goal list: goal state of DMPs
w list: tunable parameters, control amplitude of basis functions
ay int: gain on attractor term y dynamics
by int: gain on attractor term y dynamics
'''
self.dmps = dims
self.bfs = bfs
self.dt = dt
# set up the Transformation System
if ts is None:
ts = TdwFormulation()
self.ts = ts
# start and goal
if isinstance(y0, (int, float)):
y0 = np.ones(self.dmps)*y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(self.dmps)*goal
self.goal = goal
if w is None:
# default is f = 0
w = np.zeros((self.dmps, self.bfs))
self.w = w
self.f_desired = np.array([])
self.f_predicted = np.array([])
self.f = np.zeros(self.dmps)
# set up the CS
self.cs = CanonicalSystem(pattern='discrete', dt=self.dt, **kwargs)
self.time_steps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
self.prep_centers_and_variances()
self.check_offset()
def __init__(self,
n_dmps,
n_bfs,
dt=0.001,
run_time=6,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
self.run_time = run_time
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps) * y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps) * goal
self.goal = goal
if w is None:
# default is f = 0 so all the weights are null
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.ay = np.ones(
n_dmps) * 25.0 if ay is None else ay # Schaal 2012 (*25)
self.by = self.ay / 4.0 if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, run_time=self.run_time, **kwargs)
self.timesteps = self.cs.timesteps
# set up the DMP system
self.reset_state()
self.gen_centers()
# HERE WE CHOOSE TO ADD **1.5 AT THE CENTERS "self.c"
self.h = np.ones(
self.n_bfs) * self.n_bfs**1.5 / self.c**1.8 / self.cs.ax
self.check_offset()
def __init__(self,
n_dmps,
n_bfs,
decay,
dt=.01,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
"""
n_dmps int: number of dynamic motor primitives
n_bfs int: number of basis functions per DMP
dt float: timestep for simulation
y0 list: initial state of DMPs
goal list: goal state of DMPs
w list: tunable parameters, control amplitude of basis functions
ay int: gain on attractor term y dynamics
by int: gain on attractor term y dynamics
"""
self.decay = decay
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps) * y0
self.y0 = np.array(y0)
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps) * goal
self.goal = np.array(goal)
if w is None:
# default is f = 0
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.ay = np.ones(n_dmps) * 25. if ay is None else ay # Schaal 2012
self.by = self.ay / 4. if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
self.gen_centers()
self.h = np.ones(self.n_bfs) * self.n_bfs**1.5 / self.c / self.cs.ax
# set up the DMP system
self.reset_state()
def __init__(self,
n_dmps,
n_bfs,
dt=.01,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
'''
:param n_dmps: int, number of dynamic motor primitives
:param n_bfs: int, number of basis function per DMP
:param dt: float, timestep of simulation
:param y0: list, initial state of DMPs
:param goal: list, goal state of DMPs
:param w: list, control amplitude of basis functions
:param ay: int, gain on attractor term y dynamic
:param by: int, gain on attractor term y dynamic
:param kwargs: param for dict data
'''
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
if isinstance(y0, (int, float)):
y0 = np.ones(n_dmps) * y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(n_dmps) * goal
self.goal = goal
if w is None:
# default f = 0
w = np.zeros((n_dmps, n_bfs))
self.w = w
self.ay = np.ones(n_dmps) * 25. if ay is None else ay
self.by = self.ay / 4. if by is None else by
# set up the cs
self.cs = CanonicalSystem(self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
def __init__(self,
dmps,
bfs,
dt=.01,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
"""
dmps int: number of dynamic motor primitives
bfs int: number of basis functions per DMP
dt float: timestep for simulation
y0 list: initial state of DMPs
goal list: goal state of DMPs
w list: tunable parameters, control amplitude of basis functions
ay int: gain on attractor term y dynamics
by int: gain on attractor term y dynamics
"""
self.dmps = dmps
self.bfs = bfs
self.dt = dt
# check if y0 is int or float type
if isinstance(y0, (int, float)):
y0 = np.ones(self.dmps) * y0
self.y0 = y0
# check if goal is int or float type
if isinstance(goal, (int, float)):
goal = np.ones(self.dmps) * goal
self.goal = goal
# w is a array dmpsxbfs vector/matrix
if w is None:
# default is f = 0
w = np.zeros((self.dmps, self.bfs))
self.w = w
self.ay = np.ones(dmps) * 25. if ay is None else ay # Schaal 2012
self.by = self.ay / 4. if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
def __init__(self,
nDMPs,
nBFs,
dt=0.01,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
"""
Default values from Schaal (2012)
[INPUT]
[VAR NAME] [TYPE] [DESCRIPTION]
(1) nDMPs int Number of dynamic motor primitives
(2) nBFs int Number of basis functions per DMP
(3) dt float Timestep for simulation
(4) y0 list Initial state of DMPs
(5) goal list Goal state of DMPs
(6) w list Tunable parameters, control amplitude of basis functions
(7) ay int Gain on attractor term y dynamics
(8) by int Gain on attractor term y dynamics
"""
self.nDMPs = nDMPs
self.nBFs = nBFs
self.dt = dt
self.y0 = y0 * np.ones(self.nDMPs) if isinstance(y0,
(int, float)) else y0
self.goal = goal * np.ones(self.nDMPs) if isinstance(
goal, (int, float)) else goal
self.w = np.zeros((self.nDMPs, self.nBFs)) if w is None else w
self.ay = np.ones(nDMPs) * 25.0 if ay is None else ay # Schaal 2012
self.by = self.ay / 4.0 if by is None else by # Schaal 2012, 0.25 makes it a critically damped system
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.runTime / self.dt)
# set up the DMP system
self.reset_state()
def __init__(self,
n_dmps=1,
n_bfs=100,
dt=0,
alpha_y=None,
beta_y=None,
**kwargs):
self.n_dmps = n_dmps # number of data dimensions, one dmp for one degree
self.n_bfs = n_bfs # number of basis functions
self.dt = dt
self.y0 = np.zeros(n_dmps) # for multiple dimensions
self.goal = np.ones(n_dmps) # for multiple dimensions
self.alpha_y = np.ones(n_dmps) * 60.0 if alpha_y is None else alpha_y
self.beta_y = self.alpha_y / 4.0 if beta_y is None else beta_y
self.tau = 1.0
self.w = np.zeros((n_dmps, n_bfs)) # weights for forcing term
self.psi_centers = np.zeros(
self.n_bfs
) # centers over canonical system for Gaussian basis functions
self.psi_h = np.zeros(
self.n_bfs
) # variance over canonical system for Gaussian basis functions
# canonical system
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = round(self.cs.run_time / self.dt)
# generate centers for Gaussian basis functions
self.generate_centers()
# self.h = np.ones(self.n_bfs) * self.n_bfs / self.psi_centers # original
self.h = np.ones(
self.n_bfs
) * self.n_bfs**1.5 / self.psi_centers / self.cs.alpha_x # chose from trail and error
# reset state
self.reset_state()
from cs import CanonicalSystem
import numpy as np
import matplotlib.pyplot as plt
# Discrete
# ---------------------------------------------
cs = CanonicalSystem(dt=.001, pattern='discrete')
# test normal rollout
x_track1 = cs.rollout()
cs.reset_state()
# test error coupling
timesteps = int(1.0 / .001)
x_track2 = np.zeros(timesteps)
err = np.zeros(timesteps)
err[200:400] = 2
err_coup = 1.0 / (1 + err)
for i in range(timesteps):
x_track2[i] = cs.step(error_coupling=err_coup[i])
# plot results
fig, ax1 = plt.subplots(figsize=(6, 3))
ax1.plot(x_track1, lw=2)
ax1.plot(x_track2, lw=2)
plt.grid()
plt.legend(['normal rollout', 'error coupling'])
ax2 = ax1.twinx()
ax2.plot(err, 'r-', lw=2)
