Skip to content
Snippets Groups Projects
Commit 57b3e096 authored by Sylvain CALINON's avatar Sylvain CALINON
Browse files

List of examples added

parent f6f1612a
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 1
− 17
View file @ 57b3e096
......@@ -172,22 +172,6 @@ target_link_libraries(demo_LQR_infHor
${PBD_LIB}
)
add_executable(demo_proMP01
${PROJECT_SOURCE_DIR}/src/imgui_impl_glfw_gl3.cpp
${PROJECT_SOURCE_DIR}/src/gfx_ui.cpp
${PROJECT_SOURCE_DIR}/src/gfx3.cpp
${PROJECT_SOURCE_DIR}/src/imgui.cpp
${PROJECT_SOURCE_DIR}/src/imgui_draw.cpp
${PROJECT_SOURCE_DIR}/src/demo_proMP01.cpp
)
target_link_libraries(demo_proMP01
${OPENGL_LIBRARIES}
${GLEW_LIBRARIES}
${GLFW_LIB}
${ARMADILLO_LIBRARIES}
${PBD_LIB}
)
add_executable(demo_Riemannian_cov_interp02
${PROJECT_SOURCE_DIR}/src/imgui.cpp
${PROJECT_SOURCE_DIR}/src/imgui_impl_glfw.cpp
......@@ -342,4 +326,4 @@ target_link_libraries(demo_teleop
${ARMADILLO_LIBRARIES}
${GLFW_LIB}
${PBD_LIB}
)
\ No newline at end of file
)
README.md
+ 146
− 21
View file @ 57b3e096
......@@ -51,36 +51,161 @@ make
**Note:** The `-DBUILD_SHARED_LIBS` is necessary otherwise cmake will create a
static library.
### Execution
This project will build a number of executables, as listed below:
### References
- *demo_dptpgmm*: online tp-gmm learning and lqr-based trajectory generation.
Did you find PbDLib useful for your research? Please acknowledge the authors in any academic publications that used parts of these codes.
<br><br>
- *demo_glsl*: Minimalist example with glsl display
[1] Tutorial (GMM, TP-GMM, MFA, MPPCA, GMR, LWR, GPR, MPC, LQR, trajGMM):
```
@article{Calinon16JIST,
author="Calinon, S.",
title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
journal="Intelligent Service Robotics",
publisher="Springer Berlin Heidelberg",
year="2016",
volume="9",
number="1",
pages="1--29",
doi="10.1007/s11370-015-0187-9",
}
```
- *demo_LQR_infHor*: Minimalist example of discrete infinite horizon linear
quadratic regulation.
[2] HMM, HSMM:
```
@article{Rozo16Frontiers,
author="Rozo, L. and Silv\'erio, J. and Calinon, S. and Caldwell, D. G.",
title="Learning Controllers for Reactive and Proactive Behaviors in Human-Robot Collaboration",
journal="Frontiers in Robotics and {AI}",
year="2016",
month="June",
volume="3",
number="30",
pages="1--11",
doi="10.3389/frobt.2016.00030"
}
```
- *demo_online_gmm*: online gmm learning and lqr-based trajectory generation.
[3] Riemannian manifolds (S2,S3):
```
@article{Zeestraten17RAL,
author="Zeestraten, M. J. A. and Havoutis, I. and Silv\'erio, J. and Calinon, S. and Caldwell, D. G.",
title="An Approach for Imitation Learning on {R}iemannian Manifolds",
journal="{IEEE} Robotics and Automation Letters ({RA-L})",
year="2017",
month="June",
volume="2",
number="3",
pages="1240--1247"
doi="10.1109/LRA.2017.2657001",
}
```
- *demo_online_hsmm*: online hsmm learning and sampling with lqr-based trajectory
generation.
[4] Riemannian manifolds (S+):
```
@inproceedings{Jaquier17IROS,
author="Jaquier, N. and Calinon, S.",
title="Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds: Application to Wrist Motion Estimation with {sEMG}",
booktitle="Proc. {IEEE/RSJ} Intl Conf. on Intelligent Robots and Systems ({IROS})",
year="2017",
month="September",
address="Vancouver, Canada",
pages=""
}
```
- *demo_proMP01*: Conditioning on trajectory distributions with ProMP
[5] Semi-tied GMM:
```
@article{Tanwani16RAL,
author="Tanwani, A. K. and Calinon, S.",
title="Learning Robot Manipulation Tasks with Task-Parameterized Semi-Tied Hidden Semi-{M}arkov Model",
journal="{IEEE} Robotics and Automation Letters ({RA-L})",
year="2016",
month="January",
volume="1",
number="1",
pages="235--242",
doi="10.1109/LRA.2016.2517825"
}
```
- *demo_Riemannian_cov_interp02*: Covariance interpolation on Riemannian manifold
from a GMM with augmented covariances.
[6] DP-means:
```
@article{Bruno17AURO,
author="Bruno, D. and Calinon, S. and Caldwell, D. G.",
title="Learning Autonomous Behaviours for the Body of a Flexible Surgical Robot",
journal="Autonomous Robots",
year="2017",
month="February",
volume="41",
number="2",
pages="333--347",
doi="10.1007/s10514-016-9544-6"
}
```
- *demo_Riemannian_sphere_infHorLQR*: Linear quadratic regulation on a sphere by
relying on Riemannian manifold and
infinite-horizon LQR.
[7] Shared control, adaptive teleoperation:
```
@inproceedings{Havoutis17ICRA,
author="Havoutis, I. and Calinon, S.",
title="Supervisory teleoperation with online learning and optimal control",
booktitle=ICRA,
year="2017",
month="May-June",
address="Singapore",
pages="1534--1540"
}
```
- *demo_teleop*: Example of online learning of tp-hsmm model and teleoperator-like
usage example.
[8] Calligraphy, graffiti, handwriting movement generation:
```
@inproceedings{Berio17GI,
author="Berio, D. and Calinon, S. and Fol Leymarie, F.",
title="Generating Calligraphic Trajectories with Model Predictive Control",
booktitle="Proc. 43rd Conf. on Graphics Interface",
year="2017",
month="May",
address="Edmonton, AL, Canada",
pages=""
}
```
[9] Adaptive assistance:
```
@article{Pignat17RAS,
author="Pignat, E. and Calinon, S.",
title="Learning adaptive dressing assistance from human demonstration",
journal="Robotics and Autonomous Systems",
year="2017",
month="July",
volume="93",
number="",
pages="61--75",
doi="10.1016/j.robot.2017.03.017",
}
```
- *demo_tp_multiLqr*: online tp-hsmm and tp-gmm with lqr-based trajectory generation
and multiframe-lqr trajectory generation.
### List of examples
This project will build a number of executables, as listed in the table below.
| Filename | ref. | Description |
|----------|------|-------------|
| demo_dptpgmm | [9] | Online tp-gmm learning and lqr-based trajectory generation |
| demo_glsl | [8] | Rendering example with glsl display |
| demo_glsl_gfx | [8] | Rendering example with glsl display (using glfx) |
| demo_LQR_infHor | [1] | Discrete infinite horizon linear quadratic regulation |
| demo_MPC_batch_01 | [1,8] | Model predictive control (MPC) with batch linear quadratic tracking (LQT) formulation |
| demo_MPC_iterative_01 | [1,8] | Model predictive control (MPC) with iterative linear quadratic tracking (LQT) formulation |
| demo_MPC_semitied_01 | [5,8] | MPC with semi-tied covariances |
| demo_MPC_velocity_01 | [1,8] | MPC with an objective including velocity tracking |
| demo_online_gmm | [6] | Online GMM learning and LQR-based trajectory generation |
| demo_online_hsmm | [2,7] | Online HSMM learning and sampling with LQR-based trajectory generation |
| demo_Riemannian_cov_interp02 | [4] | Covariance interpolation on Riemannian manifold from a GMM with augmented covariances |
| demo_Riemannian_quat_infHorLQR | [3] | Linear quadratic regulation on hypersphere (orientation as unit quaternions) by relying on Riemannian manifold and infinite-horizon LQR |
| demo_Riemannian_sphere_infHorLQR | [3] | Linear quadratic regulation on a sphere by relying on Riemannian manifold and infinite-horizon LQR |
| demo_teleop | [7] | Online learning of tp-hsmm model and teleoperator-like usage example |
| demo_tp_multiLqr | [7] | Online tp-hsmm and tp-gmm with lqr-based trajectory generation and multiframe-lqr trajectory generation |
| demo_tp_trajMpc | [7] | Online tp-hsmm learning and trajMPC-based trajectory generation when communication is cut |
- *demo_tp_trajMpc*: online tp-hsmm learning and trajMPC-based trajectory generation
when communication is cut.
src/demo_proMP01.cpp deleted 100644 → 0
+ 0
− 468
View file @ f6f1612a
/*
* demo_proMP01.cpp
*
* Conditioning on trajectory distributions with ProMP
*
* @incollection{Paraschos13,
* title = {Probabilistic Movement Primitives},
* author = {Paraschos, A. and Daniel, C. and Peters, J. and Neumann, G.},
* booktitle = NIPS,
* pages = {2616--2624},
* year = {2013},
* publisher = {Curran Associates, Inc.}
* }
* @inproceedings{Rueckert15,
* author = "Rueckert, E. and Mundo, J. and Paraschos, A. and Peters, J. and Neumann, G.",
* title = "Extracting Low-Dimensional Control Variables for Movement Primitives",
* booktitle = ICRA,
* year = "2015",
* pages = "1511--1518",
* address = "Seattle, WA, USA"
* }
*
* Authors: Sylvain Calinon, Philip Abbet
*/
#include <stdio.h>
#include <armadillo>
#include <pbdlib/demonstration.h>
#include <pbdlib/mvn.h>
#include <gfx3.h>
#include <gfx_ui.h>
#include <GLFW/glfw3.h>
#include <imgui.h>
#include <imgui_impl_glfw_gl3.h>
using namespace pbdlib;
/*********************************** TYPES ***********************************/
struct parameters_t {
int nb_states; // Number of components in the GMM
int nb_var; // Dimension of position data (here: x1, x2)
int nb_data; // Number of datapoints in a trajectory
float dt; // Time step (without rescaling, large values such
// as 1 has the advantage of creating clusers based
// on position information)
};
/******************************** CONSTANTS **********************************/
const arma::mat COLORS({
{ 0.00000, 0.00000, 1.00000},
{ 0.00000, 0.50000, 0.00000},
{ 1.00000, 0.00000, 0.00000},
{ 0.00000, 0.75000, 0.75000},
{ 0.75000, 0.00000, 0.75000},
{ 0.75000, 0.75000, 0.00000},
{ 0.25000, 0.25000, 0.25000},
{ 0.00000, 0.00000, 1.00000},
{ 0.00000, 0.50000, 0.00000},
{ 1.00000, 0.00000, 0.00000},
});
/****************************** HELPER FUNCTIONS *****************************/
static void error_callback(int error, const char* description) {
fprintf(stderr, "Error %d: %s\n", error, description);
}
//-----------------------------------------------
// Create a demonstration (with a length of 'nb_samples') from a trajectory
// (of any length)
Demonstration sample_trajectory(const std::vector<arma::vec>& trajectory, int nb_samples) {
// Resampling of the trajectory
arma::vec x(trajectory.size());
arma::vec y(trajectory.size());
arma::vec x2(trajectory.size());
arma::vec y2(trajectory.size());
for (size_t i = 0; i < trajectory.size(); ++i) {
x(i) = trajectory[i](0);
y(i) = trajectory[i](1);
}
arma::vec from_indices = arma::linspace<arma::vec>(0, trajectory.size() - 1, trajectory.size());
arma::vec to_indices = arma::linspace<arma::vec>(0, trajectory.size() - 1, nb_samples);
interp1(from_indices, x, to_indices, x2, "*linear");
interp1(from_indices, y, to_indices, y2, "*linear");
// Create the demonstration
Demonstration demo = Demonstration(2, nb_samples);
mat& datapoints = demo.getDatapoints().getData();
for (int i = 0; i < nb_samples; ++i) {
datapoints(0, i) = x2[i];
datapoints(1, i) = y2[i];
}
return demo;
}
/********************************* FUNCTIONS *********************************/
// The actual computation
//
// Inputs: demos, parameters
// Outputs: mu_mat, mu2_mat, H
void process(std::vector<Demonstration>& demos, const parameters_t& parameters,
arma::mat &mu_mat, arma::mat &mu2_mat, arma::mat &H) {
// Create basis functions (GMM with components equally split in time)
arma::vec timesteps = arma::linspace<arma::vec>(
0, parameters.nb_data - 1, parameters.nb_data
) * parameters.dt;
arma::vec mu = arma::linspace<arma::vec>(
timesteps(0), timesteps(timesteps.n_rows - 1), parameters.nb_states
);
arma::vec sigma = arma::ones<arma::vec>(parameters.nb_states) * 0.01;
// Compute basis functions activation
H = arma::mat(parameters.nb_states, parameters.nb_data);
for (unsigned int i = 0; i < parameters.nb_states; ++i) {
arma::colvec mu_({ mu(i) });
arma::mat sigma_({ sigma(i) });
GaussianDistribution gauss(mu_, sigma_);
H.row(i) = gauss.getPDFValue(timesteps.t()).t();
}
H = H / repmat(sum(H, 0), parameters.nb_states, 1);
//_____ ProMP __________
arma::mat psi(parameters.nb_var * parameters.nb_data,
parameters.nb_var * parameters.nb_states);
for (unsigned int i = 0; i < parameters.nb_data; ++i) {
psi.rows(i * parameters.nb_var, (i + 1) * parameters.nb_var - 1) =
arma::kron(H.col(i).t(), arma::eye(parameters.nb_var, parameters.nb_var));
}
arma::mat w(parameters.nb_var * parameters.nb_states, demos.size());
for (size_t i = 0; i < demos.size(); ++i) {
w.col(i) = pinv(psi) * (arma::mat) arma::vectorise(demos[i].getDatapoints().getData());
}
// Distribution in parameter space
arma::mat mu_w = arma::mean(w, 1);
//-- First regularization term
arma::mat sigma_w = arma::eye(parameters.nb_var * parameters.nb_states,
parameters.nb_var * parameters.nb_states);
if (w.n_cols == 1) {
sigma_w += arma::rowvec(arma::cov(w.t()))[0];
}
else {
sigma_w += arma::cov(w.t());
}
// Trajectory distribution
mu = psi * mu_w;
arma::mat sigma2 = psi * sigma_w * psi.t() + // Second regularization term
0.01 * arma::eye(parameters.nb_var * parameters.nb_data,
parameters.nb_var * parameters.nb_data);
//_____ Conditioning on trajectory distribution __________
// (reconstruction from partial data)
arma::uvec in_out = arma::linspace<arma::uvec>(
0, parameters.nb_data * parameters.nb_var - 1, parameters.nb_data * parameters.nb_var
);
arma::uvec in(2 * parameters.nb_var);
in.head(parameters.nb_var) = in_out.head(parameters.nb_var);
in.tail(parameters.nb_var) = in_out.tail(parameters.nb_var);
arma::uvec out = in_out.subvec(parameters.nb_var, in_out.n_elem - parameters.nb_var - 1);
// Input data
arma::vec mu_in = arma::reshape(
demos[0].getDatapoints().getData().cols(arma::uvec({0, (unsigned) parameters.nb_data - 1})) +
arma::repmat((arma::randu(parameters.nb_var, 1) - 0.5) * 10.0, 1, 2),
parameters.nb_var * 2, 1
);
// Gaussian conditioning with trajectory distribution
arma::vec mu2(in_out.n_elem, fill::zeros);
mu2.rows(in) = mu_in;
arma::mat A = sigma_w * psi.rows(in).t(); // Used to simplify the
arma::mat B = psi.rows(in) * sigma_w * psi.rows(in).t(); // following operation
arma::mat mu_w_tmp = mu_w +
(arma::inv(B.t()) * A.t()).t() * // == A / B (not
(mu2.rows(in) - psi.rows(in) * mu_w); // supported by armadillo)
mu2.rows(out) = psi.rows(out) * mu_w_tmp;
//_____ Results __________
mu_mat = arma::conv_to<arma::mat>::from(mu);
mu_mat.reshape(parameters.nb_var, parameters.nb_data);
mu2_mat = arma::conv_to<arma::mat>::from(mu2);
mu2_mat.reshape(parameters.nb_var, parameters.nb_data);
}
//-----------------------------------------------
int main(int argc, char **argv) {
arma::arma_rng::set_seed_random();
// Parameters
parameters_t parameters;
parameters.nb_states = 6;
parameters.nb_var = 2;
parameters.dt = 0.01;
parameters.nb_data = 100;
// Initialise GLFW
glfwSetErrorCallback(error_callback);
if (!glfwInit())
return -1;
glfwWindowHint(GLFW_SAMPLES, 4);
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
// Open a window and create its OpenGL context
GLFWwindow* window = glfwCreateWindow(
800, 800, "Demo Conditioning on trajectory distributions", NULL, NULL
);
glfwMakeContextCurrent(window);
// Take 4k screens into account (framebuffer size != window size)
int win_width = ImGui::GetIO().DisplaySize.x;
int win_height = ImGui::GetIO().DisplaySize.y;
int fb_width, fb_height;
glfwGetFramebufferSize(window, &fb_width, &fb_height);
// Setup GLSL
gfx3::init();
glEnable(GL_DEPTH_TEST);
glEnable(GL_CULL_FACE);
glEnable(GL_LINE_SMOOTH);
glDepthFunc(GL_LESS);
// Setup ImGui
ImGui_ImplGlfwGL3_Init(window, true);
// Creation of the Vertex Array Object (VAO)
GLuint vertexArrayID;
glGenVertexArrays(1, &vertexArrayID);
glBindVertexArray(vertexArrayID);
// Projection matrix
arma::fmat projection = gfx3::orthographic(
(float) fb_width, (float) fb_height, 0.1f, 10.0f);
// Camera matrix
arma::fmat view = gfx3::lookAt(
arma::fvec({0, 0, 3}), // Position of the camera
arma::fvec({0, 0, 0}), // Look at the origin
arma::fvec({0, 1, 0}) // Head is up
);
// Loading of the shaders
gfx3::shader_t colored_shader = gfx3::loadShader(gfx3::VERTEX_SHADER_COLORED,
gfx3::FRAGMENT_SHADER_COLORED);
// Main loop
bool adding_line = false;
std::vector<arma::vec> current_trajectory;
std::vector< std::vector<arma::vec> > original_trajectories;
std::vector<Demonstration> demos;
arma::mat mu;
arma::mat mu2;
arma::mat H;
int current_nb_data = parameters.nb_data;
int current_nb_states = parameters.nb_states;
while (!glfwWindowShouldClose(window)) {
glfwPollEvents();
// Detect when the window was resized
if ((ImGui::GetIO().DisplaySize.x != win_width) ||
(ImGui::GetIO().DisplaySize.y != win_height)) {
win_width = ImGui::GetIO().DisplaySize.x;
win_height = ImGui::GetIO().DisplaySize.y;
glfwGetFramebufferSize(window, &fb_width, &fb_height);
// Update the projection matrix
projection = gfx3::orthographic(
(float) fb_width, (float) fb_height, 0.1f, 10.0f);
}
// If the parameters changed, recompute
if ((parameters.nb_data != current_nb_data) ||
(parameters.nb_states != current_nb_states)) {
demos.clear();
for (size_t i = 0; i < original_trajectories.size(); ++i)
demos.push_back(sample_trajectory(original_trajectories[i], parameters.nb_data));
process(demos, parameters, mu, mu2, H);
current_nb_data = parameters.nb_data;
current_nb_states = parameters.nb_states;
}
// Start the rendering
ImGui_ImplGlfwGL3_NewFrame();
glViewport(0, 0, fb_width, fb_height);
glClearColor(0.6f, 0.6f, 0.6f, 0.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Draw the currently created demonstration (if any)
if (current_trajectory.size() > 1) {
gfx3::model_t line = gfx3::create_line(
colored_shader, arma::fvec({0.33f, 0.97f, 0.33f}), current_trajectory);
gfx3::draw(line, view, projection);
gfx3::destroy(line);
}
// Draw the demonstrations (if any)
for (auto iter = demos.begin(); iter != demos.end(); ++iter) {
Datapoints& datapoints = iter->getDatapoints();
for (uint i = 0; i < datapoints.getNumPOINTS(); ++i) {
arma::fvec color = arma::conv_to<arma::fvec>::from(H.col(i).t() * COLORS.rows(0, parameters.nb_states - 1));
gfx3::model_t point = gfx3::create_square(colored_shader, color, 10.0f);
point.transforms.position = arma::conv_to<arma::fvec>::from(datapoints.getData(i));
point.transforms.position.reshape(3, point.transforms.position.n_cols);
gfx3::draw(point, view, projection);
gfx3::destroy(point);
}
}
// Draw the computed trajectories
if (mu.n_elem > 0) {
gfx3::model_t line = gfx3::create_line(
colored_shader, arma::fvec({0.0f, 0.0f, 0.0f}), mu);
gfx3::draw(line, view, projection);
gfx3::destroy(line);
line = gfx3::create_line(
colored_shader, arma::fvec({0.8f, 0.0f, 0.0f}), mu2);
gfx3::draw(line, view, projection);
gfx3::destroy(line);
}
// Parameter window
ImGui::SetNextWindowSize(ImVec2(400, 110));
ImGui::Begin("Parameters", NULL,
ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
ImGuiWindowFlags_NoMove
);
ImGui::SliderInt("Nb states", &parameters.nb_states, 2, 10);
ImGui::SliderInt("Nb data", &parameters.nb_data, 20, 300);
if (ImGui::Button("Clear")){
demos.clear();
original_trajectories.clear();
mu = arma::mat();
mu2 = arma::mat();
H = arma::mat();
}
ImGui::Text("Draw several similar trajectories");
ImGui::End();
// GUI rendering
ImGui::Render();
// Swap buffers
glfwSwapBuffers(window);
// Keyboard input
if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
break;
// Left click: start a new demonstration
if (!adding_line && ImGui::IsMouseClicked(GLFW_MOUSE_BUTTON_1)) {
// Only if not on the UI
if (!ImGui::GetIO().WantCaptureMouse)
adding_line = true;
}
if (adding_line) {
double mouse_x, mouse_y;
glfwGetCursorPos(window, &mouse_x, &mouse_y);
current_trajectory.push_back(gfx3::ui2fb({ mouse_x, mouse_y },
win_width, win_height,
fb_width, fb_height));
}
// Left mouse button release: end the demonstration creation
if (adding_line && !ImGui::IsMouseDown(GLFW_MOUSE_BUTTON_1)) {
adding_line = false;
demos.push_back(sample_trajectory(current_trajectory, parameters.nb_data));
original_trajectories.push_back(current_trajectory);
current_trajectory.clear();
process(demos, parameters, mu, mu2, H);
}
}
// Cleanup
ImGui_ImplGlfwGL3_Shutdown();
glfwTerminate();
return 0;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment