diff --git a/README.md b/README.md
index 36144ec853ab47a61448eaf4ee7c6e8337793d5c..4f8ba1dda6a9de6f1992bd4e1dd5b3b26d7f27c9 100644
--- a/README.md
+++ b/README.md
@@ -1,8 +1,8 @@
# pbdlib-matlab
-<p>PbDlib is a collection of source codes for robot programming by demonstration (learning from demonstration). It includes various functionalities at the crossroad of statistical learning, dynamical systems, optimal control and Riemannian geometry.</p>
+<p>PbDlib is a collection of source codes for robot programming by demonstration (learning from demonstration). It includes various functionalities including statistical learning, dynamical systems, optimal control and Riemannian geometry.</p>
<p>PbDlib can be used in applications requiring task adaptation, human-robot skill transfer, safe controllers based on minimal intervention principle, as well as for probabilistic motion analysis and synthesis in multiple coordinate systems.</p>
-<p>The codes are compatible with both Matlab and GNU Octave. Other versions of the library in Python and C++ are also available at http://www.idiap.ch/software/pbdlib/ (currently, the Matlab version has the most functionalities).</p>
+<p>The codes are compatible with both Matlab and GNU Octave. Other versions of the library in Python and C++ are also available at https://www.idiap.ch/software/pbdlib/ (currently, the Matlab version contains the most complete set of examples).</p>
### Usage
@@ -14,14 +14,6 @@ All the examples are located in the `demos` folder, and the functions are locate
| Filename | Ref. | Description |
|----------|------|-------------|
-| [benchmark/benchmark_DS_GP_GMM01.m](./demos/benchmark_DS_GP_GMM01.m) | [[1]](#ref-1) | Benchmark of task-parameterized model based on GPR, with trajectory model (GMM encoding) |
-| [benchmark/benchmark_DS_GP_raw01.m](./demos/benchmark_DS_GP_raw01.m) | [[1]](#ref-1) | Benchmark of task-parameterized model based on GPR, with raw trajectory |
-| [benchmark/benchmark_DS_PGMM01.m](./demos/benchmark_DS_PGMM01.m) | [[1]](#ref-1) | Benchmark of task-parameterized model based on parametric Gaussian mixture model |
-| [benchmark/benchmark_DS_TP_GMM01.m](./demos/benchmark_DS_TP_GMM01.m) | [[1]](#ref-1) | Benchmark of task-parameterized Gaussian mixture model (TP-GMM) |
-| [benchmark/benchmark_DS_TP_GP01.m](./demos/benchmark_DS_TP_GP01.m) | [[1]](#ref-1) | Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method) |
-| [benchmark/benchmark_DS_TP_LWR01.m](./demos/benchmark_DS_TP_LWR01.m) | [[1]](#ref-1) | Benchmark of task-parameterized locally weighted regression (nonparametric task-parameterized method) |
-| [benchmark/benchmark_DS_TP_MFA01.m](./demos/benchmark_DS_TP_MFA01.m) | [[1]](#ref-1) | Benchmark of task-parameterized mixture of factor analyzers (TP-MFA) |
-| [benchmark/benchmark_DS_TP_trajGMM01.m](./demos/benchmark_DS_TP_trajGMM01.m) | [[1]](#ref-1) | Benchmark of task-parameterized Gaussian mixture model (TP-GMM) |
| [demo_affineTransform01.m](./demos/demo_affineTransform01.m) | [[1]](#ref-1) | Miscellaneous affine transformations of raw data as pre-processing step to train a task-parameterized model |
| [demo_AR01.m](./demos/demo_AR01.m) | [[2]](#ref-2) | Multivariate autoregressive (AR) model parameters estimation with least-squares |
| [demo_AR_HSMM01.m](./demos/demo_AR_HSMM01.m) | [[2]](#ref-2) | Multivariate autoregressive (AR) model implemented as a hidden semi-Markov model with lognormal duration model |
@@ -111,7 +103,7 @@ All the examples are located in the `demos` folder, and the functions are locate
| [demo_HSMM_MPC01.m](./demos/demo_HSMM_MPC01.m) | [[2]](#ref-2) | Use of HSMM (with lognormal duration model) and batch LQR (with position only) for motion synthesis |
| [demo_HSMM_online01.m](./demos/demo_HSMM_online01.m) | [[2]](#ref-2) | Online HSMM |
| [demo_ID01.m](./demos/demo_ID01.m) | [[10]](#ref-10) | Inverse dynamics with dynamically consistent nullspace (requires the robotics toolbox) |
-| [demo_IK01.m](./demos/demo_IK01.m) | [[10]](#ref-10) | Inverse kinematics with nullspace control (requires the robotics toolbox) |
+| [demo_IK01.m](./demos/demo_IK01.m) | [[10]](#ref-10) | Basic forward and inverse kinematics for a planar robot, with numerical Jacobian computation |
| [demo_IK02.m](./demos/demo_IK02.m) | [[10]](#ref-10) | Inverse kinematics with two arms and nullspace control (requires the robotics toolbox) |
| [demo_IK_nullspaceAsProduct01.m](./demos/demo_IK_nullspaceAsProduct01.m) | [[1]](#ref-1) | 3-level nullspace control formulated as product of Gaussians |
| [demo_IK_nullspace_TPGMM01.m](./demos/demo_IK_nullspace_TPGMM01.m) | [[1]](#ref-1) | IK with nullspace treated with task-parameterized GMM (bimanual tracking task, version with 4 frames) |
@@ -138,43 +130,44 @@ All the examples are located in the `demos` folder, and the functions are locate
| [demo_manipulabilityTransfer01.m](./demos/demo_manipulabilityTransfer01.m) | [[6]](#ref-6) | Use of robot redundancy to track desired manipulability ellipsoid |
| [demo_manipulabilityTransfer02.m](./demos/demo_manipulabilityTransfer02.m) | [[6]](#ref-6) | Learning and reproduction of manipulability ellipsoid profiles |
| [demo_manipulabilityTransfer03.m](./demos/demo_manipulabilityTransfer03.m) | [[6]](#ref-6) | Learning and reproduction of manipulability ellipsoid profiles (numerical version) |
-| [demo_MPC01.m](./demos/demo_MPC01.m) | [[2]](#ref-2) | Batch LQR with viapoints and a double integrator system. |
-| [demo_MPC02.m](./demos/demo_MPC02.m) | [[2]](#ref-2) | Same as demo_MPC01.m but with a GMM encoding of only position data |
-| [demo_MPC03.m](./demos/demo_MPC03.m) | [[2]](#ref-2) | Control of a spring attached to a point with batch LQR |
-| [demo_MPC04.m](./demos/demo_MPC04.m) | [[2]](#ref-2) | Control of a spring attached to a point with batch LQR (with augmented state space) |
-| [demo_MPC_augmSigma01.m](./demos/demo_MPC_augmSigma01.m) | [[2]](#ref-2) | Batch LQR with augmented covariance to transform a tracking problem to a regulation problem |
-| [demo_MPC_constrained01.m](./demos/demo_MPC_constrained01.m) | [[2]](#ref-2) | Constrained batch LQT by using quadratic programming solver, with an encoding of position and velocity data |
-| [demo_MPC_fullQ01.m](./demos/demo_MPC_fullQ01.m) | [[2]](#ref-2) | Batch LQR exploiting full Q matrix by constraining the motion to pass through a common point at different time steps |
-| [demo_MPC_fullQ02.m](./demos/demo_MPC_fullQ02.m) | [[2]](#ref-2) | Batch LQR exploiting full Q matrix by constraining the motion of two agents |
-| [demo_MPC_fullQ03.m](./demos/demo_MPC_fullQ03.m) | [[2]](#ref-2) | Batch LQR exploiting full Q matrix by constraining the motion of two agents in a ballistic task |
-| [demo_MPC_infHor01.m](./demos/demo_MPC_infHor01.m) | [[2]](#ref-2) | Discrete infinite horizon linear quadratic regulation (with precision matrix only on position) |
-| [demo_MPC_infHor02.m](./demos/demo_MPC_infHor02.m) | [[2]](#ref-2) | Discrete infinite horizon linear quadratic regulation (with precision matrix on position and velocity) |
-| [demo_MPC_infHor03.m](./demos/demo_MPC_infHor03.m) | [[2]](#ref-2) | Continuous infinite horizon linear quadratic tracking, by relying on a GMM encoding of position and velocity data |
-| [demo_MPC_infHor04.m](./demos/demo_MPC_infHor04.m) | [[2]](#ref-2) | Discrete infinite horizon linear quadratic tracking, by relying on a GMM encoding of position and velocity data |
-| [demo_MPC_infHor_bicopter01.m](./demos/demo_MPC_infHor_bicopter01.m) | [[2]](#ref-2) | Infinite horizon LQR applied on a planar UAV with an iterative linearization of the system plant |
-| [demo_MPC_infHor_incremental01.m](./demos/demo_MPC_infHor_incremental01.m) | [[2]](ref-2) | Infinite horizon LQR with an iterative re-estimation of the system plant linear system (example with planar UAV) |
-| [demo_MPC_infHor_incremental02.m](./demos/demo_MPC_infHor_incremental02.m) | [[2]](ref-2) | Same as demo_MPC_infHor_incremental01.m but with pendulum) |
-| [demo_MPC_iterativeLQR01.m](./demos/demo_MPC_iterativeLQR01.m) | [[2]](#ref-2) | Iterative computation of linear quadratic tracking (with feedback and feedforward terms) |
-| [demo_MPC_iterativeLQR02.m](./demos/demo_MPC_iterativeLQR02.m) | [[2]](#ref-2) | Same as demo_MPC_iterativeLQR01.m, by relying on a GMM encoding of position and velocity data |
-| [demo_MPC_iterativeLQR03.m](./demos/demo_MPC_iterativeLQR03.m) | [[2]](#ref-2) | Same as demo_MPC_iterativeLQR01.m, by relying on a GMM encoding of only position data |
-| [demo_MPC_iterativeLQR04.m](./demos/demo_MPC_iterativeLQR04.m) | [[2]](#ref-2) | Control of a spring attached to a point with iterative linear quadratic tracking (with feedback and feedforward terms) |
-| [demo_MPC_iterativeLQR_augmSigma01.m](./demos/demo_MPC_iterativeLQR_augmSigma01.m) | [[2]](#ref-2) | Iterative LQR with augmented covariance to transform the tracking problem to a regulation problem |
-| [demo_MPC_iterativeLQR_augmSigma_online01.m](./demos/demo_MPC_iterativeLQR_augmSigma_online01.m) | [[2]](#ref-2) | Same as demo_iterativeLQR_augmSigma01 but recomputed in an online manner |
-| [demo_MPC_Lagrangian01.m](./demos/demo_MPC_Lagrangian01.m) | [[2]](#ref-2) | Batch LQR with Lagrangian in matrix form to force first and last point to coincide in order to form periodic motion |
-| [demo_MPC_MP01.m](./demos/demo_MPC_MP01.m) | [[2]](#ref-2) | Batch LQR using sparse movement primitives with radial basis functions |
-| [demo_MPC_MP02.m](./demos/demo_MPC_MP02.m) | [[2]](#ref-2) | Batch LQR using sparse movement primitives with Fourier basis functions |
-| [demo_MPC_noInitialState01.m](./demos/demo_MPC_noInitialState01.m) | [[2]](#ref-2) | Batch LQR solution finding the optimal initial state together with the optimal control commands |
-| [demo_MPC_nullspace01.m](./demos/demo_MPC_nullspace01.m) | [[2]](#ref-2) | Batch LQR with nullspace formulation |
-| [demo_MPC_nullspace_online01.m](./demos/demo_MPC_nullspace_online01.m) | [[2]](#ref-2) | Online batch LQR with nullspace formulation |
-| [demo_MPC_online01.m](./demos/demo_MPC_online01.m) | [[2]](#ref-2) | MPC recomputed in an online manner with a time horizon |
-| [demo_MPC_online02.m](./demos/demo_MPC_online02.m) | [[2]](#ref-2) | MPC recomputed in an online manner with a time horizon, by relying on a GMM encoding of position and velocity data (with animation) |
-| [demo_MPC_online03.m](./demos/demo_MPC_online03.m) | [[2]](#ref-2) | Obstacle avoidance with MPC recomputed in an online manner |
-| [demo_MPC_robot01.m](./demos/demo_MPC_robot01.m) | [[2]](#ref-2) | Robot tracking task with online batch LQR, by linearization of the system plant at each time step |
-| [demo_MPC_skillsRepr01.m](./demos/demo_MPC_skillsRepr01.m) | [[2]](#ref-2) | Representation of skills combined in parallel and in series through a batch LQT formulation |
-| [demo_MPC_viapoints01.m](./demos/demo_MPC_viapoints01.m) | [[2]](#ref-2) | Keypoint-based motion through MPC, with a GMM encoding of position and velocity |
-| [demo_MPC_viapoints02.m](./demos/demo_MPC_viapoints02.m) | [[2]](#ref-2) | Same as demo_MPC_viapoints01 with only position encoding |
-| [demo_MPC_viapoints03.m](./demos/demo_MPC_viapoints03.m) | [[8]](#ref-8) | Equivalence between cubic Bezier curve and batch LQR with double integrator |
-| [demo_MPC_viapoints_withProd01.m](./demos/demo_MPC_viapoints_withProd01.m) | [[2]](#ref-2) | Batch LQT with viapoints computed as a product of trajectory distributions in control space |
+| [demo_OC_DDP_bicopter01.m](./demos/demo_OC_DDP_bicopter01.m) | [[2]](#ref-2) | iLQR applied to a bicopter problem |
+| [demo_OC_DDP_car01.m](./demos/demo_OC_DDP_car01.m) | [[2]](#ref-2) | iLQR applied to a car parking problem |
+| [demo_OC_DDP_cartpole01.m](./demos/demo_OC_DDP_cartpole01.m) | [[2]](#ref-2) | iLQR applied to a cartpole problem |
+| [demo_OC_DDP_humanoid01.m](./demos/demo_OC_DDP_humanoid01.m) | [[2]](#ref-2) | iLQR applied to a planar 5-link humanoid problem with constraints between joints |
+| [demo_OC_DDP_manipulator01.m](./demos/demo_OC_DDP_manipulator01.m) | [[2]](#ref-2) | iLQR applied to a 2D manipulator problem |
+| [demo_OC_DDP_pendulum01.m](./demos/demo_OC_DDP_pendulum01.m) | [[2]](#ref-2) | iLQR applied to an actuated inverted pendulum problem |
+| [demo_OC_LQT01.m](./demos/demo_OC_LQT01.m) | [[2]](#ref-2) | Batch solution of linear quadratic optimal control (unconstrained linear MPC), by relying on a Gaussian mixture model (GMM) encoding of position and velocity data (see also demo_OC_LQT_iterativeLQR01.m) |
+| [demo_OC_LQT02.m](./demos/demo_OC_LQT02.m) | [[2]](#ref-2) | Same as demo_OC_LQT01.m but with a GMM encoding of only position data |
+| [demo_OC_LQT03.m](./demos/demo_OC_LQT03.m) | [[2]](#ref-2) | Same as demo_OC_LQT01.m but by tracking only position references |
+| [demo_OC_LQT04.m](./demos/demo_OC_LQT04.m) | [[2]](#ref-2) | Control of a spring attached to a point with batch LQR (with augmented state space) |
+| [demo_OC_LQT_augmSigma01.m](./demos/demo_OC_LQT_augmSigma01.m) | [[2]](#ref-2) | Batch LQR with augmented covariance to transform a tracking problem to a regulation problem |
+| [demo_OC_LQT_ballistic01.m](./demos/demo_OC_LQT_ballistic01.m) | [[2]](#ref-2) | Batch LQT with augmented state to solve simple ballistic problem |
+| [demo_OC_LQT_constrained01.m](./demos/demo_OC_LQT_constrained01.m) | [[2]](#ref-2) | Constrained batch LQT by using quadratic programming solver, with an encoding of position and velocity data |
+| [demo_OC_LQT_fullQ01.m](./demos/demo_OC_LQT_fullQ01.m) | [[2]](#ref-2) | Batch LQT problem exploiting full Q matrix by constraining the motion to pass through a common point at different time steps |
+| [demo_OC_LQT_fullQ02.m](./demos/demo_OC_LQT_fullQ02.m) | [[2]](#ref-2) | Batch LQT problem exploiting full Q matrix by constraining the motion of two agents |
+| [demo_OC_LQT_fullQ03.m](./demos/demo_OC_LQT_fullQ03.m) | [[2]](#ref-2) | Batch LQT problem exploiting full Q matrix by constraining the motion of two agents in a simple ballistic task |
+| [demo_OC_LQT_fullQ04.m](./demos/demo_OC_LQT_fullQ04.m) | [[2]](#ref-2) | Batch LQT exploiting full Q matrix to constrain the motion of two agents in a ballistic task mimicking a bimanual tennis serve problem |
+| [demo_OC_LQT_infHor01.m](./demos/demo_OC_LQT_infHor01.m) | [[2]](#ref-2) | Discrete infinite horizon linear quadratic regulation (with precision matrix only on position) |
+| [demo_OC_LQT_infHor02.m](./demos/demo_OC_LQT_infHor02.m) | [[2]](#ref-2) | Discrete infinite horizon linear quadratic regulation (with precision matrix on position and velocity) |
+| [demo_OC_LQT_infHor03.m](./demos/demo_OC_LQT_infHor03.m) | [[2]](#ref-2) | Continuous infinite horizon linear quadratic tracking, by relying on a GMM encoding of position and velocity data |
+| [demo_OC_LQT_infHor04.m](./demos/demo_OC_LQT_infHor04.m) | [[2]](#ref-2) | Discrete infinite horizon linear quadratic tracking, by relying on a GMM encoding of position and velocity data |
+| [demo_OC_LQT_recursive01.m](./demos/demo_OC_LQT_recursive01.m) | [[2]](#ref-2) | Recursive computation of linear quadratic tracking (with feedback and feedforward terms) |
+| [demo_OC_LQT_recursive02.m](./demos/demo_OC_LQT_recursive02.m) | [[2]](#ref-2) | Same as demo_OC_LQT_recursive01.m, by relying on a GMM encoding of position and velocity data, including comparison with batch LQT |
+| [demo_OC_LQT_recursive03.m](./demos/demo_OC_LQT_recursive03.m) | [[2]](#ref-2) | Same as demo_OC_LQT_recursive01.m, by relying on a GMM encoding of only position data |
+| [demo_OC_LQT_recursive_augmSigma01.m](./demos/demo_OC_LQT_recursive_augmSigma01.m) | [[2]](#ref-2) | Recursive LQR with augmented covariance to transform the tracking problem to a regulation problem |
+| [demo_OC_LQT_Lagrangian01.m](./demos/demo_OC_LQT_Lagrangian01.m) | [[2]](#ref-2) | Batch LQR with Lagrangian in matrix form to force first and last point to coincide in order to form periodic motion |
+| [demo_OC_LQT_noInitialState01.m](./demos/demo_OC_LQT_noInitialState01.m) | [[2]](#ref-2) | Batch LQR solution finding the optimal initial state together with the optimal control commands |
+| [demo_OC_LQT_nullspace01.m](./demos/demo_OC_LQT_nullspace01.m) | [[2]](#ref-2) | Batch LQR with nullspace formulation |
+| [demo_OC_LQT_nullspace02.m](./demos/demo_OC_LQT_nullspace02.m) | [[2]](#ref-2) | Batch LQR with nullspace formulation - ballistic task with an augmented state space |
+| [demo_OC_LQT_online01.m](./demos/demo_OC_LQT_online01.m) | [[2]](#ref-2) | MPC recomputed in an online manner with a time horizon |
+| [demo_OC_LQT_online02.m](./demos/demo_OC_LQT_online02.m) | [[2]](#ref-2) | MPC recomputed in an online manner with a time horizon, by relying on a GMM encoding of position and velocity data (with animation) |
+| [demo_OC_LQT_online03.m](./demos/demo_OC_LQT_online03.m) | [[2]](#ref-2) | Obstacle avoidance with MPC recomputed in an online manner |
+| [demo_OC_LQT_online_minimal01.m](./demos/demo_OC_LQT_online_minimal01.m) | [[2]](#ref-2) | Minimal example of online batch LQT |
+| [demo_OC_LQT_skillsRepr01.m](./demos/demo_OC_LQT_skillsRepr01.m) | [[2]](#ref-2) | Representation of skills combined in parallel and in series through a batch LQT formulation |
+| [demo_OC_LQT_viapoints01.m](./demos/demo_OC_LQT_viapoints01.m) | [[2]](#ref-2) | Keypoint-based motion through MPC, with a GMM encoding of position and velocity |
+| [demo_OC_LQT_viapoints02.m](./demos/demo_OC_LQT_viapoints02.m) | [[2]](#ref-2) | Same as demo_OC_LQT_viapoints01 with only position encoding |
+| [demo_OC_LQT_viapoints03.m](./demos/demo_OC_LQT_viapoints03.m) | [[2]](#ref-2) | Equivalence between cubic Bezier curve and batch LQT with double integrator |
+| [demo_OC_LQT_viapoints_withProd01.m](./demos/demo_OC_LQT_viapoints_withProd01.m) | [[2]](#ref-2) | Batch LQT with viapoints computed as a product of trajectory distributions in control space |
| [demo_PCA01.m](./demos/demo_PCA01.m) | [[10]](#ref-10) | Principal component analysis (PCA) |
| [demo_Procrustes01.m](./demos/demo_Procrustes01.m) | [[10]](#ref-10) | SVD solution of orthogonal Procrustes problem |
| [demo_proMP01.m](./demos/demo_proMP01.m) | [[5]](#ref-5) | Conditioning on trajectory distributions with Probabilistic movement primitives to estimate trajectory distribution|
@@ -254,6 +247,14 @@ All the examples are located in the `demos` folder, and the functions are locate
| [demo_trajHSMM01.m](./demos/demo_trajHSMM01.m) | [[2]](#ref-2) | Trajectory synthesis with an HSMM with dynamic features (trajectory-HSMM) |
| [demo_trajHSMM_adaptiveDuration01.m](./demos/demo_trajHSMM_adaptiveDuration01.m) | [[9]](#ref-9) | Hidden semi-Markov model with adaptive duration |
| [demo_trajHSMM_adaptiveDuration_online01.m](./demos/demo_trajHSMM_adaptiveDuration_online01.m) | [[9]](#ref-9) | Online trajectory retrieval method built on an HSMM with adaptive duration and a trajectory-GMM representation |
+| [benchmarks/benchmark_DS_GP_GMM01.m](./demos/benchmarks/benchmark_DS_GP_GMM01.m) | [[1]](#ref-1) | Benchmark of task-parameterized model based on GPR, with trajectory model (GMM encoding) |
+| [benchmarks/benchmark_DS_GP_raw01.m](./demos/benchmarks/benchmark_DS_GP_raw01.m) | [[1]](#ref-1) | Benchmark of task-parameterized model based on GPR, with raw trajectory |
+| [benchmarks/benchmark_DS_PGMM01.m](./demos/benchmarks/benchmark_DS_PGMM01.m) | [[1]](#ref-1) | Benchmark of task-parameterized model based on parametric Gaussian mixture model |
+| [benchmarks/benchmark_DS_TP_GMM01.m](./demos/benchmarks/benchmark_DS_TP_GMM01.m) | [[1]](#ref-1) | Benchmark of task-parameterized Gaussian mixture model (TP-GMM) |
+| [benchmarks/benchmark_DS_TP_GP01.m](./demos/benchmarks/benchmark_DS_TP_GP01.m) | [[1]](#ref-1) | Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method) |
+| [benchmarks/benchmark_DS_TP_LWR01.m](./demos/benchmarks/benchmark_DS_TP_LWR01.m) | [[1]](#ref-1) | Benchmark of task-parameterized locally weighted regression (nonparametric task-parameterized method) |
+| [benchmarks/benchmark_DS_TP_MFA01.m](./demos/benchmarks/benchmark_DS_TP_MFA01.m) | [[1]](#ref-1) | Benchmark of task-parameterized mixture of factor analyzers (TP-MFA) |
+| [benchmarks/benchmark_DS_TP_trajGMM01.m](./demos/benchmarks/benchmark_DS_TP_trajGMM01.m) | [[1]](#ref-1) | Benchmark of task-parameterized Gaussian mixture model (TP-GMM) |
### References
@@ -263,72 +264,72 @@ If you find PbDlib useful for your research, please cite the related publication
<p><a name="ref-1">
[1] Calinon, S. (2016). <strong>A Tutorial on Task-Parameterized Movement Learning and Retrieval</strong>. Intelligent Service Robotics (Springer), 9:1, 1-29.
</a><br>
-[[pdf]](http://calinon.ch/papers/Calinon-JIST2015.pdf)
-[[bib]](http://calinon.ch/papers/Calinon-JIST2015.bib)
+[[pdf]](https://calinon.ch/papers/Calinon-JIST2015.pdf)
+[[bib]](https://calinon.ch/papers/Calinon-JIST2015.bib)
<br><strong>(Ref. for GMM, TP-GMM, MFA, MPPCA, GPR, trajGMM)</strong>
</p>
<p><a name="ref-2">
[2] Calinon, S. and Lee, D. (2019). <strong>Learning Control</strong>. Vadakkepat, P. and Goswami, A. (eds.). Humanoid Robotics: a Reference, pp. 1261-1312. Springer.
</a><br>
-[[pdf]](http://calinon.ch/papers/Calinon-Lee-learningControl.pdf)
-[[bib]](http://calinon.ch/papers/Calinon-Lee-learningControl.bib)
-<br><strong>(Ref. for MPC, LQR, HMM, HSMM)</strong>
+[[pdf]](https://calinon.ch/papers/Calinon-Lee-learningControl.pdf)
+[[bib]](https://calinon.ch/papers/Calinon-Lee-learningControl.bib)
+<br><strong>(Ref. for LQR, MPC, HMM, HSMM)</strong>
</p>
<p><a name="ref-3">
-[3] Calinon, S. (2020). <strong>Gaussians on Riemannian Manifolds: Applications for Robot Learning and Adaptive Control</strong>. IEEE Robotics and Automation Magazine (RAM).
+[3] Calinon, S. (2020). <strong>Gaussians on Riemannian Manifolds: Applications for Robot Learning and Adaptive Control</strong>. IEEE Robotics and Automation Magazine (RAM), 27:2, 33-45.
</a><br>
-[[pdf]](http://calinon.ch/papers/Calinon-RAM2020.pdf)
-[[bib]](http://calinon.ch/papers/Calinon-RAM2020.bib)
+[[pdf]](https://calinon.ch/papers/Calinon-RAM2020.pdf)
+[[bib]](https://calinon.ch/papers/Calinon-RAM2020.bib)
<br><strong>(Ref. for Riemannian manifolds)</strong>
</p>
<p><a name="ref-4">
[4] Jaquier, N. and Calinon, S. (2017). <strong>Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds: Application to Wrist Motion Estimation with sEMG</strong>. In Proc. of the IEEE/RSJ Intl Conf. on Intelligent Robots and Systems (IROS), pp. 59-64.
</a><br>
-[[pdf]](http://calinon.ch/papers/Jaquier-IROS2017.pdf)
-[[bib]](http://calinon.ch/papers/Jaquier-IROS2017.bib)
+[[pdf]](https://calinon.ch/papers/Jaquier-IROS2017.pdf)
+[[bib]](https://calinon.ch/papers/Jaquier-IROS2017.bib)
<br><strong>(Ref. for S^+ Riemannian manifolds)</strong>
</p>
<p><a name="ref-5">
[5] Calinon, S. (2019). <strong>Mixture Models for the Analysis, Edition, and Synthesis of Continuous Time Series</strong>. Bouguila, N. and Fan, W. (eds). Mixture Models and Applications, pp. 39-57. Springer.
</a><br>
-[[pdf]](http://calinon.ch/papers/Calinon_MMchapter2019.pdf)
-[[bib]](http://calinon.ch/papers/Calinon_MMchapter2019.bib)
+[[pdf]](https://calinon.ch/papers/Calinon_MMchapter2019.pdf)
+[[bib]](https://calinon.ch/papers/Calinon_MMchapter2019.bib)
<br><strong>(Ref. for ergodic control, Bezier curves, LWR, GMR, probabilistic movement primitives)</strong>
</p>
<p><a name="ref-6">
-[6] Jaquier, N., Rozo, L., Caldwell, D.G. and Calinon, S. (2019). <strong>Geometry-aware Manipulability Transfer</strong>. arXiv:1811.11050.
+[6] Jaquier, N., Rozo, L., Caldwell, D.G. and Calinon, S. (2020). <strong>Geometry-aware Manipulability Learning, Tracking and Transfer</strong>. International Journal of Robotics Research (IJRR).
</a><br>
-[[pdf]](http://calinon.ch/papers/Jaquier-arXiv2018.pdf)
-[[bib]](http://calinon.ch/papers/Jaquier-arXiv2018.bib)
+[[pdf]](https://calinon.ch/papers/Jaquier-IJRR2020.pdf)
+[[bib]](https://calinon.ch/papers/Jaquier-IJRR2020.bib)
<br><strong>(Ref. for manipulability ellipsoids)</strong>
</p>
<p><a name="ref-7">
[7] Bruno, D., Calinon, S. and Caldwell, D.G. (2017). <strong>Learning Autonomous Behaviours for the Body of a Flexible Surgical Robot</strong>. Autonomous Robots, 41:2, 333-347.
</a><br>
-[[pdf]](http://calinon.ch/papers/Bruno-AURO2017.pdf)
-[[bib]](http://calinon.ch/papers/Bruno-AURO2017.bib)
+[[pdf]](https://calinon.ch/papers/Bruno-AURO2017.pdf)
+[[bib]](https://calinon.ch/papers/Bruno-AURO2017.bib)
<br><strong>(Ref. for DP-means)</strong>
</p>
<p><a name="ref-8">
[8] Berio, D., Calinon, S. and Fol Leymarie, F. (2017). <strong>Generating Calligraphic Trajectories with Model Predictive Control</strong>. In Proc. of the 43rd Conf. on Graphics Interface, pp. 132-139.
</a><br>
-[[pdf]](http://calinon.ch/papers/Berio-GI2017.pdf)
-[[bib]](http://calinon.ch/papers/Berio-GI2017.bib)
-<br><strong>(Ref. for Bezier curves as MPC with viapoints)</strong>
+[[pdf]](https://calinon.ch/papers/Berio-GI2017.pdf)
+[[bib]](https://calinon.ch/papers/Berio-GI2017.bib)
+<br><strong>(Ref. for Bezier curves as batch LQT with viapoints)</strong>
</p>
<p><a name="ref-9">
[9] Rozo, L., Silvério, J., Calinon, S. and Caldwell, D.G. (2016). <strong>Learning Controllers for Reactive and Proactive Behaviors in Human-Robot Collaboration</strong>. Frontiers in Robotics and AI, 3:30, 1-11.
</a><br>
-[[pdf]](http://calinon.ch/papers/Rozo-Frontiers2016.pdf)
-[[bib]](http://calinon.ch/papers/Rozo-Frontiers2016.bib)
+[[pdf]](https://calinon.ch/papers/Rozo-Frontiers2016.pdf)
+[[bib]](https://calinon.ch/papers/Rozo-Frontiers2016.bib)
<br><strong>(Ref. for HSMM with adaptive time duration)</strong>
</p>
@@ -356,7 +357,7 @@ If you find PbDlib useful for your research, please cite the related publication
The Matlab/GNU Octave version of PbDlib is maintained by Sylvain Calinon, Idiap Research Institute.
-Copyright (c) 2015-2019 Idiap Research Institute, http://idiap.ch/
+Copyright (c) 2015-2021 Idiap Research Institute, https://idiap.ch/
PbDlib is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License version 3 as published by the Free Software Foundation.
diff --git a/demos/demo_IK01.m b/demos/demo_IK01.m
index 03a4d2aa379dd50381c8a68907473d56d644c9ec..0521ea1adfd8c36354a9d9682a40020274977c20 100644
--- a/demos/demo_IK01.m
+++ b/demos/demo_IK01.m
@@ -37,7 +37,7 @@ addpath('./m_fcts/');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = 1E-2; %Time step
nbData = 100;
-
+ use
%% Robot parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
diff --git a/demos/demo_MPC_MP01.m b/demos/demo_MPC_MP01.m
deleted file mode 100644
index 6451406c1f52a7a4e8d02699a9ae841121c91430..0000000000000000000000000000000000000000
--- a/demos/demo_MPC_MP01.m
+++ /dev/null
@@ -1,191 +0,0 @@
-function demo_MPC_MP01
-% Batch LQR using sparse movement primitives with radial basis functions
-%
-% If this code is useful for your research, please cite the related publication:
-% @incollection{Calinon19MM,
-% author="Calinon, S.",
-% title="Mixture Models for the Analysis, Edition, and Synthesis of Continuous Time Series",
-% booktitle="Mixture Models and Applications",
-% publisher="Springer",
-% editor="Bouguila, N. and Fan, W.",
-% year="2019",
-% pages="39--57",
-% doi="10.1007/978-3-030-23876-6_3"
-% }
-%
-% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
-% Written by Sylvain Calinon, http://calinon.ch/
-%
-% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
-%
-% PbDlib is free software: you can redistribute it and/or modify
-% it under the terms of the GNU General Public License version 3 as
-% published by the Free Software Foundation.
-%
-% PbDlib is distributed in the hope that it will be useful,
-% but WITHOUT ANY WARRANTY; without even the implied warranty of
-% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-% GNU General Public License for more details.
-%
-% You should have received a copy of the GNU General Public License
-% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
-
-addpath('./m_fcts/');
-
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbVarPos = 2; %Dimension of position data (here: x1,x2)
-nbDeriv = 1; %Number of static & dynamic features (nbDeriv=2 for [x,dx])
-nbData = 200; %Number of datapoints in a trajectory
-
-dt = 0.01; %Time step duration
-rfactor = 1E2; %Control cost in LQR
-R = speye((nbData-1)*nbVarPos) * rfactor;
-
-
-%% Dynamical System settings (discrete version)
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-A1d = zeros(nbDeriv);
-for i=0:nbDeriv-1
- A1d = A1d + diag(ones(nbDeriv-i,1),i) .* dt^i ./ factorial(i); %Discrete 1D
-end
-B1d = zeros(nbDeriv,1);
-for i=1:nbDeriv
- B1d(nbDeriv-i+1) = dt^i ./ factorial(i); %Discrete 1D
-end
-A = kron(A1d, eye(nbVarPos)); %Discrete nD
-B = kron(B1d, eye(nbVarPos)); %Discrete nD
-C = kron([1, zeros(1,nbDeriv-1)], eye(nbVarPos));
-
-%Build CSx and CSu matrices for batch LQR
-CSu = zeros(nbVarPos*nbData, nbVarPos*(nbData-1));
-CSx = kron(ones(nbData,1), [eye(nbVarPos) zeros(nbVarPos, nbVarPos*(nbDeriv-1))]);
-M = B;
-for n=2:nbData
- id1 = (n-1)*nbVarPos+1:n*nbVarPos;
- CSx(id1,:) = CSx(id1,:) * A;
- id1 = (n-1)*nbVarPos+1:n*nbVarPos;
- id2 = 1:(n-1)*nbVarPos;
- CSu(id1,id2) = C * M;
- M = [A*M(:,1:nbVarPos), M]; %Also M = [A^(n-1)*B, M] or M = [Sx(id1,:)*B, M]
-end
-
-
-%% Probabilistic movement primitives with radial basis functions
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Generate data
-nbSamples = 4; %Number of demonstrations
-for n=1:nbSamples
- t = linspace(.2*n/3, 1.5*pi+.4*n/3, nbData);
- xtmp = (1+n.*1E-2) .* [cos(t); sin(t)] + (.7+n.*1E-2) .* [zeros(1,nbData); cos(t*2-pi/3)] + randn(nbVarPos,nbData) .* 1E-2;
- x(:,n) = xtmp(:);
-end
-
-%Compute basis functions Psi and activation weights w
-nbFct = 12;
-t = linspace(0,1,nbData);
-tMu = linspace(t(1), t(end), nbFct);
-phi = zeros(nbData,nbFct);
-
-for i=1:nbFct
- phi(:,i) = gaussPDF(t, tMu(i), 1E-2);
-% phi(:,i) = mvnpdf(t', tMu(i), 1E-2);
-end
-% phi = phi ./ repmat(sum(phi,2),1,nbFct); %Optional rescaling
-
-% for i=0:nbFct-1
-% phi(:,i+1) = factorial(nbFct-1) ./ (factorial(i) .* factorial(nbFct-1-i)) .* (1-t).^(nbFct-1-i) .* t.^i; %Bernstein basis functions
-% end
-
-Psi = kron(phi, eye(nbVarPos));
-
-% w = (Psi' * Psi + eye(nbFct*nbVarPos).*1E-18) \ Psi' * x;
-w = pinv(Psi) * x;
-
-%Distribution in parameter space
-Mu_w = mean(w,2); %Mean
-Sigma_w = cov(w') + eye(nbFct*nbVarPos) .* 1E-4; %Covariance
-
-%Distribution in trajectory space
-MuQ = Psi * Mu_w; %Mean
-Q = Psi / Sigma_w * Psi'; %Precision matrix
-% rank(Q) %Q is sparse
-
-
-%% Reproduction
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-x0 = [x(1:2,1)+.2; zeros(nbVarPos*(nbDeriv-1),1)];
-
-% % u = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - Sx*x0));
-% % rx = reshape(Sx*x0+Su*u, nbVarPos, nbData);
-% u = (CSu' * Q * CSu + R) \ (CSu' * Q * (MuQ - CSx*x0));
-% rx = reshape(CSx*x0+CSu*u, nbVarPos, nbData);
-
-%Alternative computation highlighting the linear relation to Mu_w and x0 in the solution
-A0 = (CSu' * Q * CSu + R) \ CSu' * Q;
-A1 = CSu * A0 * Psi;
-A2 = CSx - CSu * A0 * CSx;
-rx = reshape(A1 * Mu_w + A2 * x0, nbVarPos, nbData);
-
-%Comparison with Gaussian conditioning
-in = 1:nbVarPos;
-out = nbVarPos+1:nbVarPos*nbData;
-Sigma = Psi * Sigma_w * Psi';
-rx_out = MuQ(out) + Sigma(out,in) / Sigma(in,in) * (x0(1:nbVarPos) - MuQ(in));
-% rx_out = MuQ(out) - Q(out,out) \ Q(out,in) * (x0(1:nbVarPos) - MuQ(in));
-rx2 = reshape([x0(1:nbVarPos); rx_out], nbVarPos, nbData);
-
-% %Computation of covariances
-% uSigma = inv(Rq) .* 1E-1;
-% xSigma = Su * uSigma * Su';
-
-
-%% 2D Plots
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,2300,1200]);
-subplot(3,2,[1,3,5]); hold on; axis off;
-for n=1:nbSamples
- plot(x(1:2:end,n), x(2:2:end,n), '-','linewidth',4,'color',[.7,.7,.7]);
-end
-plot(MuQ(1:2:end), MuQ(2:2:end), '-','linewidth',4,'color',[.8 0 0]);
-plot(rx(1,1), rx(2,1), '.','markersize',28,'color',[0,0,0]);
-plot(rx(1,:), rx(2,:), '-','linewidth',4,'color',[0,0,0]);
-plot(rx2(1,:), rx2(2,:), '-','linewidth',4,'color',[0,0,.8]);
-axis equal;
-
-%Timeline Plots
-for j=1:2
- subplot(3,2,2*(j-1)+2); hold on; %axis off;
- for n=1:nbSamples
- plot(1:nbData, x(j:2:end,n), '-','linewidth',3,'color',[.7,.7,.7]);
- end
-% %Plot uncertainty
-% id = j:nbVar:nbVar*nbData;
-% S = diag(xSigma(id,id));
-% patch([1:nbData nbData:-1:1], [rx(j,:)-S(:)' fliplr(rx(j,:)+S(:)')], [.8 0 0],'edgecolor',[.6 0 0],'facealpha', .2, 'edgealpha', .2);
- plot(1:nbData, MuQ(j:2:end), '-','linewidth',4,'color',[.8 0 0]);
- plot(1:nbData, rx(j,:), '-','linewidth',4,'color',[0 0 0]);
- set(gca,'xtick',[],'ytick',[]);
- xlabel('$t$','interpreter','latex','fontsize',34);
- ylabel(['$x_' num2str(j) '$'],'interpreter','latex','fontsize',34);
-end
-
-%Basis functions plot
-subplot(3,2,6); hold on;
-clrmap = lines(nbFct);
-for i=1:nbFct
- plot(1:nbData, phi(:,i), '-','linewidth',3,'color',clrmap(i,:)); %axis tight;
-end
-set(gca,'xtick',[],'ytick',[]);
-xlabel('$t$','interpreter','latex','fontsize',34); ylabel('$\phi_k$','interpreter','latex','fontsize',34);
-
-% %Psi*Psi' plot
-% figure; hold on; axis off; title('\Psi\Psi^T','fontsize',16);
-% colormap(flipud(gray));
-% imagesc(abs(Psi * Psi'));
-% axis tight; axis square; axis ij;
-
-% print('-dpng','graphs/MPC_MP01.png');
-pause;
-close all;
\ No newline at end of file
diff --git a/demos/demo_MPC_MP02.m b/demos/demo_MPC_MP02.m
deleted file mode 100644
index 853c910a70f2ba0b8962170b1c95a30c8f383c8d..0000000000000000000000000000000000000000
--- a/demos/demo_MPC_MP02.m
+++ /dev/null
@@ -1,195 +0,0 @@
-function demo_MPC_MP02
-% Batch LQR using sparse movement primitives with Fourier basis functions
-%
-% If this code is useful for your research, please cite the related publication:
-% @incollection{Calinon19MM,
-% author="Calinon, S.",
-% title="Mixture Models for the Analysis, Edition, and Synthesis of Continuous Time Series",
-% booktitle="Mixture Models and Applications",
-% publisher="Springer",
-% editor="Bouguila, N. and Fan, W.",
-% year="2019",
-% pages="39--57",
-% doi="10.1007/978-3-030-23876-6_3"
-% }
-%
-% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
-% Written by Sylvain Calinon, http://calinon.ch/
-%
-% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
-%
-% PbDlib is free software: you can redistribute it and/or modify
-% it under the terms of the GNU General Public License version 3 as
-% published by the Free Software Foundation.
-%
-% PbDlib is distributed in the hope that it will be useful,
-% but WITHOUT ANY WARRANTY; without even the implied warranty of
-% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-% GNU General Public License for more details.
-%
-% You should have received a copy of the GNU General Public License
-% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
-
-addpath('./m_fcts/');
-
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbVarPos = 2; %Dimension of position data (here: x1,x2)
-nbDeriv = 1; %Number of static & dynamic features (D=2 for [x,dx])
-nbVar = nbVarPos * nbDeriv; %Dimension of state vector
-nbData = 500; %Number of datapoints in a trajectory
-
-dt = 0.01; %Time step duration
-rfactor = 1E-8; %Control cost in LQR
-R = speye((nbData-1)*nbVarPos) * rfactor;
-
-
-%% Dynamical System settings (discrete version)
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Integration with higher order Taylor series expansion
-A1d = zeros(nbDeriv);
-for i=0:nbDeriv-1
- A1d = A1d + diag(ones(nbDeriv-i,1),i) * dt^i * 1/factorial(i); %Discrete 1D
-end
-B1d = zeros(nbDeriv,1);
-for i=1:nbDeriv
- B1d(nbDeriv-i+1) = dt^i * 1/factorial(i); %Discrete 1D
-end
-A = kron(A1d, speye(nbVarPos)); %Discrete nD
-B = kron(B1d, speye(nbVarPos)); %Discrete nD
-
-%Construct Su and Sx matrices (transfer matrices in batch LQR)
-Su = zeros(nbVar*nbData, nbVarPos*(nbData-1));
-Sx = kron(ones(nbData,1),eye(nbVar));
-M = B;
-for n=2:nbData
- %Build Sx matrix
- id1 = (n-1)*nbVar+1:nbData*nbVar;
- Sx(id1,:) = Sx(id1,:) * A;
- %Build Su matrix
- id1 = (n-1)*nbVar+1:n*nbVar;
- id2 = 1:(n-1)*nbVarPos;
- Su(id1,id2) = M;
- M = [A*M(:,1:nbVarPos), M];
-end
-
-
-%% Probabilistic movement primitives with Fourier basis functions
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% t = linspace(0, 4*pi, nbData);
-% MuTmp = [fft(cos(t) + .4 * cos(t*2+pi/3)); fft(sin(t))];
-% MuTmp = [ifft(MuTmp(1,:)); ifft(MuTmp(2,:))];
-% MuQ = MuTmp(:);
-% Q = eye(nbVar*nbData);
-
-%Generate periodic data
-nbSamples = 4; %Number of demonstrations
-for n=1:nbSamples
- t = linspace(2*n/3, 4*pi+2*n/3, nbData);
- xtmp = (1+n.*1E-2) .* [cos(t); sin(t)] + (.7+n.*1E-2) .* [zeros(1,nbData); cos(t*2-pi/3)] + randn(nbVar,nbData) .* 1E-2;
- x(:,n) = xtmp(:);
-end
-
-%Compute basis functions Psi and activation weights w
-k = -5:5;
-% nbFct = length(k);
-t = linspace(0, 1, nbData);
-phi = exp(t' * k * 2 * pi * 1i) ./ nbData;
-Psi = kron(phi, eye(nbVar));
-
-% w = (Psi' * Psi + eye(nbFct).*1E-18) \ Psi' * x;
-w = pinv(Psi) * x;
-
-%Distribution in parameter space
-Mu_R = mean(abs(w), 2); %Magnitude average
-Mu_theta = mean_angle(angle(w), 2); %Phase average
-Mu_w = Mu_R .* exp(1i * Mu_theta); %Reconstruction
-
-Sigma_R = cov(abs(w')); %Magnitude spread
-Sigma_theta = cov_angle(angle(w')); %Phase spread
-
-% %Remove correlations
-% Sigma_R = diag(diag(Sigma_R));
-% Sigma_theta = diag(diag(Sigma_theta));
-
-Sigma_w = Sigma_R .* exp(1i * Sigma_theta); %+ eye(size(Sigma_R)) * 1E-4; %Reconstruction
-
-% % [V_R, D_R] = eig(Sigma_R);
-% % [V_theta, D_theta] = eig(Sigma_theta);
-% % U_R = V_R * D_R.^.5;
-% % U_theta = V_theta * D_theta.^.5;
-% U_R = sqrtm(Sigma_R);
-% U_theta = sqrtm(Sigma_theta);
-% U_w = U_R .* exp(1i * U_theta)
-% Sigma_w = U_w * U_w';
-
-
-MuQ = Psi * Mu_w;
-Q = Psi / Sigma_w * Psi';
-
-
-%% Reproduction
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% x0 = zeros(nbVarPos,1);
-x0 = x(11:12,1); %[10; 0];
-u = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - Sx*x0));
-rx = reshape(Sx*x0+Su*u, nbVar, nbData);
-
-% uSigma = inv(Rq) .* 1E-1;
-% xSigma = Su * uSigma * Su';
-
-
-%% 2D Plots
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% rxi = imag(rx);
-rx = real(rx);
-figure('position',[10,10,2300,1200]);
-subplot(2,2,[1,3]); hold on; axis off;
-for n=1:nbSamples
- plot(x(1:2:end,n), x(2:2:end,n), '-','linewidth',4,'color',[.7,.7,.7]);
-end
-plot(MuQ(1:2:end), MuQ(2:2:end), '-','linewidth',4,'color',[.8 0 0]);
-plot(rx(1,:), rx(2,:), '-','linewidth',4,'color',[0,0,0]);
-plot(rx(1,1), rx(2,1), '.','markersize',28,'color',[0,0,0]);
-% plot(rxi(1,:), rxi(2,:), ':','linewidth',4,'color',[0,0,0]);
-axis equal;
-
-%Timeline Plots
-for j=1:2
- subplot(2,2,2*(j-1)+2); hold on; %axis off;
- for n=1:nbSamples
- plot(1:nbData, x(j:2:end,n), '-','linewidth',3,'color',[.7,.7,.7]);
- end
-% %Plot uncertainty
-% id = j:nbVar:nbVar*nbData;
-% S = diag(xSigma(id,id));
-% patch([1:nbData nbData:-1:1], [rx(j,:)-S(:)' fliplr(rx(j,:)+S(:)')], [.8 0 0],'edgecolor',[.6 0 0],'facealpha', .2, 'edgealpha', .2);
-% plot(1:nbData, rxi(j,:), ':','linewidth',4,'color',[0 0 0]);
- plot(1:nbData, rx(j,:), '-','linewidth',4,'color',[0 0 0]);
- plot(1:nbData, MuQ(j:2:end), '-','linewidth',4,'color',[.8 0 0]);
-
- set(gca,'xtick',[],'ytick',[]);
- xlabel('$t$','interpreter','latex','fontsize',34);
- ylabel(['$x_' num2str(j) '$'],'interpreter','latex','fontsize',34);
-end
-
-% print('-dpng','graphs/MPC_MP02.png');
-pause;
-close all;
-end
-
-%%%%%%%%%%%%%%%%%%
-function Mu = mean_angle(phi, dim)
- if nargin<2
- dim = 1;
- end
- Mu = angle(mean(exp(1i*phi), dim));
-end
-
-%%%%%%%%%%%%%%%%%%
-function Sigma = cov_angle(phi)
- Mu = mean_angle(phi);
- e = phi - repmat(Mu, size(phi,1), 1);
- Sigma = cov(angle(exp(1i*e)));
-end
\ No newline at end of file
diff --git a/demos/demo_MPC_infHor_bicopter01.m b/demos/demo_MPC_infHor_bicopter01.m
deleted file mode 100644
index 7ceee4c39b7e4c326997fcd3265d5a0c4ba8305c..0000000000000000000000000000000000000000
--- a/demos/demo_MPC_infHor_bicopter01.m
+++ /dev/null
@@ -1,310 +0,0 @@
-function demo_MPC_infHor_bicopter01
-% Infinite horizon LQR applied on a planar UAV with an iterative linearization of the system plant.
-%
-% If this code is useful for your research, please cite the related publication:
-% @incollection{Calinon19chapter,
-% author="Calinon, S. and Lee, D.",
-% title="Learning Control",
-% booktitle="Humanoid Robotics: a Reference",
-% publisher="Springer",
-% editor="Vadakkepat, P. and Goswami, A.",
-% year="2019",
-% pages="1261--1312",
-% doi="10.1007/978-94-007-6046-2_68"
-% }
-%
-% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
-% Written by Antonio Paolillo and Sylvain Calinon
-%
-% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
-%
-% PbDlib is free software: you can redistribute it and/or modify
-% it under the terms of the GNU General Public License version 3 as
-% published by the Free Software Foundation.
-%
-% PbDlib is distributed in the hope that it will be useful,
-% but WITHOUT ANY WARRANTY; without even the implied warranty of
-% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-% GNU General Public License for more details.
-%
-% You should have received a copy of the GNU General Public License
-% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
-
-close all;
-addpath('./m_fcts/');
-
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbData = 400; %Number of datapoints
-nbIter = 1; %Number of reproductions
-
-model.nbVarPos = 3; %Dimension of position data (here: x1,x2)
-model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
-model.nbVarX = model.nbVarPos * model.nbDeriv; %Dimension of state vector in the tangent space
-model.nbVarU = 2;
-model.dt = 1E-2; %Time step duration
-model.rfactor = 0.5; %Control cost in LQR
-
-% UAV parameters
-robot.m = 2.5; % mass [kg]
-robot.g = 9.81; % gravity acceleration [m/s^2]
-robot.l = 0.5; % arms length [m]
-robot.I = 1.2; % inertia [kg*m^2]
-
-% Control cost matrix
-R = eye(model.nbVarU) * model.rfactor;
-
-%Task description
-model.Mu = [2; 5; 0; zeros(model.nbVarPos,1)];
-model.Sigma = blkdiag(eye(model.nbVarPos-1).*1E-2, 1E-1,eye(model.nbVarPos).*1E1);
-
-% Control input at the reference point
-u0 = 0.5 * robot.m * robot.g * ones(2,1);
-
-animation = true;
-
-% Waitbar
-h_bar = waitbar(0, '0.00%','name','PROGRESS');
-
-
-%% iLQR reproduction
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-for n=1:nbIter
- u = zeros(model.nbVarU, 1);
- x = zeros(model.nbVarX, 1);
- for t=1:nbData
-
- frac = t / nbData;
- waitbar(frac, h_bar, sprintf ('Iteration n. %d of %d: %.2f%%', n, nbIter, 100*frac));
-
- % Log data for plots
- r(n).x(:,t) = x;
- r(n).u(:,t) = u;
- r(n).time(t) = t*model.dt;
-
- % Simulate UAV motion
- % 1- Update the system (acceleration)
- ddx = dynSys(x, u, robot);
-
- % 2- Update velocity (integration)
- x(model.nbVarPos+1:end) = x(model.nbVarPos+1:end) + ddx .* model.dt;
- % 3- Update position (integration)
- x(1:model.nbVarPos) = x(1:model.nbVarPos) + x(model.nbVarPos+1:end) .* model.dt;
-
- % Linearization of the system plant around the reference point
- [A, B] = computeTransferMatrices(model.Mu, u0, model, robot);
- % Linearization of the system plant around the current point
- %[A, B] = computeTransferMatrices(x, u, model, robot);
-
- % Iterative discrete LQR with infinite horizon
- % 1- Compute precision matrix
- Q = inv(model.Sigma);
- % 2- Solve Riccati's equation
- P = solveAlgebraicRiccati_eig_discrete(A, B*(R\B'), (Q+Q')/2);
- % 3- Compute the gain
- L = (B'*P*B + R) \ B'*P*A;
-
- % Control law (steering method)
- u = u0 + L * (model.Mu-x);
- end
-end
-
-close(h_bar)
-
-%% Animation
-if animation
- figure('name', 'animation');
-
- subplot(2,2,[1 3])
- hold on
- h1 = plot(zeros(1,nbData),zeros(1,nbData),'k--');
- h2 = plot(0,0,'r.','markersize',10);
- h3 = plot( zeros(3,1), zeros(3,1), 'b.-', 'linewidth',2,'markersize',10);
- h4 = plot(0,0,'k.','markersize',10,'linewidth',1.5);
-
- margin = robot.l + 0.1;
- var = [r(end).x(1,:), model.Mu(1)];
- x_min = min(var) - margin;
- x_max = max(var) + margin;
- var = [r(end).x(2,:), model.Mu(2,:)];
- y_min = min(var) - margin;
- y_max = max(var) + margin;
-
- axis([x_min x_max y_min y_max])
- xlabel('x [m]')
- ylabel('y [m]')
- axis equal; grid on; axis on; box on;
-
- subplot(2,2,2)
- hold on;
- h5 = plot(0,0,'k.','markersize',5,'linewidth',2);
- h6 = plot(zeros(1,nbData), zeros(1,nbData),'k');
- var = r(end).u(1,:); margin = 0.05;
- x_min = min(r(end).time(:)); x_max = max(r(end).time(:));
- margin = 0.1*abs(max(var)-min(var));
- y_min = min(var) - margin;
- y_max = max(var) + margin;
- axis([x_min x_max y_min y_max]);
- axis on; grid on; box on;
- xlabel('time [s]')
- ylabel('u_1 [kg m s^{-2}]')
-
- subplot(2,2,4)
- hold on;
- h7 = plot(0,0,'k.','markersize',5,'linewidth',2);
- h8 = plot(zeros(1,nbData), zeros(1,nbData),'k');
- var = r(end).u(2,:); margin = 0.05;
- margin = 0.1*abs(max(var)-min(var));
- y_min = min(var) - margin;
- y_max = max(var) + margin;
- axis([x_min x_max y_min y_max]);
- axis on; grid on; box on;
- xlabel('time [s]')
- ylabel('u_2 [kg m s^{-2}]')
-
- for i=1:nbData
- tic
- x_left = [r(end).x(1,i)-robot.l*cos(r(end).x(3,i)); r(end).x(2,i)-robot.l*sin(r(end).x(3,i))];
- x_right= [r(end).x(1,i)+robot.l*cos(r(end).x(3,i)); r(end).x(2,i)+robot.l*sin(r(end).x(3,i))];
- set(h1, 'xdata', [zeros(1,nbData-i),r(end).x(1,1:i)], 'ydata', [zeros(1,nbData-i),r(end).x(2,1:i)]);
- set(h2, 'xdata', model.Mu(1), 'ydata', model.Mu(2));
- set(h3, 'xdata', [x_left(1); r(end).x(1,i); x_right(1)],'ydata',[x_left(2); r(end).x(2,i); x_right(2)])
- set(h4, 'xdata', r(end).x(1,1),'ydata',r(end).x(2,1));
- set(h5, 'xdata', r(end).time(i), 'ydata', r(end).u(1,i));
- set(h6, 'xdata', [zeros(1,nbData-i),r(end).time(1:i)], 'ydata', [zeros(1,nbData-i),r(end).u(1,1:i)]);
- set(h7, 'xdata', r(end).time(i), 'ydata', r(end).u(2,i));
- set(h8, 'xdata', [zeros(1,nbData-i),r(end).time(1:i)], 'ydata', [zeros(1,nbData-i),r(end).u(2,1:i)]);
- drawnow()
- pause(model.dt - 0.001*toc)
- if i==1 || i==nbData
- pause(2)
- end
- end
-end
-
-%% Plot 2D
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%figure('position',[10 10 700 700],'color',[1 1 1],'name','x1-x2 plot');
-figure('name', 'trajectory')
-hold on; axis on; grid on; box on;
-plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [1 0 0], .5);
-plot(model.Mu(1), model.Mu(2), 'r.','markersize',15);
-%Plot reproduction samples
-x_min = 0;
-x_max = 0;
-y_min = 0;
-y_max = 0;
-for n=1:nbIter
- plot(r(n).x(1,:), r(n).x(2,:), '-','linewidth',2,'color',ones(1,3)-ones(1,3).*.8.*n./nbIter);
- plot(r(n).x(1,1), r(n).x(2,1), '.','markersize',15,'color',ones(1,3)-ones(1,3).*.8.*n./nbIter);
- var = [r(n).x(1,:), model.Mu(1)];
- margin = 0.1 * abs(min(var)-max(var));
- if min(var) - margin < x_min
- x_min = min(var) - margin;
- end
- if max(var) + margin > x_max
- x_max = max(var) + margin;
- end
- var = [r(n).x(2,:), model.Mu(2,:)];
- margin = 0.1 * abs(min(var)-max(var));
- if min(var) - margin < y_min
- y_min = min(var) - margin;
- end
- if max(var) + margin > y_max
- y_max = max(var) + margin;
- end
-end
-axis([x_min x_max y_min y_max])
-xlabel('x [m]')
-ylabel('y [m]')
-axis equal;
-
-
-%% Timeline plot
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-labList = {'$x_1$','$x_2$','$x_3$','$\dot{x}_1$','$\dot{x}_2$','$\dot{x}_3$'};
-%figure('position',[720 10 1000 1300],'color',[1 1 1]);
-figure('name', 'system state')
-for j=1:model.nbVarX
- subplot(model.nbVarX,1,j);
- axis on; grid on; hold on; box on;
- if j>model.nbVarPos
- plot([1,nbData],[0,0],':','color',[.5 .5 .5]);
- end
- %Plot reproduction samples
- for n=1:nbIter
- plot(r(n).x(j,:), '-','linewidth',1,'color',ones(1,3)-ones(1,3).*.8.*n./nbIter);
- end
- ylabel(labList{j},'fontsize',14,'interpreter','latex');
-end
-xlabel('$t$','fontsize',14,'interpreter','latex');
-% control input
-figure('name','system input')
-for j=1:model.nbVarU
- subplot(model.nbVarU,1,j);
- axis on; grid on; hold on; box on;
- %Plot reproduction samples
- for n=1:nbIter
- plot(r(n).u(j,:), '-','linewidth',1,'color',ones(1,3)-ones(1,3).*.8.*n./nbIter);
- end
- ylabel(['$u_' num2str(j) '$'],'fontsize',14,'interpreter','latex');
-end
-xlabel('$t$','fontsize',14,'interpreter','latex');
-
-end
-
-
-%%%%%%%%%%%%%%%%%
-function f = dynSys(x,u,robot)
- m = robot.m;
- g = robot.g;
- l = robot.l;
- I = robot.I;
-
- f = zeros(3,1);
-
- f(1,1) = -m.^-1 .* (u(1) + u(2)) .* sin(x(3)); %ddx
- f(2,1) = m.^-1 .* (u(1) + u(2)) .* cos(x(3)) - g; %ddy
- f(3,1) = l/I .* (u(1) - u(2)); %ddt
-end
-
-%%%%%%%%%%%%%%%%%
-function [A, B] = computeTransferMatrices(x, u, model, robot)
- m = robot.m;
- l = robot.l;
- I = robot.I;
-
- dfdx = zeros(3,1);
- dfdy = zeros(3,1);
- dfdt = zeros(3,1);
- dfdu1 = zeros(3,1);
- dfdu2 = zeros(3,1);
-
- dfdx(1) = 0;
- dfdx(2) = 0;
- dfdx(3) = 0;
-
- dfdy(1) = 0;
- dfdy(2) = 0;
- dfdy(3) = 0;
-
- dfdt(1) = -m.^-1 .* (u(1) + u(2)) .* cos(x(3)); %*dx?
- dfdt(2) = -m.^-1 .* (u(1) + u(2)) .* sin(x(3)); %*dx?
- dfdt(3) = 0;
-
- dfdu1(1) = -m.^-1 .* sin(x(3));
- dfdu1(2) = m.^-1 .* cos(x(3));
- dfdu1(3) = l/I;
-
- dfdu2(1) = -m.^-1 .* sin(x(3));
- dfdu2(2) = m.^-1 .* cos(x(3));
- dfdu2(3) = -l/I;
-
- Ac = [zeros(model.nbVarPos), eye(model.nbVarPos); [dfdx, dfdy, dfdt], zeros(model.nbVarPos)];
- Bc = [zeros(model.nbVarPos,model.nbVarU); [dfdu1, dfdu2]];
-
- %Conversion to discrete linear system
- A = eye(model.nbVarX) + Ac * model.dt;
- B = Bc * model.dt;
-end
\ No newline at end of file
diff --git a/demos/demo_MPC_infHor_incremental01.m b/demos/demo_MPC_infHor_incremental01.m
deleted file mode 100644
index 409116ba4dcf05c360f4fac9cc7d3e3d7c20d422..0000000000000000000000000000000000000000
--- a/demos/demo_MPC_infHor_incremental01.m
+++ /dev/null
@@ -1,183 +0,0 @@
-function demo_MPC_infHor_incremental01
-% Infinite horizon LQR with an iterative re-estimation of the system plant linear system (example with planar UAV).
-%
-% If this code is useful for your research, please cite the related publication:
-% @incollection{Calinon19chapter,
-% author="Calinon, S. and Lee, D.",
-% title="Learning Control",
-% booktitle="Humanoid Robotics: a Reference",
-% publisher="Springer",
-% editor="Vadakkepat, P. and Goswami, A.",
-% year="2019",
-% pages="1261--1312",
-% doi="10.1007/978-94-007-6046-2_68"
-% }
-%
-% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
-% Written by Sylvain Calinon and Danilo Bruno
-%
-% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
-%
-% PbDlib is free software: you can redistribute it and/or modify
-% it under the terms of the GNU General Public License version 3 as
-% published by the Free Software Foundation.
-%
-% PbDlib is distributed in the hope that it will be useful,
-% but WITHOUT ANY WARRANTY; without even the implied warranty of
-% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-% GNU General Public License for more details.
-%
-% You should have received a copy of the GNU General Public License
-% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
-
-addpath('./m_fcts/');
-
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbData = 300; %Number of datapoints
-nbIter = 1; %Number of reproductions
-
-model.nbVarPos = 3; %Dimension of position data (here: x=x1,x2,theta)
-model.nbDeriv = 2; %Number of derivatives for the state space
-model.nbVarX = model.nbVarPos * model.nbDeriv; %Dimension of state space (here: [x,dx])
-model.nbVarU = 2; %Dimension of control command (here: force of two propellers)
-model.dt = 1E-2; %Time step duration
-model.rfactor = 1E-4; %Control cost in LQR
-
-%Control cost matrix
-R = eye(model.nbVarU) * model.rfactor;
-
-
-%% Task description
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.Mu = [1; 1; 0; zeros(model.nbVarPos,1)];
-model.Sigma = blkdiag(eye(model.nbVarPos).*1E-2, eye(model.nbVarPos).*1E-1);
-Q = inv(model.Sigma); %Precision matrix
-u00 = repmat(0.5 * 2.5 * 9.81, 2, 1);
-
-
-%% iLQR reproduction
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-for n=1:nbIter
- u = zeros(model.nbVarU, 1);
- x = zeros(model.nbVarX, 1);
- for t=1:nbData
- %Log data
- r(n).x(:,t) = x;
- r(n).u(:,t) = u;
-
- %Simulation
- ddx = dynSys(x, u); %Compute acceleration
- x(model.nbVarPos+1:end) = x(model.nbVarPos+1:end) + ddx .* model.dt; %Update velocity
- x(1:model.nbVarPos) = x(1:model.nbVarPos) + x(model.nbVarPos+1:end) .* model.dt; %Update position
-
- %Linearization of system plant
- u1 = 0.5 * ( (2.5*(ddx(2)+9.81))/cos(x(3)) + (1.2*ddx(3))/0.5 );
- u2 = 0.5 * ( (2.5*(ddx(2)+9.81))/cos(x(3)) - (1.2*ddx(3))/0.5 );
- [A, B] = computeTransferMatrices(x, [u1; u2], model);
-
- %Iterative discrete LQR with infinite horizon
- P = solveAlgebraicRiccati_eig_discrete(A, B*(R\B'), (Q+Q')/2);
- L = (B'*P*B + R) \ B'*P*A; %Feedback gain (discrete version)
- u = L * (model.Mu-x) + u00; %Compute acceleration (with only feedback terms)
-% x = A*x + B*u;
- end
-end
-
-
-%% Plot 2D
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 700 700],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
-plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [.8 0 0], .5);
-plot(model.Mu(1,:), model.Mu(2,:), '.','markersize',20,'color',[.8 0 0]);
-%Plot reproduction samples
-for n=1:nbIter
- plot(r(n).x(1,:), r(n).x(2,:), '-','linewidth',2,'color',ones(1,3)-ones(1,3).*.8.*n./nbIter);
- plot(r(n).x(1,1), r(n).x(2,1), '.','markersize',20,'color',ones(1,3)-ones(1,3).*.8.*n./nbIter);
-end
-axis equal;
-
-
-%% Timeline plot
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-labList = {'$x_1$','$x_2$','$x_3$','$\dot{x}_1$','$\dot{x}_2$','$\dot{x}_3$'};
-figure('position',[720 10 1000 1300],'color',[1 1 1]);
-for j=1:model.nbVarX
- subplot(model.nbVarX+model.nbVarU,1,j); hold on;
- plot([1,nbData],[model.Mu(j) model.Mu(j)],':','color',[.5 .5 .5]);
- %Plot reproduction samples
- for n=1:nbIter
- plot(r(n).x(j,:), '-','linewidth',1,'color',ones(1,3)-ones(1,3).*.8.*n./nbIter);
- end
- ylabel(labList{j},'fontsize',14,'interpreter','latex');
-end
-for j=1:model.nbVarU
- subplot(model.nbVarX+model.nbVarU,1,model.nbVarX+j); hold on;
- %Plot reproduction samples
- for n=1:nbIter
- plot(r(n).u(j,:), '-','linewidth',1,'color',ones(1,3)-ones(1,3).*.8.*n./nbIter);
- end
- ylabel(['$u_' num2str(j) '$'],'fontsize',14,'interpreter','latex');
-end
-xlabel('$t$','fontsize',14,'interpreter','latex');
-
-pause;
-close all;
-end
-
-%%%%%%%%%%%%%%%%%
-function f = dynSys(x,u)
- m = 2.5;
- g = 9.81;
- l = 0.5;
- I = 1.2;
- f = zeros(3,1);
- f(1,1) = -m.^-1 .* (u(1) + u(2)) .* sin(x(3)); %ddx
- f(2,1) = m.^-1 .* (u(1) + u(2)) .* cos(x(3)) - g; %ddy
- f(3,1) = l/I .* (u(1) - u(2)); %ddt
-end
-
-%%%%%%%%%%%%%%%%%
-function [A, B] = computeTransferMatrices(x, u, model)
- m = 2.5;
-% g = 9.81;
- l = 0.5;
- I = 1.2;
-
- %syms x;
- dfdx = zeros(3,1);
- dfdy = zeros(3,1);
- dfdt = zeros(3,1);
- dfdu1 = zeros(3,1);
- dfdu2 = zeros(3,1);
-
-% f = dynSys(x(:,t), u(:,t));
-
- dfdx(1) = 0;
- dfdx(2) = 0;
- dfdx(3) = 0;
-
- dfdy(1) = 0;
- dfdy(2) = 0;
- dfdy(3) = 0;
-
- dfdt(1) = -m.^-1 .* (u(1) + u(2)) .* cos(x(3)); %*dx?
- dfdt(2) = -m.^-1 .* (u(1) + u(2)) .* sin(x(3)); %*dx?
- dfdt(3) = 0;
-
- dfdu1(1) = -m.^-1 .* sin(x(3));
- dfdu1(2) = m.^-1 .* cos(x(3));
- dfdu1(3) = l/I;
-
- dfdu2(1) = -m.^-1 .* sin(x(3));
- dfdu2(2) = m.^-1 .* cos(x(3));
- dfdu2(3) = -l/I;
-
- Ac = [zeros(model.nbVarPos), eye(model.nbVarPos); [dfdx, dfdy, dfdt], zeros(model.nbVarPos)];
- Bc = [zeros(model.nbVarPos,model.nbVarU); [dfdu1, dfdu2]];
-
- %Conversion to discrete linear system
- A = eye(model.nbVarX) + Ac * model.dt;
- B = Bc * model.dt;
-end
\ No newline at end of file
diff --git a/demos/demo_MPC_infHor_incremental02.m b/demos/demo_MPC_infHor_incremental02.m
deleted file mode 100644
index f76814f7db5b7a8d40126bee5578a7ad7f59eb82..0000000000000000000000000000000000000000
--- a/demos/demo_MPC_infHor_incremental02.m
+++ /dev/null
@@ -1,166 +0,0 @@
-function demo_MPC_infHor_incremental02
-% Infinite horizon LQR with an iterative re-estimation of the system plant linear system (example with pendulum).
-% (similar example as in "LQR-RRT∗: Optimal Sampling-Based Motion Planning with Automatically Derived Extension Heuristics")
-%
-% If this code is useful for your research, please cite the related publication:
-% @incollection{Calinon19chapter,
-% author="Calinon, S. and Lee, D.",
-% title="Learning Control",
-% booktitle="Humanoid Robotics: a Reference",
-% publisher="Springer",
-% editor="Vadakkepat, P. and Goswami, A.",
-% year="2019",
-% pages="1261--1312",
-% doi="10.1007/978-94-007-6046-2_68"
-% }
-%
-% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
-% Written by Sylvain Calinon and Danilo Bruno
-%
-% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
-%
-% PbDlib is free software: you can redistribute it and/or modify
-% it under the terms of the GNU General Public License version 3 as
-% published by the Free Software Foundation.
-%
-% PbDlib is distributed in the hope that it will be useful,
-% but WITHOUT ANY WARRANTY; without even the implied warranty of
-% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-% GNU General Public License for more details.
-%
-% You should have received a copy of the GNU General Public License
-% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
-
-addpath('./m_fcts/');
-
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbData = 200; %Number of datapoints
-nbIter = 1; %Number of reproductions
-
-model.nbVarPos = 1; %Dimension of position data (here: q)
-model.nbDeriv = 2; %Number of derivatives for the state space
-model.nbVarX = model.nbVarPos * model.nbDeriv; %Dimension of state space (here: [q,dq])
-model.nbVarU = 1; %Dimension of control command (here: torque)
-model.dt = 1E-2; %Time step duration
-model.rfactor = 1E-4; %Control cost in LQR
-
-%Control cost matrix
-R = eye(model.nbVarU) * model.rfactor;
-
-
-%% Task description
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.Mu = [pi/2; 0]; %pi/2
-model.Sigma = diag([1E-2, 1E-1]);
-Q = inv(model.Sigma); %Precision matrix
-
-
-%% iLQR reproduction
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-for n=1:nbIter
- u = 0;
- x = [-pi/2; 0];
-
- for t=1:nbData
- %Log data
- r(n).x(:,t) = x;
-
- %Simulation
- ddx = dynSys(x, u); %Compute acceleration
- x(2) = x(2) + ddx .* model.dt; %Update velocity
- x(1) = x(1) + x(2) .* model.dt; %Update position
-
- %Linearization of system plant
- [A, B] = computeTransferMatrices(x, model);
-
- %Iterative discrete LQR with infinite horizon
- P = solveAlgebraicRiccati_eig_discrete(A, B*(R\B'), (Q+Q')/2);
- K = (B'*P*B + R) \ B'*P*A; %Feedback gain (discrete version)
- u = K * (model.Mu - x); %Compute acceleration (with only feedback terms)
-% x = A*x + B*u;
-
- r(n).c(t) = (model.Mu-x)'*Q*(model.Mu-x) + u'*R*u; %Value of the cost function
- r(n).u(:,t) = u;
- end
-end
-
-
-%% Plot 2D
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 700 700],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
-plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [.8 0 0], .5);
-plot(model.Mu(1,:), model.Mu(2,:), '.','markersize',20,'color',[.8 0 0]);
-%Plot reproduction samples
-for n=1:nbIter
- plot(r(n).x(1,:), r(n).x(2,:), '-','linewidth',2,'color',ones(1,3)-ones(1,3).*.8.*n./nbIter);
- plot(r(n).x(1,1), r(n).x(2,1), '.','markersize',20,'color',ones(1,3)-ones(1,3).*.8.*n./nbIter);
-end
-axis equal;
-
-
-%% Timeline plot
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-labList = {'$x_1$','$x_2$','$x_3$','$\dot{x}_1$','$\dot{x}_2$','$\dot{x}_3$'};
-figure('position',[720 10 1000 1300],'color',[1 1 1]);
-for j=1:model.nbVarX
- subplot(model.nbVarX+model.nbVarU,1,j); hold on;
- plot([1,nbData],[model.Mu(j) model.Mu(j)],':','color',[.5 .5 .5]);
- %Plot reproduction samples
- for n=1:nbIter
- plot(r(n).x(j,:), '-','linewidth',1,'color',ones(1,3)-ones(1,3).*.8.*n./nbIter);
- end
- ylabel(labList{j},'fontsize',14,'interpreter','latex');
-end
-for j=1:model.nbVarU
- subplot(model.nbVarX+model.nbVarU,1,model.nbVarX+j); hold on;
- %Plot reproduction samples
- for n=1:nbIter
- plot(r(n).u(j,:), '-','linewidth',1,'color',ones(1,3)-ones(1,3).*.8.*n./nbIter);
- end
- ylabel(['$u_' num2str(j) '$'],'fontsize',14,'interpreter','latex');
-end
-xlabel('$t$','fontsize',14,'interpreter','latex');
-
-
-%% Animated plot
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('PaperPosition',[0 0 8 8],'position',[10,10,1000,1000],'color',[1 1 1]); hold on; axis off;
-plotArm(model.Mu(1), 1, [0;0;0], .03, [.8 0 0]);
-for t=round(linspace(1,nbData,20))
- colTmp = [.9,.9,.9] - [.7,.7,.7] * t/nbData;
- plotArm(r(1).x(1,t), 1, [0;0;t*0.1], .02, colTmp);
-end
-axis equal;
-
-figure; hold on;
-plot(r(1).c,'k-');
-
-pause;
-close all;
-end
-
-%%%%%%%%%%%%%%%%%
-function f = dynSys(x,u)
- b = 0.1;
- g = 9.81;
- f = u - b .* x(2,:) - g .* cos(x(1,:));
-end
-
-%%%%%%%%%%%%%%%%%
-function [A, B] = computeTransferMatrices(x, model)
- b = 0.1;
- g = 9.81;
-
- dfdx = g .* sin(x(1,:));
- dfddx = -b;
- dfdu = 1;
-
- Ac = [0, 1; dfdx, dfddx];
- Bc = [0; dfdu];
-
- %Conversion to discrete linear system
- A = eye(model.nbVarX) + Ac * model.dt;
- B = Bc * model.dt;
-end
\ No newline at end of file
diff --git a/demos/demo_MPC_iterativeLQR04.m b/demos/demo_MPC_iterativeLQR04.m
deleted file mode 100644
index 6f90751747d8093d6a2c7ec7e1e244593e7eb1ea..0000000000000000000000000000000000000000
--- a/demos/demo_MPC_iterativeLQR04.m
+++ /dev/null
@@ -1,123 +0,0 @@
-function demo_MPC_iterativeLQR04
-% Control of a spring attached to a point with linear quadratic tracking (with feedback and feedforward terms using the recursive formulation).
-%
-% If this code is useful for your research, please cite the related publication:
-% @incollection{Calinon19chapter,
-% author="Calinon, S. and Lee, D.",
-% title="Learning Control",
-% booktitle="Humanoid Robotics: a Reference",
-% publisher="Springer",
-% editor="Vadakkepat, P. and Goswami, A.",
-% year="2019",
-% pages="1261--1312",
-% doi="10.1007/978-94-007-6046-2_68"
-% }
-%
-% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
-% Written by Sylvain Calinon, http://calinon.ch/
-%
-% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
-%
-% PbDlib is free software: you can redistribute it and/or modify
-% it under the terms of the GNU General Public License version 3 as
-% published by the Free Software Foundation.
-%
-% PbDlib is distributed in the hope that it will be useful,
-% but WITHOUT ANY WARRANTY; without even the implied warranty of
-% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-% GNU General Public License for more details.
-%
-% You should have received a copy of the GNU General Public License
-% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
-
-addpath('./m_fcts/');
-
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbVarPos = 2; %Dimension of position data (here: x1,x2)
-nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
-nbVar = nbVarPos * (nbDeriv+1); %Dimension of state vector
-dt = 1E-2; %Time step duration
-rfactor = 1E-4; %Control cost in LQR
-kP = 0; %Stiffness gain (initial estimate)
-kV = (2*kP)^.5; %Damping gain (with ideal underdamped damping ratio)
-%kV = 2*kP^.5; %Damping (for critically damped system)
-
-nbRepros = 5; %Number of reproductions
-nbData = 200; %Number of datapoints
-
-
-%% Dynamical System settings (discrete version)
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Dynamical System settings (discrete version)
-Ac = kron([0, 1, 0; -kP, -kV, kP; 0, 0, 0], eye(nbVarPos));
-A = eye(nbVar) + Ac * dt;
-B = kron([dt^2/2; dt; 0], eye(nbVarPos));
-%Control cost matrix
-R = eye(nbVarPos) * rfactor;
-
-
-%% Setting tracking task
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-xTar = [0; 3]; %Equilibrium point of the spring
-Mu = [zeros(nbVarPos,1); zeros(nbVarPos,1); xTar];
-Gamma = blkdiag(eye(nbVarPos)*1E1, zeros(nbVarPos), zeros(nbVarPos));
-
-
-%% Iterative LQR reproduction (finite horizon, discrete version)
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-P = zeros(nbVar,nbVar,nbData);
-P(:,:,end) = Gamma;
-d = zeros(nbVar, nbData);
-Q = Gamma;
-
-%Backward computation
-for t=nbData-1:-1:1
- P(:,:,t) = Q - A' * (P(:,:,t+1) * B / (B' * P(:,:,t+1) * B + R) * B' * P(:,:,t+1) - P(:,:,t+1)) * A;
- d(:,t) = (A' - A' * P(:,:,t+1) * B / (R + B' * P(:,:,t+1) * B) * B' ) * (P(:,:,t+1) * (A * Mu - Mu) + d(:,t+1));
-end
-
-%Reproduction with feedback (FB) and feedforward (FF) terms
-for n=1:nbRepros
- X = [10; 10; zeros(2,1); xTar] + [randn(2,1)*3E0; zeros(4,1)];
- r(n).X0 = X;
- for t=1:nbData
- r(n).Data(:,t) = X; %Log data
- K = (B' * P(:,:,t) * B + R) \ B' * P(:,:,t) * A; %FB gain
-
-% %Test ratio between kp and kv
-% kp = eigs(K(:,1:2));
-% kv = eigs(K(:,3:4));
-% ratio = kv ./ (2.*kp).^.5
-
-% figure; hold on;
-% plotGMM(zeros(2,1), K(:,1:2), [.8 0 0],.3);
-% plotGMM(zeros(2,1), K(:,3:4), [.8 0 0],.3);
-% axis equal;
-% pause;
-% close all;
-
- M = -(B' * P(:,:,t) * B + R) \ B' * (P(:,:,t) * (A * Mu - Mu) + d(:,t)); %FF term
- u = K * (Mu - X) + M; %Acceleration command with FB and FF terms
- X = A * X + B * u; %Update of state vector
- end
-end
-
-
-%% Plot
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1200 1200],'color',[1 1 1]); hold on; axis off;
-for n=1:nbRepros
- plot(r(n).Data(1,:), r(n).Data(2,:), '-','linewidth',1,'color',[.8 0 0]);
- plot(r(n).Data(1,:), r(n).Data(2,:), '.','markersize',6,'color',[.6 0 0]);
-end
-%plotGMM(Mu(1:2,:), Sigma(1:2,1:2,:), [0.5 0.5 0.5]);
-h(1) = plot(Mu(1,:), Mu(2,:), 'k.','markersize',20, 'color', [0.5 0.5 0.5]);
-h(2) = plot(xTar(1),xTar(2),'k+','markersize',20,'linewidth',2);
-legend(h,{'Target to reach','Equilibrium point of the spring'});
-axis equal;
-
-%print('-dpng','graphs/demo_iterativeLQR04.png');
-pause;
-close all;
\ No newline at end of file
diff --git a/demos/demo_MPC_iterativeLQR_augmSigma_online01.m b/demos/demo_MPC_iterativeLQR_augmSigma_online01.m
deleted file mode 100644
index ffdb1e2489ba1f44609486cff42ae736418df72c..0000000000000000000000000000000000000000
--- a/demos/demo_MPC_iterativeLQR_augmSigma_online01.m
+++ /dev/null
@@ -1,159 +0,0 @@
-function demo_MPC_iterativeLQR_augmSigma_online01
-% Recursive LQR with augmented covariance to transform the tracking problem to a regulation problem, recomputed in an online manner.
-%
-% If this code is useful for your research, please cite the related publication:
-% @incollection{Calinon19chapter,
-% author="Calinon, S. and Lee, D.",
-% title="Learning Control",
-% booktitle="Humanoid Robotics: a Reference",
-% publisher="Springer",
-% editor="Vadakkepat, P. and Goswami, A.",
-% year="2019",
-% pages="1261--1312",
-% doi="10.1007/978-94-007-6046-2_68"
-% }
-%
-% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
-% Written by Sylvain Calinon, http://calinon.ch/
-%
-% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
-%
-% PbDlib is free software: you can redistribute it and/or modify
-% it under the terms of the GNU General Public License version 3 as
-% published by the Free Software Foundation.
-%
-% PbDlib is distributed in the hope that it will be useful,
-% but WITHOUT ANY WARRANTY; without even the implied warranty of
-% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-% GNU General Public License for more details.
-%
-% You should have received a copy of the GNU General Public License
-% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
-
-addpath('./m_fcts/');
-
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbSamples = 5; %Number of demonstrations
-nbRepros = 1; %Number of reproductions
-nbData = 100; %Number of datapoints
-nbD = 20; %Time window for LQR computation
-
-model.nbStates = 5; %Number of Gaussians in the GMM
-model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
-model.nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
-model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
-model.dt = 0.01; %Time step duration
-model.rfactor = 1E-6; %Control cost in LQR
-
-%Control cost matrix
-R = eye(model.nbVarPos) * model.rfactor;
-
-
-%% Dynamical System settings (discrete version)
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Integration with higher order Taylor series expansion
-A1d = zeros(model.nbDeriv);
-for i=0:model.nbDeriv-1
- A1d = A1d + diag(ones(model.nbDeriv-i,1),i) * model.dt^i * 1/factorial(i); %Discrete 1D
-end
-B1d = zeros(model.nbDeriv,1);
-for i=1:model.nbDeriv
- B1d(model.nbDeriv-i+1) = model.dt^i * 1/factorial(i); %Discrete 1D
-end
-A0 = kron(A1d, eye(model.nbVarPos)); %Discrete nD
-B0 = kron(B1d, eye(model.nbVarPos)); %Discrete nD
-
-A = [A0, zeros(model.nbVar,1); zeros(1,model.nbVar), 1]; %Augmented A
-B = [B0; zeros(1,model.nbVarPos)]; %Augmented B
-
-
-%% Load handwriting data
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-load('data/2Dletters/A.mat');
-Data=[];
-for n=1:nbSamples
- s(n).Data=[];
- for m=1:model.nbDeriv
- if m==1
- dTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
- else
- dTmp = gradient(dTmp) / model.dt; %Compute derivatives
- end
- s(n).Data = [s(n).Data; dTmp];
- end
- Data = [Data s(n).Data];
-end
-
-
-%% Learning
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model = init_GMM_kbins(Data,model,nbSamples);
-[model, H] = EM_GMM(Data, model);
-
-%Transform model to the corresponding version with augmented covariance
-model0 = model;
-model.nbVar = model0.nbVar+1;
-model.Mu = zeros(model.nbVar, model.nbStates);
-model.Sigma = zeros(model.nbVar, model.nbVar, model.nbStates);
-for i=1:model.nbStates
- model.Sigma(:,:,i) = [model0.Sigma(:,:,i)+model0.Mu(:,i)*model0.Mu(:,i)', model0.Mu(:,i); model0.Mu(:,i)', 1];
-end
-
-
-%% Iterative LQR with augmented state space, recomputed in an online manner
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-tic
-for n=1:nbRepros
- X = [Data(:,1); 1];
- for t=1:nbData
- r(n).Data(:,t) = X; %Log data
- %Set list of states according to first demonstration (alternatively, an HSMM can be used)
- id = [t:min(t+nbD-1,nbData), repmat(nbData,1,t-nbData+nbD-1)];
- [~,q] = max(H(:,id),[],1); %works also for nbStates=1
- %Riccati equation
- P = zeros(model.nbVar,model.nbVar,nbD);
- P(:,:,end) = inv(model.Sigma(:,:,q(end)));
- for s=nbD-1:-1:1
- Q = inv(model.Sigma(:,:,q(s)));
- P(:,:,s) = Q - A' * (P(:,:,s+1) * B / (B' * P(:,:,s+1) * B + R) * B' * P(:,:,s+1) - P(:,:,s+1)) * A;
- end
- K = (B' * P(:,:,1) * B + R) \ B' * P(:,:,1) * A; %FB gain
- u = -K * X; %Acceleration command with FB terms on augmented state (resulting in FB and FF terms)
- X = A * X + B * u; %Update of state vector
- end
-end
-toc
-
-%% Plot
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Plot position
-figure('position',[10 10 1000 1000],'color',[1 1 1]); hold on;
-plotGMM(model0.Mu(1:2,:), model0.Sigma(1:2,1:2,:), [0.5 0.5 0.5], .3);
-for n=1:nbSamples
- plot(Data(1,(n-1)*nbData+1:n*nbData), Data(2,(n-1)*nbData+1:n*nbData), '-','color',[.7 .7 .7]);
-end
-for n=1:nbRepros
- h(1) = plot(r(n).Data(1,:), r(n).Data(2,:), '-','linewidth',2,'color',[.8 0 0]);
-end
-axis equal;
-xlabel('x_1'); ylabel('x_2');
-
-% %Plot velocity
-% figure('position',[1020 10 1000 1000],'color',[1 1 1]); hold on;
-% plotGMM(model0.Mu(3:4,:), model0.Sigma(3:4,3:4,:), [0.5 0.5 0.5], .3);
-% for n=1:nbSamples
-% plot(Data(3,(n-1)*nbData+1:n*nbData), Data(4,(n-1)*nbData+1:n*nbData), '-','color',[.7 .7 .7]);
-% end
-% for n=1:nbRepros
-% plot(r(n).Data(3,:), r(n).Data(4,:), '-','linewidth',2,'color',[.8 0 0]);
-% plot(r(n).Data(3,1), r(n).Data(4,1), '.','markersize',18,'color',[.6 0 0]);
-% end
-% plot(0,0,'k+');
-% axis equal;
-% xlabel('dx_1'); ylabel('dx_2');
-
-%print('-dpng','graphs/demo_iterativeLQR_augmSigma_online01.png');
-pause;
-close all;
\ No newline at end of file
diff --git a/demos/demo_MPC_robot01.m b/demos/demo_MPC_robot01.m
deleted file mode 100644
index ba1b8c9a8403d84b6fb039699736eb12b7f7a325..0000000000000000000000000000000000000000
--- a/demos/demo_MPC_robot01.m
+++ /dev/null
@@ -1,198 +0,0 @@
-function demo_MPC_robot01
-% Robot manipulator tracking task with MPC, by relinearization of the system plant at each time step.
-%
-% If this code is useful for your research, please cite the related publication:
-% @incollection{Calinon19chapter,
-% author="Calinon, S. and Lee, D.",
-% title="Learning Control",
-% booktitle="Humanoid Robotics: a Reference",
-% publisher="Springer",
-% editor="Vadakkepat, P. and Goswami, A.",
-% year="2019",
-% pages="1261--1312",
-% doi="10.1007/978-94-007-6046-2_68"
-% }
-%
-% The commented parts of this demo require the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
-% First run 'startup_rvc' from the robotics toolbox if you uncomment these parts.
-%
-% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
-% Written by Sylvain Calinon, http://calinon.ch/
-%
-% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
-%
-% PbDlib is free software: you can redistribute it and/or modify
-% it under the terms of the GNU General Public License version 3 as
-% published by the Free Software Foundation.
-%
-% PbDlib is distributed in the hope that it will be useful,
-% but WITHOUT ANY WARRANTY; without even the implied warranty of
-% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-% GNU General Public License for more details.
-%
-% You should have received a copy of the GNU General Public License
-% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
-
-addpath('./m_fcts/');
-
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbData = 50; %Number of datapoints in a trajectory
-nbD = 10; %Time horizon
-nbDOFs = 3; %Number of articulations
-
-model.nbStates = 1; %Number of Gaussians in the GMM
-model.nbVarPos = 2; %Dimension of position data
-model.nbDeriv = 1; %Number of static & dynamic features (D=2 for [x,dx])
-model.nbVarX = model.nbVarPos * model.nbDeriv; %Dimension of state space
-model.nbVarU = nbDOFs; %Dimension of control space
-
-model.dt = 1E-3; %Time step duration
-model.rfactor = 1E-2; %Control cost in LQR
-
-%Control cost matrix
-R = eye(model.nbVarU) * model.rfactor;
-R = kron(eye(nbD-1),R);
-
-%Robot description
-model.l = 0.6;
-
-% L1 = Link('d', 0, 'a', model.l, 'alpha', 0);
-% robot = SerialLink(repmat(L1,nbDOFs,1));
-
-
-%% Task description
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.Mu = ones(model.nbVarPos,1) * .7;
-model.Sigma = eye(model.nbVarPos) * 1E-3;
-MuQ = kron(ones(nbD,1), model.Mu);
-Q = kron(eye(nbD), inv(model.Sigma));
-
-
-%% Online batch LQR with relinearization of the system plant
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-q = ones(nbDOFs,1) * 0.1;
-% Htmp = robot.fkine(q);
-% x = Htmp.t(1:model.nbVarPos);
-x = fkine(q, model);
-
-for t=1:nbData
-% [Su, Sx] = computeTransferMatrices(q, model, robot, nbD); %Linearization of system plant
- [Su, Sx] = computeTransferMatrices(q, model, nbD); %Linearization of system plant
- u = (Su' * Q * Su + R) \ Su' * Q * (MuQ - Sx * x); %Control trajectory
- %Log data
- r(1).q(:,t) = q;
- r(1).x(:,t) = x;
- r(1).u(:,t) = u(1:model.nbVarU);
- %Update state
- q = q + u(1:model.nbVarU) * model.dt;
-% Htmp = robot.fkine(q); %Forward kinematics
-% x = Htmp.t(1:model.nbVarPos);
- x = fkine(q, model);
-end
-
-
-%% Plot 2D
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 800 800],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
-for t=round(linspace(1,nbData,2))
- colTmp = [.9,.9,.9] - [.7,.7,.7] * t/nbData;
- plotArm(r(1).q(:,t), ones(nbDOFs,1)*model.l, [0;0;-10+t*0.1], .06, colTmp);
-end
-% plotArm(model.Mu, ones(nbDOFs,1)*model.l, [0;0;10], .02, [.8 0 0]);
-
-plotGMM(model.Mu, model.Sigma, [.8 0 0], .5);
-plot(model.Mu(1,:), model.Mu(2,:), '.','markersize',20,'color',[.8 0 0]);
-for n=1:1
- plot(r(n).x(1,:), r(n).x(2,:), '-','linewidth',2,'color',[0 0 0]);
- plot(r(n).x(1,:), r(n).x(2,:), '.','markersize',12,'color',[.2 .2 .2]);
-end
-axis equal;
-
-
-%% Timeline plot
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-labList = {'$x_1$','$x_2$','$\dot{x}_1$','$\dot{x}_2$'};
-figure('position',[830 10 800 800],'color',[1 1 1]);
-for j=1:model.nbVarX
- subplot(model.nbVarX+model.nbVarU,1,j); hold on;
- plot([1,nbData],[model.Mu(j) model.Mu(j)],':','color',[.5 .5 .5]);
- %Plot reproduction samples
- for n=1:1
- plot(r(n).x(j,:), '-','linewidth',1,'color',[0 0 0]);
- end
- errorbar(nbData, model.Mu(j), model.Sigma(j,j)^.5, 'color',[.8 0 0]);
- plot(nbData, model.Mu(j), '.','markersize',15,'color',[.8 0 0]);
- ylabel(labList{j},'fontsize',14,'interpreter','latex');
-end
-for j=1:model.nbVarU
- subplot(model.nbVarX+model.nbVarU,1,model.nbVarX+j); hold on;
- %Plot reproduction samples
- for n=1:1
- plot(r(n).u(j,:), '-','linewidth',1,'color',[0 0 0]);
- end
- ylabel(['$u_' num2str(j) '$'],'fontsize',14,'interpreter','latex');
-end
-xlabel('$t$','fontsize',14,'interpreter','latex');
-
-%print('-dpng','graphs/demo_MPC_robot01.png');
-pause;
-close all;
-end
-
-%%%%%%%%%%%%%%%%%%%%%%
-function [Su, Sx] = transferMatrices(A, B, nbD)
- [nbVarX, nbVarU] = size(B);
- Su = zeros(nbVarX*nbD, nbVarU*(nbD-1));
- Sx = kron(ones(nbD,1), eye(nbVarX));
- M = B;
- for t=2:nbD
- id1 = (t-1)*nbVarX+1:nbD*nbVarX;
- Sx(id1,:) = Sx(id1,:) * A;
- id1 = (t-1)*nbVarX+1:t*nbVarX;
- id2 = 1:(t-1)*nbVarU;
- Su(id1,id2) = M;
- M = [A*M(:,1:nbVarU), M]; %Also M = [A^(n-1)*B, M] or M = [Sx(id1,:)*B, M]
- end
-end
-
-%%%%%%%%%%%%%%%%%%%%%%
-% function [Su, Sx, A, B] = computeTransferMatrices(q, model, robot, nbD)
-function [Su, Sx, A, B] = computeTransferMatrices(q, model, nbD)
-% J = robot.jacob0(q,'trans');
- J = jacob0(q, model);
- %Discrete linear system x(t+1) = x(t) + J * dq
- A = eye(model.nbVarX);
- B = J * model.dt;
- [Su, Sx] = transferMatrices(A, B, nbD);
-end
-
-%%%%%%%%%%%%%%%%%%%%%%
-%Forward kinematics
-function [x, Tf] = fkine(q, model)
- Tf = eye(4);
- T = repmat(Tf, [1,1,size(q,1)]);
- for n=1:size(q,1)
- c = cos(q(n));
- s = sin(q(n));
- T(:,:,n) = [c, -s, 0, model.l * c; ...
- s, c, 0, model.l * s; ...
- 0, 0, 1, 0;
- 0, 0, 0, 1]; %Homogeneous matrix
- Tf = Tf * T(:,:,n);
- end
- x = Tf(1:2,end);
-end
-
-%%%%%%%%%%%%%%%%%%%%%%
-%Jacobian with numerical computation
-function J = jacob0(q, model)
- e = 1E-4;
- J = zeros(2,size(q,1));
- for n=1:size(q,1)
- qtmp = q;
- qtmp(n) = qtmp(n) + e;
- J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
- end
-end
\ No newline at end of file
diff --git a/demos/demo_OC_DDP_bicopter01.m b/demos/demo_OC_DDP_bicopter01.m
new file mode 100644
index 0000000000000000000000000000000000000000..ec201b75164f01d9c4c86e35814efe636f821616
--- /dev/null
+++ b/demos/demo_OC_DDP_bicopter01.m
@@ -0,0 +1,279 @@
+function demo_OC_DDP_bicopter01
+% iLQR applied to a bicopter problem
+%
+% Sylvain Calinon, 2020
+%
+% Reference:
+% W. Li and E. Todorov, "Iterative linear quadratic regulator design for nonlinear biological movement systems,"
+% in Intl Conf. on InfoRatics in Control, Automation and Robotics, 2004, pp. 222–229
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.sim_time = 3; %Simulation time
+model.dt = 1E-1; %Time step size
+model.nbData = model.sim_time / model.dt; %Number of datapoints
+model.nbIter = 20; %Number of iterations for iLQR
+
+model.nbVarPos = 3; %Dimension of position (x1,x2,theta)
+model.nbDeriv = 2; %Number of derivatives (nbDeriv=2 for [x; dx] state)
+model.nbVarX = model.nbVarPos * model.nbDeriv; %State space dimension
+model.nbVarU = 2; %Control space dimension (u1,u2)
+
+model.rfactor = 1E-6; %Control weight teR
+model.Mu = [4; 4; 0; zeros(model.nbVarPos,1)]; %Target
+
+model.m = 2.5; %Mass
+model.l = 0.5; %Length
+model.g = 9.81; %Acceleration due to gravity
+model.I = 1.2; %Inertia
+
+R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix (at trajectory level)
+Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
+
+Q = blkdiag(sparse((model.nbData-1)*model.nbVarX, (model.nbData-1)*model.nbVarX), diag([1E2, 1E2, 1E1, 1E0, 1E0, 1E0])); %Sparse precision matrix (at trajectory level)
+MuQ = [sparse((model.nbData-1)*model.nbVarX,1); model.Mu]; %Sparse reference (at trajectory level)
+
+
+%% Iterative LQR (iLQR)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% delta_x0 = zeros(model.nbVarX, 1);
+% u = zeros(model.nbVarU, model.nbData-1);
+u0 = 0.5 * model.m * model.g * ones(model.nbVarU,1);
+u = repmat(u0, 1, model.nbData-1);
+x0 = zeros(model.nbVarX,1);
+% x0 = [-1; 1; -pi/8; .3; .1; .2];
+
+% uSigma = zeros(model.nbVarU*(model.nbData-1));
+% xSigma = zeros(model.nbVarX*model.nbData);
+
+for n=1:model.nbIter
+ %System evolution
+ x = dynSysSimulation(x0, u, model);
+% if n>1
+% x = reshape(Sx * x0 + Su * u(:), model.nbVarX, model.nbData); %Approximated dynamics
+% end
+
+ %Linearization
+ [A, B] = linSys(x, u, model);
+ [Su, Sx] = transferMatrices(A, B);
+
+% du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:) - Su' * Q * Sx * delta_x0); %Control trajectory
+ du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:)); %Control trajectory
+ delta_u = reshape(du_vec, model.nbVarU, model.nbData-1);
+% delta_x = reshape(Sx * delta_x0 + Su * delta_u(:), model.nbVarX, model.nbData); %Forward recursion
+
+ %Estimate step size with line search method
+ alpha = 1;
+ cost0 = (MuQ - x(:))' * Q * (MuQ - x(:)) + u(:)' * R * u(:);
+ while 1
+ utmp = u + delta_u * alpha;
+ xtmp = dynSysSimulation(x0, utmp, model);
+% xtmp = reshape(Sx * x0 + Su * utmp(:), model.nbVarX, model.nbData); %Approximated dynamics
+
+ cost = (MuQ - xtmp(:))' * Q * (MuQ - xtmp(:)) + utmp(:)' * R * utmp(:); %Compute the cost
+ if cost < cost0 || alpha < 1E-4
+ break;
+ end
+ alpha = alpha * 0.5;
+ end
+ disp(alpha);
+ u = u + delta_u * alpha; %Update control by following gradient
+
+% uSigmaTmp = inv(Su' * Q * Su + R);
+% uSigma = uSigma + uSigmaTmp * alpha^2;
+% xSigma = xSigma + Su * uSigmaTmp * Su' * alpha^2;
+
+ %Log data
+ r(n).x = x;
+ r(n).u = u;
+end
+
+
+%% Stochastic sampling by exploiting the nullspace structure of the problem
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% %Standard stochastic sampling
+% uSigma = inv(Su' * Q * Su + R);
+% % xSigma = Su * uSigma * Su';
+% [V, D] = eig(full(uSigma));
+% U = real(V * D.^.5);
+% nbSamples = 50;
+% sx = zeros(model.nbData*model.nbVarX, nbSamples);
+% for n=1:nbSamples
+% utmp = u(:) + U * randn(model.nbVarU*(model.nbData-1), 1) * 1E-3;
+% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
+% sx(:,n) = xtmp(:);
+% end
+
+% %Generate structured stochastic u through Bezier curves
+% nbSamples = 50;
+% nbRBF = 18;
+% H = zeros(nbRBF, model.nbData-1);
+% tl = linspace(0, 1, model.nbData-1);
+% nbDeg = nbRBF - 1;
+% for i=0:nbDeg
+% H(i+1,:) = factorial(nbDeg) / (factorial(i) * factorial(nbDeg-i)) * (1-tl).^(nbDeg-i) .* tl.^i; %Bernstein basis functions
+% end
+% %Nullspace planning
+% [V,D] = eig(full(Q));
+% U = V * D.^.5;
+% N = eye((model.nbData-1)*model.nbVarU) - Su' * U / (U' * (Su * Su') * U + Rx) * U' * Su; %Nullspace projection matrix
+% sx = zeros(model.nbData*model.nbVarX, nbSamples);
+% for n=1:nbSamples
+% w = randn(model.nbVarU, nbRBF) * 1E0; %Random weights
+% un = w * H; %Reconstruction of signals by weighted superposition of basis functions
+% utmp = u(:) + N * un(:);
+% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
+% sx(:,n) = xtmp(:);
+% end
+
+
+%% Plot state space
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,800,800]); hold on; axis off; grid on; box on;
+% for t=1:model.nbData
+% plotGMM(r(end).x(1:2,t), xSigma(model.nbVarX*(t-1)+[1,2],model.nbVarX*(t-1)+[1,2]).*1E-5, [.8 0 0], .1);
+% end
+
+%iLQR refinement steps
+for k=1:length(r)
+ plot(r(k).x(1,:), r(k).x(2,:), '-','linewidth',1,'color',[.7 .7 .7]-.7*k/length(r));
+end
+
+% %Stochastic samples
+% for n=1:nbSamples
+% plot(sx(1:model.nbVarX:end,n), sx(2:model.nbVarX:end,n), '-','linewidth',1,'color',(ones(1,3)-n/nbSamples)*.9);
+% end
+
+% plot(r(end).x(1,:),r(end).x(2,:), '-','linewidth',2,'color',[.8 0 0]);
+h(1) = plot(r(end).x(1,1),r(end).x(2,1), '.','markersize',40,'color',[0 0 0]);
+%Animation
+for t=floor(linspace(1,model.nbData,20))
+ plot(r(end).x(1,t), r(end).x(2,t), '.','markersize',40,'color',[0 0 0]);
+ msh = [r(end).x(1:2,t), r(end).x(1:2,t)] + [cos(r(end).x(3,t)); sin(r(end).x(3,t))] * [-.4, .4];
+ plot(msh(1,:), msh(2,:), '-','linewidth',5,'color',[0 0 0]);
+end
+msh = [model.Mu(1:2), model.Mu(1:2)] + [cos(model.Mu(3)); sin(model.Mu(3))] * [-.4, .4];
+plot(msh(1,:), msh(2,:), '-','linewidth',5,'color',[.8 0 0]);
+h(2) = plot(model.Mu(1), model.Mu(2), '.','markersize',40,'color',[.8 0 0]);
+legend(h,{'Starting point','Ending point'},'location','southeast','fontsize',40);
+axis equal; axis([-.5 4.5 -.2 4.4]);
+xlabel('x_1'); ylabel('x_2');
+print('-dpng','graphs/DDP_bicopter01.png');
+
+
+% %% Plot control space
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[1030,10,1000,1000]);
+% t = model.dt:model.dt:model.dt*model.nbData;
+% for kk=1:size(r(1).u,1)
+% subplot(size(r(1).u,1), 1, kk); grid on; hold on; box on; box on;
+% for k=1:length(r)
+% plot(t(1:end-1), r(k).u(kk,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
+% end
+% plot(t(1:end-1), r(end).u(kk,:), '-','linewidth',2,'color',[0 0 0]);
+% axis([min(t), max(t(1:end-1)), min(r(end).u(kk,:))-2, max(r(end).u(kk,:))+2]);
+% xlabel('t'); ylabel(['u_' num2str(kk)]);
+% end
+
+
+% %% Plot animated robot
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10,10,1000,1000]); hold on; axis off;
+% plot(model.Mu(1), model.Mu(2), '.','markersize',40,'color',[.8 0 0]);
+% msh = [model.Mu(1:2), model.Mu(1:2)] + [cos(model.Mu(3)); sin(model.Mu(3))] * [-.4, .4];
+% plot(msh(1,:), msh(2,:), '-','linewidth',5,'color',[.8 0 0]);
+% axis equal; axis([-.5 4.5 -.2 4.2]);
+% for t=1:model.nbData
+% h(1) = plot(r(end).x(1,t), r(end).x(2,t), '.','markersize',40,'color',[0 0 0]);
+% msh = [r(end).x(1:2,t), r(end).x(1:2,t)] + [cos(r(end).x(3,t)); sin(r(end).x(3,t))] * [-.4, .4];
+% h(2) = plot(msh(1,:), msh(2,:), '-','linewidth',5,'color',[0 0 0]);
+% drawnow;
+% % print('-dpng',['graphs/anim/DDP_bicopter_anim' num2str(t,'%.3d') '.png']);
+% if t<model.nbData
+% delete(h);
+% end
+% end
+% % for t=model.nbData+1:model.nbData+10
+% % print('-dpng',['graphs/anim/DDP_bicopter_anim' num2str(t,'%.3d') '.png']);
+% % end
+
+pause;
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+function [Su, Sx] = transferMatrices(A, B)
+ [nbVarX, nbVarU, nbData] = size(B);
+ nbData = nbData+1;
+ Sx = kron(ones(nbData,1), speye(nbVarX));
+ Su = sparse(zeros(nbVarX*(nbData-1), nbVarU*(nbData-1)));
+ for t=1:nbData-1
+ id1 = (t-1)*nbVarX+1:t*nbVarX;
+ id2 = t*nbVarX+1:(t+1)*nbVarX;
+ id3 = (t-1)*nbVarU+1:t*nbVarU;
+ Sx(id2,:) = squeeze(A(:,:,t)) * Sx(id1,:);
+ Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
+ Su(id2,id3) = B(:,:,t);
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Given the control trajectory u and initial state x0, compute the whole state trajectory
+function x = dynSysSimulation(x0, u, model)
+ m = model.m;
+ l = model.l;
+ I = model.I;
+ g = model.g;
+ x = zeros(model.nbVarX, model.nbData);
+ f = zeros(model.nbVarX, 1);
+ x(:,1) = x0;
+ for t=1:model.nbData-1
+ f(1) = x(4,t);
+ f(2) = x(5,t);
+ f(3) = x(6,t);
+ f(4) = -m^-1 * (u(1,t) + u(2,t)) * sin(x(3,t));
+ f(5) = m^-1 * (u(1,t) + u(2,t)) * cos(x(3,t)) - g;
+ f(6) = l/I * (u(1,t) - u(2,t));
+ x(:,t+1) = x(:,t) + f * model.dt;
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Linearize the system along the trajectory computing the matrices A and B
+function [A, B] = linSys(x, u, model)
+ m = model.m;
+ l = model.l;
+ I = model.I;
+ A = zeros(model.nbVarX, model.nbVarX, model.nbData-1);
+ B = zeros(model.nbVarX, model.nbVarU, model.nbData-1);
+ Ac = zeros(model.nbVarX);
+ Ac(1:3,4:6) = eye(model.nbVarPos);
+ Bc = zeros(model.nbVarX, model.nbVarU);
+ for t=1:model.nbData-1
+ %Linearize the system
+ Ac(4,3) = -m^-1 * (u(1) + u(2)) * cos(x(3));
+ Ac(5,3) = -m^-1 * (u(1) + u(2)) * sin(x(3));
+ Bc(4,1) = -m^-1 * sin(x(3)); Bc(4,2) = Bc(4,1);
+ Bc(5,1) = m^-1 * cos(x(3)); Bc(5,2) = Bc(5,1);
+ Bc(6,1) = l / I; Bc(6,2) = -Bc(6,1);
+ %Discretize the linear system
+ A(:,:,t) = eye(model.nbVarX) + Ac * model.dt;
+ B(:,:,t) = Bc * model.dt;
+ end
+
+% %Symbolic expressions to find linearized system
+% syms m l g I
+% x = sym('x',[6 1]);
+% u = sym('u',[2 1]);
+% f(1) = x(4);
+% f(2) = x(5);
+% f(3) = x(6);
+% f(4) = -m^-1 * (u(1) + u(2)) * sin(x(3));
+% f(5) = m^-1 * (u(1) + u(2)) * cos(x(3)) - g;
+% f(6) = l/I * (u(1) - u(2));
+% jacobian(f, x)
+% jacobian(f, u)
+end
\ No newline at end of file
diff --git a/demos/demo_OC_DDP_car01.m b/demos/demo_OC_DDP_car01.m
new file mode 100644
index 0000000000000000000000000000000000000000..ba5dc0b671d3da6285efb5d111c01d6de064f70b
--- /dev/null
+++ b/demos/demo_OC_DDP_car01.m
@@ -0,0 +1,279 @@
+function demo_OC_DDP_car01
+% iLQR applied to a car parking problem
+%
+% Sylvain Calinon, 2020
+%
+% Reference:
+% W. Li and E. Todorov, "Iterative linear quadratic regulator design for nonlinear biological movement systems,"
+% in Intl Conf. on InfoRatics in Control, Automation and Robotics, 2004, pp. 222–229
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.sim_time = 3; %Simulation time
+model.dt = 1E-1; %Time step size
+model.nbData = model.sim_time / model.dt; %Number of datapoints
+model.nbIter = 10; %Number of iterations for iLQR
+
+model.nbVarX = 4; %Dimension of state (x1,x2,theta,phi)
+model.nbVarU = 2; %Control space dimension (v,dphi)
+
+model.rfactor = 1E-6; %Control weight teR
+model.Mu = [4; 3; pi/2; 0]; %Target
+% model.Mu = [.2; 2; pi/2; 0]; %Target
+
+
+model.l = 0.5; %Length of car
+
+R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix (at trajectory level)
+Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
+
+Q = blkdiag(sparse((model.nbData-1)*model.nbVarX, (model.nbData-1)*model.nbVarX), speye(model.nbVarX)*1E2); %Sparse precision matrix (at trajectory level)
+MuQ = [sparse((model.nbData-1)*model.nbVarX,1); model.Mu]; %Sparse reference (at trajectory level)
+
+
+%% Iterative LQR (iLQR)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% delta_x0 = zeros(model.nbVarX, 1);
+u = zeros(model.nbVarU, model.nbData-1);
+% u = repmat([2; 0], 1, model.nbData-1);
+x0 = zeros(model.nbVarX,1);
+
+% uSigma = zeros(model.nbVarU*(model.nbData-1));
+% xSigma = zeros(model.nbVarX*model.nbData);
+
+for n=1:model.nbIter
+ %System evolution
+ x = dynSysSimulation(x0, u, model);
+% if n>1
+% x = reshape(Sx * x0 + Su * u(:), model.nbVarX, model.nbData); %Approximated dynamics
+% end
+
+ %Linearization
+ [A, B] = linSys(x, u, model);
+ [Su, Sx] = transferMatrices(A, B);
+
+% du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:) - Su' * Q * Sx * delta_x0); %Control trajectory
+ du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:)); %Control trajectory
+ delta_u = reshape(du_vec, model.nbVarU, model.nbData-1);
+% delta_x = reshape(Sx * delta_x0 + Su * delta_u(:), model.nbVarX, model.nbData); %Forward recursion
+
+ %Estimate step size with line search method
+ alpha = 1;
+ cost0 = (MuQ - x(:))' * Q * (MuQ - x(:)) + u(:)' * R * u(:);
+ while 1
+ utmp = u + delta_u * alpha;
+ xtmp = dynSysSimulation(x0, utmp, model);
+% xtmp = reshape(Sx * x0 + Su * utmp(:), model.nbVarX, model.nbData); %Approximated dynamics
+
+ cost = (MuQ - xtmp(:))' * Q * (MuQ - xtmp(:)) + utmp(:)' * R * utmp(:); %Compute the cost
+ if cost < cost0 || alpha < 1E-4
+ break;
+ end
+ alpha = alpha * 0.5;
+ end
+% disp(alpha);
+ u = u + delta_u * alpha; %Update control by following gradient
+
+% uSigmaTmp = inv(Su' * Q * Su + R);
+% uSigma = uSigma + uSigmaTmp * alpha^2;
+% xSigma = xSigma + Su * uSigmaTmp * Su' * alpha^2;
+
+ %Log data
+ r(n).x = x;
+ r(n).u = u;
+end
+
+
+%% Stochastic sampling by exploiting the nullspace structure of the problem
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% %Standard stochastic sampling
+% uSigma = inv(Su' * Q * Su + R);
+% % xSigma = Su * uSigma * Su';
+% [V, D] = eig(full(uSigma));
+% U = real(V * D.^.5);
+% sx = zeros(model.nbData*model.nbVarX, nbSamples);
+% nbSamples = 50;
+% for n=1:nbSamples
+% utmp = u(:) + U * randn(model.nbVarU*(model.nbData-1), 1) * 1E-3;
+% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
+% sx(:,n) = xtmp(:);
+% end
+
+% %Generate structured stochastic u through Bezier curves
+% nbSamples = 50;
+% nbRBF = 18;
+% H = zeros(nbRBF, model.nbData-1);
+% tl = linspace(0, 1, model.nbData-1);
+% nbDeg = nbRBF - 1;
+% for i=0:nbDeg
+% H(i+1,:) = factorial(nbDeg) / (factorial(i) * factorial(nbDeg-i)) * (1-tl).^(nbDeg-i) .* tl.^i; %Bernstein basis functions
+% end
+% %Nullspace planning
+% [V,D] = eig(full(Q));
+% U = V * D.^.5;
+% N = eye((model.nbData-1)*model.nbVarU) - Su' * U / (U' * (Su * Su') * U + Rx) * U' * Su; %Nullspace projection matrix
+% sx = zeros(model.nbData*model.nbVarX, nbSamples);
+% for n=1:nbSamples
+% w = randn(model.nbVarU, nbRBF) * 1E0; %Random weights
+% un = w * H; %Reconstruction of signals by weighted superposition of basis functions
+% utmp = u(:) + N * un(:);
+% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
+% sx(:,n) = xtmp(:);
+% end
+
+
+%% Plot state space
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,800,800]); hold on; axis off;
+% for t=1:model.nbData
+% plotGMM(r(end).x(1:2,t), xSigma(model.nbVarX*(t-1)+[1,2],model.nbVarX*(t-1)+[1,2]).*1E-6, [.8 0 0], .1);
+% end
+%Animation
+for t=round(linspace(1,model.nbData,model.nbData))
+ R = [cos(r(end).x(3,t)) -sin(r(end).x(3,t)); sin(r(end).x(3,t)) cos(r(end).x(3,t))];
+ msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(r(end).x(1:2,t),1,5);
+ plot(msh(1,:), msh(2,:), '-','linewidth',2,'color',[.8 .8 .8]);
+end
+
+% %iLQR refinement steps
+% for k=1:length(r)
+% plot(r(k).x(1,:), r(k).x(2,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
+% end
+
+% %Stochastic samples
+% for n=1:nbSamples
+% plot(sx(1:model.nbVarX:end,n), sx(2:model.nbVarX:end,n), '-','linewidth',1,'color',(ones(1,3)-n/nbSamples)*.9);
+% end
+
+plot(r(end).x(1,:),r(end).x(2,:), '-','linewidth',2,'color',[0 0 0]);
+%Initial pose
+R = [cos(r(end).x(3,1)) -sin(r(end).x(3,1)); sin(r(end).x(3,1)) cos(r(end).x(3,1))];
+msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(r(end).x(1:2,1),1,5);
+plot(msh(1,:), msh(2,:), '-','linewidth',2,'color',[0 0 0]);
+h(1) = plot(r(end).x(1,1),r(end).x(2,1), '.','markersize',20,'color',[0 0 0]);
+%Final pose
+R = [cos(r(end).x(3,end)) -sin(r(end).x(3,end)); sin(r(end).x(3,end)) cos(r(end).x(3,end))];
+msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(r(end).x(1:2,end),1,5);
+plot(msh(1,:), msh(2,:), '-','linewidth',4,'color',[0 0 0]);
+h(1) = plot(r(end).x(1,1),r(end).x(2,1), '.','markersize',20,'color',[0 0 0]);
+%Target pose
+R = [cos(model.Mu(3)) -sin(model.Mu(3)); sin(model.Mu(3)) cos(model.Mu(3))];
+msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(model.Mu(1:2),1,5);
+plot(msh(1,:), msh(2,:), '-','linewidth',2,'color',[.8 0 0]);
+h(2) = plot(model.Mu(1), model.Mu(2), '.','markersize',20,'color',[.8 0 0]);
+
+legend(h,{'Initial pose','Target pose'},'location','northwest','fontsize',20);
+axis equal; axis([-1 5 -1 4]);
+% print('-dpng','graphs/DDP_car01_Sigma.png');
+
+
+% %% Plot control space
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[1030,10,1000,1000]);
+% t = model.dt:model.dt:model.dt*model.nbData;
+% for kk=1:size(r(1).u,1)
+% subplot(size(r(1).u,1), 1, kk); grid on; hold on; box on; box on;
+% for k=1:length(r)
+% plot(t(1:end-1), r(k).u(kk,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
+% end
+% plot(t(1:end-1), r(end).u(kk,:), '-','linewidth',2,'color',[0 0 0]);
+% axis([min(t), max(t(1:end-1)), min(r(end).u(kk,:))-.2, max(r(end).u(kk,:))+.2]);
+% xlabel('t'); ylabel(['u_' num2str(kk)]);
+% end
+
+
+% %% Plot animated robot
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10,10,1000,1000]); hold on; axis off;
+% %Target pose
+% R = [cos(model.Mu(3)) -sin(model.Mu(3)); sin(model.Mu(3)) cos(model.Mu(3))];
+% msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(model.Mu(1:2),1,5);
+% plot(msh(1,:), msh(2,:), '-','linewidth',4,'color',[.8 0 0]);
+% plot(model.Mu(1), model.Mu(2), '.','markersize',40,'color',[.8 0 0]);
+% axis equal; axis([-1 5 -1 4]);
+% for t=1:model.nbData
+% R = [cos(r(end).x(3,t)) -sin(r(end).x(3,t)); sin(r(end).x(3,t)) cos(r(end).x(3,t))];
+% msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(r(end).x(1:2,t),1,5);
+% h(1) = plot(msh(1,:), msh(2,:), '-','linewidth',4,'color',[0 0 0]);
+% h(2) = plot(r(end).x(1,t),r(end).x(2,t), '.','markersize',40,'color',[0 0 0]);
+% % print('-dpng',['graphs/anim/DDP_car_anim' num2str(t,'%.3d') '.png']);
+% if t<model.nbData
+% delete(h);
+% end
+% end
+% % for t=model.nbData+1:model.nbData+10
+% % print('-dpng',['graphs/anim/DDP_car_anim' num2str(t,'%.3d') '.png']);
+% % end
+
+pause;
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+function [Su, Sx] = transferMatrices(A, B)
+ [nbVarX, nbVarU, nbData] = size(B);
+ nbData = nbData+1;
+ Sx = kron(ones(nbData,1), speye(nbVarX));
+ Su = sparse(zeros(nbVarX*(nbData-1), nbVarU*(nbData-1)));
+ for t=1:nbData-1
+ id1 = (t-1)*nbVarX+1:t*nbVarX;
+ id2 = t*nbVarX+1:(t+1)*nbVarX;
+ id3 = (t-1)*nbVarU+1:t*nbVarU;
+ Sx(id2,:) = squeeze(A(:,:,t)) * Sx(id1,:);
+ Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
+ Su(id2,id3) = B(:,:,t);
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Given the control trajectory u and initial state x0, compute the whole state trajectory
+function x = dynSysSimulation(x0, u, model)
+ l = model.l;
+ x = zeros(model.nbVarX, model.nbData);
+ f = zeros(model.nbVarX, 1);
+ x(:,1) = x0;
+ for t=1:model.nbData-1
+ f(1) = cos(x(3,t)) * u(1,t);
+ f(2) = sin(x(3,t)) * u(1,t);
+ f(3) = tan(x(4,t)) * u(1,t) / l;
+ f(4) = u(2,t);
+ x(:,t+1) = x(:,t) + f * model.dt;
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Linearize the system along the trajectory computing the matrices A and B
+function [A, B] = linSys(x, u, model)
+ l = model.l;
+ A = zeros(model.nbVarX, model.nbVarX, model.nbData-1);
+ B = zeros(model.nbVarX, model.nbVarU, model.nbData-1);
+ Ac = zeros(model.nbVarX);
+ Bc = zeros(model.nbVarX, model.nbVarU);
+ for t=1:model.nbData-1
+ %Linearize the system
+ Ac(1,3) = -u(1,t) * sin(x(3,t));
+ Ac(2,3) = u(1,t) * cos(x(3,t));
+ Ac(3,4) = u(1,t) * tan(x(4,t)^2 + 1) / l;
+ Bc(1,1) = cos(x(3,t));
+ Bc(2,1) = sin(x(3,t));
+ Bc(3,1) = tan(x(4,t)) / l;
+ Bc(4,2) = 1;
+ %Discretize the linear system
+ A(:,:,t) = eye(model.nbVarX) + Ac * model.dt;
+ B(:,:,t) = Bc * model.dt;
+ end
+
+% %Symbolic expressions to find linearized system
+% syms l
+% x = sym('x',[4 1]);
+% u = sym('u',[2 1]);
+% f(1) = cos(x(3)) * u(1);
+% f(2) = sin(x(3)) * u(1);
+% f(3) = tan(x(4)) * u(1) / l;
+% f(4) = u(2);
+% jacobian(f, x)
+% jacobian(f, u)
+end
\ No newline at end of file
diff --git a/demos/demo_OC_DDP_cartpole01.m b/demos/demo_OC_DDP_cartpole01.m
new file mode 100644
index 0000000000000000000000000000000000000000..0c3be4c620ec3dcca956a635ea5185f0daaefa24
--- /dev/null
+++ b/demos/demo_OC_DDP_cartpole01.m
@@ -0,0 +1,269 @@
+function demo_OC_DDP_cartpole01
+% iLQR applied to a cartpole problem
+%
+% Sylvain Calinon, 2020
+%
+% Reference for cartpole system:
+% http://databookuw.com/databook.pdf, p. 372
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.sim_time = 3; %Simulation time
+model.dt = 2E-2; %Time step size
+model.nbData = model.sim_time / model.dt; %Number of datapoints
+model.nbIter = 30; %Number of iterations for iLQR
+
+model.nbVarPos = 2; %Dimension of position
+model.nbDeriv = 2; %Number of derivatives (nbDeriv=2 for [x; dx] state)
+model.nbVarX = model.nbVarPos * model.nbDeriv; %State space dimension
+model.nbVarU = 1; %Control space dimension
+
+model.rfactor = 1E-5; %Control weight teR
+model.Mu = [2; pi; 0; 0]; %Target
+
+model.m = 1; %Pendulum mass
+model.M = 5; %Cart mass
+model.L = 1; %Pendulum length
+model.g = 9.81; %Acceleration due to gravity
+model.d = 1; %Cart damping
+% model.b = 0.01; %Friction
+
+R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix (at trajectory level)
+Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
+
+Q = blkdiag(sparse((model.nbData-1)*model.nbVarX, (model.nbData-1)*model.nbVarX), diag([1E4, 1E4, 1E3, 1E3])); %Sparse precision matrix (at trajectory level)
+MuQ = [sparse((model.nbData-1)*model.nbVarX,1); model.Mu]; %Sparse reference (at trajectory level)
+
+
+%% Iterative LQR (iLQR)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% delta_x0 = zeros(model.nbVarX, 1);
+u = zeros(model.nbVarU, model.nbData-1);
+x0 = zeros(model.nbVarX,1);
+% x0 = [0; pi/4; .4; 0];
+
+% uSigma = zeros(model.nbVarU*(model.nbData-1));
+% xSigma = zeros(model.nbVarX*model.nbData);
+
+for n=1:model.nbIter
+ %sxstem evolution
+ x = dynSysSimulation(x0, u, model);
+
+ %Linearization
+ [A, B] = linSys(x, u, model);
+ [Su, Sx] = transferMatrices(A, B);
+
+% du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:) - Su' * Q * Sx * delta_x0); %Control trajectory
+ du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:)); %Control trajectory
+ delta_u = reshape(du_vec, model.nbVarU, model.nbData-1);
+% delta_x = reshape(Sx * delta_x0 + Su * delta_u(:), model.nbVarX, model.nbData); %Forward recursion
+
+ %Estimate step size with line search method
+ alpha = 1;
+ cost0 = (MuQ - x(:))' * Q * (MuQ - x(:)) + u(:)' * R * u(:);
+ while 1
+ utmp = u + delta_u * alpha;
+ xtmp = dynSysSimulation(x0, utmp, model);
+ cost = (MuQ - xtmp(:))' * Q * (MuQ - xtmp(:)) + utmp(:)' * R * utmp(:); %Compute the cost
+ if cost < cost0 || alpha < 1E-4
+ break;
+ end
+ alpha = alpha * 0.5;
+ end
+% disp(alpha);
+ u = u + delta_u * alpha; %Update control by following gradient
+
+% uSigmaTmp = inv(Su' * Q * Su + R);
+% uSigma = uSigma + uSigmaTmp * alpha^2;
+% xSigma = xSigma + Su * uSigmaTmp * Su' * alpha^2;
+
+ %Log data
+ r(n).x = x;
+ r(n).u = u;
+end
+
+%% Stochastic sampling by exploiting the nullspace structure of the problem
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% %Standard stochastic sampling
+% uSigma = inv(Su' * Q * Su + R);
+% % xSigma = Su * uSigma * Su';
+% [V, D] = eig(full(uSigma));
+% U = real(V * D.^.5);
+% nbSamples = 50;
+% sx = zeros(model.nbData*model.nbVarX, nbSamples);
+% for n=1:nbSamples
+% utmp = u(:) + U * randn(model.nbVarU*(model.nbData-1), 1) * 1E-1;
+% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
+% sx(:,n) = xtmp(:);
+% end
+
+% %Generate structured stochastic u through Bezier curves
+% nbSamples = 50;
+% nbRBF = 18;
+% H = zeros(nbRBF, model.nbData-1);
+% tl = linspace(0, 1, model.nbData-1);
+% nbDeg = nbRBF - 1;
+% for i=0:nbDeg
+% H(i+1,:) = factorial(nbDeg) / (factorial(i) * factorial(nbDeg-i)) * (1-tl).^(nbDeg-i) .* tl.^i; %Bernstein basis functions
+% end
+% %Nullspace planning
+% [V,D] = eig(full(Q));
+% U = V * D.^.5;
+% N = eye((model.nbData-1)*model.nbVarU) - Su' * U / (U' * (Su * Su') * U + Rx) * U' * Su; %Nullspace projection matrix
+% sx = zeros(model.nbData*model.nbVarX, nbSamples);
+% for n=1:nbSamples
+% w = randn(model.nbVarU, nbRBF) * 1E1; %Random weights
+% un = w * H; %Reconstruction of signals by weighted superposition of basis functions
+% utmp = u(:) + N * un(:);
+% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
+% sx(:,n) = xtmp(:);
+% end
+
+
+%% Plot state space
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,800,800]); hold on; axis on; grid on; box on;
+% for t=1:model.nbData
+% plotGMM(r(end).x(1:2,t), xSigma(model.nbVarX*(t-1)+[1,2],model.nbVarX*(t-1)+[1,2]).*5E-2, [.8 0 0], .05);
+% end
+
+% %iLQR refinement steps
+% for k=1:length(r)
+% plot(r(k).x(1,:), r(k).x(2,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
+% end
+
+% %Stochastic samples
+% for n=1:nbSamples
+% plot(sx(1:model.nbVarX:end,n), sx(2:model.nbVarX:end,n), '-','linewidth',1,'color',(ones(1,3)-n/nbSamples)*.9);
+% end
+
+plot(r(end).x(1,:),r(end).x(2,:), '-','linewidth',2,'color',[.8 0 0]);
+h(1) = plot(r(end).x(1,1),r(end).x(2,1), '.','markersize',20,'color',[0 0 0]);
+h(2) = plot(model.Mu(1), model.Mu(2), '.','markersize',20,'color',[.8 0 0]);
+legend(h,{'Starting point','Ending point'},'location','southwest');
+axis equal; %axis([-5 5 -4 6]);
+xlabel('x'); ylabel('\theta');
+
+
+% %% Plot control space
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[1020,10,1000,1000]);
+% t = model.dt:model.dt:model.dt*model.nbData;
+% for kk=1:size(r(1).u,1)
+% subplot(size(r(1).u,1), 1, kk); grid on; hold on; box on; box on;
+% for k=1:length(r)
+% plot(t(1:end-1), r(k).u(kk,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
+% end
+% plot(t(1:end-1), r(end).u(kk,:), '-','linewidth',2,'color',[0 0 0]);
+% axis([min(t), max(t(1:end-1)), min(r(end).u(kk,:))-2, max(r(end).u(kk,:))+2]);
+% xlabel('t'); ylabel(['u_' num2str(kk)]);
+% end
+
+
+% %% Plot animated robot
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10 10 2200 700],'color',[1 1 1]); hold on; axis off;
+% plotAR(model.Mu(2)-pi/2, model.L, [0;0;-1], .1, [.8 0 0]);
+% axis equal; axis([-4.2 4.2 -1.2 1.2]*2);
+% h=[];
+% for t=1:model.nbData
+% delete(h);
+% h = plotAR(r(end).x(2,t)-pi/2, model.L, [r(end).x(1,t);0;0], .1, [0 0 0]);
+% drawnow;
+% end
+
+pause;
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+function [Su, Sx] = transferMatrices(A, B)
+ [nbVarX, nbVarU, nbData] = size(B);
+ nbData = nbData+1;
+ Sx = kron(ones(nbData,1), speye(nbVarX));
+ Su = sparse(zeros(nbVarX*(nbData-1), nbVarU*(nbData-1)));
+ for t=1:nbData-1
+ id1 = (t-1)*nbVarX+1:t*nbVarX;
+ id2 = t*nbVarX+1:(t+1)*nbVarX;
+ id3 = (t-1)*nbVarU+1:t*nbVarU;
+ Sx(id2,:) = squeeze(A(:,:,t)) * Sx(id1,:);
+ Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
+ Su(id2,id3) = B(:,:,t);
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Given the control trajectory u and initial state x0, compute the whole state trajectory
+function x = dynSysSimulation(x0, u, model)
+ m = model.m;
+ M = model.M;
+ L = model.L;
+ g = model.g;
+ d = model.d;
+ x = zeros(model.nbVarX, model.nbData);
+ f = zeros(model.nbVarX, 1);
+ x(:,1) = x0;
+ for t=1:model.nbData-1
+ sx = sin(x(2,t));
+ cx = cos(x(2,t));
+ D = m*L^2*(M+m*(1-cx^2));
+ f(1) = x(3,t);
+ f(2) = x(4,t);
+% f(3) = (1/D)*(-m^2*L^2*g*cx*sx + m*L^2*(m*L*x(4,t)^2*sx - d*x(3,t))) + m*L^2*(1/D) * u(1,t);
+% f(4) = (1/D)*((m+M)*m*g*L*sx - m*L*cx*(m*L*x(4,t)^2*sx - d*x(3,t))) - m*L*cx*(1/D) * u(1,t);
+ f(3) = (u(1,t) + m*sx*(L*x(4,t)^2+g*cx)) / (M+m*sx^2); %From RLSC Homework (Part 2: Optimal Control)
+ f(4) = (-u(1,t) - m*L*x(4,t)*cx*sx - (M+m)*g*sx) / (L*(M+m*sx^2)); %From RLSC Homework (Part 2: Optimal Control)
+ x(:,t+1) = x(:,t) + f * model.dt;
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Linearize the sxstem along the trajectory computing the matrices A and B
+function [A, B] = linSys(x, u, model)
+ m = model.m;
+ M = model.M;
+ L = model.L;
+ g = model.g;
+ d = model.d;
+
+ A = zeros(model.nbVarX, model.nbVarX, model.nbData-1);
+ B = zeros(model.nbVarX, model.nbVarU, model.nbData-1);
+
+% %Linearization at target (pendulum up corresponds to s=1)
+% s = 1;
+% Ac = [0 0 1 0;
+% 0 0 0 1;
+% 0 -m*g/M -d/M 0;
+% 0 -s*(m+M)*g/(M*L) -s*d/(M*L) 0];
+% Bc = [0; 0; 1/M; s*1/(M*L)];
+
+ for t=1:model.nbData-1
+ %Linearization
+ Ac = [0, 0, 1, 0; ...
+ 0, 0, 0, 1; ...
+ 0, ((m*cos(x(2,t))*(L*x(4,t)^2 + g*cos(x(2,t))) - g*m*sin(x(2,t))^2)/(m*sin(x(2,t))^2 + M) - (2*m*cos(x(2,t))*sin(x(2,t))*(u(1,t) + m*sin(x(2,t))*(L*x(4,t)^2 + g*cos(x(2,t)))))/(m*sin(x(2,t))^2 + M)^2), 0, ((2*L*m*x(4,t)*sin(x(2,t)))/(m*sin(x(2,t))^2 + M)); ...
+ 0, (2*m*cos(x(2,t))*sin(x(2,t))*(u(1,t) + g*sin(x(2,t))*(M + m) + L*m*x(4,t)*cos(x(2,t))*sin(x(2,t))))/(L*(m*sin(x(2,t))^2 + M)^2) - (g*cos(x(2,t))*(M + m) + L*m*x(4,t)*cos(x(2,t))^2 - L*m*x(4,t)*sin(x(2,t))^2)/(L*(m*sin(x(2,t))^2 + M)), 0, (-(m*cos(x(2,t))*sin(x(2,t)))/(m*sin(x(2,t))^2 + M))];
+ Bc = [0; 0; 1/(m*sin(x(2,t))^2 + M); -1/(L*(m*sin(x(2,t))^2 + M))];
+ %Discretize the linear sxstem
+ A(:,:,t) = eye(model.nbVarX) + Ac * model.dt;
+ B(:,:,t) = Bc * model.dt;
+ end
+
+% %Symbolic expressions to find linearized system
+% syms m M L g d u
+% x = sym('x',[4 1]);
+% sx = sin(x(2));
+% cx = cos(x(2));
+% D = m*L^2*(M+m*(1-cx^2));
+% f(1) = x(3);
+% f(2) = x(4);
+% % f(3) = (1/D)*(-m^2*L^2*g*cx*sx + m*L^2*(m*L*x(4)^2*sx - d*x(3))) + m*L^2*(1/D) * u;
+% % f(4) = (1/D)*((m+M)*m*g*L*sx - m*L*cx*(m*L*x(4)^2*sx - d*x(3))) - m*L*cx*(1/D) * u;
+% f(3) = (u + m*sx*(L*x(4)^2+g*cx)) / (M+m*sx^2);
+% f(4) = (-u - m*L*x(4)*cx*sx - (M+m)*g*sx) / (L*(M+m*sx^2));
+% jacobian(f, x)
+% jacobian(f, u)
+end
\ No newline at end of file
diff --git a/demos/demo_OC_DDP_humanoid01.m b/demos/demo_OC_DDP_humanoid01.m
new file mode 100644
index 0000000000000000000000000000000000000000..419ea951abd72eb4ebdf6864709f377207c922d3
--- /dev/null
+++ b/demos/demo_OC_DDP_humanoid01.m
@@ -0,0 +1,259 @@
+function demo_OC_DDP_humanoid01
+% iLQR applied to a planar 5-link humanoid problem with constraints between joints
+%
+% Sylvain Calinon, 2020
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.sim_time = 1; %Simulation time
+model.dt = 1E-2; %Time step size
+model.nbData = model.sim_time / model.dt; %Number of datapoints
+model.nbPoints = 2; %Number of viapoints
+model.nbIter = 20; %Number of iterations for iLQR
+model.nbVarX = 5; %Dimension of state (x1,x2, q1,q2,q3)
+model.nbVarU = 3; %Control space dimension (dq1,dq2,dq3)
+
+model.rfactor = 1E-2; %Control weight term
+model.Mu(:,1) = [3; 1.5; zeros(model.nbVarU,1)]; %Target (x1h,x2h, q1h,q2h,q3h)
+model.Mu(:,2) = [2; 5; zeros(model.nbVarU,1)]; %Target (x1h,x2h, q1h,q2h,q3h)
+
+model.l = 2; %Link length
+
+R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix (at trajectory level)
+Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
+
+% Q = blkdiag(sparse((model.nbData-1)*model.nbVarX, (model.nbData-1)*model.nbVarX), diag([1E2, 1E2, zeros(1,model.nbVarU)])); %Sparse precision matrix (at trajectory level)
+% MuQ = [sparse((model.nbData-1)*model.nbVarX,1); model.Mu]; %Sparse reference (at trajectory level)
+
+Q = blkdiag(sparse((model.nbData/2-1)*model.nbVarX, (model.nbData/2-1)*model.nbVarX), diag([1E2, 1E2, zeros(1,model.nbVarU)]), ...
+ sparse((model.nbData/2-1)*model.nbVarX, (model.nbData/2-1)*model.nbVarX), diag([1E2, 1E2, zeros(1,model.nbVarU)])); %Sparse precision matrix (at trajectory level)
+MuQ = [sparse((model.nbData/2-1)*model.nbVarX,1); model.Mu(:,1); ...
+ sparse((model.nbData/2-1)*model.nbVarX,1); model.Mu(:,2)]; %Sparse reference (at trajectory level)
+
+% Q = blkdiag(zeros((model.nbData-2)*model.nbVarX), diag([1E2, 1E2, 0, 0, 0, 1E2, 1E2, 0, 0, 0])); %Sparse precision matrix (at trajectory level)
+% MuQ = [zeros((model.nbData-2)*model.nbVarX,1); model.Mu; model.Mu]; %Sparse reference (at trajectory level)
+
+% Q = kron(eye(model.nbData), diag([1E0, 1E0, 0, 0, 0])); %Sparse precision matrix (at trajectory level)
+% MuQ = kron(ones(model.nbData,1), model.Mu); %Sparse reference (at trajectory level)
+
+% Q = kron(eye(model.nbData), inv(model.Sigma)); %Sparse precision matrix (at trajectory level)
+% MuQ = kron(ones(model.nbData,1), model.Mu); %Sparse reference (at trajectory level)
+
+
+%% Iterative LQR (iLQR)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+u = zeros(model.nbVarU, model.nbData-1);
+% u = repmat(linspace(0,1E-3,model.nbData-1), model.nbVarU, 1);
+% u = ones(model.nbVarU, model.nbData-1) * 1E-1;
+
+% q0 = zeros(model.nbVarU,1);
+% q0 = ones(model.nbVarU,1) * pi/20;
+q0 = [.7; pi-.2; pi/2];
+x0 = [fkine(q0, model); q0];
+
+for n=1:model.nbIter
+ %System evolution
+ x = dynSysSimulation(x0, u, model);
+
+ %Linearization
+ [A, B] = linSys(x, u, model);
+ [Su, Sx] = transferMatrices(A, B);
+
+% du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:) - Su' * Q * Sx * delta_x0); %Control trajectory
+ du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:)); %Control trajectory
+ delta_u = reshape(du_vec, model.nbVarU, model.nbData-1);
+
+ %Estimate step size with line search method
+ alpha = 1;
+ cost0 = (MuQ - x(:))' * Q * (MuQ - x(:)) + u(:)' * R * u(:);
+ while 1
+ utmp = u + delta_u * alpha;
+ xtmp = dynSysSimulation(x0, utmp, model);
+ cost = (MuQ - xtmp(:))' * Q * (MuQ - xtmp(:)) + utmp(:)' * R * utmp(:); %Compute the cost
+ if cost < cost0 || alpha < 1E-3
+ break;
+ end
+ alpha = alpha * 0.5;
+ end
+ u = u + delta_u * alpha; %Update control by following gradient
+
+ %Log data
+ r(n).x = [x(1:2,:); pi/2-x(3,:); 2*x(3,:); -x(3,:); x(4:5,:)];
+ r(n).u = u;
+end
+
+
+% %% Plot control space
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[1020,10,1000,1000]);
+% t = model.dt:model.dt:model.dt*model.nbData;
+% for kk=1:size(r(1).u,1)
+% subplot(size(r(1).u,1), 1, kk); grid on; hold on; box on; box on;
+% for k=1:length(r)
+% plot(t(1:end-1), r(k).u(kk,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
+% end
+% plot(t(1:end-1), r(end).u(kk,:), '-','linewidth',2,'color',[0 0 0]);
+% axis([min(t), max(t(1:end-1)), min(r(end).u(kk,:))-.1, max(r(end).u(kk,:))+.1]);
+% xlabel('t'); ylabel(['u_' num2str(kk)]);
+% end
+
+
+%% Plot static robot
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,1600,800]); hold on; axis off;
+% for t=1:model.nbData
+% plotGMM(r(end).x(1:2,t), xSigma(model.nbVarX*(t-1)+[1,2],model.nbVarX*(t-1)+[1,2]).*1E-2, [.8 0 0], .1);
+% end
+% plotArm(r(end).x(3:end,1), ones(5,1)*model.l, [0; 0; -3], .2, [.8 .8 .8]);
+% plotArm(r(end).x(3:end,end/2), ones(5,1)*model.l, [ofxa; 0; -2], .2, [.6 .6 .6]);
+% plotArm(r(end).x(3:end,end), ones(5,1)*model.l, [2*ofxa; 0; -1], .2, [.4 .4 .4]);
+
+% %iLQR refinement steps
+% for k=1:length(r)
+% plot(r(k).x(1,:), r(k).x(2,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
+% end
+
+% %Stochastic samples
+% for n=1:nbSamples
+% plot(sx(1:model.nbVarX:end,n), sx(2:model.nbVarX:end,n), '-','linewidth',1,'color',(ones(1,3)-n/nbSamples)*.9);
+% end
+
+colMat = lines(model.nbPoints);
+tl = floor(linspace(1,model.nbData,3));
+msh = [-1 -1 1 1 -1; -1 1 1 -1 -1] * 6E-1;
+ofxa = 6;
+for i=1:3
+ ofx = (i-1)*ofxa;
+ plotArm(r(end).x(3:end,tl(i)), ones(5,1)*model.l, [ofx; 0; -2], .2, [.8 .8 .8]-(i-1)*[.2 .2 .2]);
+ for j=1:model.nbPoints
+ plot2Dframe(eye(2)*4E-1, model.Mu(1:2,j)+[ofx;0], repmat(colMat(j,:),3,1), 6);
+ plot(msh(1,:)+model.Mu(1,j)+ofx, msh(2,:)+model.Mu(2,j), '-','linewidth',2,'color',colMat(j,:));
+ end
+% for t=floor(linspace(model.nbData/2,model.nbData,2))
+% msh = [-1 -1 1 1 -1; -1 1 1 -1 -1]*6E-1 + repmat(r(end).x(1:2,t), 1, 5);
+% plot3(msh(1,:)+ofx, msh(2,:), ones(5,1)*-2, '-','linewidth',2,'color',[0 0 0]);
+% end
+ plot(r(end).x(1,1:tl(i))+ofx, r(end).x(2,1:tl(i)), '-','linewidth',2,'color',[0 0 0]);
+ plot(r(end).x(1,1)+ofx, r(end).x(2,1), '.','markersize',40,'color',[0 0 0]);
+end
+% msh = [-1 -1 1 1 -1; -1 1 1 -1 -1]*6E-1 + repmat(model.Mu(1:2,1), 1, 5);
+% plot3(msh(1,:), msh(2,:), ones(5,1)*-2, '-','linewidth',2,'color',[0 0 0]);
+
+axis equal; %axis([-3.5 6.5 -1 5.5]);
+% print('-dpng','graphs/DDP_humanoid01.png');
+
+
+% %% Plot animated robot
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off;
+% plot3(model.Mu(1,:), model.Mu(2,:), [1,1], '.','markersize',40,'color',[.8 0 0]);
+%
+% msh = [-1 -1 1 1 -1; -1 1 1 -1 -1]*6E-1 + repmat(model.Mu(1:2,1), 1, 5);
+% plot3(msh(1,:), msh(2,:), ones(5,1)*-2, ':','linewidth',2,'color',[.8 0 0]);
+% hb = plot3(msh(1,:), msh(2,:), ones(5,1)*-2, '-','linewidth',2,'color',[0 0 0]);
+%
+% msh = [-1 -1 1 1 -1; -1 1 1 -1 -1]*6E-1 + repmat(model.Mu(1:2,2), 1, 5);
+% plot3(msh(1,:), msh(2,:), ones(5,1)*-2, ':','linewidth',2,'color',[.8 0 0]);
+%
+% axis equal; axis([-1 3.7 -.6 5.6]);
+% for t=1:model.nbData
+% h = plotArm(r(end).x(3:end,t), ones(5,1)*model.l, zeros(3,1), .2, [0 0 0]);
+% if t>model.nbData/2
+% msh = [-1 -1 1 1 -1; -1 1 1 -1 -1]*6E-1 + repmat(r(end).x(1:2,t), 1, 5);
+% delete(hb);
+% hb = plot3(msh(1,:), msh(2,:), ones(5,1)*-2, '-','linewidth',2,'color',[0 0 0]);
+% end
+% drawnow;
+% % print('-dpng',['graphs/anim/DDP_humanoid_anim' num2str(t,'%.3d') '.png']);
+% if t<model.nbData
+% delete(h);
+% end
+% end
+% % for t=model.nbData+1:model.nbData+10
+% % print('-dpng',['graphs/anim/DDP_humanoid_anim' num2str(t,'%.3d') '.png']);
+% % end
+
+pause;
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+function [Su, Sx] = transferMatrices(A, B)
+ [nbVarX, nbVarU, nbData] = size(B);
+ nbData = nbData+1;
+ Sx = kron(ones(nbData,1), speye(nbVarX));
+ Su = sparse(zeros(nbVarX*(nbData-1), nbVarU*(nbData-1)));
+ for t=1:nbData-1
+ id1 = (t-1)*nbVarX+1:t*nbVarX;
+ id2 = t*nbVarX+1:(t+1)*nbVarX;
+ id3 = (t-1)*nbVarU+1:t*nbVarU;
+ Sx(id2,:) = squeeze(A(:,:,t)) * Sx(id1,:);
+ Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
+ Su(id2,id3) = B(:,:,t);
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Given the control trajectory u and initial state x0, compute the whole state trajectory
+function x = dynSysSimulation(x0, u, model)
+ x = zeros(model.nbVarX, model.nbData);
+ x(:,1) = x0;
+ for t=1:model.nbData-1
+ x(3:end,t+1) = x(3:end,t) + u(:,t) * model.dt;
+% Htmp = model.rob.fkine(x(3:end,t+1)); %Forward kinematics
+% x(1:2,t+1) = Htmp.t(1:2);
+ x(1:2,t+1) = fkine(x(3:end,t+1), model); %Forward kinematics
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Linearize the system along the trajectory computing the matrices A and B
+function [A, B] = linSys(x, u, model)
+ A = zeros(model.nbVarX, model.nbVarX, model.nbData-1);
+ B = zeros(model.nbVarX, model.nbVarU, model.nbData-1);
+ Ac = zeros(model.nbVarX);
+ Bc = zeros(model.nbVarX, model.nbVarU);
+ for t=1:model.nbData-1
+ %Linearize the system
+% J = model.rob.jacob0(x(3:end,t),'trans');
+% Bc(1:2,:) = J(1:2,:);
+ Bc(1:2,:) = jacob0(x(3:end,t), model);
+ Bc(3:end,:) = eye(model.nbVarU);
+ %Discretize the linear system
+ A(:,:,t) = eye(model.nbVarX) + Ac * model.dt;
+ B(:,:,t) = Bc * model.dt;
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Forward kinematics
+function [x, Tf] = fkine(q, model)
+ q = [pi/2-q(1); 2*q(1); -q(1); q(2); q(3)];
+ Tf = eye(4);
+ T = repmat(Tf, [1,1,size(q,1)]);
+ for n=1:size(q,1)
+ c = cos(q(n));
+ s = sin(q(n));
+ T(:,:,n) = [c, -s, 0, model.l * c; ...
+ s, c, 0, model.l * s; ...
+ 0, 0, 1, 0; ...
+ 0, 0, 0, 1]; %Homogeneous matrix
+ Tf = Tf * T(:,:,n);
+ end
+ x = Tf(1:2,end);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Jacobian with numerical computation
+function J = jacob0(q, model)
+ e = 1E-6;
+ J = zeros(2,size(q,1));
+ for n=1:size(q,1)
+ qtmp = q;
+ qtmp(n) = qtmp(n) + e;
+ J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
+ end
+end
\ No newline at end of file
diff --git a/demos/demo_OC_DDP_manipulator01.m b/demos/demo_OC_DDP_manipulator01.m
new file mode 100644
index 0000000000000000000000000000000000000000..3331f67add369c42371919cce974566d2403c3cd
--- /dev/null
+++ b/demos/demo_OC_DDP_manipulator01.m
@@ -0,0 +1,329 @@
+function demo_OC_DDP_manipulator01
+% iLQR applied to a 2D manipulator problem
+%
+% Sylvain Calinon, 2020
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.dt = 1E-2; %Time step size
+model.nbData = 100; %Number of datapoints
+model.nbIter = 20; %Number of iterations for iLQR
+model.nbPoints = 2; %Number of viapoints
+model.nbVarX = 3; %State space dimension (q1,q2,q3)
+model.nbVarU = 3; %Control space dimension (dq1,dq2,dq3)
+model.nbVarF = 2; %Objective function dimension (x1,x2)
+
+model.rfactor = 1E-2; %Control weight term
+% model.Mu = [2; 4]; %Target (x1h,x2h)
+model.Mu = rand(model.nbVarF, model.nbPoints) * 5; %Viapoints
+
+model.l = [2,2,2]; %Link length
+
+R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix (at trajectory level)
+% Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
+
+% Q = kron(eye(model.nbData), inv(model.Sigma)); %Sparse precision matrix (at trajectory level)
+% MuQ = kron(ones(model.nbData,1), model.Mu); %Sparse reference (at trajectory level)
+
+% Q = blkdiag(sparse((model.nbData-1)*2,(model.nbData-1)*2), diag([1E1, 1E1])); %Sparse precision matrix (at trajectory level)
+% MuQ = [sparse((model.nbData-1)*2,1); model.Mu]; %Sparse reference (at trajectory level)
+
+%Sparse reference with a set of via-points
+tl = linspace(1, model.nbData, model.nbPoints+1);
+tl = round(tl(2:end));
+MuQ = sparse(model.nbVarF * model.nbData, 1);
+Q = sparse(model.nbVarF * model.nbData, model.nbVarF * model.nbData);
+for t=1:length(tl)
+ id(:,t) = (tl(t) - 1) * model.nbVarF + [1:model.nbVarF];
+ Q(id(:,t), id(:,t)) = eye(model.nbVarF) * 1E3;
+ MuQ(id(:,t)) = model.Mu(:,t);
+end
+
+
+%% Iterative LQR (iLQR)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% u = zeros(model.nbVarU, model.nbData-1);
+% u = repmat(linspace(0,1E-3,model.nbData-1), model.nbVarU, 1);
+u = ones(model.nbVarU, model.nbData-1) * pi/8;
+x0 = zeros(model.nbVarX,1);
+
+% uSigma = zeros(model.nbVarU*(model.nbData-1));
+% xSigma = zeros(model.nbVarX*model.nbData);
+
+for n=1:model.nbIter
+ %System evolution
+ x = dynSysSimulation(x0, u, model);
+% x = reshape(Su * u(:) + Sx * x0, model.nbVarX, model.nbData);
+
+ %Linearization
+ if n==1 %Constant Su and Sx for the proposed system
+ [A, B] = linSys(x, u, model);
+ [Su, Sx] = transferMatrices(A, B);
+ end
+
+% f=[];
+% % J=[];
+% % JQJ0=[]; JQ0=[];
+% for t=1:model.nbData
+% ftmp = fkine(x(:,t), model);
+% f = [f; ftmp]; %Endeffector positions in lifted form
+%
+% % Jtmp = jacob0(x(:,t), model);
+% % J = blkdiag(J, Jtmp); %Jacobians in lifted form
+%
+% % %Alternative computation (equivalent if no constraint between time steps)
+% % Jtmp = jacob0(x(:,t), model);
+% % id = (t-1) * model.nbVarF + [1:model.nbVarF];
+% % JQJ0 = blkdiag(JQJ0, Jtmp' * Q(id,id) * Jtmp);
+% % JQ0 = blkdiag(JQ0, Jtmp' * Q(id,id));
+% end
+% f = fkine(x, model);
+
+ %Alternative computation (fast version exploiting sparsity of Q)
+ f = sparse(model.nbData*model.nbVarF, 1);
+ JQJ = sparse(model.nbData*model.nbVarX, model.nbData*model.nbVarX);
+ JQ = sparse(model.nbData*model.nbVarX, model.nbData*model.nbVarF);
+ for t=1:length(tl)
+ Jtmp = jacob0(x(:,tl(t)), model);
+ idu = (tl(t) - 1) * model.nbVarU + [1:model.nbVarU];
+ idf = (tl(t) - 1) * model.nbVarF + [1:model.nbVarF];
+ JQJ(idu,idu) = Jtmp' * Q(idf,idf) * Jtmp;
+ JQ(idu,idf) = Jtmp' * Q(idf,idf);
+ f(idf) = fkine(x(:,tl(t)), model);
+ end
+
+% du_vec = (Su' * J' * Q * J * Su + R) \ (Su' * J' * Q * (MuQ - f(:)) - R * u(:)); %Control trajectory
+ du_vec = (Su' * JQJ * Su + R) \ (Su' * JQ * (MuQ - f(:)) - R * u(:)); %Control trajectory (alternative computation)
+
+ delta_u = reshape(du_vec, model.nbVarU, model.nbData-1);
+
+ %Estimate step size with line search method
+ alpha = 1;
+ cost0 = (MuQ - f(:))' * Q * (MuQ - f(:)) + u(:)' * R * u(:);
+ while 1
+ utmp = u + delta_u * alpha;
+% xtmp = dynSysSimulation(x0, utmp, model);
+ xtmp = reshape(Su * utmp(:) + Sx * x0, model.nbVarX, model.nbData);
+% ftmp = [];
+% for t=1:model.nbData
+% ftmp = [ftmp; fkine(xtmp(:,t), model)]; %Endeffector positions in lifted form
+% end
+ ftmp = fkine(xtmp, model);
+ cost = (MuQ - ftmp(:))' * Q * (MuQ - ftmp(:)) + utmp(:)' * R * utmp(:); %Compute the cost
+ if cost < cost0 || alpha < 1E-3
+ break;
+ end
+ alpha = alpha * 0.5;
+ end
+ u = u + delta_u * alpha; %Update control by following gradient
+
+% uSigmaTmp = inv(Su' * J' * Q * J * Su + R);
+% uSigma = uSigma + uSigmaTmp * alpha^2;
+% xSigma = xSigma + Su * uSigmaTmp * Su' * alpha^2;
+
+% %Log data
+% r(n).x = x;
+% r(n).f = f; %reshape(f, model.nbVarF, model.nbData);
+% r(n).u = u;
+end
+
+%Log data
+r(1).x = x;
+r(1).f = fkine(x, model);
+r(1).u = u;
+
+
+%% Stochastic sampling by exploiting the nullspace structure of the problem
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% %Standard stochastic sampling
+% uSigma = inv(Su' * J' * Q * J * Su + R);
+% % xSigma = Su * uSigma * Su';
+% [V, D] = eig(full(uSigma));
+% U = real(V * D.^.5);
+% nbSamples = 50;
+% for n=1:nbSamples
+% utmp = u(:) + U * randn(model.nbVarU*(model.nbData-1), 1) * 5E-2;
+% qtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
+% ftmp = [];
+% for t=1:model.nbData
+% % Htmp = model.rob.fkine(qtmp(:,t)); %Forward kinematics
+% % ftmp = [ftmp; Htmp.t(1:2)]; %Endeffector positions in lifted form
+% ftmp = [ftmp; fkine(qtmp(:,t), model)]; %Endeffector positions in lifted form
+% end
+% sx(:,n) = ftmp;
+% end
+
+% %Generate structured stochastic u through Bezier curves
+% nbSamples = 50;
+% nbRBF = 18;
+% H = zeros(nbRBF, model.nbData-1);
+% tl = linspace(0, 1, model.nbData-1);
+% nbDeg = nbRBF - 1;
+% for i=0:nbDeg
+% H(i+1,:) = factorial(nbDeg) / (factorial(i) * factorial(nbDeg-i)) * (1-tl).^(nbDeg-i) .* tl.^i; %Bernstein basis functions
+% end
+% %Nullspace planning
+% [V,D] = eig(full(J' * Q * J));
+% U = V * D.^.5;
+% N = eye((model.nbData-1)*model.nbVarU) - Su' * U / (U' * (Su * Su') * U + Rx) * U' * Su; %Nullspace projection matrix
+% sx = zeros(model.nbData*2, nbSamples);
+% for n=1:nbSamples
+% w = randn(model.nbVarU, nbRBF) * 5E-1; %Random weights
+% un = w * H; %Reconstruction of signals by weighted superposition of basis functions
+% utmp = u(:) + N * un(:);
+% qtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
+% ftmp = [];
+% for t=1:model.nbData
+% % Htmp = model.rob.fkine(qtmp(:,t)); %Forward kinematics
+% % f = [f; Htmp.t(1:2)]; %Endeffector positions in lifted form
+% ftmp = [ftmp; fkine(qtmp(:,t), model)]; %Endeffector positions in lifted form
+% end
+% sx(:,n) = ftmp;
+% end
+
+
+%% Plot state space
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,1000,800],'color',[1,1,1]); hold on; axis off;
+plotArm(r(end).x(:,1), ones(model.nbVarU,1)*model.l, [0; 0; -3], .2, [.8 .8 .8]);
+plotArm(r(end).x(:,model.nbData/2+1), ones(model.nbVarU,1)*model.l, [0; 0; -2], .2, [.6 .6 .6]);
+plotArm(r(end).x(:,model.nbData), ones(model.nbVarU,1)*model.l, [0; 0; -1], .2, [.4 .4 .4]);
+
+% for k=1:length(r)
+% plot(r(k).f(1,:), r(k).f(2,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
+% end
+
+% %Stochastic samples
+% for n=1:nbSamples
+% plot(sx(1:2:end,n), sx(2:2:end,n), '-','linewidth',1,'color',(ones(1,3)-n/nbSamples)*.9);
+% end
+
+colMat = lines(model.nbPoints);
+for i=1:model.nbPoints
+ plot2Dframe(eye(2)*4E-1, model.Mu(:,i), repmat(colMat(i,:),3,1), 6);
+end
+
+plot(r(end).f(1,:), r(end).f(2,:), '-','linewidth',2,'color',[0 0 0]);
+h(1) = plot(r(end).f(1,1), r(end).f(2,1), '.','markersize',40,'color',[0 0 0]);
+% h(2) = plot3(model.Mu(1,:), model.Mu(2,:), ones(1,size(model.Mu,2)), '.','markersize',40,'color',[.8 0 0]);
+% legend(h,{'Initial endeffector position','Target endeffector viapoint position'},'location','northwest','fontsize',20);
+axis equal; %axis([-3.5 6.5 -2 5.5]);
+% print('-dpng','graphs/DDP_manipulator01.png');
+
+
+% %% Plot control space
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[1020,10,1000,1000]);
+% t = model.dt:model.dt:model.dt*model.nbData;
+% for kk=1:size(r(1).u,1)
+% subplot(size(r(1).u,1), 1, kk); grid on; hold on; box on; box on;
+% for k=1:length(r)
+% plot(t(1:end-1), r(k).u(kk,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
+% end
+% plot(t(1:end-1), r(end).u(kk,:), '-','linewidth',2,'color',[0 0 0]);
+% axis([min(t), max(t(1:end-1)), min(r(end).u(kk,:))-.1, max(r(end).u(kk,:))+.1]);
+% xlabel('t'); ylabel(['u_' num2str(kk)]);
+% end
+
+
+% %% Plot animated robot
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off;
+% plot3(model.Mu(1), model.Mu(2), 1, '.','markersize',40,'color',[.8 0 0]);
+% axis equal; axis([-3.5 6.5 -1 5.5]);
+% for t=1:model.nbData
+% h = plotArm(r(end).x(:,t), ones(model.nbVarU,1)*model.l, zeros(3,1), .2, [0 0 0]);
+% drawnow;
+% % print('-dpng',['graphs/anim/DDP_manipulator_anim' num2str(t,'%.3d') '.png']);
+% if t<model.nbData
+% delete(h);
+% end
+% end
+% % for t=model.nbData+1:model.nbData+10
+% % print('-dpng',['graphs/anim/DDP_manipulator_anim' num2str(t,'%.3d') '.png']);
+% % end
+
+pause;
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+function [Su, Sx] = transferMatrices(A, B)
+ [nbVarX, nbVarU, nbData] = size(B);
+ nbData = nbData+1;
+ Sx = kron(ones(nbData,1), speye(nbVarX));
+ Su = sparse(zeros(nbVarX*(nbData-1), nbVarU*(nbData-1)));
+ for t=1:nbData-1
+ id1 = (t-1)*nbVarX+1:t*nbVarX;
+ id2 = t*nbVarX+1:(t+1)*nbVarX;
+ id3 = (t-1)*nbVarU+1:t*nbVarU;
+ Sx(id2,:) = squeeze(A(:,:,t)) * Sx(id1,:);
+ Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
+ Su(id2,id3) = B(:,:,t);
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Given the control trajectory u and initial state x0, compute the whole state trajectory
+function x = dynSysSimulation(x0, u, model)
+ x = zeros(model.nbVarX, model.nbData);
+ x(:,1) = x0;
+ for t=1:model.nbData-1
+ x(:,t+1) = x(:,t) + u(:,t) * model.dt;
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Linearize the system along the trajectory computing the matrices A and B
+function [A, B] = linSys(x, u, model)
+ A = zeros(model.nbVarX, model.nbVarX, model.nbData-1);
+ B = zeros(model.nbVarX, model.nbVarU, model.nbData-1);
+ Ac = zeros(model.nbVarX);
+ Bc = eye(model.nbVarX, model.nbVarU);
+ for t=1:model.nbData-1
+ %Discretize the linear system
+ A(:,:,t) = eye(model.nbVarX) + Ac * model.dt;
+ B(:,:,t) = Bc * model.dt;
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Forward kinematics
+function [x, Tf] = fkine(q, model)
+% Tf = eye(4);
+% T = repmat(Tf, [1,1,size(q,1)]);
+% for n=1:size(q,1)
+% c = cos(q(n));
+% s = sin(q(n));
+% T(:,:,n) = [c, -s, 0, model.l(n) * c; ...
+% s, c, 0, model.l(n) * s; ...
+% 0, 0, 1, 0; ...
+% 0, 0, 0, 1]; %Homogeneous matrix
+% Tf = Tf * T(:,:,n);
+% end
+% x = Tf(1:2,end);
+
+% x = [model.l * cos(tril(ones(model.nbVarU)) * q); model.l * sin(tril(ones(model.nbVarU)) * q)];
+
+ x = model.l * exp(1i * tril(ones(model.nbVarU)) * q); %Computation in matrix form
+ x = [real(x); imag(x)];
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Jacobian
+function J = jacob0(q, model)
+% e = 1E-6;
+% J = zeros(model.nbVarF, model.nbVarX);
+% for n=1:size(q,1)
+% qtmp = q;
+% qtmp(n) = qtmp(n) + e;
+% J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e; %Numeric computation
+% end
+% J
+
+ J = 1i * exp(1i * tril(ones(model.nbVarU)) * q)' * tril(ones(model.nbVarU)) * diag(model.l); %Computation in matrix form
+ J = [-real(J); imag(J)];
+end
\ No newline at end of file
diff --git a/demos/demo_OC_DDP_pendulum01.m b/demos/demo_OC_DDP_pendulum01.m
new file mode 100644
index 0000000000000000000000000000000000000000..fc4e5a1c6e8e98e0abda06058aa387cd2d04597d
--- /dev/null
+++ b/demos/demo_OC_DDP_pendulum01.m
@@ -0,0 +1,266 @@
+function demo_OC_DDP_pendulum01
+% iLQR applied to an inverted pendulum problem
+%
+% Sylvain Calinon, 2020
+%
+% Reference:
+% W. Li and E. Todorov, "Iterative linear quadratic regulator design for nonlinear biological movement systems,"
+% in Intl Conf. on InfoRatics in Control, Automation and Robotics, 2004, pp. 222–229
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.sim_time = 2; %Simulation time
+model.dt = 2E-2; %Time step size
+model.nbData = model.sim_time / model.dt; %Number of datapoints
+model.nbIter = 10; %Number of iterations for iLQR
+
+model.nbVarPos = 1; %Dimension of position
+model.nbDeriv = 2; %Number of derivatives (nbDeriv=2 for [x; dx] state)
+model.nbVarX = model.nbVarPos * model.nbDeriv; %State space dimension
+model.nbVarU = 1; %Control space dimension
+
+model.rfactor = 1E-5; %Control weight teR
+model.Mu = [pi/2; 0]; %Target
+
+model.l = 1; %Link length
+model.m = 1; %Mass
+model.g = 9.81; %Acceleration due to gravity
+model.b = 0.01; %Friction (optional)
+
+R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix (at trajectory level)
+Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
+
+Q = blkdiag(sparse((model.nbData-1)*model.nbVarX, (model.nbData-1)*model.nbVarX), speye(model.nbVarX)*1E0); %Sparse precision matrix (at trajectory level)
+MuQ = [sparse((model.nbData-1)*model.nbVarX,1); model.Mu]; %Sparse reference (at trajectory level)
+
+
+%% Iterative LQR (iLQR)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% delta_x0 = zeros(model.nbVarX, 1);
+u = zeros(model.nbVarU, model.nbData-1);
+% x0 = zeros(model.nbVarX,1);
+x0 = [-pi/2; 0];
+
+% uSigma = zeros(model.nbVarU*(model.nbData-1));
+% xSigma = zeros(model.nbVarX*model.nbData);
+
+for n=1:model.nbIter
+ %System evolution
+ x = dynSysSimulation(x0, u, model);
+% if n>1
+% x = reshape(Sx * x0 + Su * u(:), model.nbVarX, model.nbData); %Approximated dynamics
+% end
+
+ %Linearization
+ [A, B] = linSys(x, u, model);
+ [Su, Sx] = transferMatrices(A, B);
+
+% du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:) - Su' * Q * Sx * delta_x0); %Control trajectory
+ du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:)); %Control trajectory
+ delta_u = reshape(du_vec, model.nbVarU, model.nbData-1);
+% delta_x = reshape(Sx * delta_x0 + Su * delta_u(:), model.nbVarX, model.nbData);
+
+ %Estimate step size with line search method
+ alpha = 1;
+ cost0 = (MuQ - x(:))' * Q * (MuQ - x(:)) + u(:)' * R * u(:);
+ while 1
+ utmp = u + delta_u * alpha;
+ xtmp = dynSysSimulation(x0, utmp, model);
+% xtmp = reshape(Sx * x0 + Su * utmp(:), model.nbVarX, model.nbData); %Approximated dynamics
+
+ cost = (MuQ - xtmp(:))' * Q * (MuQ - xtmp(:)) + utmp(:)' * R * utmp(:); %Compute the cost
+ if cost < cost0 || alpha < 1E-4
+ break;
+ end
+ alpha = alpha * 0.5;
+ end
+% disp(alpha);
+ u = u + delta_u * alpha; %Update control by following gradient
+
+% uSigmaTmp = inv(Su' * Q * Su + R);
+% uSigma = uSigma + uSigmaTmp * alpha^2;
+% xSigma = xSigma + Su * uSigmaTmp * Su' * alpha^2;
+
+ %Log data
+ r(n).x = x;
+ r(n).u = u;
+end
+
+
+%% Stochastic sampling by exploiting the nullspace structure of the problem
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% %Standard stochastic sampling
+% uSigma = inv(Su' * Q * Su + R);
+% % xSigma = Su * uSigma * Su';
+% [V, D] = eig(full(uSigma));
+% U = real(V * D.^.5);
+% nbSamples = 50;
+% sx = zeros(model.nbData*model.nbVarX, nbSamples);
+% for n=1:nbSamples
+% utmp = u(:) + U * randn(model.nbVarU*(model.nbData-1), 1) * 1E-2;
+% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
+% sx(:,n) = xtmp(:);
+% end
+
+% %Generate structured stochastic u through Bezier curves
+% nbSamples = 50;
+% nbRBF = 18;
+% H = zeros(nbRBF, model.nbData-1);
+% tl = linspace(0, 1, model.nbData-1);
+% nbDeg = nbRBF - 1;
+% for i=0:nbDeg
+% H(i+1,:) = factorial(nbDeg) / (factorial(i) * factorial(nbDeg-i)) * (1-tl).^(nbDeg-i) .* tl.^i; %Bernstein basis functions
+% end
+% %Nullspace planning
+% [V,D] = eig(full(Q));
+% U = V * D.^.5;
+% N = eye((model.nbData-1)*model.nbVarU) - Su' * U / (U' * (Su * Su') * U + Rx) * U' * Su; %Nullspace projection matrix
+% sx = zeros(model.nbData*model.nbVarX, nbSamples);
+% for n=1:nbSamples
+% w = randn(model.nbVarU, nbRBF) * 2E0; %Random weights
+% un = w * H; %Reconstruction of signals by weighted superposition of basis functions
+% utmp = u(:) + N * un(:);
+% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
+% sx(:,n) = xtmp(:);
+% end
+
+
+%% Plot state space
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,800,800]); hold on; axis on; grid on; box on;
+% for t=1:model.nbData
+% plotGMM(r(end).x(1:2,t), xSigma(2*(t-1)+[1,2],2*(t-1)+[1,2]).*3E-4, [.8 0 0], .05);
+% end
+
+%iLQR refinement steps
+for k=1:length(r)
+ plot(r(k).x(1,:), r(k).x(2,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
+end
+
+% %Stochastic samples
+% for n=1:nbSamples
+% plot(sx(1:model.nbVarX:end,n), sx(2:model.nbVarX:end,n), '-','linewidth',1,'color',(ones(1,3)-n/nbSamples)*.9);
+% end
+
+plot(r(end).x(1,:),r(end).x(2,:), '-','linewidth',2,'color',[.8 0 0]);
+h(1) = plot(r(end).x(1,1),r(end).x(2,1), '.','markersize',30,'color',[0 0 0]);
+h(2) = plot(model.Mu(1), model.Mu(2), '.','markersize',30,'color',[.8 0 0]);
+legend(h,{'Initial state','Target'},'location','southeast','fontsize',20);
+axis equal; axis([-pi pi -2 5]);
+set(gca,'xtick',[-pi, -pi/2, 0, pi/2, pi],'xticklabel',{'-\pi','-\pi/2','0','\pi/2','\pi'},'ytick',0,'fontsize',22);
+xlabel('$q$','interpreter','latex','fontsize',30);
+ylabel('$\dot{q}$','interpreter','latex','fontsize',30);
+% print('-dpng','graphs/DDP_pendulum_q_dq01.png');
+
+
+% %% Plot control space
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[1020,10,1000,1000]);
+% t = model.dt:model.dt:model.dt*model.nbData;
+% for kk=1:size(r(1).u,1)
+% subplot(size(r(1).u,1), 1, kk); grid on; hold on; box on; box on;
+% for k=1:length(r)
+% plot(t(1:end-1), r(k).u(kk,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
+% end
+% plot(t(1:end-1), r(end).u(kk,:), '-','linewidth',2,'color',[0 0 0]);
+% axis([min(t), max(t(1:end-1)), min(r(end).u(kk,:))-2, max(r(end).u(kk,:))+2]);
+% xlabel('t'); ylabel(['u_' num2str(kk)]);
+% end
+
+
+% %% Plot robot (static)
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off;
+% plotArm(model.Mu(1), model.l, [0;0;20], .05, [.8 0 0]);
+% for t=floor(linspace(1, model.nbData, 30))
+% plotArm(r(end).x(1,t), model.l, [0; 0; t*.1], .05, [.7 .7 .7]-.7*t/model.nbData);
+% end
+% axis equal; axis([-1.2 1.2 -1.2 1.2]);
+% % print('-dpng','graphs/DDP_pendulum01.png');
+
+
+% %% Plot animated robot
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off;
+% plotArm(model.Mu(1), model.l, [0;0;-1], .05, [.8 0 0]);
+% axis equal; axis([-1.2 1.2 -1.2 1.2]);
+% h=[];
+% for t=1:model.nbData
+% % delete(h);
+% h = plotArm(r(end).x(1,t), model.l, zeros(3,1), .05, [0 0 0]);
+% drawnow;
+% % print('-dpng',['graphs/anim/DDP_pendulum_anim' num2str(t,'%.3d') '.png']);
+% end
+% % for t=model.nbData+1:model.nbData+10
+% % print('-dpng',['graphs/anim/DDP_pendulum_anim' num2str(t,'%.3d') '.png']);
+% % end
+
+pause;
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+function [Su, Sx] = transferMatrices(A, B)
+ [nbVarX, nbVarU, nbData] = size(B);
+ nbData = nbData+1;
+ Sx = kron(ones(nbData,1), speye(nbVarX));
+ Su = sparse(zeros(nbVarX*(nbData-1), nbVarU*(nbData-1)));
+ for t=1:nbData-1
+ id1 = (t-1)*nbVarX+1:t*nbVarX;
+ id2 = t*nbVarX+1:(t+1)*nbVarX;
+ id3 = (t-1)*nbVarU+1:t*nbVarU;
+ Sx(id2,:) = squeeze(A(:,:,t)) * Sx(id1,:);
+ Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
+ Su(id2,id3) = B(:,:,t);
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Given the control trajectory u and initial state x0, compute the whole state trajectory
+function x = dynSysSimulation(x0, u, model)
+ b = model.b;
+ l = model.l;
+ m = model.m;
+ g = model.g;
+ x = zeros(model.nbVarX, model.nbData);
+ f = zeros(model.nbVarX, 1);
+ x(:,1) = x0;
+ for t=1:model.nbData-1
+ f(1) = x(2,t);
+% f(2) = u(t) / (m * l^2) - cos(x(1,t)) * g / l - (b / (m * l^2)) * x(2,t); %With friction
+ f(2) = u(t) / (m * l^2) - cos(x(1,t)) * g / l; %Without friction
+ x(:,t+1) = x(:,t) + f * model.dt;
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Linearize the system along the trajectory computing the matrices A and B
+function [A, B] = linSys(x, u, model)
+ b = model.b;
+ g = model.g;
+ l = model.l;
+ m = model.m;
+ A = zeros(model.nbVarX, model.nbVarX, model.nbData-1);
+ B = zeros(model.nbVarX, model.nbVarU, model.nbData-1);
+ Bc = [0; (m * l^2)^2];
+ for t=1:model.nbData-1
+ %Linearize the system
+% Ac = [0, 1; sin(x(1,t)) * g / l, -b / (m * l^2)]; %With friction
+ Ac = [0, 1; sin(x(1,t)) * g / l, 0]; %Without friction
+
+ %Discretize the linear system
+ A(:,:,t) = eye(model.nbVarX) + Ac * model.dt;
+ B(:,:,t) = Bc * model.dt;
+ end
+
+% %Symbolic expressions to find linearized system
+% syms b m l g x u
+% x = sym('x',[2 1]);
+% f(1) = x(2);
+% f(2) = u / (m * l^2) - (g / l) * sin(x(1)) - (b / (m * l^2)) * x(2);
+% jacobian(f, x)
+% jacobian(f, u)
+end
\ No newline at end of file
diff --git a/demos/demo_MPC01.m b/demos/demo_OC_LQT01.m
similarity index 95%
rename from demos/demo_MPC01.m
rename to demos/demo_OC_LQT01.m
index a6bac4861b914c654c953fbbafe2465cbd7354e8..6a06826be8ab106f1f8d0698ed053a88deb4ef6b 100644
--- a/demos/demo_MPC01.m
+++ b/demos/demo_OC_LQT01.m
@@ -1,5 +1,5 @@
-function demo_MPC01
-% Batch LQR with viapoints and a double integrator system.
+function demo_OC_LQT01
+% Batch LQR with viapoints and simple/double/triple integrator system.
%
% If this code is useful for your research, please cite the related publication:
% @incollection{Calinon19chapter,
@@ -36,7 +36,7 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbData = 200; %Number of datapoints
-nbPoints = 3; %Number of keypoints
+nbPoints = 3; %Number of viapoints
nbVarPos = 2; %Dimension of position data (here: x1,x2)
nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
nbVar = nbVarPos * nbDeriv; %Dimension of state vector
@@ -72,7 +72,7 @@ for n=2:nbData
end
-%% Task setting
+%% Task setting (viapoints passing)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tl = linspace(1, nbData, nbPoints+1);
tl = round(tl(2:end));
diff --git a/demos/demo_MPC02.m b/demos/demo_OC_LQT02.m
similarity index 99%
rename from demos/demo_MPC02.m
rename to demos/demo_OC_LQT02.m
index 36135c3a0a909dc19ed98e68b6f8f8dc31619b72..837ddfdfd1afd6c5064615f4ab678d865c614f6f 100644
--- a/demos/demo_MPC02.m
+++ b/demos/demo_OC_LQT02.m
@@ -1,4 +1,4 @@
-function demo_MPC02
+function demo_OC_LQT02
% Batch computation of linear quadratic tracking problem, by tracking position and velocity references.
%
% If this code is useful for your research, please cite the related publication:
diff --git a/demos/demo_MPC03.m b/demos/demo_OC_LQT03.m
similarity index 99%
rename from demos/demo_MPC03.m
rename to demos/demo_OC_LQT03.m
index 0c7fb581194044b1aeef132e961fec844e47024b..6a9af095c637cb43cf8ac88fc9c9f7408bed91d5 100644
--- a/demos/demo_MPC03.m
+++ b/demos/demo_OC_LQT03.m
@@ -1,4 +1,4 @@
-function demo_MPC03
+function demo_OC_LQT03
% Batch computation of linear quadratic tracking problem, by tracking only position references.
%
% If this code is useful for your research, please cite the related publication:
diff --git a/demos/demo_MPC04.m b/demos/demo_OC_LQT04.m
similarity index 99%
rename from demos/demo_MPC04.m
rename to demos/demo_OC_LQT04.m
index 3d28f9a93b14deaccb8cd3d34791e9ac088da098..7a7a6462417416323bfcce8c0604ba19bb978b99 100644
--- a/demos/demo_MPC04.m
+++ b/demos/demo_OC_LQT04.m
@@ -1,4 +1,4 @@
-function demo_MPC04
+function demo_MPC_LQT04
% Control of a spring attached to a point with batch LQR (with augmented state space)
%
% If this code is useful for your research, please cite the related publication:
diff --git a/demos/demo_MPC_Lagrangian01.m b/demos/demo_OC_LQT_Lagrangian01.m
similarity index 91%
rename from demos/demo_MPC_Lagrangian01.m
rename to demos/demo_OC_LQT_Lagrangian01.m
index dc414780f0778c69d4b1b2f2afcfb2750686b80e..b9e4f38b90ef75c3e870b211239a6771f3c37f30 100644
--- a/demos/demo_MPC_Lagrangian01.m
+++ b/demos/demo_OC_LQT_Lagrangian01.m
@@ -1,5 +1,6 @@
-function demo_MPC_Lagrangian01
+function demo_OC_LQT_Lagrangian01
% Batch LQT with Lagrangian in matrix form to force first and last point to coincide in order to form a periodic motion.
+% (see also demo_MPC_noInitialState01.m)
%
% If this code is useful for your research, please cite the related publication:
% @incollection{Berio19MM,
@@ -87,6 +88,11 @@ end
Mu0 = [[-1;1], [1;1], [1;-1], [-1;-1]];
tl = linspace(1,nbData,nbPoints+2);
tl = round(tl(2:end-1)); %[nbData/2, nbData];
+
+% Mu0 = [[-1;1], [1;1], [1;-1], [-1;-1], [-1;1]];
+% tl = linspace(1,nbData,nbPoints); %+2);
+% tl = round(tl); %%(2:end-1)); %[nbData/2, nbData];
+
MuQ = zeros(nbVar*nbData,1);
Q = zeros(nbVar*nbData);
for t=1:length(tl)
@@ -106,8 +112,8 @@ end
%% Batch LQT with Lagrangian in matrix form
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
C = zeros(nbVar,nbData*nbVar); %Linear constraint C*x=0
-C(:,1:nbVar) = -eye(nbVar) .* 1E3; %p1-p2=0 (for both position and speed)
-C(:,end-nbVar+1:end) = eye(nbVar) .* 1E3; %p1-p2=0 (for both position and speed)
+C(:,1:nbVar) = -eye(nbVar) * 1E3; %p1-p2=0 (for both position and speed)
+C(:,end-nbVar+1:end) = eye(nbVar) * 1E3; %p1-p2=0 (for both position and speed)
%Augmented solution by finding the optimal initial state together with the optimal control commands
S = [Su, Sx];
@@ -133,7 +139,7 @@ end
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1000 1000],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
+figure('position',[10 10 800 800],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
for t=1:nbData
plotGMM(rx(nbVar*(t-1)+[1,2]), xSigma(nbVar*(t-1)+[1,2],nbVar*(t-1)+[1,2]).*1E-7, [.2 .2 .2], .1);
end
diff --git a/demos/demo_MPC_augmSigma01.m b/demos/demo_OC_LQT_augmSigma01.m
similarity index 99%
rename from demos/demo_MPC_augmSigma01.m
rename to demos/demo_OC_LQT_augmSigma01.m
index dde31d88b59d57b0d6bb85c26d7ceadc168d8412..eff9c7b18756819eb3a8891fd610f9b87d939105 100644
--- a/demos/demo_MPC_augmSigma01.m
+++ b/demos/demo_OC_LQT_augmSigma01.m
@@ -1,4 +1,4 @@
-function demo_MPC_augmSigma01
+function demo_OC_LQT_augmSigma01
% Batch LQR with augmented covariance to transform the tracking problem to a regulation problem.
%
% If this code is useful for your research, please cite the related publication:
diff --git a/demos/demo_OC_LQT_ballistic01.m b/demos/demo_OC_LQT_ballistic01.m
new file mode 100644
index 0000000000000000000000000000000000000000..c2f0af6a73fdbf37115ebe4451d4b3433ba2ee00
--- /dev/null
+++ b/demos/demo_OC_LQT_ballistic01.m
@@ -0,0 +1,134 @@
+function demo_OC_LQT_ballistic01
+% Batch LQT with augmented state to solve simple ballistic problem
+%
+% Sylvain Calinon, 2020
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+nbData = 100; %Number of datapoints
+nbVarX = 6; %Dimension of state vector
+nbVarU = 2; %Dimension of state vector
+dt = 1E-2; %Time step duration
+rfactor = 1E-7; %dt^nbDeriv; %Control cost in LQR
+R = speye((nbData-1)*nbVarU) * rfactor; %Control cost matrix
+
+m = 1; %Object mass
+f = [0; -m*9.81]; %Gravity vector
+x01 = [0; 0]; %Initial position
+xTar = [1; 0]; %Target position
+tRelease = 10; %Time step when ball is released
+
+
+%% Linear dynamical system parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+Ac = kron([0, 1, 0; 0, 0, 1; 0, 0, 0], eye(2));
+Bc = kron([0; 1; 0], eye(2));
+Ad = eye(nbVarX) + Ac * dt; %Parameters for discrete dynamical system
+Bd = Bc * dt; %Parameters for discrete dynamical system
+
+%Build Sx and Su matrices for heterogeneous A and B
+A = repmat(Ad,[1,1,nbData-1]);
+B = repmat(Bd,[1,1,nbData-1]);
+% B(:,:,2:end) = 0; %Control command at first time step only
+B(:,:,tRelease:end) = 0; %Ball released at time step tRelease
+
+[Su, Sx] = transferMatrices(A, B);
+
+
+%% Task setting (sparse reference to reach target at end of the motion)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+Q = blkdiag(sparse((nbData-1)*nbVarX,(nbData-1)*nbVarX), diag([1E4, 1E4, 0, 0, 0, 0])); %Sparse precision matrix (at trajectory level)
+MuQ = [sparse((nbData-1)*nbVarX,1); [xTar; 0; 0; 0; 0]]; %Sparse reference (at trajectory level)
+
+
+%% Batch LQT
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+x0 = [x01; zeros(2,1); f];
+u = (Su' * Q * Su + R) \ Su' * Q * (MuQ - Sx * x0);
+rx = reshape(Sx*x0+Su*u, nbVarX, nbData); %Reshape data for plotting
+
+uSigma = inv(Su' * Q * Su + R);
+xSigma = Su * uSigma * Su';
+
+
+%% 2D plot
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10 10 800 800],'color',[1 1 1]); hold on; axis off;
+set(0,'DefaultAxesLooseInset',[0,0,0,0]);
+set(gca,'LooseInset',[0,0,0,0]);
+% %Uncertainty
+% for t=1:nbData
+% plotGMM(rx(1:2,t), xSigma(nbVar*(t-1)+[1,2],nbVar*(t-1)+[1,2]) * 1E-3, [0 0 0], .04); %Ball motion
+% end
+%Ball motion
+plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
+h(1) = plot(rx(1,1), rx(2,1), '.','markersize',70,'color',[0 0 0]); %Initial position
+h(2) = plot(rx(1,tRelease), rx(2,tRelease), '.','markersize',70,'color',[0 .6 0]); %Release position
+h(3) = plot(xTar(1), xTar(2), '.','markersize',70,'color',[.8 0 0]); %Target position
+axis equal; %axis([-.05 1.2,-.05 1.1]);
+% %Animation
+% for t=1:nbData
+% ha(1) = plot(rx(1,t), rx(2,t), '.','markersize',70,'color',[0 0 0]);
+% drawnow;
+% delete(ha);
+% end
+legend(h,{'Initial point','Release point','Target point'},'fontsize',30,'location','northwest');
+legend('boxoff');
+% print('-dpng','graphs/MPC_ballistic_basic01.png');
+
+
+% %% 2D sampling plot
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10 10 1200 1200],'color',[1 1 1]); hold on; axis off;
+% set(0,'DefaultAxesLooseInset',[0,0,0,0]);
+% set(gca,'LooseInset',[0,0,0,0]);
+% nbSamples = 50;
+% [V,D] = eig(uSigma);
+% for n=1:nbSamples
+% u2 = u + real(V*D.^.5) * randn(size(u)).*1E-2;
+% rx2 = reshape(Sx*x0+Su*u2, nbVar, nbData); %Reshape data for plotting
+% patch('xdata',rx2(1,[1:t,t:-1:1]),'ydata',rx2(2,[1:t,t:-1:1]),'linewidth',4,'edgecolor',[0 0 0],'edgealpha',.05); %Ball motion
+% plot(rx2(1,tRelease), rx2(2,tRelease), '.','markersize',30,'color',[0 .6 0]); %Release of ball
+% % plot2DArrow(rx(1:2,tEvent(1)), diff(rx(1:2,tEvent(1):tEvent(1)+1),1,2)*12E0, [0 .6 0], 4, .01);
+% % plot2DArrow(rx(7:8,tEvent(2)), diff(rx(7:8,tEvent(2):tEvent(2)+1),1,2)*12E0, [0 0 .8], 4, .01);
+% end
+%
+% plot(rx(1,1), rx(2,1), '.','markersize',70,'color',[0 0 0]); %Initial position
+% plot(xTar(1), xTar(2), '.','markersize',70,'color',[.8 0 0]); %Target position
+% axis equal; axis([-.05 1.2,-.05 1.1]);
+% % print('-dpng','graphs/MPC_ballistic02.png');
+
+
+% %% Additional plot
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10 10 1600 1200]); hold on; axis off;
+% colormap(repmat(linspace(1,0,64),3,1)');
+% imagesc(Su);
+% axis tight; axis equal; axis ij;
+
+pause;
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%
+function [Su, Sx] = transferMatrices(A, B)
+ [nbVarX, nbVarU, nbData] = size(B);
+ nbData = nbData+1;
+ Sx = kron(ones(nbData,1), eye(nbVarX));
+ for t=1:nbData-1
+ id1 = (t-1)*nbVarX+1:t*nbVarX;
+ id2 = t*nbVarX+1:(t+1)*nbVarX;
+ Sx(id2,:) = squeeze(A(:,:,t)) * Sx(id1,:);
+ end
+ Su = zeros(nbVarX*(nbData-1), nbVarU*(nbData-1));
+ for t=1:nbData-1
+ id1 = (t-1)*nbVarX+1:t*nbVarX;
+ id2 = t*nbVarX+1:(t+1)*nbVarX;
+ id3 = (t-1)*nbVarU+1:t*nbVarU;
+ Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
+ Su(id2,id3) = B(:,:,t);
+ end
+end
\ No newline at end of file
diff --git a/demos/demo_MPC_constrained01.m b/demos/demo_OC_LQT_constrained01.m
similarity index 99%
rename from demos/demo_MPC_constrained01.m
rename to demos/demo_OC_LQT_constrained01.m
index cf4c9eadd1b277dc8304dc2623853c3bae02fdde..72cf633fb6542400e923b8627d1d2628c04d0eac 100644
--- a/demos/demo_MPC_constrained01.m
+++ b/demos/demo_OC_LQT_constrained01.m
@@ -1,4 +1,4 @@
-function demo_MPC_constrained01
+function demo_OC_LQT_constrained01
% Constrained batch LQT by using quadratic programming solver, with an encoding of position and velocity data.
%
% If this code is useful for your research, please cite the related publication:
diff --git a/demos/demo_MPC_fullQ01.m b/demos/demo_OC_LQT_fullQ01.m
similarity index 88%
rename from demos/demo_MPC_fullQ01.m
rename to demos/demo_OC_LQT_fullQ01.m
index d0c1746a9289056bf16f7a7c078ce20734419a98..e33c71963551a7d0e57027c2714c7d72d39a54a1 100644
--- a/demos/demo_MPC_fullQ01.m
+++ b/demos/demo_OC_LQT_fullQ01.m
@@ -1,4 +1,4 @@
-function demo_MPC_fullQ01
+function demo_OC_LQT_fullQ01
% Batch LQT exploiting full Q matrix (e.g., by constraining the motion to pass through a common point at different time steps).
%
% If this code is useful for your research, please cite the related publication:
@@ -39,7 +39,7 @@ nbData = 200; %Number of datapoints
% nbData = 40; %Number of datapoints
nbPoints = 3; %Number of keypoints
nbVarPos = 2; %Dimension of position data (here: x1,x2)
-nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
+nbDeriv = 1; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
nbVar = nbVarPos * nbDeriv; %Dimension of state vector
dt = 1E-2; %Time step duration
rfactor = 1E-8; %dt^nbDeriv; %Control cost in LQR
@@ -109,18 +109,18 @@ end
% Q(id2(:,2), id2(:,1)) = -eye(nbVarPos)*1E0;
% end
-%Constraining the velocity before and after the keypoints to be correlated, effectively creating a straight line (version 2)
-toff = 12;
-for k=1:nbPoints-1
- for t=1:toff
- id2(1:2,1) = id(:,k) - t * nbVar + nbVarPos;
- id2(1:2,2) = id(:,k) + t * nbVar + nbVarPos;
- Q(id2(:,1), id2(:,1)) = eye(nbVarPos)*1E0;
- Q(id2(:,2), id2(:,2)) = eye(nbVarPos)*1E0;
- Q(id2(:,1), id2(:,2)) = -eye(nbVarPos)*1E0;
- Q(id2(:,2), id2(:,1)) = -eye(nbVarPos)*1E0;
- end
-end
+% %Constraining the velocity before and after the keypoints to be correlated, effectively creating a straight line (version 2)
+% toff = 12;
+% for k=1:nbPoints-1
+% for t=1:toff
+% id2(1:2,1) = id(:,k) - t * nbVar + nbVarPos;
+% id2(1:2,2) = id(:,k) + t * nbVar + nbVarPos;
+% Q(id2(:,1), id2(:,1)) = eye(nbVarPos)*1E0;
+% Q(id2(:,2), id2(:,2)) = eye(nbVarPos)*1E0;
+% Q(id2(:,1), id2(:,2)) = -eye(nbVarPos)*1E0;
+% Q(id2(:,2), id2(:,1)) = -eye(nbVarPos)*1E0;
+% end
+% end
% %Constraining the velocity before and after the keypoints to form a desired angle (version 1)
% a = pi/2; %desired angle
@@ -192,6 +192,15 @@ end
% Q(id2(:,2), id2(:,1)) = -eye(nbVarPos)*1E0;
% end
+%Constraining R (same control command applied during a time interval)
+toff = 12;
+for k=1:nbPoints-1
+ tt = tl(k) + [-toff,toff];
+ id2 = tt(1) * nbVarPos +1 : tt(2) * nbVarPos;
+ R(id2,id2) = -1E-1 * kron(ones(2*toff), eye(nbVarPos)) + 2 * 1E-1 * eye(length(id2));
+end
+
+
%% Batch LQR reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -202,7 +211,7 @@ rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1000 1000],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
+figure('position',[10 10 800 800],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
plot(rx(1,1), rx(2,1), '.','markersize',35,'color',[0 0 0]);
for k=1:nbPoints-1
@@ -213,6 +222,12 @@ plot(MuQ(id(1,:)), MuQ(id(2,:)), '.','markersize',35,'color',[.8 0 0]);
axis equal;
% print('-dpng','graphs/MPC_fullQ01a.png');
+%Plot 1D
+figure; hold on;
+plot(u(1:nbVarPos:end),'k-');
+plot(tl, 0, '.','markersize',25,'color',[0 .6 0]);
+xlabel('t'); ylabel('u_1');
+
% %Visualize Q
% xlim = [1 size(Q,1); 1 size(Q,1)];
% figure('position',[1030 10 1000 1000],'color',[1 1 1]); hold on; axis off;
diff --git a/demos/demo_MPC_fullQ02.m b/demos/demo_OC_LQT_fullQ02.m
similarity index 99%
rename from demos/demo_MPC_fullQ02.m
rename to demos/demo_OC_LQT_fullQ02.m
index 7e4f57773f0048db6d562b780d85df2f5dcea567..b19be31e5c82df5766d487ab0e8ce3270eb53d9f 100644
--- a/demos/demo_MPC_fullQ02.m
+++ b/demos/demo_OC_LQT_fullQ02.m
@@ -1,4 +1,4 @@
-function demo_MPC_fullQ02
+function demo_OC_LQT_fullQ02
% Batch LQT exploiting full Q matrix to constrain the motion of two agents.
%
% If this code is useful for your research, please cite the related publication:
@@ -187,7 +187,7 @@ end
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 2500 600]);
+figure('position',[10 10 1200 600]);
subplot(1,4,1); hold on; axis off;
% set(0,'DefaultAxesLooseInset',[1,1,1,1]*1E2);
% set(gca,'LooseInset',[1,1,1,1]*1E2);
diff --git a/demos/demo_MPC_fullQ03.m b/demos/demo_OC_LQT_fullQ03.m
similarity index 98%
rename from demos/demo_MPC_fullQ03.m
rename to demos/demo_OC_LQT_fullQ03.m
index 7b12623e545c19b9931ca0a8339c195e5712e292..2c5f41425ec2f35bf54a00e4a16f9349cac6920d 100644
--- a/demos/demo_MPC_fullQ03.m
+++ b/demos/demo_OC_LQT_fullQ03.m
@@ -1,4 +1,4 @@
-function demo_MPC_fullQ03
+function demo_OC_LQT_fullQ03
% Batch LQT problem exploiting full Q matrix to constrain the motion of two agents in a simple ballistic task.
%
% If this code is useful for your research, please cite the related publication:
@@ -117,7 +117,7 @@ rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1000 1000],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
+figure('position',[10 10 800 800],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
%Agent 1
% for t=1:nbData
diff --git a/demos/demo_OC_LQT_fullQ04.m b/demos/demo_OC_LQT_fullQ04.m
new file mode 100644
index 0000000000000000000000000000000000000000..9ab144e73ff0c3f56e764eb6c7c704596d5d5688
--- /dev/null
+++ b/demos/demo_OC_LQT_fullQ04.m
@@ -0,0 +1,347 @@
+function demo_OC_LQT_fullQ04
+% Batch LQT problem exploiting full Q matrix to constrain the motion of two agents in a ballistic task mimicking a bimanual tennis serve problem
+%
+% If this code is useful for your research, please cite the related publication:
+% @incollection{Calinon19chapter,
+% author="Calinon, S. and Lee, D.",
+% title="Learning Control",
+% booktitle="Humanoid Robotics: a Reference",
+% publisher="Springer",
+% editor="Vadakkepat, P. and Goswami, A.",
+% year="2019",
+% pages="1261--1312",
+% doi="10.1007/978-94-007-6046-2_68"
+% }
+%
+% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
+% Written by Sylvain Calinon, http://calinon.ch/
+%
+% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
+%
+% PbDlib is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License version 3 as
+% published by the Free Software Foundation.
+%
+% PbDlib is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU General Public License for more details.
+%
+% You should have received a copy of the GNU General Public License
+% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+nbData = 200; %Number of datapoints
+nbAgents = 3; %Number of agents
+nbVarPos = 2 * nbAgents; %Dimension of position data (here: x1,x2 for the three agents)
+nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
+nbVar = nbVarPos * (nbDeriv+1); %Dimension of state vector (incl. offset to apply gravity effect)
+nbVarU = 4; %Number of control variables (acceleration commands of the two hands)
+dt = 1E-2; %Time step duration
+rfactor = 1E-8; %dt^nbDeriv; %Control cost in LQR
+R = speye((nbData-1)*nbVarU) * rfactor; %Control cost matrix
+
+m = [2*1E-1, 2*1E-1, 2*1E-1]; %Mass of agents (left hand, right hand and ball)
+g = [0; -9.81]; %Gravity vector
+
+tEvent = [50, 100, 150]; %Time steps for ball release and ball hitting
+x01 = [1.6; 0]; %Inital position of Agent 1 (left hand)
+x02 = [2; 0]; %Inital position of Agent 2 (right hand)
+xTar = [1; -.2]; %Final position of Agent 3 (ball)
+
+% tEvent = [40, 110, 190]; %40 50 %Time steps for ball release and ball hitting
+% x01 = [1.4; .2]; %Inital position of Agent 1 (left hand)
+% x02 = [2; -.1]; %Inital position of Agent 2 (right hand)
+% xTar = [1.3; 0]; %Final position of Agent 3 (ball)
+
+
+%% Linear dynamical system parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+Ac1 = kron([0, 1, 0; 0, 0, 1; 0, 0, 0], eye(2));
+Bc1 = kron([0; 1; 0], eye(2));
+Ac = kron(eye(nbAgents), Ac1);
+Bc = [kron(eye(2), Bc1); zeros(6,nbVarU)]; %Ball is not directly controlled
+
+Ad = eye(nbVar) + Ac * dt; %Parameters for discrete dynamical system
+Bd = Bc * dt; %Parameters for discrete dynamical system
+
+%Set heterogeneous A and B
+A = repmat(Ad, [1,1,nbData-1]);
+B = repmat(Bd, [1,1,nbData-1]);
+
+%Set Agent 3 state (ball) equals to Agent 1 state (left hand) until tEvent(1)
+A(13:16,:,1:tEvent(1)) = 0;
+% A(13:14,13:14,1:tEvent(1)) = repmat(eye(2),[1,1,tEvent(1)]);
+A(13:14,1:2,1:tEvent(1)) = repmat(eye(2),[1,1,tEvent(1)]);
+A(13:14,3:4,1:tEvent(1)) = repmat(eye(2),[1,1,tEvent(1)]) * dt;
+A(15:16,3:4,1:tEvent(1)) = repmat(eye(2),[1,1,tEvent(1)]);
+A(15:16,5:6,1:tEvent(1)) = repmat(eye(2),[1,1,tEvent(1)]) * dt;
+% A(13:18,:,1)
+
+%Set Agent 3 state (ball) equals to Agent 2 state (right hand) at tEvent(2)
+A(13:16,:,tEvent(2)) = 0;
+% A(13:14,13:14,tEvent(2)) = eye(2);
+A(13:14,7:8,tEvent(2)) = eye(2);
+A(13:14,9:10,tEvent(2)) = eye(2) * dt;
+A(15:16,9:10,tEvent(2)) = eye(2);
+A(15:16,11:12,tEvent(2)) = eye(2) * dt;
+% A(13:18,:,tEvent(2))
+
+[Su, Sx] = transferMatrices(A, B); %Build transfer matrices
+
+% figure('position',[10 10 1600 1200]); hold on; axis off;
+% imagesc(Su);
+% axis tight; axis equal; axis ij;
+
+
+%% Task setting
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+MuQ = zeros(nbVar*nbData,1);
+Q = zeros(nbVar*nbData);
+
+%Agent 1 (left hand) and Agent 2 (right hand) must come back to initial pose at tEvent(3) and stay here
+for t=tEvent(3):nbData
+ id = [1:4] + (t-1) * nbVar; %Left hand
+ Q(id,id) = eye(4) * 1E3;
+ MuQ(id) = [x01; zeros(2,1)];
+ id = [7:10] + (t-1) * nbVar; %Right hand
+ Q(id,id) = eye(4) * 1E3;
+ MuQ(id) = [x02; zeros(2,1)];
+
+% id = [1:2] + (t-1) * nbVar; %Left hand
+% Q(id,id) = eye(2);
+% MuQ(id) = x01;
+% id = [7:8] + (t-1) * nbVar; %Right hand
+% Q(id,id) = eye(2);
+% MuQ(id) = x02;
+
+% id = [3:4] + (t-1) * nbVar; %Left hand
+% Q(id,id) = eye(2);
+% MuQ(id) = zeros(2,1);
+% id = [9:10] + (t-1) * nbVar; %Right hand
+% Q(id,id) = eye(2);
+% MuQ(id) = zeros(2,1);
+end
+
+%Agent 3 (ball) must reach desired target at the end of the movement
+id = [13:14] + (nbData-1) * nbVar;
+Q(id,id) = eye(2);
+MuQ(id) = xTar;
+
+%Agent 2 and Agent 3 must meet at tEvent(2) (right hand hitting the ball)
+id = [7:8,13,14] + (tEvent(2)-1) * nbVar;
+MuQ(id) = rand(4,1);
+MuQ(id(3:4)) = MuQ(id(1:2)); %Proposed common meeting point for the two agents (can be any point)
+Q(id,id) = eye(4);
+Q(id(1:2), id(3:4)) = -eye(2);
+Q(id(3:4), id(1:2)) = -eye(2);
+
+
+%% Batch LQT reproduction
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+x0 = [x01; zeros(2,1); m(1)*g; ...
+ x02; zeros(2,1); m(2)*g; ...
+ x01; zeros(2,1); m(3)*g];
+u = (Su' * Q * Su + R) \ Su' * Q * (MuQ - Sx * x0);
+rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
+
+% uSigma = inv(Su' * Q * Su + R);
+% xSigma = Su * uSigma * Su';
+
+
+%% 2D plot
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10 10 1200 800],'color',[1 1 1]); hold on; axis off;
+% %Uncertainty
+% for t=1:nbData
+% plotGMM(rx(1:2,t), xSigma(nbVar*(t-1)+[1,2],nbVar*(t-1)+[1,2])*3E-7, [0 0 0], .04); %Agent 1 (left hand)
+% end
+% for t=1:nbData
+% plotGMM(rx(7:8,t), xSigma(nbVar*(t-1)+[7,8],nbVar*(t-1)+[7,8])*3E-7, [.6 .6 .6], .08); %Agent 2 (right hand)
+% end
+% for t=1:nbData
+% plotGMM(rx(13:14,t), xSigma(nbVar*(t-1)+[13,14],nbVar*(t-1)+[13,14])*3E-7, [.8 .4 0], .08); %Agent 3 (ball)
+% end
+
+%Agents
+h(1) = plot(rx(1,:), rx(2,:), '-','linewidth',4,'color',[0 0 0]); %Agent 1 (left hand)
+h(2) = plot(rx(7,:), rx(8,:), '-','linewidth',4,'color',[.6 .6 .6]); %Agent 2 (right hand)
+h(3) = plot(rx(13,:), rx(14,:), ':','linewidth',4,'color',[.8 .4 0]); %Agent 3 (ball)
+
+%Events
+h(4) = plot(rx(1,1), rx(2,1), '.','markersize',40,'color',[0 0 0]); %Initial position (left hand)
+h(5) = plot(rx(7,1), rx(8,1), '.','markersize',40,'color',[.6 .6 .6]); %Initial position (right hand)
+h(6) = plot(rx(1,tEvent(1)), rx(2,tEvent(1)), '.','markersize',40,'color',[0 .6 0]); %Release of ball
+h(7) = plot(rx(7,tEvent(2)), rx(8,tEvent(2)), '.','markersize',40,'color',[0 0 .8]); %Hitting of ball
+h(8) = plot(xTar(1), xTar(2), '.','markersize',40,'color',[.8 0 0]); %Ball target
+plot2DArrow(rx(1:2,tEvent(1)), diff(rx(1:2,tEvent(1):tEvent(1)+1),1,2)*12E0, [0 .6 0], 4, .01);
+plot2DArrow(rx(7:8,tEvent(2)), diff(rx(7:8,tEvent(2):tEvent(2)+1),1,2)*12E0, [0 0 .8], 4, .01);
+axis equal; axis([1, 2.2, -0.3, 0.2]);
+
+% text(rx(1,1)+.01, rx(2,1)-.01,'$t=0$','interpreter','latex','fontsize',20);
+% text(rx(1,1)+.01, rx(2,1)-.03,'$t=\frac{3T}{4}$','interpreter','latex','fontsize',20);
+% text(rx(7,1)+.01, rx(8,1)+.02,'$t=0$','interpreter','latex','fontsize',20);
+% text(rx(7,1)+.01, rx(8,1),'$t=\frac{3T}{4}$','interpreter','latex','fontsize',20);
+% text(rx(1,tEvent(1))-.01, rx(2,tEvent(1))-.02,'$t=\frac{T}{4}$','interpreter','latex','fontsize',20);
+% text(rx(13,tEvent(2))+.01, rx(14,tEvent(2))+.01,'$t=\frac{T}{2}$','interpreter','latex','fontsize',20);
+% text(xTar(1)+.02, xTar(2),'$t=T$','interpreter','latex','fontsize',20);
+% legend(h, {'Left hand motion (Agent 1)','Right hand motion (Agent 2)','Ball motion (Agent 3)', ...
+% 'Left hand initial point','Right hand initial point','Ball releasing point','Ball hitting point','Ball target'}, 'fontsize',20,'location','northwest');
+% legend('boxoff');
+% print('-dpng','graphs/MPC_tennisServe01.png');
+
+
+% %% 2D animated plot
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10 10 1100 450],'color',[1 1 1]); hold on; axis off;
+% plot(rx(1,1), rx(2,1), '.','markersize',40,'color',[0 0 0]); %Initial position (left hand)
+% plot(rx(7,1), rx(8,1), '.','markersize',40,'color',[.6 .6 .6]); %Initial position (right hand)
+% plot(xTar(1), xTar(2), '.','markersize',40,'color',[.8 0 0]); %Ball target
+% axis equal; axis([1, 2.2, -0.3, 0.2]);
+% %Animation
+% for t=1:nbData
+% h(1) = plot(rx(1,1:t), rx(2,1:t), '-','linewidth',4,'color',[0 0 0]); %Agent 1 (left hand)
+% h(2) = plot(rx(7,1:t), rx(8,1:t), '-','linewidth',4,'color',[.6 .6 .6]); %Agent 2 (right hand)
+% h(3) = plot(rx(13,1:t), rx(14,1:t), ':','linewidth',4,'color',[.8 .4 0]); %Agent 3 (ball)
+% % h(1) = patch('xdata',rx(1,[1:t,t:-1:1]),'ydata',rx(2,[1:t,t:-1:1]),'linewidth',4,'edgecolor',[0 0 0],'edgealpha',.2); %Agent 1 (left hand)
+% % h(2) = patch('xdata',rx(7,[1:t,t:-1:1]),'ydata', rx(8,[1:t,t:-1:1]),'linewidth',4,'edgecolor',[.6 .6 .6],'edgealpha',.2); %Agent 2 (right hand)
+% % h(3) = patch('xdata',rx(13,[1:t,t:-1:1]),'ydata', rx(14,[1:t,t:-1:1]),'linewidth',4,'edgecolor',[.8 .4 0],'edgealpha',.2); %Agent 3 (ball) %'linestyle',':'
+%
+% h(4) = plot(rx(1,t), rx(2,t), '.','markersize',60,'color',[0 0 0]);
+% h(5) = plot(rx(7,t), rx(8,t), '.','markersize',60,'color',[.6 .6 .6]);
+% h(6) = plot(rx(13,t), rx(14,t), '.','markersize',50,'color',[.8 .4 0]);
+% drawnow;
+% print('-dpng',['graphs/anim/mpc01_' num2str(t,'%.3d') '.png']);
+% delete(h);
+% end
+
+
+% %% Sampling plot
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10 10 2200 900],'color',[1 1 1]); hold on; axis off;
+% nbSamples = 50;
+% [V,D] = eig(uSigma);
+% plot(rx(1,1), rx(2,1), '.','markersize',40,'color',[0 0 0]); %Initial position (left hand)
+% plot(rx(7,1), rx(8,1), '.','markersize',40,'color',[.6 .6 .6]); %Initial position (right hand)
+% plot(xTar(1), xTar(2), '.','markersize',40,'color',[.8 0 0]); %Ball target
+% for n=1:nbSamples
+% u2 = u + real(V*D.^.5) * randn(size(u)).*2E-4;
+% rx = reshape(Sx*x0+Su*u2, nbVar, nbData); %Reshape data for plotting
+% %Agents
+% patch('xdata',rx(1,[1:t,t:-1:1]),'ydata',rx(2,[1:t,t:-1:1]),'linewidth',4,'edgecolor',[0 0 0],'edgealpha',.2); %Agent 1 (left hand)
+% patch('xdata',rx(7,[1:t,t:-1:1]),'ydata', rx(8,[1:t,t:-1:1]),'linewidth',4,'edgecolor',[.6 .6 .6],'edgealpha',.2); %Agent 2 (right hand)
+% patch('xdata',rx(13,[1:t,t:-1:1]),'ydata', rx(14,[1:t,t:-1:1]),'linewidth',4,'edgecolor',[.8 .4 0],'edgealpha',.2); %Agent 3 (ball) %'linestyle',':
+% %Events
+% plot(rx(1,tEvent(1)), rx(2,tEvent(1)), '.','markersize',20,'color',[0 .6 0]); %Release of ball
+% plot(rx(7,tEvent(2)), rx(8,tEvent(2)), '.','markersize',20,'color',[0 0 .8]); %Hitting of ball
+% % plot2DArrow(rx(1:2,tEvent(1)), diff(rx(1:2,tEvent(1):tEvent(1)+1),1,2)*12E0, [0 .6 0], 4, .01);
+% % plot2DArrow(rx(7:8,tEvent(2)), diff(rx(7:8,tEvent(2):tEvent(2)+1),1,2)*12E0, [0 0 .8], 4, .01);
+% end
+% axis equal; axis([1, 2.2, -0.3, 0.2]);
+% % print('-dpng','graphs/MPC_tennisServe01f.png'); %convert -gravity Center -crop 80x80%+10-50 MPC_tennisServe01f.png MPC_tennisServe_cropped01f.png
+
+
+% %% Timeline plots
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10 10 2200 1200],'color',[1 1 1]);
+% ttl = {'Left hand','','Right hand','','Ball',''};
+% %x
+% id = [1:2,7:8,13:14];
+% for n=1:6
+% subplot(6,2,1+2*(n-1)); hold on; grid on; box on; title(ttl(n));
+% plot(rx(id(n),:),'k.');
+% if n<3
+% plot([1,tEvent(3)], [rx(id(n),1) rx(id(n),tEvent(3))], '.','markersize',40,'color',[0 0 0]);
+% end
+% if n>2 && n<5
+% plot([1,tEvent(3)], [rx(id(n),1) rx(id(n),tEvent(3))], '.','markersize',40,'color',[.6 .6 .6]);
+% end
+% if n<3 || n>4
+% plot(tEvent(1), rx(id(n),tEvent(1)), '.','markersize',40,'color',[0 .6 0]);
+% end
+% if n>2
+% plot(tEvent(2), rx(id(n),tEvent(2)), '.','markersize',40,'color',[0 0 .8]);
+% end
+% if n>4
+% plot(nbData, rx(id(n),nbData), '.','markersize',40,'color',[.8 0 0]);
+% end
+% m = round(min(rx(id(n),:)) + (max(rx(id(n),:)) - min(rx(id(n),:))) / 2, 1);
+% axis([1, nbData, m-.6, m+.6]);
+% set(gca,'xtick',[1,tEvent,nbData],'xticklabel',{},'ytick',sort([m-.6, m+.6, 0]),'fontsize',14);
+% ylabel(['$x_' num2str(n) '$'],'interpreter','latex','fontsize',22);
+% end
+% set(gca,'xtick',[1,tEvent,nbData],'xticklabel',{'0','T/4','T/2','3T/4','T'},'fontsize',14);
+% ylabel(['$x_' num2str(n) '$'],'interpreter','latex','fontsize',22);
+% %dx
+% id = [3:4,9:10,15:16];
+% for n=1:6
+% subplot(6,2,2*n); hold on; grid on; box on; title(ttl(n));
+% plot(rx(id(n),:),'k.');
+% if n<3
+% plot([1,tEvent(3)], [rx(id(n),1) rx(id(n),tEvent(3))], '.','markersize',40,'color',[0 0 0]);
+% end
+% if n>2 && n<5
+% plot([1,tEvent(3)], [rx(id(n),1) rx(id(n),tEvent(3))], '.','markersize',40,'color',[.6 .6 .6]);
+% end
+% if n<3 || n>4
+% plot(tEvent(1), rx(id(n),tEvent(1)), '.','markersize',40,'color',[0 .6 0]);
+% end
+% if n>2 && n<5
+% plot(tEvent(2), rx(id(n),tEvent(2)), '.','markersize',40,'color',[0 0 .8]);
+% end
+% if n>4
+% plot(tEvent(2)+1, rx(id(n),tEvent(2)+1), '.','markersize',40,'color',[0 0 .8]);
+% end
+% axis([1, nbData, -1.2, 1.2]);
+% set(gca,'xtick',[1,tEvent,nbData],'xticklabel',{},'ytick',[-1.2, 0, 1.2],'fontsize',14);
+% ylabel(['$\dot{x}_' num2str(n) '$'],'interpreter','latex','fontsize',22);
+% end
+% set(gca,'xtick',[1,tEvent,nbData],'xticklabel',{'0','T/4','T/2','3T/4','T'},'fontsize',14);
+% ylabel(['$\dot{x}_' num2str(n) '$'],'interpreter','latex','fontsize',22);
+% % print('-dpng','graphs/MPC_tennisServe02.png');
+
+
+%% Additional plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% %Timeline plots (u)
+% figure('position',[10 10 2200 1200],'color',[1 1 1]);
+% ttl = {'Left hand','','Right hand','','Ball (not controlled)',''};
+% for n=1:nbVarPos
+% subplot(6,1,n); hold on; grid on; box on; title(ttl(n));
+% plot(u(n:nbVarPos:end),'k.');
+% set(gca,'xtick',[1,tEvent,nbData],'ytick',[0]);
+% ylabel(['$u_' num2str(n) '$'],'interpreter','latex','fontsize',14);
+% end
+
+% %Visualize Q
+% figure('position',[1030 10 1000 1000],'color',[1 1 1],'name','Covariances'); hold on; box on;
+% set(gca,'linewidth',2); title('Q','fontsize',14);
+% colormap(gca, flipud(gray));
+% pcolor(abs(Q));
+% set(gca,'xtick',[1,size(Q,1)],'ytick',[1,size(Q,1)]);
+% axis square; axis([1 size(Q,1) 1 size(Q,1)]); shading flat;
+
+pause;
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%
+function [Su, Sx] = transferMatrices(A, B)
+ [nbVarX, nbVarU, nbData] = size(B);
+ nbData = nbData+1;
+ Sx = kron(ones(nbData,1), speye(nbVarX));
+ Su = sparse(zeros(nbVarX*(nbData-1), nbVarU*(nbData-1)));
+ for t=1:nbData-1
+ id1 = (t-1)*nbVarX+1:t*nbVarX;
+ id2 = t*nbVarX+1:(t+1)*nbVarX;
+ id3 = (t-1)*nbVarU+1:t*nbVarU;
+ Sx(id2,:) = squeeze(A(:,:,t)) * Sx(id1,:);
+ Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
+ Su(id2,id3) = B(:,:,t);
+ end
+end
\ No newline at end of file
diff --git a/demos/demo_MPC_infHor01.m b/demos/demo_OC_LQT_infHor01.m
similarity index 97%
rename from demos/demo_MPC_infHor01.m
rename to demos/demo_OC_LQT_infHor01.m
index c0272a5e691780e68bc31ef86a84ec18466b2f48..a9d46802022fb9cfa7875df2ac43bbdcd8dd785c 100644
--- a/demos/demo_MPC_infHor01.m
+++ b/demos/demo_OC_LQT_infHor01.m
@@ -1,4 +1,4 @@
-function demo_MPC_infHor01
+function demo_OC_LQT_infHor01
% Discrete infinite horizon linear quadratic regulation (with precision matrix only on position).
%
% If this code is useful for your research, please cite the related publication:
@@ -39,7 +39,7 @@ nbData = 100; %Number of datapoints
nbRepros = 10; %Number of reproductions
nbVarPos = 2; %Dimension of position data (here: x1,x2)
-nbDeriv = 1; %Number of static & dynamic features (D=2 for [x,dx])
+nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
nbVar = nbVarPos * nbDeriv; %Dimension of state vector in the tangent space
dt = 1E-2; %Time step duration
rfactor = 4E-2; %Control cost in LQR
@@ -80,7 +80,7 @@ L = (B' * P * B + R) \ B' * P * A; %Feedback gain (discrete version)
if nbDeriv>1
kp = eigs(L(:,1:nbVarPos));
kv = eigs(L(:,nbVarPos+1:end));
- ratio = kv ./ (2.*kp).^.5
+ ratio = kv ./ (2 * kp.^.5)
end
% figure; hold on;
diff --git a/demos/demo_MPC_infHor02.m b/demos/demo_OC_LQT_infHor02.m
similarity index 91%
rename from demos/demo_MPC_infHor02.m
rename to demos/demo_OC_LQT_infHor02.m
index d2f2c4fcca7992bcccbd663a82691894d08425a2..5b6a0ef644bc98bb339112cb16b8f93326e45a03 100644
--- a/demos/demo_MPC_infHor02.m
+++ b/demos/demo_OC_LQT_infHor02.m
@@ -1,4 +1,4 @@
-function demo_MPC_infHor02
+function demo_OC_LQT_infHor02
% Discrete infinite horizon linear quadratic regulation (with precision matrix on position and velocity).
%
% If this code is useful for your research, please cite the related publication:
@@ -50,11 +50,11 @@ R = eye(nbVarPos) * rfactor;
% R = Ar*diag([1,.1])*Ar' * rfactor;
%Target and desired covariance
-Utar = [-1; .2; zeros(nbVarPos*(nbDeriv-1),1)];
+xTar = [-1; .2; zeros(nbVarPos*(nbDeriv-1),1)];
% [Ar,~] = qr(randn(nbVarPos));
% uCov = blkdiag(Ar*diag([.1,.1])*Ar' * 1E-1, eye(nbVarPos));
-uCov = diag([.1, .1, .1/dt, .1/dt]) * 1E-1;
+xCov = diag([.1, .1, .1/dt, .1/dt]) * 1E-1;
%% Discrete dynamical System settings (in tangent space)
@@ -73,16 +73,16 @@ B = kron(B1d, eye(nbVarPos)); %Discrete nD
%% Iterative discrete LQR with infinite horizon
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-Q = inv(uCov); %Precision matrix
+Q = inv(xCov); %Precision matrix
P = solveAlgebraicRiccati_eig_discrete(A, B*(R\B'), (Q+Q')/2);
L = (B'*P*B + R) \ B'*P*A; %Feedback gain (discrete version)
for n=1:nbRepros
- U = [[-1; -1]+randn(nbVarPos,1)*5E-1; zeros(nbVarPos*(nbDeriv-1),1)];
+ x = [[-1; -1]+randn(nbVarPos,1)*5E-1; zeros(nbVarPos*(nbDeriv-1),1)];
for t=1:nbData
- r(n).Data(:,t) = U;
- ddu = L * (Utar-U); %Compute acceleration (with only feedback terms)
- U = A*U + B*ddu;
+ r(n).Data(:,t) = x;
+ u = L * (xTar - x); %Compute acceleration (with only feedback terms)
+ x = A*x + B*u;
end
end
@@ -91,7 +91,7 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plots
figure('position',[10,10,650,650]); hold on; axis off; grid off;
-plotGMM(Utar(1:nbVarPos), uCov(1:2,1:2), [.8 0 0],.3);
+plotGMM(xTar(1:nbVarPos), xCov(1:2,1:2), [.8 0 0],.3);
for n=1:nbRepros
% for t=1:nbData
% coltmp = [.3 1 .3] * (nbData-t)/nbData;
diff --git a/demos/demo_MPC_infHor03.m b/demos/demo_OC_LQT_infHor03.m
similarity index 98%
rename from demos/demo_MPC_infHor03.m
rename to demos/demo_OC_LQT_infHor03.m
index 5d6bdfbfdb8735b566313bad49bef7b83a4c2a04..4c122776a692a95d7b3aa13d3072a29b812586b5 100644
--- a/demos/demo_MPC_infHor03.m
+++ b/demos/demo_OC_LQT_infHor03.m
@@ -1,4 +1,4 @@
-function demo_MPC_infHor03
+function demo_OC_LQT_infHor03
% Continuous infinite horizon linear quadratic tracking, by relying on a GMM encoding of position and velocity data.
% (see also demo_MPC_infHor02.m for the corresponding discrete version, recommended for stability)
%
@@ -61,6 +61,7 @@ R = eye(model.nbVarPos) * model.rfactor;
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
load('data/2Dletters/G.mat');
Data=[];
for n=1:nbSamples
@@ -167,7 +168,7 @@ end
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot position
-figure('position',[10 10 1000 1000],'color',[1 1 1]); hold on;
+figure('position',[10 10 800 800],'color',[1 1 1]); hold on;
plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [0.5 0.5 0.5], .3);
for n=1:nbSamples
plot(Data(1,(n-1)*nbData+1:n*nbData), Data(2,(n-1)*nbData+1:n*nbData), '-','color',[.7 .7 .7]);
diff --git a/demos/demo_MPC_infHor04.m b/demos/demo_OC_LQT_infHor04.m
similarity index 97%
rename from demos/demo_MPC_infHor04.m
rename to demos/demo_OC_LQT_infHor04.m
index 0d2b9a939aaa76c1413e9b6dfddd03c491a4f09e..f2744dab1f947a139a17b599210003cb278725a4 100644
--- a/demos/demo_MPC_infHor04.m
+++ b/demos/demo_OC_LQT_infHor04.m
@@ -1,4 +1,4 @@
-function demo_MPC_infHor04
+function demo_OC_LQT_infHor04
% Discrete infinite horizon linear quadratic tracking, by relying on a GMM encoding of position and velocity data.
% Compared to the continuous version, the discrete version is less sensitive to numerical instability when defining the R cost.
%
@@ -83,6 +83,7 @@ B = kron(B1d, eye(model.nbVarPos)); %Discrete nD
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
load('data/2Dletters/G.mat')
Data=[];
for n=1:nbSamples
@@ -170,7 +171,7 @@ end
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot position
-figure('position',[10 10 1000 1000],'color',[1 1 1]); hold on;
+figure('position',[10 10 800 800],'color',[1 1 1]); hold on;
plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [0.5 0.5 0.5], .3);
for n=1:nbSamples
plot(Data(1,(n-1)*nbData+1:n*nbData), Data(2,(n-1)*nbData+1:n*nbData), '-','color',[.7 .7 .7]);
@@ -200,7 +201,7 @@ xlabel('x_1'); ylabel('x_2');
%% Timeline plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
labList = {'$x_1$','$x_2$','$\dot{x}_1$','$\dot{x}_2$','$\ddot{x}_1$','$\ddot{x}_2$'};
-figure('position',[1020 10 1000 1000],'color',[1 1 1]); hold on;
+figure('position',[820 10 800 800],'color',[1 1 1]); hold on;
for j=1:model.nbVar
subplot(model.nbVar+1,1,j); hold on;
for n=1:nbSamples
diff --git a/demos/demo_MPC_noInitialState01.m b/demos/demo_OC_LQT_noInitialState01.m
similarity index 94%
rename from demos/demo_MPC_noInitialState01.m
rename to demos/demo_OC_LQT_noInitialState01.m
index 194f4f3d9307cc1baa932048e945d0d148196021..64da55a059a122e23d46ca0df31702b037085035 100644
--- a/demos/demo_MPC_noInitialState01.m
+++ b/demos/demo_OC_LQT_noInitialState01.m
@@ -1,5 +1,6 @@
-function demo_MPC_noInitialState01
+function demo_OC_LQT_noInitialState01
% Batch LQT solution determining the optimal initial state together with the optimal control commands.
+% (see also demo_MPC_Lagrangian01.m)
%
% If this code is useful for your research, please cite the related publication:
% @incollection{Calinon19chapter,
@@ -41,7 +42,7 @@ nbVarPos = 2; %Dimension of position data (here: x1,x2)
nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
nbVar = nbVarPos * nbDeriv; %Dimension of state vector
dt = 1E-2; %Time step duration
-rfactor = 1E-8; %dt^nbDeriv; %Control cost in LQR
+rfactor = 1E-8; %1E10; dt^nbDeriv; %Control cost in LQR
R = speye((nbData-1)*nbVarPos) * rfactor; %Control cost matrix
@@ -92,7 +93,7 @@ Ra = blkdiag(zeros(nbVar), R);
ua = (S' * Q * S + Ra) \ S' * Q * MuQ;
rx = reshape(S*ua, nbVar, nbData); %Reshape data for plotting
-% x0 = ua(1:nbVar,1); %Optimal starting point
+x0 = ua(1:nbVar,1); %Optimal starting point
% u = ua(nbVar+1:end,1); %Optimal control commands
% %Standard LQT solution
@@ -106,6 +107,7 @@ rx = reshape(S*ua, nbVar, nbData); %Reshape data for plotting
figure('position',[10 10 1000 1000],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
plot(rx(1,1), rx(2,1), '.','markersize',40,'color',[0 0 0]);
+plot(x0(1,1), x0(2,1), 'o','markersize',20,'color',[0 0 0]);
plot(MuQ(id(1,:)), MuQ(id(2,:)), '.','markersize',40,'color',[.8 0 0]);
axis equal;
% print('-dpng','graphs/demo_MPC_noInitialState01.png');
diff --git a/demos/demo_MPC_nullspace01.m b/demos/demo_OC_LQT_nullspace01.m
similarity index 96%
rename from demos/demo_MPC_nullspace01.m
rename to demos/demo_OC_LQT_nullspace01.m
index 0b18b8f795140da5142b4698ef00c19b238ee35e..95d8ff5b3497df62e0c9f2da7d19c242ffee872d 100644
--- a/demos/demo_MPC_nullspace01.m
+++ b/demos/demo_OC_LQT_nullspace01.m
@@ -1,5 +1,6 @@
-function demo_MPC_nullspace01
+function demo_OC_LQT_nullspace01
% Batch LQT with nullspace formulation.
+% (see also demo_MPC_legibility01.m)
%
% If this code is useful for your research, please cite the related publication:
% @article{Girgin19,
@@ -130,7 +131,7 @@ end
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1000 1000],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
+figure('position',[10 10 800 800],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
plotGMM(Mu, Sigma, [.8 0 0], .2);
for n=1:nbRepros
plot(r(n).x(1,:), r(n).x(2,:), '-','linewidth',1,'color',.9-[.7 .7 .7].*rand(1));
diff --git a/demos/demo_OC_LQT_nullspace02.m b/demos/demo_OC_LQT_nullspace02.m
new file mode 100644
index 0000000000000000000000000000000000000000..6ea330143d7abdf105779fb9166c4f9f7cdabde2
--- /dev/null
+++ b/demos/demo_OC_LQT_nullspace02.m
@@ -0,0 +1,177 @@
+function demo_OC_LQT_nullspace02
+% Batch LQT with nullspace formulation: ballistic task with an augmented state space
+%
+% If this code is useful for your research, please cite the related publication:
+% @article{Girgin19,
+% author="Girgin, H. and Calinon, S.",
+% title="Nullspace Structure in Model Predictive Control",
+% journal="arXiv:1905.09679",
+% year="2019",
+% pages="1--16"
+% }
+%
+% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
+% Written by Sylvain Calinon and Hakan Girgin
+%
+% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
+%
+% PbDlib is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License version 3 as
+% published by the Free Software Foundation.
+%
+% PbDlib is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU General Public License for more details.
+%
+% You should have received a copy of the GNU General Public License
+% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+nbData = 200; %Number of datapoints
+t_release = 50; %Time step when object is released
+nbVarPos = 2; %Dimension of position data (here: x1,x2)
+nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
+nbVar = nbVarPos * (nbDeriv+1); %Dimension of augmented state vector
+dt = 1E-2; %Time step duration
+rfactor = 1E-8; %dt^nbDeriv; %Control cost in LQR
+R = speye((nbData-1)*nbVarPos) * rfactor;
+Rx = speye(nbData*nbVar) * rfactor;
+
+
+%% Dynamical System settings (augmented state space, discrete version)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+Ac0 = kron([0, 1, 0; 0, 0, 0; 0, 0, 0], speye(nbVarPos));
+Ad0 = speye(nbVar) + Ac0 * dt;
+
+% Ac = kron([0, 1, 0; 0, 0, 0; 0, 0, 0], speye(nbVarPos));
+% Ac(4,end) = -9.81 * 1E0; %gravity
+% Ad = speye(nbVar) + Ac * dt;
+
+Ac = kron([0, 1, 0; 0, 0, 1; 0, 0, 0], speye(nbVarPos));
+Ad = speye(nbVar) + Ac * dt;
+
+% Bd = kron([dt^2/2; dt; 0], speye(nbVarPos));
+Bc = kron([0; 1; 0], speye(nbVarPos));
+Bd = Bc * dt;
+
+% %Build Sx and Su transfer matrices(for homogeneous A and B)
+% Su = sparse(nbVar*nbData, nbVarPos*(nbData-1));
+% Sx = kron(ones(nbData,1), speye(nbVar));
+% M = Bd;
+% for n=2:nbData
+% id1 = (n-1)*nbVar+1:nbData*nbVar;
+% Sx(id1,:) = Sx(id1,:) * Ad;
+% id1 = (n-1)*nbVar+1:n*nbVar;
+% id2 = 1:(n-1)*nbVarPos;
+% Su(id1,id2) = M;
+% M = [Ad*M(:,1:nbVarPos), M];
+% end
+
+%Build Sx and Su transfer matrices(for heterogeneous A and B)
+A = zeros(nbVar,nbVar,nbData-1);
+B = zeros(nbVar,nbVarPos,nbData-1);
+for t=1:nbData-1
+ A(:,:,t) = Ad;
+ if t<t_release
+% A(:,:,t) = Ad0;
+ B(:,:,t) = Bd;
+ end
+end
+[Su, Sx] = transferMatrices(A, B);
+
+
+%% Task setting
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+MuQ = zeros(nbVar*nbData,1);
+Q = zeros(nbVar*nbData);
+id = [1:nbVarPos] + (nbData-1) * nbVar;
+Q(id,id) = eye(nbVarPos);
+MuQ(id) = rand(nbVarPos,1) - 0.5;
+
+
+%% Batch LQR reproduction
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%Generate structured stochastic u through Bezier curves
+nbRBF = 18;
+H = zeros(nbRBF,nbData-1);
+tl = linspace(0,1,nbData-1);
+nbDeg = nbRBF - 1;
+for i=0:nbDeg
+ H(i+1,:) = factorial(nbDeg) ./ (factorial(i) .* factorial(nbDeg-i)) .* (1-tl).^(nbDeg-i) .* tl.^i; %Bernstein basis functions
+end
+w = randn(nbVarPos,nbRBF) .* 1E1; %Random weights
+un = w * H; %Reconstruction of signals by weighted superposition of basis functions
+un(:,t_release+1:end) = 0;
+
+%Nullspace planning
+% x0 = [zeros(nbVarPos*nbDeriv,1); ones(nbVarPos,1)];
+x0 = [zeros(nbVarPos*nbDeriv,1); 0; -9.81*1E-1];
+u0 = (Su' * Q * Su + R) \ Su' * Q * (MuQ - Sx * x0);
+rx0 = reshape(Sx*x0+Su*u0, nbVar, nbData); %Reshape data for plotting
+
+[V,D] = eig(Q);
+U = V * D.^.5;
+N = eye((nbData-1)*nbVarPos) - Su' * U / (U' * (Su * Su') * U + Rx) * U' * Su; %Nullspace projection matrix
+u = Su' * U / (U' * (Su * Su') * U + Rx) * U' * (MuQ - Sx * x0) + N * un(:);
+rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
+
+
+%% Plot
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10 10 800 800],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
+plot(rx0(1,:), rx0(2,:), '-','linewidth',2,'color',[.7 .7 .7]);
+plot(rx0(1,1), rx0(2,1), '.','markersize',30,'color',[.7 .7 .7]);
+plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
+h(1) = plot(rx(1,1), rx(2,1), '.','markersize',30,'color',[0 0 0]);
+h(2) = plot(rx(1,t_release), rx(2,t_release), '.','markersize',30,'color',[0 .6 0]);
+plot(rx0(1,t_release), rx0(2,t_release), '.','markersize',30,'color',[.7 1 .7]);
+h(3) = plot(MuQ(id(1)), MuQ(id(2)), '.','markersize',30,'color',[.8 0 0]);
+legend(h, {'Initial point','Release of object','Target to reach'});
+axis equal;
+% print('-dpng','graphs/demo_MPC_nullspace02.png');
+
+%Timeline Plots
+figure('position',[830,10,800,800]);
+for j=1:nbVarPos
+ subplot(nbVarPos,1,j); hold on; %axis off;
+ plot(1:nbData-1, u0(j:nbVarPos:end), '-','linewidth',1,'color',[.7 .7 .7]);
+ plot(1:nbData-1, u(j:nbVarPos:end), '-','linewidth',1,'color',[0 0 0]);
+% set(gca,'xtick',[],'ytick',[]);
+ plot([t_release,t_release], [min(u(j:nbVarPos:end)), max(u(j:nbVarPos:end))], '-','linewidth',2,'color',[0 .6 0]);
+ xlabel('$t$','interpreter','latex','fontsize',28);
+ ylabel(['$u_' num2str(j) '$'],'interpreter','latex','fontsize',28);
+end
+axis tight;
+
+% %Visualize Su matrix
+% figure('position',[10 1050 400 650],'name','Su');
+% axes('Position',[0.01 0.01 .98 .98]); hold on; set(gca,'linewidth',2);
+% colormap(flipud(gray));
+% pcolor([abs(Su) zeros(size(Su,1),1); zeros(1,size(Su,2)+1)]); %dummy values for correct display
+% shading flat; axis ij; axis equal tight;
+% set(gca,'xtick',[],'ytick',[]);
+
+pause;
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%
+function [Su, Sx] = transferMatrices(A, B)
+ [nbVarX, nbVarU, nbData] = size(B);
+ nbData = nbData+1;
+ Sx = kron(ones(nbData,1), speye(nbVarX));
+ Su = sparse(zeros(nbVarX*(nbData-1), nbVarU*(nbData-1)));
+ for t=1:nbData-1
+ id1 = (t-1)*nbVarX+1:t*nbVarX;
+ id2 = t*nbVarX+1:(t+1)*nbVarX;
+ id3 = (t-1)*nbVarU+1:t*nbVarU;
+ Sx(id2,:) = squeeze(A(:,:,t)) * Sx(id1,:);
+ Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
+ Su(id2,id3) = B(:,:,t);
+ end
+end
\ No newline at end of file
diff --git a/demos/demo_MPC_online01.m b/demos/demo_OC_LQT_online01.m
similarity index 97%
rename from demos/demo_MPC_online01.m
rename to demos/demo_OC_LQT_online01.m
index e8fc21d4aeb491e20499ec6b47da8b0f6804a1d4..5d36cd1340763f0b5c53acf2da3246e51fcf788c 100644
--- a/demos/demo_MPC_online01.m
+++ b/demos/demo_OC_LQT_online01.m
@@ -1,4 +1,4 @@
-function demo_MPC_online01
+function demo_OC_LQT_online01
% MPC recomputed in an online manner with a time horizon.
%
% If this code is useful for your research, please cite the related publication:
@@ -115,7 +115,7 @@ end
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1000 1000]); hold on; axis off;
+figure('position',[10 10 800 800]); hold on; axis off;
plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
plot(rx(1,1), rx(2,1), '.','markersize',35,'color',[0 0 0]);
plot(MuQ0(id0(1,:)), MuQ0(id0(2,:)), '.','markersize',35,'color',[.8 0 0]);
diff --git a/demos/demo_MPC_online02.m b/demos/demo_OC_LQT_online02.m
similarity index 94%
rename from demos/demo_MPC_online02.m
rename to demos/demo_OC_LQT_online02.m
index 1a79a4068f10ddc776552d5ca01b9a784dfd30cf..c3b713febb752b002144e3ade5f12c86191c6ce4 100644
--- a/demos/demo_MPC_online02.m
+++ b/demos/demo_OC_LQT_online02.m
@@ -1,4 +1,4 @@
-function demo_MPC_online02
+function demo_OC_LQT_online02
% MPC recomputed in an online manner with a time horizon, by relying on a GMM encoding of position and velocity data (with animation).
%
% If this code is useful for your research, please cite the related publication:
@@ -35,12 +35,12 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbSamples = 5; %Number of demonstrations
+nbSamples = 8; %Number of demonstrations
nbRepros = 1; %Number of reproductions in new situations
nbData = 100; %Number of datapoints
-nbD = 20; %Time window for LQR computation
+nbD = 30; %Time window for LQR computation
-model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbStates = 5; %Number of Gaussians in the GMM
model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
model.nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
@@ -82,7 +82,8 @@ end
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-load('data/2Dletters/C.mat');
+demos=[];
+load('data/2Dletters/M.mat');
Data=[];
for n=1:nbSamples
s(n).Data=[];
@@ -100,7 +101,7 @@ end
%% Learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%model = init_GMM_kmeans(Data,model);
+% model = init_GMM_kmeans(Data,model);
model = init_GMM_kbins(Data,model,nbSamples);
[model, H] = EM_GMM(Data, model);
@@ -180,7 +181,7 @@ end
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 2500 1300],'color',[1 1 1]);
+figure('position',[10 10 1600 800],'color',[1 1 1]);
%Plot 2D
subplot(model.nbVar+model.nbVarPos, 2, [1:2:(model.nbVar+model.nbVarPos)*2]); hold on; axis off;
limAxes = [min(r(1).x(1,:))-8E0, max(r(1).x(1,:))+8E0, min(r(1).x(2,:))-8E0, max(r(1).x(2,:))+8E0];
@@ -233,9 +234,10 @@ for j=1:model.nbVar
plot(r(n).x(j,:), '-','linewidth',2,'color',[.8 0 0]);
end
if j<7
- ylabel(labList{j},'fontsize',14,'interpreter','latex');
+ ylabel(labList{j},'fontsize',22,'interpreter','latex');
end
axis(v(j).limAxes);
+ set(gca,'xtick',[],'ytick',[]);
end
%Control profile
@@ -247,10 +249,11 @@ for j=1:model.nbVarPos
for n=1:nbRepros
plot(r(n).u(j,:), '.','markersize',10,'color',[.8 0 0]);
end
- ylabel(['$u_' num2str(j) '$'],'fontsize',14,'interpreter','latex');
+ ylabel(['$u_' num2str(j) '$'],'fontsize',22,'interpreter','latex');
axis(v(model.nbVar+j).limAxes);
+ set(gca,'xtick',[],'ytick',[]);
end
-xlabel('$t$','fontsize',14,'interpreter','latex');
+xlabel('$t$','fontsize',22,'interpreter','latex');
%Time window anim
for tt=1:nbData
diff --git a/demos/demo_MPC_online03.m b/demos/demo_OC_LQT_online03.m
similarity index 98%
rename from demos/demo_MPC_online03.m
rename to demos/demo_OC_LQT_online03.m
index 388b462ef03b164b27c9e85b86aef8227e0c3e3e..86289a5586ac864453f95bc6eade2534496ad34b 100644
--- a/demos/demo_MPC_online03.m
+++ b/demos/demo_OC_LQT_online03.m
@@ -1,4 +1,4 @@
-function demo_MPC_online03
+function demo_OC_LQT_online03
% Obstacle avoidance with MPC recomputed in an online manner.
%
% If this code is useful for your research, please cite the related publication:
@@ -82,6 +82,7 @@ end
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
load('data/2Dletters/S.mat');
Data=[];
for n=1:nbSamples
@@ -162,7 +163,7 @@ end
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1200 1250],'color',[1 1 1]); hold on; axis off;
+figure('position',[10 10 800 800],'color',[1 1 1]); hold on; axis off;
plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [.8 .8 .8]);
% for n=1:nbSamples
% plot(Data(1,(n-1)*nbData+1:n*nbData), Data(2,(n-1)*nbData+1:n*nbData), '-','lineWidth',1,'color',[.7 .7 .7]);
@@ -185,7 +186,7 @@ axis equal;
%% Timeline plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
labList = {'$x_1$','$x_2$','$\dot{x}_1$','$\dot{x}_2$','$\ddot{x}_1$','$\ddot{x}_2$'};
-figure('position',[1220 10 1000 1250],'color',[1 1 1]);
+figure('position',[820 10 800 800],'color',[1 1 1]);
for j=1:model.nbVar
subplot(model.nbVar+1,1,j); hold on;
limAxes = [1, nbData, min(Data(j,:))-4E0, max(Data(j,:))+4E0];
diff --git a/demos/demo_MPC_nullspace_online01.m b/demos/demo_OC_LQT_online_minimal01.m
similarity index 63%
rename from demos/demo_MPC_nullspace_online01.m
rename to demos/demo_OC_LQT_online_minimal01.m
index f042c2d7c94d9efc3b664dafa1c7a3fb9cfba89e..37cef020f5d76407c4b3bb5899c7ec31a7a805db 100644
--- a/demos/demo_MPC_nullspace_online01.m
+++ b/demos/demo_OC_LQT_online_minimal01.m
@@ -1,17 +1,20 @@
-function demo_MPC_nullspace_online01
-% Batch LQR recomputed in an online manner with nullspace formulation.
+function demo_OC_LQT_online_minimal01
+% MPC recomputed in an online manner with a time horizon.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Girgin19,
-% author="Girgin, H. and Calinon, S.",
-% title="Nullspace Structure in Model Predictive Control",
-% journal="arXiv:1905.09679",
+% @incollection{Calinon19chapter,
+% author="Calinon, S. and Lee, D.",
+% title="Learning Control",
+% booktitle="Humanoid Robotics: a Reference",
+% publisher="Springer",
+% editor="Vadakkepat, P. and Goswami, A.",
% year="2019",
-% pages="1--16"
+% pages="1261--1312",
+% doi="10.1007/978-94-007-6046-2_68"
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
-% Written by Sylvain Calinon and Hakan Girgin
+% Written by Sylvain Calinon, http://calinon.ch/
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
%
@@ -70,7 +73,7 @@ for n=2:nbD
end
-%% Principal task setting
+%% Task setting
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tl = linspace(1,nbData,nbPoints+1);
tl = round(tl(2:end));
@@ -83,71 +86,45 @@ for t=1:length(tl)
end
-%% Secondary task setting (simulating smooth random perturbations)
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Precomputation of basis functions to generate structured stochastic u through Bezier curves
-nbRBF = 8;
-H = zeros(nbRBF,nbData-1);
-tlr = linspace(0,1,nbData-1);
-nbDeg = nbRBF - 1;
-for i=0:nbDeg
- H(i+1,:) = factorial(nbDeg) ./ (factorial(i) .* factorial(nbDeg-i)) .* (1-tlr).^(nbDeg-i) .* tlr.^i; %Bernstein basis functions
-end
-w = randn(nbVarPos,nbRBF) .* 1E1; %Random weights
-u0 = reshape(w * H, [nbVarPos*(nbData-1), 1]); %Reconstruction of signals by weighted superposition of basis functions
-
-% figure; hold on;
-% % plot(u0(1:2:end), u0(2:2:end), 'k-');
-% x = zeros(nbVar,1);
-% for t=1:nbData-1
-% x = A * x + B * u0((t-1)*nbVarPos+[1:nbVarPos]); %Update state with first control command
-% plot(x(1), x(2), 'k.');
-% end
-% pause;
-% close all;
-% return
-
-
-%% Reproduction
+%% Batch LQR reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = zeros(nbVar,1);
rx = zeros(nbVar,nbData);
+% figure; hold on;
+% h = [];
for t=1:nbData
rx(:,t) = x; %Log data
- id = [t:min(t+nbD-1,nbData), ones(1,t-nbData+nbD-1)]; %Time steps involved in the MPC computation
- id2=[];
- for s=1:nbD
- id2 = [id2, [1:nbVar] + (id(s)-1) * nbVar];
+ if t==20
+ tl = linspace(1,nbData-20,nbPoints+1);
+ tl = round(tl(2:end));
+ MuQ0 = zeros(nbVar*nbData,1);
+ Q0 = zeros(nbVar*nbData);
+ for t=1:length(tl)
+ id0(:,t) = [1:nbVarPos] + (tl(t)-1) * nbVar;
+ Q0(id0(:,t), id0(:,t)) = eye(nbVarPos);
+ MuQ0(id0(:,t)) = rand(nbVarPos,1) - 0.5;
+ end
end
- id = [t:min(t+nbD-1,nbData-1), ones(1,t-nbData+nbD-1)]; %Time steps involved in the MPC computation
- id3=[];
- for s=1:nbD-1
- id3 = [id3, [1:nbVarPos] + (id(s)-1) * nbVarPos];
+ id = [t:min(t+nbD-1,nbData), ones(1,t-nbData+nbD-1)]; %Time steps involved in the MPC computation
+ id2 = [];
+ for s=1:nbD
+ id2 = [id2; [1:nbVar]' + (id(s)-1) * nbVar];
end
-
MuQ = MuQ0(id2,1);
Q = Q0(id2,id2);
-
- %Reproduction with nullspace planning
- [V,D] = eig(Q);
- U = V * D.^.5;
- J = U' * Su; %Jacobian
- %Left pseudoinverse solution
- % pinvJ = pinv(J);
- pinvJ = (J' * J + R) \ J'; %Left pseudoinverse
- N = speye((nbD-1)*nbVarPos) - pinvJ * J; %Nullspace projection matrix
- u1 = pinvJ * U' * (MuQ - Sx * x); %Principal task
-
- u = u1 + N * u0(id3);
-% u = u0(id3);
-
+ u = (Su' * Q * Su + R) \ Su' * Q * (MuQ - Sx * x); %Compute control commands
x = A * x + B * u(1:nbVarPos); %Update state with first control command
+
+% delete(h);
+% h(1) = plot(MuQ(1:nbVar:end),'k.');
+% h(2) = plot(x(1),'r.');
+% axis([1,nbD,min(MuQ0),max(MuQ0)]);
+% pause(.001);
end
-
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10 10 1000 1000]); hold on; axis off;
@@ -158,7 +135,7 @@ for t=1:length(tl)
text(MuQ0(id0(1,t)), MuQ0(id0(2,t))+2E-2, num2str(t));
end
axis equal;
-% print('-dpng','graphs/MPC_nullspace_online01.png');
+% print('-dpng','graphs/MPC_minimal01.png');
pause;
close all;
\ No newline at end of file
diff --git a/demos/demo_MPC_iterativeLQR01.m b/demos/demo_OC_LQT_recursive01.m
similarity index 87%
rename from demos/demo_MPC_iterativeLQR01.m
rename to demos/demo_OC_LQT_recursive01.m
index 1bdddf87b42f44b72cc11b672ad5e82e0916d26e..2189ed5b153e3c03493bda06e9a657b5e2ace911 100644
--- a/demos/demo_MPC_iterativeLQR01.m
+++ b/demos/demo_OC_LQT_recursive01.m
@@ -1,4 +1,4 @@
-function demo_MPC_iterativeLQR01
+function demo_OC_LQT_recursive01
% Recursive computation of linear quadratic tracking (with feedback and feedforward terms).
%
% If this code is useful for your research, please cite the related publication:
@@ -61,10 +61,10 @@ B = kron(B1d, speye(nbVarPos)); %Discrete nD
%% Task setting
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-tl = linspace(1,nbData/2,nbPoints+1);
+tl = linspace(1, nbData/2, nbPoints+1);
tl = round(tl(2:end));
-MuQ = zeros(nbVar*nbData,1);
-Q = zeros(nbVar*nbData);
+% MuQ = zeros(nbVar*nbData,1);
+% Q = zeros(nbVar*nbData);
for t=1:length(tl)
id(:,t) = [1:nbVarPos] + (tl(t)-1) * nbVar;
% MuQ(id(:,t)) = rand(nbVarPos,1) - 0.5;
@@ -75,8 +75,8 @@ for t=1:length(tl)
end
% Mu = reshape(MuQ, nbVar, nbData);
-MuQ = kron(ones(nbData,1), [1;1;0;0]);
-Q = blkdiag(zeros((nbData/2)*nbVar), kron(eye(nbData/2), diag([1,1,0,0])) .* 1E1);
+MuQ = kron(ones(nbData,1), [1; 1; zeros(nbVar-nbVarPos,1)]);
+Q = blkdiag(zeros((nbData/2)*nbVar), kron(eye(nbData/2), diag([1,1,zeros(1,nbVar-nbVarPos)])) * 1E1);
Mu = reshape(MuQ, nbVar, nbData);
% Q(end-1:end,end-1:end) = eye(2) .* 1E3;
@@ -120,26 +120,26 @@ Mu = reshape(MuQ, nbVar, nbData);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K = zeros(nbVarPos, nbVar, nbData-1);
Kff = zeros(nbVarPos, nbVar, nbData-1);
-S = zeros(nbVar, nbVar, nbData);
-S(:,:,end) = Q(end-nbVar+1:end,end-nbVar+1:end);
+P = zeros(nbVar, nbVar, nbData);
+P(:,:,end) = Q(end-nbVar+1:end,end-nbVar+1:end);
v = zeros(nbVar, nbData);
v(:,end) = Q(end-nbVar+1:end,end-nbVar+1:end) * Mu(:,end);
%Backward computation
for t=nbData-1:-1:1
- Kff(:,:,t) = (B' * S(:,:,t+1) * B + R) \ B'; %FF
- K(:,:,t) = Kff(:,:,t) * S(:,:,t+1) * A; %FB
- S(:,:,t) = A' * S(:,:,t+1) * (A - B * K(:,:,t)) + Q((t-1)*nbVar+1:t*nbVar,(t-1)*nbVar+1:t*nbVar);
+ Kff(:,:,t) = (B' * P(:,:,t+1) * B + R) \ B'; %FF
+ K(:,:,t) = Kff(:,:,t) * P(:,:,t+1) * A; %FB
+ P(:,:,t) = A' * P(:,:,t+1) * (A - B * K(:,:,t)) + Q((t-1)*nbVar+1:t*nbVar,(t-1)*nbVar+1:t*nbVar);
v(:,t) = (A - B * K(:,:,t))' * v(:,t+1) + Q((t-1)*nbVar+1:t*nbVar,(t-1)*nbVar+1:t*nbVar) * Mu(:,t);
kp(:,t) = eigs(K(:,1:2,t));
kv(:,t) = eigs(K(:,3:4,t));
-% %Test ratio between kp and kv
-% ratio(:,t) = kv(:,t) ./ (2.*kp(:,t)).^.5;
+ %Test ratio between kp and kv
+ ratio(:,t) = kv(:,t) ./ (2*kp(:,t)).^.5;
end
%Reproduction with feedback (FB) and feedforward (FF) terms
x = zeros(nbVar,1);
-rx(:,1) = X; %Log data
+rx(:,1) = x; %Log data
for t=2:nbData
u = -K(:,:,t-1) * x + Kff(:,:,t-1) * v(:,t); %Acceleration command with FB and FF terms
x = A * x + B * u; %Update of state vector
@@ -149,14 +149,17 @@ end
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1000 1000]); hold on; axis off;
+figure('position',[10 10 800 800]); hold on; axis off;
plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
plot(rx(1,1), rx(2,1), '.','markersize',50,'color',[0 0 0]);
plot(MuQ(id(1,:)), MuQ(id(2,:)), '.','markersize',50,'color',[.8 0 0]);
axis equal;
%print('-dpng','graphs/LQT_x01.png');
-figure('position',[1020 10 800 1000]);
+
+%% Plot timeline
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[820 10 800 800]);
% %phase
% alpha = 2;
% tIn = [1:nbData]*dt;
@@ -175,6 +178,7 @@ figure('position',[1020 10 800 1000]);
% subplot(2,1,1); hold on; grid on;
% plot(ratio(1,:), '-','linewidth',2,'color',[0 0 0]);
% xlabel('t'); ylabel('kp-kv ratio');
+% axis([1, nbData/2+20, 0, max(ratio(1,:))+.1]);
%x
subplot(2,1,1); hold on; grid on;
@@ -194,6 +198,12 @@ set(gca,'linewidth',2,'xtick',100,'ytick',[kv(1,120),kp(1,120)],'xticklabel',{},
xlabel('$t$','interpreter','latex','fontsize',34);
ylabel('$\kappa^{\scriptscriptstyle{P}},\kappa^{\scriptscriptstyle{V}}$','interpreter','latex','fontsize',34);
%print('-dpng','graphs/demo_MPC_iterativeLQT01.png');
-
+
+% %Plot P
+% figure('position',[820 10 800 800]); hold on; axis off;
+% for t=round(linspace(nbData-1, 1, 20))
+% plotGMM(zeros(2,1), P(1:2,1:2,t), [.7 .7 .7], .1);
+% end
+
pause;
close all;
\ No newline at end of file
diff --git a/demos/demo_MPC_iterativeLQR02.m b/demos/demo_OC_LQT_recursive02.m
similarity index 95%
rename from demos/demo_MPC_iterativeLQR02.m
rename to demos/demo_OC_LQT_recursive02.m
index ac4505e1211da57e23d89c220a5a82b43ebbed09..50caf8fe03db1bed88399b4825a3806b760c7bbe 100644
--- a/demos/demo_MPC_iterativeLQR02.m
+++ b/demos/demo_OC_LQT_recursive02.m
@@ -1,4 +1,4 @@
-function demo_MPC_iterativeLQR02
+function demo_OC_LQT_recursive02
% Recursive computation of linear quadratic tracking (with feedback and feedforward terms),
% by relying on a GMM encoding of position and velocity data, including comparison with batch LQT.
%
@@ -81,6 +81,7 @@ B = kron(B1d, eye(model.nbVarPos)); %Discrete nD
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
load('data/2Dletters/S.mat');
Data=[];
for n=1:nbSamples
@@ -164,19 +165,19 @@ end
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% K = zeros(model.nbVarPos, model.nbVar, nbData-1);
% Kff = zeros(model.nbVarPos, model.nbVar, nbData-1);
-% S = zeros(model.nbVar, model.nbVar, nbData);
-% S(:,:,end) = inv(model.Sigma(:,:,qList(nbData)));
+% P = zeros(model.nbVar, model.nbVar, nbData);
+% P(:,:,end) = inv(model.Sigma(:,:,qList(nbData)));
% v = zeros(model.nbVar, nbData);
% v(:,end) = inv(model.Sigma(:,:,qList(nbData))) * model.Mu(:,qList(nbData));
% % r(1).xx = zeros(model.nbVar, nbData-1);
% %Backward computation
% for t=nbData-1:-1:1
% Q = inv(model.Sigma(:,:,qList(t)));
-% Kff(:,:,t) = (B' * S(:,:,t+1) * B + R) \ B'; %FF
-% K(:,:,t) = Kff(:,:,t) * S(:,:,t+1) * A; %FB
-% S(:,:,t) = A' * S(:,:,t+1) * (A - B * K(:,:,t)) + Q;
+% Kff(:,:,t) = (B' * P(:,:,t+1) * B + R) \ B'; %FF
+% K(:,:,t) = Kff(:,:,t) * P(:,:,t+1) * A; %FB
+% P(:,:,t) = A' * P(:,:,t+1) * (A - B * K(:,:,t)) + Q;
% v(:,t) = (A - B * K(:,:,t))' * v(:,t+1) + Q * model.Mu(:,qList(t));
-% % r(1).xx(:,t) = (S(:,:,t) * A) \ v(:,t);
+% % r(1).xx(:,t) = (P(:,:,t) * A) \ v(:,t);
% end
% %Reproduction with feedback (FB) and feedforward (FF) terms
% for n=1:nbRepros
@@ -223,7 +224,7 @@ end
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot position
-figure('position',[10 10 1000 1000],'color',[1 1 1]); hold on;
+figure('position',[10 10 800 800],'color',[1 1 1]); hold on;
plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [0.5 0.5 0.5], .3);
for n=1:nbSamples
plot(Data(1,(n-1)*nbData+1:n*nbData), Data(2,(n-1)*nbData+1:n*nbData), '-','color',[.7 .7 .7]);
diff --git a/demos/demo_MPC_iterativeLQR03.m b/demos/demo_OC_LQT_recursive03.m
similarity index 98%
rename from demos/demo_MPC_iterativeLQR03.m
rename to demos/demo_OC_LQT_recursive03.m
index f8fc44416de4d9723b9f8263696572f02f179f5e..7ee66e9d94b439a5bb01e6fca8b56d4541324737 100644
--- a/demos/demo_MPC_iterativeLQR03.m
+++ b/demos/demo_OC_LQT_recursive03.m
@@ -1,4 +1,4 @@
-function demo_MPC_iterativeLQR03
+function demo_OC_LQT_recursive03
% Recursive computation of linear quadratic tracking (with feedback and feedforward terms),
% by relying on a GMM encoding of only position data.
%
@@ -87,6 +87,7 @@ C = kron([1, 0], eye(model.nbVarPos));
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
load('data/2Dletters/G.mat');
Data=[];
for n=1:nbSamples
@@ -142,7 +143,7 @@ end
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot position
-figure('position',[10 10 1000 1000],'color',[1 1 1]); hold on;
+figure('position',[10 10 800 800],'color',[1 1 1]); hold on;
plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [0.5 0.5 0.5], .3);
for n=1:nbSamples
plot(Data(1,(n-1)*nbData+1:n*nbData), Data(2,(n-1)*nbData+1:n*nbData), '-','color',[.7 .7 .7]);
diff --git a/demos/demo_MPC_iterativeLQR_augmSigma01.m b/demos/demo_OC_LQT_recursive_augmSigma01.m
similarity index 99%
rename from demos/demo_MPC_iterativeLQR_augmSigma01.m
rename to demos/demo_OC_LQT_recursive_augmSigma01.m
index a17f034107cf19210f8874ad375a8e73a82996fb..93fed8540554036d1f596114c5a9a4cc0460554d 100644
--- a/demos/demo_MPC_iterativeLQR_augmSigma01.m
+++ b/demos/demo_OC_LQT_recursive_augmSigma01.m
@@ -1,4 +1,4 @@
-function demo_MPC_iterativeLQR_augmSigma01
+function demo_OC_LQT_recursive_augmSigma01
% Recursive LQR with augmented covariance to transform the tracking problem to a regulation problem.
%
% If this code is useful for your research, please cite the related publication:
diff --git a/demos/demo_MPC_skillsRepr01.m b/demos/demo_OC_LQT_skillsRepr01.m
similarity index 99%
rename from demos/demo_MPC_skillsRepr01.m
rename to demos/demo_OC_LQT_skillsRepr01.m
index 749bd97bfa72075671b0b356cf62e8112048b6c8..d2a3c5ee10f4d486e395977f0259c4c2536b5016 100644
--- a/demos/demo_MPC_skillsRepr01.m
+++ b/demos/demo_OC_LQT_skillsRepr01.m
@@ -1,4 +1,4 @@
-function demo_MPC_skillsRepr01
+function demo_OC_LQT_skillsRepr01
% Representation of skills combined in parallel and in series through a batch LQT formulation.
%
% If this code is useful for your research, please cite the related publication:
diff --git a/demos/demo_MPC_viapoints01.m b/demos/demo_OC_LQT_viapoints01.m
similarity index 98%
rename from demos/demo_MPC_viapoints01.m
rename to demos/demo_OC_LQT_viapoints01.m
index 3041a97a9a788ac7eb33fd548dfcfe650e10ccfc..a4ec38bfce7e7bb75265bbb710941b6db1c15d0e 100644
--- a/demos/demo_MPC_viapoints01.m
+++ b/demos/demo_OC_LQT_viapoints01.m
@@ -1,5 +1,5 @@
-function demo_MPC_viapoints01
-% Batch LQR with viapoints and a double integrator system, and an encoding of position and velocity.
+function demo_OC_LQT_viapoints01
+% Batch LQT with viapoints and a double integrator system, and an encoding of position and velocity.
%
% If this code is useful for your research, please cite the related publication:
% @incollection{Calinon19chapter,
@@ -242,7 +242,7 @@ end
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%x1-x2
-figure('position',[10 10 600 600],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
+figure('position',[10 10 500 500],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
%Plot uncertainty
for t=1:nbData
plotGMM(r(1).x(1:2,t), xSigma(model.nbVar*(t-1)+[1,2],model.nbVar*(t-1)+[1,2]).*1E1, [.2 .2 .2], .1);
@@ -284,7 +284,7 @@ axis equal;
% end %model.nbDeriv
%u1-u2
-figure('position',[10 700 600 600],'color',[1 1 1],'name','u1-u2 plot'); hold on; axis off;
+figure('position',[260 560 250 250],'color',[1 1 1],'name','u1-u2 plot'); hold on; axis off;
%Plot uncertainty
for t=1:nbData-1
plotGMM(r(1).u(1:2,t), uSigma(model.nbVarPos*(t-1)+[1,2],model.nbVarPos*(t-1)+[1,2]).*1E0, [.2 .2 .2], .02);
@@ -300,7 +300,7 @@ end
%% Timeline plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
labList = {'$x_1$','$x_2$','$\dot{x}_1$','$\dot{x}_2$','$\ddot{x}_1$','$\ddot{x}_2$'};
-figure('position',[620 10 1000 1290],'color',[1 1 1]);
+figure('position',[520 10 600 800],'color',[1 1 1]);
for j=1:model.nbVar
subplot(model.nbVar+model.nbVarPos+1,1,j); hold on;
if j>model.nbVarPos
@@ -497,4 +497,4 @@ xlabel('$t$','fontsize',14,'interpreter','latex');
% print('-dpng','graphs/batchLQR_Su01.png');
pause;
-close all;
\ No newline at end of file
+close all;
diff --git a/demos/demo_MPC_viapoints02.m b/demos/demo_OC_LQT_viapoints02.m
similarity index 99%
rename from demos/demo_MPC_viapoints02.m
rename to demos/demo_OC_LQT_viapoints02.m
index 96d1f1de036611d0531c0f127ed2ced1501382c6..9501ca0f90ed160945fcf665f025826f77ddfb8f 100644
--- a/demos/demo_MPC_viapoints02.m
+++ b/demos/demo_OC_LQT_viapoints02.m
@@ -1,4 +1,4 @@
-function demo_MPC_viapoints02
+function demo_OC_LQT_viapoints02
% Batch LQR with viapoints and a double integrator system, and an encoding of only position.
%
% If this code is useful for your research, please cite the related publication:
diff --git a/demos/demo_MPC_viapoints03.m b/demos/demo_OC_LQT_viapoints03.m
similarity index 99%
rename from demos/demo_MPC_viapoints03.m
rename to demos/demo_OC_LQT_viapoints03.m
index 9af0132bd2c05c0f38cce4581a464b0ec58bac1d..d10e6c9d4d5ef805216a361ff3048a728735718b 100644
--- a/demos/demo_MPC_viapoints03.m
+++ b/demos/demo_OC_LQT_viapoints03.m
@@ -1,4 +1,4 @@
-function demo_MPC_viapoints03
+function demo_OC_LQT_viapoints03
% Equivalence between cubic Bezier curve and batch LQR with double integrator (formulation with position and velocity).
%
% If this code is useful for your research, please cite the related publication:
diff --git a/demos/demo_MPC_viapoints_withProd01.m b/demos/demo_OC_LQT_viapoints_withProd01.m
similarity index 99%
rename from demos/demo_MPC_viapoints_withProd01.m
rename to demos/demo_OC_LQT_viapoints_withProd01.m
index 1caeace13f1fea187b109565d84c247b5bc98bd5..468f473c2fb2168c027267989fabec8a7ae688d5 100644
--- a/demos/demo_MPC_viapoints_withProd01.m
+++ b/demos/demo_OC_LQT_viapoints_withProd01.m
@@ -1,4 +1,4 @@
-function demo_MPC_viapoints_withProd01
+function demo_OC_LQT_viapoints_withProd01
% Batch LQT with viapoints computed as a product of trajectory distributions in control space.
%
% If this code is useful for your research, please cite the related publication: