def main():
""" Main function for script. """
# Train a DMP with a trajectory
traj = Trajectory.loadtxt("trajectory.txt")
function_apps = [
FunctionApproximatorRBFN(10, 0.7) for _ in range(traj.dim)
]
dmp = Dmp.from_traj(traj, function_apps, dmp_type="KULVICIUS_2012_JOINING")
# Compute analytical solution
xs, xds, _, _ = dmp.analytical_solution(traj.ts)
traj_reproduced = dmp.states_as_trajectory(traj.ts, xs, xds) # noqa
# Numerical integration
dt = 0.001
n_time_steps = int(1.3 * traj.duration / dt)
x, xd = dmp.integrate_start()
for tt in range(1, n_time_steps):
x, xd = dmp.integrate_step(dt, x)
# Convert complete DMP state to end-eff state
y, yd, ydd = dmp.states_as_pos_vel_acc(x, xd)
print(y)
# Save the DMP to a json file that can be read in C++
filename = "dmp_for_cpp.json"
json_for_cpp.savejson_for_cpp(filename, dmp)
print(f'Saved {filename} to local directory.')
def tell(self, obj, name, i_update=None, i_sample=None):
""" Add an object to the database.
@param obj: The object to add
@param name: The name of the file
@param i_update: The update number
@param i_sample: The sample number
"""
# If it's a Dmp, save it in a C++-readable format also
if "dmp" in name:
if self._root_dir is not None:
basename = self.get_base_name(name, i_update, i_sample)
abs_basename = Path(self._root_dir, basename)
jc.savejson(f"{abs_basename}.json", obj)
jc.savejson_for_cpp(f"{abs_basename}_for_cpp.json", obj)
filename = super().tell(obj, name, i_update, i_sample)
return filename
def train(directory, fa_name, n_dims, **kwargs):
""" Train a function approximator and plot it
@param directory: Directory to write data to.
@param fa_name: Name of the function approximator
@param n_dims: Dimensionality of the input
@param kwargs: The booleans "show", "save" and "verbose"
"""
show = kwargs.get("show", False)
save = kwargs.get("save", False)
verbose = kwargs.get("verbose", False)
# Generate training data
n_samples_per_dim = 30 if n_dims == 1 else [10, 10]
inputs, targets = target_function(n_samples_per_dim)
n_rfs = 9 if n_dims == 1 else [
5, 5
] # Number of basis functions. To be used later.
# Initialize function approximator
if fa_name == "LWR":
# This value for intersection is quite low. But for the demo it is nice
# because it makes the linear segments quite obvious.
intersection = 0.2
fa = FunctionApproximatorLWR(n_rfs, intersection)
else:
intersection = 0.7
fa = FunctionApproximatorRBFN(n_rfs, intersection)
# Train function approximator with data
fa.train(inputs, targets)
# Make predictions on a grid
n_samples_per_dim_grid = 200 if n_dims == 1 else [30, 30]
inputs_grid, _ = target_function(n_samples_per_dim_grid)
# Make predictions for the targets
outputs_grid = fa.predict(inputs_grid)
# Save the function approximator to a json file
basename = f"{fa_name}_{n_dims}D"
jc.savejson(Path(directory, f"{basename}.json"), fa)
jc.savejson_for_cpp(Path(directory, f"{basename}_for_cpp.json"), fa)
# Save the inputs to a directory
filename = os.path.join(directory, f"{basename}_inputs.txt")
np.savetxt(
filename,
inputs_grid) # noqa https://youtrack.jetbrains.com/issue/PY-35025
# Call the binary, which does analytical_solution and integration in C++
exec_name = "testFunctionApproximators"
arguments = f"{directory} {fa_name} {n_dims}"
execute_binary(exec_name, arguments)
outputs_grid_cpp = np.loadtxt(
os.path.join(directory, f"{basename}_outputs.txt"))
max_diff = np.max(np.abs(outputs_grid - outputs_grid_cpp))
if verbose:
print(f" max_diff = {max_diff} ({fa_name}, {n_dims}D)")
assert max_diff < 10e-7
if show or save:
h_pyt, ax = fa.plot(inputs, targets=targets)
if n_dims == 1:
h_cpp = ax.plot(inputs_grid, outputs_grid_cpp, "-")
elif n_dims == 2:
inputs_0_on_grid = np.reshape(inputs_grid[:, 0],
n_samples_per_dim_grid)
inputs_1_on_grid = np.reshape(inputs_grid[:, 1],
n_samples_per_dim_grid)
outputs_on_grid = np.reshape(outputs_grid_cpp,
n_samples_per_dim_grid)
h_cpp = ax.plot_wireframe(inputs_0_on_grid,
inputs_1_on_grid,
outputs_on_grid,
rstride=1,
cstride=1)
else:
raise ValueError(
f"Cannot plot input data with a dimensionality of {n_dims}")
plt.setp(h_pyt, linestyle="-", linewidth=4, color=(0.8, 0.8, 0.8))
plt.setp(h_cpp, linestyle="--", linewidth=2, color=(0.2, 0.2, 0.8))
plt.gcf().suptitle(basename)
if save:
plt.gcf().savefig(Path(directory, f"{basename}.png"))
def main(directory, **kwargs):
""" Main function of the script. """
show = kwargs.get("show", False)
save = kwargs.get("save", False)
verbose = kwargs.get("verbose", False)
directory.mkdir(parents=True, exist_ok=True)
###########################################################################
# Create all systems and add them to a dictionary
# ExponentialSystem
tau = 0.6 # Time constant
x_init_2d = np.array([0.5, 1.0])
x_attr_2d = np.array([0.8, 0.1])
x_init_1d = np.array([0.5])
x_attr_1d = np.array([0.8])
alpha = 6.0
dyn_systems = {} # noqa
dyn_systems["ExponentialSystem_1D"] = ExponentialSystem(
tau, x_init_1d, x_attr_1d, alpha)
dyn_systems["ExponentialSystem_2D"] = ExponentialSystem(
tau, x_init_2d, x_attr_2d, alpha)
# SigmoidSystem
max_rate = -10
inflection_ratio = 0.8
dyn_systems["SigmoidSystem_1D"] = SigmoidSystem(tau, x_init_1d, max_rate,
inflection_ratio)
dyn_systems["SigmoidSystem_2D"] = SigmoidSystem(tau, x_init_2d, max_rate,
inflection_ratio)
# SpringDamperSystem
alpha = 12.0
dyn_systems["SpringDamperSystem_1D"] = SpringDamperSystem(
tau, x_init_1d, x_attr_1d, alpha)
dyn_systems["SpringDamperSystem_2D"] = SpringDamperSystem(
tau, x_init_2d, x_attr_2d, alpha)
# TimeSystem
dyn_systems["TimeSystem"] = TimeSystem(tau)
# TimeSystem (but counting down instead of up)
count_down = True
dyn_systems["TimeCountDownSystem"] = TimeSystem(tau, count_down)
###########################################################################
# Start integration of all systems
# Settings for the integration of the system
dt = 0.01 # Integration step duration
integration_duration = 1.25 * tau # Integrate for longer than the time constant
n_time_steps = int(np.ceil(integration_duration / dt)) + 1
# 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)
# https://youtrack.jetbrains.com/issue/PY-35025
np.savetxt(os.path.join(directory, "ts.txt"), ts) # noqa
for name in dyn_systems.keys():
dyn_system = dyn_systems[name]
# Save the dynamical system to a json file
jc.savejson(Path(directory, f"{name}.json"), dyn_system)
jc.savejson_for_cpp(Path(directory, f"{name}_for_cpp.json"),
dyn_system)
# Call the binary, which does analytical_solution and integration in C++
exec_name = "testDynamicalSystems"
arguments = f"{directory} {name}"
execute_binary(exec_name, arguments)
if verbose:
print("===============")
print("Python Analytical solution")
xs, xds = dyn_system.analytical_solution(ts)
xs_cpp = np.loadtxt(os.path.join(directory, "xs_analytical.txt"))
xds_cpp = np.loadtxt(os.path.join(directory, "xds_analytical.txt"))
max_diff = np.max(np.abs(xs - np.reshape(xs_cpp, xs.shape)))
if verbose:
print(f" max_diff = {max_diff} ({name}System, Analytical)")
assert max_diff < 10e-7
if save or show:
fig1 = plt.figure(figsize=(10, 10))
plot_comparison(ts, xs, xds, xs_cpp, xds_cpp, fig1)
fig1.suptitle(f"{name}System - Analytical")
if verbose:
print("===============")
print("Python Integrating with Euler")
xs[0, :], xds[0, :] = dyn_system.integrate_start()
for ii in range(1, n_time_steps):
xs[ii, :], xds[ii, :] = dyn_system.integrate_step_euler(
dt, xs[ii - 1, :])
xs_cpp = np.loadtxt(os.path.join(directory, "xs_euler.txt"))
xds_cpp = np.loadtxt(os.path.join(directory, "xds_euler.txt"))
max_diff = np.max(np.abs(xs - np.reshape(xs_cpp, xs.shape)))
if verbose:
print(f" max_diff = {max_diff} ({name}System, Euler)")
assert max_diff < 10e-7
if save or show:
fig2 = plt.figure(figsize=(10, 10))
plot_comparison(ts, xs, xds, xs_cpp, xds_cpp, fig2)
fig2.suptitle(f"{name}System - Euler")
if verbose:
print("===============")
print("Python Integrating with Runge-Kutta")
xs[0, :], xds[0, :] = dyn_system.integrate_start()
for ii in range(1, n_time_steps):
xs[ii, :], xds[ii, :] = dyn_system.integrate_step_runge_kutta(
dt, xs[ii - 1, :])
xs_cpp = np.loadtxt(os.path.join(directory, "xs_rungekutta.txt"))
xds_cpp = np.loadtxt(os.path.join(directory, "xds_rungekutta.txt"))
max_diff = np.max(np.abs(xs - np.reshape(xs_cpp, xs.shape)))
if verbose:
print(f" max_diff = {max_diff} ({name}System, Runge-Kutta)")
assert max_diff < 10e-7
if save or show:
fig3 = plt.figure(figsize=(10, 10))
plot_comparison(ts, xs, xds, xs_cpp, xds_cpp, fig3)
fig3.suptitle(f"{name}System - Runge-Kutta")
if save:
fig1.savefig(Path(directory,
f"{name}System_analytical.png")) # noqa
fig2.savefig(Path(directory, f"{name}System_euler.png")) # noqa
fig2.savefig(Path(directory, f"{name}System_rungekutta.png"))
if show:
plt.show()
def main():
""" Main function that is called when executing the script. """
parser = argparse.ArgumentParser()
parser.add_argument("trajectory_file", help="file to read trajectory from")
parser.add_argument("output_directory",
help="directory to write dmp and other results to")
parser.add_argument("--n",
help="max number of basis functions",
type=int,
default=15)
parser.add_argument("--show", action="store_true", help="Show plots")
parser.add_argument("--save",
action="store_true",
help="save result plots to png")
args = parser.parse_args()
os.makedirs(args.output_directory, exist_ok=True)
################################################
# Read trajectory and train DMP with it.
print(f"Reading trajectory from: {args.trajectory_file}\n")
traj = Trajectory.loadtxt(args.trajectory_file)
filename_traj = Path(args.output_directory, "trajectory.txt")
traj.savetxt(filename_traj)
# jc.savejson(traj,Path(args.output_directory,'trajectory.json'))
n_dims = traj.dim
peak_to_peak = np.ptp(traj.ys, axis=0) # Range of data; used later on
mean_absolute_errors = []
n_bfs_list = list(range(3, args.n + 1))
for n_bfs in n_bfs_list:
function_apps = [
FunctionApproximatorRBFN(n_bfs, 0.7) for _ in range(n_dims)
]
dmp = Dmp.from_traj(traj,
function_apps,
dmp_type="KULVICIUS_2012_JOINING")
# These are the parameters that will be optimized.
dmp.set_selected_param_names("weights")
################################################
# Save DMP to file
d = args.output_directory
filename = Path(d, f"dmp_trained_{n_bfs}.json")
print(f"Saving trained DMP to: {filename}")
jc.savejson(filename, dmp)
jc.savejson_for_cpp(Path(d, f"dmp_trained_{n_bfs}_for_cpp.json"), dmp)
################################################
# Analytical solution to compute difference
ts = traj.ts
xs_ana, xds_ana, _, _ = dmp.analytical_solution(ts)
traj_reproduced_ana = dmp.states_as_trajectory(ts, xs_ana, xds_ana)
mae = np.mean(abs(traj.ys - traj_reproduced_ana.ys))
mean_absolute_errors.append(mae)
print()
print(f" Number of basis functions: {n_bfs}")
print(f"MAE between demonstration and reproduced: {mae}")
print(f" Range of data: {peak_to_peak}")
print()
################################################
# Integrate DMP
tau_exec = 1.3 * traj.duration
dt = 0.01
n_time_steps = int(tau_exec / dt)
ts = np.zeros([n_time_steps, 1])
xs_step = np.zeros([n_time_steps, dmp.dim_x])
xds_step = np.zeros([n_time_steps, dmp.dim_x])
x, xd = dmp.integrate_start()
xs_step[0, :] = x
xds_step[0, :] = xd
for tt in range(1, n_time_steps):
ts[tt] = dt * tt
xs_step[tt, :], xds_step[tt, :] = dmp.integrate_step(
dt, xs_step[tt - 1, :])
traj_reproduced = dmp.states_as_trajectory(ts, xs_step, xds_step)
if args.show or args.save:
################################################
# Plot results
# h, axs = dmp.plot(dmp.tau,ts,xs_step,xds_step)
# fig.canvas.set_window_title(f'Step-by-step integration (n_bfs={n_bfs})')
# fig.savefig(Path(args.output_directory,f'dmp_trained_{n_bfs}.png'))
h_demo, axs = traj.plot()
h_repr, _ = traj_reproduced.plot(axs)
d = "demonstration"
plt.setp(h_demo,
linestyle="-",
linewidth=4,
color=(0.8, 0.8, 0.8),
label=d)
plt.setp(h_repr,
linestyle="--",
linewidth=2,
color=(0.0, 0.0, 0.5),
label="reproduced")
plt.legend()
plt.gcf().canvas.set_window_title(
f"Comparison {d}/reproduced (n_bfs={n_bfs})")
plt.gcf().suptitle(f"Comparison {d}/reproduced (n_bfs={n_bfs})")
if args.save:
plt.gcf().savefig(
Path(args.output_directory,
f"trajectory_comparison_{n_bfs}.png"))
if args.show or args.save:
if len(n_bfs_list) > 1:
# Plot the mean absolute error
ax = plt.figure().add_subplot(111)
print(n_bfs_list)
print(mean_absolute_errors)
ax.plot(n_bfs_list, mean_absolute_errors)
ax.set_xlabel("number of basis functions")
ax.set_ylabel(
"mean absolute error between demonstration and reproduced")
filename = "mean_absolute_errors.png"
if args.save:
plt.gcf().savefig(Path(args.output_directory, filename))
if args.show:
plt.show()
def main():
""" Main function that is called when executing the script. """
parser = argparse.ArgumentParser()
parser.add_argument("dmp", help="input dmp")
parser.add_argument("output_directory",
help="directory to write results to")
parser.add_argument("--sigma",
help="sigma of covariance matrix",
type=float,
default=3.0)
parser.add_argument("--n", help="number of samples", type=int, default=10)
parser.add_argument("--traj",
action="store_true",
help="integrate DMP and save trajectory")
parser.add_argument("--show",
action="store_true",
help="show result plots")
parser.add_argument("--save",
action="store_true",
help="save result plots to png")
args = parser.parse_args()
sigma_dir = "sigma_%1.3f" % args.sigma
directory = Path(args.output_directory, sigma_dir)
filename = args.dmp
print(f"Loading DMP from: {filename}")
dmp = jc.loadjson(filename)
ts = dmp.ts_train
parameter_vector = dmp.get_param_vector()
n_samples = args.n
sigma = args.sigma
covar_init = sigma * sigma * np.eye(parameter_vector.size)
distribution = DistributionGaussian(parameter_vector, covar_init)
filename = Path(directory, f"distribution.json")
print(f"Saving sampling distribution to: {filename}")
os.makedirs(directory, exist_ok=True)
jc.savejson(filename, distribution)
samples = distribution.generate_samples(n_samples)
if args.show or args.save:
fig = plt.figure()
ax1 = fig.add_subplot(121) # noqa
distribution.plot(ax1)
ax1.plot(samples[:, 0], samples[:, 1], "o", color="#BBBBBB")
ax2 = fig.add_subplot(122)
xs, xds, _, _ = dmp.analytical_solution()
traj_mean = dmp.states_as_trajectory(ts, xs, xds)
lines, _ = traj_mean.plot([ax2])
plt.setp(lines, linewidth=4, color="#007700")
for i_sample in range(n_samples):
dmp.set_param_vector(samples[i_sample, :])
filename = Path(directory, f"{i_sample:02}_dmp")
print(f"Saving sampled DMP to: {filename}.json")
jc.savejson(str(filename) + ".json", dmp)
jc.savejson_for_cpp(str(filename) + "_for_cpp.json", dmp)
if args.show or args.save or args.traj:
xs, xds, forcing, fa_outputs = dmp.analytical_solution()
traj_sample = dmp.states_as_trajectory(ts, xs, xds)
if args.traj:
filename = Path(directory, f"{i_sample:02}_traj.txt")
print(f"Saving sampled trajectory to: {filename}")
traj_sample.savetxt(filename)
if args.show or args.save:
lines, _ = traj_sample.plot([ax2]) # noqa
plt.setp(lines, color="#BBBBBB", alpha=0.5)
if args.save:
filename = "exploration_dmp_traj.png"
plt.gcf().savefig(Path(directory, filename))
if args.show:
plt.show()
def main(directory, **kwargs):
""" Main function of the script. """
show = kwargs.get("show", False)
save = kwargs.get("save", False)
verbose = kwargs.get("verbose", False)
directory.mkdir(parents=True, exist_ok=True)
################################
# Read trajectory and train DMP with it.
# trajectory_file = Path("..", "fixtures", "trajectory.txt")
# if verbose:
# print(f"Reading trajectory from: {trajectory_file}\n")
# traj = Trajectory.loadtxt(trajectory_file)
traj = get_trajectory()
n_dims = traj.dim
n_bfs = 10
function_apps = [
FunctionApproximatorRBFN(n_bfs, 0.7) for _ in range(n_dims)
]
dmp = Dmp.from_traj(traj, function_apps, dmp_type="KULVICIUS_2012_JOINING")
################
# Analytical solution to compute difference
if verbose:
print("===============\nPython Analytical solution")
ts = traj.ts
xs_ana, xds_ana, forcing_terms_ana, fa_outputs_ana = dmp.analytical_solution(
ts)
################################################
# Numerically integrate the DMP
if verbose:
print("===============\nPython Numerical integration")
n_time_steps = len(ts)
dim_x = xs_ana.shape[1]
xs_step = np.zeros([n_time_steps, dim_x])
xds_step = np.zeros([n_time_steps, dim_x])
x, xd = dmp.integrate_start()
xs_step[0, :] = x
xds_step[0, :] = xd
for tt in range(1, n_time_steps):
dt = ts[tt] - ts[tt - 1]
xs_step[tt, :], xds_step[tt, :] = dmp.integrate_step_runge_kutta(
dt, xs_step[tt - 1, :])
################################################
# Call the binary, which does analytical_solution and numerical integration in C++
# Save the dynamical system to a json file
jc.savejson(Path(directory, "dmp.json"), dmp)
jc.savejson_for_cpp(Path(directory, "dmp_for_cpp.json"), dmp)
np.savetxt(Path(directory, "ts.txt"), ts)
exec_name = "testDmp"
execute_binary(exec_name, f"{directory} dmp")
if verbose:
print("===============\nPython reading output from C++")
d = directory
xs_ana_cpp = np.loadtxt(os.path.join(d, "xs_ana.txt"))
xds_ana_cpp = np.loadtxt(os.path.join(d, "xds_ana.txt"))
forcing_terms_ana_cpp = np.loadtxt(os.path.join(d,
"forcing_terms_ana.txt"))
fa_outputs_ana_cpp = np.loadtxt(os.path.join(d, "fa_outputs_ana.txt"))
xs_step_cpp = np.loadtxt(os.path.join(d, "xs_step.txt"))
xds_step_cpp = np.loadtxt(os.path.join(d, "xds_step.txt"))
max_diff_ana = np.max(np.abs(xs_ana -
np.reshape(xs_ana_cpp, xs_ana.shape)))
max_diff_step = np.max(
np.abs(xs_step - np.reshape(xs_step_cpp, xs_step.shape)))
if verbose:
print(f" max_diff_ana = {max_diff_ana}")
print(f" max_diff_step = {max_diff_step}")
assert max_diff_ana < 10e-7
assert max_diff_step < 10e-7
# Plotting
if save or show:
if verbose:
print("===============\nPython Plotting")
h_pyt, axs1 = dmp.plot(ts,
xs_ana,
xds_ana,
forcing_terms=forcing_terms_ana,
fa_outputs=fa_outputs_ana)
h_cpp, _ = dmp.plot(
ts,
xs_ana_cpp,
xds_ana_cpp,
axs=axs1,
forcing_terms=forcing_terms_ana_cpp,
fa_outputs=fa_outputs_ana_cpp,
)
plt.setp(h_pyt,
linestyle="-",
linewidth=4,
color=(0.8, 0.8, 0.8),
label="Python")
plt.setp(h_cpp,
linestyle="--",
linewidth=2,
color=(0.2, 0.2, 0.8),
label="C++")
axs1[0].legend()
plt.gcf().suptitle("Analytical solution")
if save:
plt.gcf().savefig(Path(directory, "analytical.png"))
h_diff, axs1d = dmp.plot(
ts,
xs_ana - xs_ana_cpp,
xds_ana - xds_ana_cpp,
forcing_terms=forcing_terms_ana - forcing_terms_ana_cpp,
fa_outputs=fa_outputs_ana - fa_outputs_ana_cpp,
plot_tau=False,
)
plt.setp(h_diff, linestyle="-", linewidth=1, color=(0.8, 0.2, 0.2))
plt.gcf().suptitle("Analytical solution (diff)")
if save:
plt.gcf().savefig(Path(directory, "analytical_diff.png"))
h_pyt, axs2 = dmp.plot(ts, xs_step, xds_step)
h_cpp, _ = dmp.plot(ts, xs_step_cpp, xds_step_cpp, axs=axs2)
plt.setp(h_pyt,
linestyle="-",
linewidth=4,
color=(0.8, 0.8, 0.8),
label="Python")
plt.setp(h_cpp,
linestyle="--",
linewidth=2,
color=(0.2, 0.2, 0.8),
label="C++")
axs2[0].legend()
plt.gcf().suptitle("Numerical integration")
if save:
plt.gcf().savefig(Path(directory, "numerical.png"))
h_diff, axs2d = dmp.plot(ts,
xs_step - xs_step_cpp,
xds_step - xds_step_cpp,
plot_tau=False)
plt.setp(h_diff, linestyle="-", linewidth=1, color=(0.8, 0.2, 0.2))
plt.gcf().suptitle("Numerical integration (diff)")
if save:
plt.gcf().savefig(Path(directory, "numerical_diff.png"))
if show:
plt.show()