diff --git a/README.md b/README.md
index 274a7d01bbc2b7fa3e3c2e05cdf85d3107e888dc..fa10352acc835d7d9d6f57f8e9f8e54c11101669 100644
--- a/README.md
+++ b/README.md
@@ -96,6 +96,12 @@ See [examples.md](./examples.md)
### Gallery
+[demo\_demo\_ergodicControl\_2D01.cpp](./src/demo_ergodicControl_2D01.cpp)
+
+
+
+***
+
[demo\_GMR01.cpp](./src/demo_GMR01.cpp)
@@ -108,6 +114,24 @@ See [examples.md](./examples.md)
***
+[demo\_HSMM\_batchLQR01.cpp](./src/demo_HSMM_batchLQR01.cpp)
+
+
+
+***
+
+[demo\_LWR\_batch01.cpp](./src/demo_LWR_batch01.cpp)
+
+
+
+***
+
+[demo\_LWR\_iterative01.cpp](./src/demo_LWR_iterative01.cpp)
+
+
+
+***
+
[demo\_MPC\_batch01.cpp](./src/demo_MPC_batch01.cpp)
diff --git a/examples.md b/examples.md
index 83d8dcab1babc6c46cd0af982b5c7af6b7a49618..1136581deedc590967341b87b7c95ae20c99730b 100644
--- a/examples.md
+++ b/examples.md
@@ -156,7 +156,12 @@ This project will build a number of executables, as listed in the table below.
| Filename | ref. | Description |
|----------|------|-------------|
+| [demo_ergodicControl_2D01.cpp](./src/demo_ergodicControl_2D01.cpp) | [] | 2D ergodic control with spectral multiscale coverage (SMC) algorithm |
| [demo_GMR01.cpp](./src/demo_GMR01.cpp) | [[1]](#ref-1) | Gaussian mixture model (GMM) and time-based Gaussian mixture regression (GMR) used for reproduction |
+| [demo_GPR01.cpp](./src/demo_GPR01.cpp) | [[1]](#ref-1) | Gaussian process regression (GPR) |
+| [demo_HSMM_batchLQR01.cpp](./src/demo_HSMM_batchLQR01.cpp) | [[2]](#ref-2) | Use of HSMM (with lognormal duration model) and batch LQR (with position only) for motion synthesis |
+| [demo_LWR_batch01.cpp](./src/demo_LWR_batch01.cpp) | [[1]](#ref-1) | Locally weighted regression (LWR) with radial basis functions (RBF), using batch computation |
+| [demo_LWR_iterative01.cpp](./src/demo_LWR_iterative01.cpp) | [[1]](#ref-1) | Locally weighted regression (LWR) with radial basis functions (RBF), using iterative computation |
| [demo_MPC_batch01.cpp](./src/demo_MPC_batch01.cpp) | [[1]](#ref-1), [[8]](#ref-8) | Model predictive control (MPC) with batch linear quadratic tracking (LQT) formulation |
| [demo_MPC_iterative01.cpp](./src/demo_MPC_iterative01.cpp) | [[1]](#ref-1), [[8]](#ref-8) | Model predictive control (MPC) with iterative linear quadratic tracking (LQT) formulation |
| [demo_MPC_semitied01.cpp](./src/demo_MPC_semitied01.cpp) | [[5]](#ref-5), [[8]](#ref-8) | MPC with semi-tied covariances |
diff --git a/images/demo_HSMM_batchLQR01.gif b/images/demo_HSMM_batchLQR01.gif
new file mode 100644
index 0000000000000000000000000000000000000000..e75d72aae8e62540b717a99b10348ac51a71e05f
Binary files /dev/null and b/images/demo_HSMM_batchLQR01.gif differ
diff --git a/images/demo_LWR_batch01.gif b/images/demo_LWR_batch01.gif
new file mode 100644
index 0000000000000000000000000000000000000000..85eec0020cab975e295d8e1787fc0191ea8f2d4d
Binary files /dev/null and b/images/demo_LWR_batch01.gif differ
diff --git a/images/demo_LWR_iterative01.gif b/images/demo_LWR_iterative01.gif
new file mode 100644
index 0000000000000000000000000000000000000000..a5acda716efd2e30cb39ed2502df7ff40d02a329
Binary files /dev/null and b/images/demo_LWR_iterative01.gif differ
diff --git a/images/demo_MPC_iterative01.gif b/images/demo_MPC_iterative01.gif
index cbe3974e58509ac06d8233f2e94904271a1d17f9..5298143568038475659d819f0eef17f2e280868c 100644
Binary files a/images/demo_MPC_iterative01.gif and b/images/demo_MPC_iterative01.gif differ
diff --git a/images/demo_MPC_semitied01.gif b/images/demo_MPC_semitied01.gif
index d37e73c54190561de448c3d3679fedd520b8dd55..7cc87588c0cb03b188a5b67eb773b6f36e515a6d 100644
Binary files a/images/demo_MPC_semitied01.gif and b/images/demo_MPC_semitied01.gif differ
diff --git a/images/demo_MPC_velocity01.gif b/images/demo_MPC_velocity01.gif
index a944fc448fee63fb3254de89f8d17eb66fe1e0d4..7f00705fbe2e2263f6f2534b0435006494fa1b50 100644
Binary files a/images/demo_MPC_velocity01.gif and b/images/demo_MPC_velocity01.gif differ
diff --git a/images/demo_TPGMMProduct01.gif b/images/demo_TPGMMProduct01.gif
index 824d935a70aef2ebe3c05527a6541160a39f8df5..4a44bb364091e72a15ac5f5e8a420b118ff1338e 100644
Binary files a/images/demo_TPGMMProduct01.gif and b/images/demo_TPGMMProduct01.gif differ
diff --git a/images/demo_TPGMR01.gif b/images/demo_TPGMR01.gif
index 8801dcaa8c9c3d6af3cc0ee0b08b7f355a2a0fa6..a7105bf3d6435e74405fccb2ef2b3640d6967604 100644
Binary files a/images/demo_TPGMR01.gif and b/images/demo_TPGMR01.gif differ
diff --git a/images/demo_TPbatchLQR01.gif b/images/demo_TPbatchLQR01.gif
index e1fa02c9ff96190f8dc5061eb61a7a93b962b87e..78125f45b78704b861db2bc0d4fbf50110bbeae1 100644
Binary files a/images/demo_TPbatchLQR01.gif and b/images/demo_TPbatchLQR01.gif differ
diff --git a/images/demo_ergodicControl_2D01.gif b/images/demo_ergodicControl_2D01.gif
new file mode 100644
index 0000000000000000000000000000000000000000..9e3863fd82d809a6d801097d65cacee86a61b563
Binary files /dev/null and b/images/demo_ergodicControl_2D01.gif differ
diff --git a/src/demo_HSMM_batchLQR01.cpp b/src/demo_HSMM_batchLQR01.cpp
index c5dc42041e8ba337400c434f9dd0efb76ba0be7a..d6c93bfcab69ef7aa18faf26bded92793099e914 100644
--- a/src/demo_HSMM_batchLQR01.cpp
+++ b/src/demo_HSMM_batchLQR01.cpp
@@ -123,7 +123,7 @@ void EM_HMM(const matrix_list_t& data, model_t &model) {
const int nb_min_steps = 5; // Minimum number of iterations allowed
const double max_diff_log_likelihood = 1e-4; // Likelihood increase threshold
// to stop the algorithm
- const double diag_reg_fact = 1e-8; //Regularization term
+ const double diag_reg_fact = 1e-4; //Regularization term
mat all_data(nb_var, nb_data);
for (int i = 0; i < nb_samples; ++i)
@@ -322,9 +322,9 @@ mat compute_LQR(const model_t& model, const vec& start_point) {
//----------------------------------------------
// Precomputation of duration probabilities
- mat Pd(model.parameters.nb_states, nbD);
+ mat Pd(model.parameters.nb_states, nbD + 1);
- vec logs = log(linspace<vec>(0, nbD - 1, nbD));
+ vec logs = log(linspace<vec>(0, nbD, nbD + 1));
for (int i = 0; i < model.parameters.nb_states; ++i)
Pd(i, span::all) = gaussPDF(logs.t(), Mu_Pd[i], Sigma_Pd[i]).t();
@@ -337,8 +337,8 @@ mat compute_LQR(const model_t& model, const vec& start_point) {
if (t < nbD)
h(i, t) = model.states_priors(i) * Pd(i, t);
- for (int d = 0; d < std::min(t - 1, nbD); ++d)
- h(i, t) = h(i, t) + mat(h(span::all, t - d).t() * model.transitions(span::all, i) * Pd(i, d))(0, 0);
+ for (int d = 1; d <= std::min(t, nbD); ++d)
+ h(i, t) = h(i, t) + mat(h(span::all, t - d).t() * model.transitions(span::all, i) * Pd(i, d - 1))(0, 0);
}
}
@@ -379,7 +379,6 @@ mat compute_LQR(const model_t& model, const vec& start_point) {
// Create single Gaussian N(MuQ,SigmaQ) based on optimal state sequence q, see Eq. (27)
urowvec qList = index_max(h, 0);
-
mat MuQ(nb_var_pos, qList.size());
mat sigma_(nb_var_pos, nb_var_pos * qList.size());
@@ -426,9 +425,6 @@ const mat COLORS({
{ 0.75, 0.0, 0.75 },
{ 0.75, 0.75, 0.0 },
{ 0.25, 0.25, 0.25 },
- { 0.0, 0.0, 1.0 },
- { 0.0, 0.5, 0.0 },
- { 1.0, 0.0, 0.0 },
});
@@ -436,7 +432,7 @@ const mat COLORS({
// Create a demonstration (with a length of 'timestamps.size()') from a
// trajectory (of any length)
//-----------------------------------------------------------------------------
-mat sample_trajectory(const vector_list_t& trajectory, const vec& time_steps) {
+mat sample_trajectory(const vector_list_t& trajectory, int nb_data) {
// Resampling of the trajectory
vec x(trajectory.size());
@@ -450,14 +446,14 @@ mat sample_trajectory(const vector_list_t& trajectory, const vec& time_steps) {
}
vec from_indices = linspace<vec>(0, trajectory.size() - 1, trajectory.size());
- vec to_indices = linspace<vec>(0, trajectory.size() - 1, time_steps.size());
+ vec to_indices = linspace<vec>(0, trajectory.size() - 1, nb_data);
interp1(from_indices, x, to_indices, x2, "*linear");
interp1(from_indices, y, to_indices, y2, "*linear");
// Create the demonstration
- mat demo(2, time_steps.size());
- for (int i = 0; i < time_steps.size(); ++i) {
+ mat demo(2, nb_data);
+ for (int i = 0; i < nb_data; ++i) {
demo(0, i) = x2[i];
demo(1, i) = y2[i];
}
@@ -485,6 +481,7 @@ struct gui_state_t {
// in parameters_t)
int parameter_nb_data;
int parameter_nb_states;
+ float parameter_rfactor;
};
@@ -499,7 +496,7 @@ int main(int argc, char **argv){
// Parameters
model.parameters.nb_data = 200;
model.parameters.nb_states = 6;
- model.parameters.rfactor = 1e-3;
+ model.parameters.rfactor = 1e-2;
model.parameters.dt = 0.01;
@@ -548,10 +545,10 @@ int main(int argc, char **argv){
gui_state.must_recompute = false;
gui_state.parameter_nb_data = model.parameters.nb_data;
gui_state.parameter_nb_states = model.parameters.nb_states;
+ gui_state.parameter_rfactor = model.parameters.rfactor;
// Main loop
- vec time_steps;
vector_list_t current_trajectory;
std::vector<vector_list_t> original_trajectories;
matrix_list_t demonstrations;
@@ -588,7 +585,6 @@ int main(int argc, char **argv){
}
gui_state.are_parameters_modified = true;
- time_steps.clear();
}
}
@@ -596,16 +592,12 @@ int main(int argc, char **argv){
// If the parameters changed, learn the model again
if (gui_state.are_parameters_modified) {
- if (time_steps.size() != gui_state.parameter_nb_data) {
+ if (!demonstrations.empty() && (demonstrations[0].n_cols != gui_state.parameter_nb_data)) {
demonstrations.clear();
- time_steps = linspace<vec>(
- 0, gui_state.parameter_nb_data - 1, gui_state.parameter_nb_data
- );
-
for (size_t i = 0; i < original_trajectories.size(); ++i) {
mat sampled_trajectory = sample_trajectory(original_trajectories[i],
- time_steps);
+ gui_state.parameter_nb_data);
sampled_trajectory.row(0) /= window_size.fb_width;
sampled_trajectory.row(1) /= window_size.fb_height;
@@ -615,6 +607,7 @@ int main(int argc, char **argv){
model.parameters.nb_data = gui_state.parameter_nb_data;
model.parameters.nb_states = gui_state.parameter_nb_states;
+ model.parameters.rfactor = gui_state.parameter_rfactor;
gui_state.are_parameters_modified = false;
gui_state.must_recompute = !demonstrations.empty();
@@ -663,7 +656,7 @@ int main(int argc, char **argv){
});
gfx2::draw_gaussian(
- conv_to<fvec>::from(COLORS.row(i % 10).t()), mu,
+ conv_to<fvec>::from(COLORS.row(i % COLORS.n_rows).t()), mu,
scaling * model.sigma[i] * scaling.t()
);
}
@@ -700,7 +693,7 @@ int main(int argc, char **argv){
// Control panel GUI
ImGui::SetNextWindowPos(ImVec2(2,2));
- ImGui::SetNextWindowSize(ImVec2(300, 116));
+ ImGui::SetNextWindowSize(ImVec2(500, 126));
ImGui::Begin("Control Panel", NULL,
ImGuiWindowFlags_NoTitleBar|ImGuiWindowFlags_NoResize|
@@ -708,9 +701,9 @@ int main(int argc, char **argv){
);
ImGui::Text("Left-click to collect demonstrations");
- ImGui::Text("");
ImGui::SliderInt("Nb states", &gui_state.parameter_nb_states, 2, 20);
ImGui::SliderInt("Nb data", &gui_state.parameter_nb_data, 100, 300);
+ ImGui::SliderFloat("LQR control cost", &gui_state.parameter_rfactor, 1e-3, 1e-1);
if (ImGui::Button("Apply"))
gui_state.are_parameters_modified = true;
@@ -771,7 +764,10 @@ int main(int argc, char **argv){
gui_state.is_drawing_demonstration = false;
if (current_trajectory.size() > 1) {
- mat sampled_trajectory = sample_trajectory(current_trajectory, time_steps);
+ mat sampled_trajectory = sample_trajectory(
+ current_trajectory, gui_state.parameter_nb_data
+ );
+
sampled_trajectory.row(0) /= window_size.fb_width;
sampled_trajectory.row(1) /= window_size.fb_height;
diff --git a/src/demo_LWR_batch01.cpp b/src/demo_LWR_batch01.cpp
index 61143d4ede1aa9ce43479d652a59202236941451..731aab2a6d07eaf157c13d1d3acf22a626264ade 100644
--- a/src/demo_LWR_batch01.cpp
+++ b/src/demo_LWR_batch01.cpp
@@ -2,7 +2,7 @@
* demo_LWR_batch01.cpp
*
* Locally weighted regression (LWR) with radial basis functions (RBF), using batch
- * computation
+ * computation
*
* Authors: Sylvain Calinon, Philip Abbet
*/
@@ -568,7 +568,7 @@ int main(int argc, char **argv) {
// Recompute the LWR (if necessary)
- if (gui_state.must_recompute_LWR) {
+ if (gui_state.must_recompute_LWR && (demonstration.n_cols > 0)) {
std::tie(reproduction, H) = compute_LWR(parameters, demonstration);
gui_state.must_recompute_LWR = false;
}
diff --git a/src/demo_LWR_iterative01.cpp b/src/demo_LWR_iterative01.cpp
index cda48b99ccf0c9b56bad4d0bad55005e64e793fd..df3ec34d801c88e258e6fc9835be747de58a2387 100644
--- a/src/demo_LWR_iterative01.cpp
+++ b/src/demo_LWR_iterative01.cpp
@@ -251,15 +251,24 @@ void draw_demo_viewport(const viewport_t& viewport,
glLoadIdentity();
// Draw the demonstration
- if (current_trajectory.size() > 1)
+ int reproduction_size;
+
+ if (current_trajectory.size() > 1) {
gfx2::draw_line(arma::fvec({0.33f, 0.97f, 0.33f}), current_trajectory);
- else if (demonstration.n_cols > 0)
+ reproduction_size = current_trajectory.size();
+ }
+ else if (demonstration.n_cols > 0) {
gfx2::draw_line({0.0f, 0.0f, 0.0f}, demonstration);
+ reproduction_size = demonstration.n_cols;
+ }
// Draw the reproduction
if (!reproductions.empty()) {
glLineWidth(4.0f);
- gfx2::draw_line({1.0f, 0.0f, 0.0f}, reproductions[reproductions.size() - 1]);
+ gfx2::draw_line(
+ {1.0f, 0.0f, 0.0f},
+ reproductions[reproductions.size() - 1](span::all, span(0, reproduction_size- 1 ))
+ );
glLineWidth(1.0f);
}
}
@@ -475,7 +484,7 @@ int main(int argc, char **argv) {
// Parameters
parameters_t parameters;
- parameters.nb_RBF = 8;
+ parameters.nb_RBF = 16;
parameters.nb_data = 100;
@@ -570,7 +579,7 @@ int main(int argc, char **argv) {
// If the parameters changed, learn the model again
if (gui_state.are_parameters_modified) {
- if (parameters.nb_data > gui_state.parameter_nb_data)
+ if ((parameters.nb_data > gui_state.parameter_nb_data) && (demonstration.n_cols > 0))
demonstration = demonstration(span::all, span(0, gui_state.parameter_nb_data - 1));
parameters.nb_RBF = gui_state.parameter_nb_RBF;
diff --git a/src/demo_ergodicControl_2D01.cpp b/src/demo_ergodicControl_2D01.cpp
index d6bc3f9cec810aa4f9e8a241a523e96fe7675db6..1d4bbc18c31d8837e7bd4ce6e3517f39657a7a34 100644
--- a/src/demo_ergodicControl_2D01.cpp
+++ b/src/demo_ergodicControl_2D01.cpp
@@ -179,11 +179,15 @@ static void error_callback(int error, const char* description){
//-----------------------------------------------
std::tuple<vec, mat> trans2d_to_gauss(const ui::Trans2d& gaussian_transforms,
- const gfx2::window_size_t& window_size) {
+ const gfx2::window_size_t& window_size,
+ int background_size) {
vec mu = gfx2::ui2fb_centered(vec({ gaussian_transforms.pos.x, gaussian_transforms.pos.y }),
window_size);
+ mu(0) *= (float) window_size.fb_width / background_size;
+ mu(1) *= (float) window_size.fb_height / background_size;
+
vec t_x({
gaussian_transforms.x.x * window_size.scale_x(),
gaussian_transforms.x.y * window_size.scale_y()
@@ -228,7 +232,8 @@ void gauss_to_trans2d(const vec& mu, const mat& sigma,
//-----------------------------------------------
std::tuple<mat, cube, std::vector<ui::Trans2d> > create_random_gaussians(
- const parameters_t& parameters, const gfx2::window_size_t& window_size) {
+ const parameters_t& parameters, const gfx2::window_size_t& window_size,
+ int background_size) {
mat Mu = randu(2, parameters.nb_gaussians);
Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
@@ -260,7 +265,10 @@ std::tuple<mat, cube, std::vector<ui::Trans2d> > create_random_gaussians(
vec mu_;
mat sigma_;
- std::tie(mu_, sigma_) = trans2d_to_gauss(gaussians[i], window_size);
+
+ std::tie(mu_, sigma_) = trans2d_to_gauss(
+ gaussians[i], window_size, background_size
+ );
Sigma.slice(i) = sigma_;
}
@@ -338,6 +346,7 @@ int main(int argc, char **argv){
const float speed = 1.0f / 20.0f;
float t = 0.0f;
bool must_recompute = false;
+ int background_size;
gfx2::texture_t background_texture = {0};
@@ -360,6 +369,8 @@ int main(int argc, char **argv){
glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+ background_size = std::max(window_size.fb_width, window_size.fb_height);
+
// Move and rescale the various objects so they stay in the window
if (!first) {
float scale = std::min((float) window_size.fb_width / previous_fb_width,
@@ -384,7 +395,9 @@ int main(int argc, char **argv){
// At the very first frame: random initialisation of the gaussians (taking 4K
// screens into account)
else if ((window_size.fb_width > 0) && (window_size.win_width > 0)) {
- std::tie(Mu, Sigma, gaussians) = create_random_gaussians(parameters, window_size);
+ std::tie(Mu, Sigma, gaussians) = create_random_gaussians(
+ parameters, window_size, background_size
+ );
}
must_recompute = true;
@@ -429,13 +442,13 @@ int main(int argc, char **argv){
}
}
- gfx2::draw_rectangle(background_texture, window_size.fb_width, window_size.fb_height);
+ gfx2::draw_rectangle(background_texture, background_size, background_size);
glClear(GL_DEPTH_BUFFER_BIT);
mat result2(result.n_rows, result.n_cols);
- result2(0, span::all) = (result(0, span::all) - 0.5) * window_size.fb_width;
- result2(1, span::all) = (result(1, span::all) - 0.5) * window_size.fb_height;
+ result2(0, span::all) = (result(0, span::all) - 0.5) * background_size;
+ result2(1, span::all) = (result(1, span::all) - 0.5) * background_size;
gfx2::draw_line(fvec({ 0.0f, 0.0f, 1.0f }), result2);
@@ -488,8 +501,11 @@ int main(int argc, char **argv){
// Gaussian widgets
- if (parameters.nb_gaussians != gaussians.size())
- std::tie(Mu, Sigma, gaussians) = create_random_gaussians(parameters, window_size);
+ if (parameters.nb_gaussians != gaussians.size()) {
+ std::tie(Mu, Sigma, gaussians) = create_random_gaussians(
+ parameters, window_size, background_size
+ );
+ }
ui::begin("Gaussian");
@@ -501,7 +517,10 @@ int main(int argc, char **argv){
vec mu;
mat sigma;
- std::tie(mu, sigma) = trans2d_to_gauss(gaussians[i], window_size);
+
+ std::tie(mu, sigma) = trans2d_to_gauss(
+ gaussians[i], window_size, background_size
+ );
Mu.col(i) = mu;
Sigma.slice(i) = sigma;
@@ -525,7 +544,16 @@ int main(int argc, char **argv){
mu(0, span::all) = mu(0, span::all) / window_size.fb_width + 0.5;
mu(1, span::all) = mu(1, span::all) / window_size.fb_height + 0.5;
- std::tie(result, G) = compute(parameters, mu, Sigma / (window_size.fb_width * window_size.fb_height));
+ mat scaling({
+ { 1.0 / background_size, 0.0 },
+ { 0.0, 1.0 / background_size }
+ });
+
+ cube sigma(Sigma.n_rows, Sigma.n_cols, Sigma.n_slices);
+ for (int i = 0; i < Sigma.n_slices; ++i)
+ sigma.slice(i) = scaling * Sigma.slice(i) * scaling.t();
+
+ std::tie(result, G) = compute(parameters, mu, sigma);
t = 0.0f;
if (background_texture.width > 0)