diff --git a/.gitignore b/.gitignore
old mode 100644
new mode 100755
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 23743ce6e8b6b80ab696d58fbf433ff8207a8313..48777f3dbc17b1f97cb52ef7fa0e7e6dda7990ca 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,13 +1,26 @@
-cmake_minimum_required(VERSION 2.8)
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
PROJECT(pbdlib)
+set(CMAKE_EXPORT_COMPILE_COMMANDS 1)
-find_package(Armadillo 4.4 REQUIRED)
-set(CMAKE_MAJOR_VERSION 1)
-set(CMAKE_MINOR_VERSION 0)
-set(CMAKE_PATCH_VERSION 0)
+INCLUDE(CheckCXXCompilerFlag)
+check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_CXX11)
+check_cxx_compiler_flag("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+IF(COMPILER_SUPPORTS_CXX11)
+ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -g")
+ELSEIF(COMPILER_SUPPORTS_CXX0X)
+ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+ELSE()
+ MESSAGE(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+ENDIF()
+
+FIND_PACKAGE(Armadillo 5.4 REQUIRED)
+SET(CMAKE_MAJOR_VERSION 1)
+SET(CMAKE_MINOR_VERSION 0)
+SET(CMAKE_PATCH_VERSION 0)
+
+INCLUDE_DIRECTORIES(include)
+INCLUDE_DIRECTORIES(${ARMADILLO_INCLUDE_DIRS})
-include_directories(include)
-include_directories(${ARMADILLO_INCLUDE_DIRS})
ADD_LIBRARY(pbd
src/datapoints.cpp
@@ -21,11 +34,39 @@ ADD_LIBRARY(pbd
src/lqr.cpp
src/hmm.cpp
src/hsmm.cpp
+ src/adhsmm.cpp
+ src/quaternion.cpp
+ src/trajgmm.cpp
+ src/mpc.cpp
+ src/trajMPC.cpp
+ src/tphsmm.cpp
+ src/tpdpgmm.cpp
+ src/trajdist.cpp
)
#Uncomment for debugging
#add_definitions(-g)
+################################################################################
+### Mac specific
+################################################################################
+
+if(APPLE)
+ set(ARMADILLO_LIBRARIES
+ ${ARMADILLO_LIBRARIES}
+ lapack
+ blas)
+
+ include_directories(
+ /usr/local/include
+ )
+
+ link_directories(
+ /usr/local/Frameworks
+ /usr/local/lib
+ )
+endif()
+
################################################################################
### samples
################################################################################
@@ -34,15 +75,18 @@ IF (PBDLIB_BUILD_TEST)
ADD_SUBDIRECTORY(examples)
ENDIF ()
-target_link_libraries(pbd ${ARMADILLO_LIBRARIES})
+TARGET_LINK_LIBRARIES(pbd ${ARMADILLO_LIBRARIES})
+
+# Copy data/ to build so that the examples can load from that location
+FILE(COPY ${PROJECT_SOURCE_DIR}/data DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/examples/)
################################################################################
### install
################################################################################
-set(ROOT_INSTALL_LIBDIR lib)
-set(ROOT_INSTALL_INCLUDEDIR include)
-file(GLOB headers ${CMAKE_CURRENT_SOURCE_DIR}/include/pbdlib/*.h)
+SET(ROOT_INSTALL_LIBDIR lib)
+SET(ROOT_INSTALL_INCLUDEDIR include)
+FILE(GLOB headers ${CMAKE_CURRENT_SOURCE_DIR}/include/pbdlib/*.h)
-install(TARGETS pbd DESTINATION ${ROOT_INSTALL_LIBDIR})
-install(FILES ${headers} DESTINATION ${ROOT_INSTALL_INCLUDEDIR}/pbdlib/)
+INSTALL(TARGETS pbd DESTINATION ${ROOT_INSTALL_LIBDIR})
+INSTALL(FILES ${headers} DESTINATION ${ROOT_INSTALL_INCLUDEDIR}/pbdlib/)
diff --git a/README.md b/README.md
index 415cc1d93a874388b5ceadd23f9e396a8e2fdd38..467171c06c619fb311e1672c5ead4b3fa638ab40 100644
--- a/README.md
+++ b/README.md
@@ -14,12 +14,20 @@ To install these dependencies, you can run:
sudo apt-get install cmake liblapack3 liblapack-dev libopenblas-dev
```
+If you don't have C++11 support, you can install it with:
+```
+sudo add-apt-repository ppa:ubuntu-toolchain-r/test
+sudo apt-get update
+sudo apt-get install g++-4.9
+```
+
The latest version of Armadillo can then be downloaded from [here](http://arma.sourceforge.net/download.html).
### Installation instructions
```
-cd pbdlib
+git clone git@gitlab.idiap.ch:rli/pbdlib-sandbox.git
+cd pbdlib-sandbox
mkdir build
cd build
cmake ..
@@ -28,7 +36,7 @@ sudo make install
```
A GUI can be used after installation of the library:
-https://gitlab.idiap.ch/rli/pbdlib_gui
+https://gitlab.idiap.ch/rli/pbdlib_gui_sandbox
### Test
@@ -52,9 +60,9 @@ include_directories(include ${ARMADILLO_INCLUDE_DIRS})
The installation of PbDLib copies the header files to /usr/local/include, so the command include_directories(include)
adds this directory to the search path.
-Header files can then be included in source code using:
+Header files can then be included in source code, by using for example (for the GMM class):
```
-#include "pbdlib/gmm.h" // includes header file of the GMM class
+#include "pbdlib/gmm.h"
```
#### 2) Linking your program with PbD and Armadillo
diff --git a/data/adhsmm/ADHSMM_test_durMu.txt b/data/adhsmm/ADHSMM_test_durMu.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7cbe5e335aa09caf1c0681ee52c5aee2bc1c0bd7
--- /dev/null
+++ b/data/adhsmm/ADHSMM_test_durMu.txt
@@ -0,0 +1,2 @@
+0.000000 1.000000 0.000000 1.000000 0.000000 1.000000 0.000000 1.000000 0.000000 1.000000 0.000000 1.000000 0.000000 1.000000
+20.000000 100.000000 20.000000 100.000000 20.000000 100.000000 20.000000 100.000000 20.000000 100.000000 20.000000 100.000000 20.000000 100.000000
diff --git a/data/adhsmm/ADHSMM_test_durPriors.txt b/data/adhsmm/ADHSMM_test_durPriors.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f1e6196e9e01a0f073ec0f4072e614cc87270187
--- /dev/null
+++ b/data/adhsmm/ADHSMM_test_durPriors.txt
@@ -0,0 +1,2 @@
+1.000000 1.000000 1.000000 1.000000 1.000000 1.000000 1.000000
+1.000000 1.000000 1.000000 1.000000 1.000000 1.000000 1.000000
diff --git a/data/adhsmm/ADHSMM_test_durSigma.txt b/data/adhsmm/ADHSMM_test_durSigma.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6c8ebf3f8eb769b4dd6ead4a628336b60b2c2f9f
--- /dev/null
+++ b/data/adhsmm/ADHSMM_test_durSigma.txt
@@ -0,0 +1,2 @@
+0.010000 0.000000 0.010000 0.000000 0.010000 0.000000 0.010000 0.000000 0.010000 0.000000 0.010000 0.000000 0.010000 0.000000 0.010000 0.000000 0.010000 0.000000 0.010000 0.000000 0.010000 0.000000 0.010000 0.000000 0.010000 0.000000 0.010000 0.000000
+0.000000 5.000000 0.000000 800.000000 0.000000 5.000000 0.000000 800.000000 0.000000 5.000000 0.000000 800.000000 0.000000 5.000000 0.000000 800.000000 0.000000 5.000000 0.000000 800.000000 0.000000 5.000000 0.000000 800.000000 0.000000 5.000000 0.000000 800.000000
diff --git a/data/adhsmm/ADHSMM_test_mu.txt b/data/adhsmm/ADHSMM_test_mu.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c54fbb6712df039896caffaae8ef1bc10150bea0
--- /dev/null
+++ b/data/adhsmm/ADHSMM_test_mu.txt
@@ -0,0 +1,2 @@
+8.330649 -10.226152 -21.643923 -5.522649 15.209979 12.934711 2.548406
+17.167884 15.136602 -5.204755 -21.714712 -13.790094 -0.516919 0.287984
diff --git a/data/adhsmm/ADHSMM_test_priors.txt b/data/adhsmm/ADHSMM_test_priors.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a1f9283282dd80a63d59f45150241d82171c21bd
--- /dev/null
+++ b/data/adhsmm/ADHSMM_test_priors.txt
@@ -0,0 +1 @@
+1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000
diff --git a/data/adhsmm/ADHSMM_test_sigma.txt b/data/adhsmm/ADHSMM_test_sigma.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8df6a62b6f2cb9d54a4c0e31621ea12883835595
--- /dev/null
+++ b/data/adhsmm/ADHSMM_test_sigma.txt
@@ -0,0 +1,2 @@
+8.955796 -2.491576 44.306038 13.330600 6.649426 -3.428034 61.466981 -7.276294 20.474603 14.117891 17.645156 -2.622628 4.859608 0.745922
+-2.491576 6.184153 13.330600 10.484544 -3.428034 55.029148 -7.276294 6.050033 14.117891 32.604623 -2.622628 2.955720 0.745922 0.226175
diff --git a/data/adhsmm/ADHSMM_test_testData.txt b/data/adhsmm/ADHSMM_test_testData.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6600242121356fe637a39d4e2e6d71d326b50855
--- /dev/null
+++ b/data/adhsmm/ADHSMM_test_testData.txt
@@ -0,0 +1,2 @@
+ 1.1890490e+01 1.1880214e+01 1.1851202e+01 1.1812441e+01 1.1767406e+01 1.1710190e+01 1.1637318e+01 1.1544851e+01 1.1426255e+01 1.1267460e+01 1.1057851e+01 1.0791127e+01 1.0461826e+01 1.0063513e+01 9.5977469e+00 9.0722330e+00 8.4898689e+00 7.8533351e+00 7.1701645e+00 6.4516252e+00 5.7084986e+00 4.9471188e+00 4.1748196e+00 3.4004226e+00 2.6189148e+00 1.8206824e+00 1.0127316e+00 2.0069172e-01 -6.1443515e-01 -1.4232346e+00 -2.2193939e+00 -2.9993104e+00 -3.7613077e+00 -4.5040414e+00 -5.2216453e+00 -5.9061014e+00 -6.5622597e+00 -7.1952674e+00 -7.8056759e+00 -8.4011977e+00 -8.9916453e+00 -9.5830146e+00 -1.0175157e+01 -1.0769476e+01 -1.1370704e+01 -1.1978759e+01 -1.2587911e+01 -1.3194319e+01 -1.3799241e+01 -1.4400087e+01 -1.4988964e+01 -1.5559922e+01 -1.6108763e+01 -1.6626468e+01 -1.7101038e+01 -1.7531385e+01 -1.7918216e+01 -1.8260764e+01 -1.8557095e+01 -1.8807211e+01 -1.9011887e+01 -1.9175012e+01 -1.9304909e+01 -1.9409509e+01 -1.9496323e+01 -1.9569643e+01 -1.9631850e+01 -1.9682830e+01 -1.9725187e+01 -1.9761358e+01 -1.9792759e+01 -1.9818266e+01 -1.9834289e+01 -1.9839850e+01 -1.9830317e+01 -1.9801093e+01 -1.9753021e+01 -1.9686715e+01 -1.9594745e+01 -1.9478446e+01 -1.9337332e+01 -1.9166545e+01 -1.8964265e+01 -1.8728767e+01 -1.8457664e+01 -1.8146063e+01 -1.7789704e+01 -1.7387288e+01 -1.6941881e+01 -1.6450221e+01 -1.5922560e+01 -1.5366727e+01 -1.4780411e+01 -1.4171135e+01 -1.3551583e+01 -1.2920430e+01 -1.2272799e+01 -1.1618797e+01 -1.0957414e+01 -1.0282159e+01 -9.5857329e+00 -8.8724705e+00 -8.1500914e+00 -7.4151178e+00 -6.6637069e+00 -5.9122912e+00 -5.1685549e+00 -4.4186435e+00 -3.6729764e+00 -2.9423492e+00 -2.2281142e+00 -1.5243587e+00 -8.3779152e-01 -1.6885820e-01 4.8180114e-01 1.1230058e+00 1.7608156e+00 2.3884530e+00 3.0124444e+00 3.6412950e+00 4.2701572e+00 4.8914887e+00 5.5178824e+00 6.1480543e+00 6.7742326e+00 7.3963114e+00 8.0057631e+00 8.5966922e+00 9.1619636e+00 9.7015796e+00 1.0218136e+01 1.0708300e+01 1.1168059e+01 1.1598311e+01 1.1995759e+01 1.2356521e+01 1.2683612e+01 1.2974439e+01 1.3228875e+01 1.3445016e+01 1.3623827e+01 1.3769475e+01 1.3881943e+01 1.3963969e+01 1.4018021e+01 1.4039259e+01 1.4029491e+01 1.3995357e+01 1.3939225e+01 1.3862729e+01 1.3768079e+01 1.3651752e+01 1.3510204e+01 1.3341988e+01 1.3153600e+01 1.2953092e+01 1.2743790e+01 1.2526010e+01 1.2299640e+01 1.2063434e+01 1.1818358e+01 1.1563134e+01 1.1297646e+01 1.1023094e+01 1.0736101e+01 1.0432802e+01 1.0116598e+01 9.7877942e+00 9.4412196e+00 9.0734450e+00 8.6792286e+00 8.2614058e+00 7.8253002e+00 7.3748462e+00 6.9130398e+00 6.4458523e+00 5.9678235e+00 5.4786523e+00 4.9836783e+00 4.4881819e+00 3.9979569e+00 3.5185540e+00 3.0520802e+00 2.6023690e+00 2.1718902e+00 1.7616452e+00 1.3797703e+00 1.0339660e+00 7.2815392e-01 4.6828699e-01 2.5929963e-01 9.8193642e-02 -1.9800498e-02 -1.0251379e-01 -1.5106768e-01 -1.7087974e-01 -1.6395777e-01 -1.3778863e-01 -8.8330620e-02 0.0000000e+00
+ 1.4102674e+01 1.4102674e+01 1.4102674e+01 1.4102674e+01 1.4103073e+01 1.4106708e+01 1.4115861e+01 1.4130821e+01 1.4154611e+01 1.4189307e+01 1.4232790e+01 1.4286957e+01 1.4352422e+01 1.4429006e+01 1.4516146e+01 1.4609847e+01 1.4704387e+01 1.4798449e+01 1.4885280e+01 1.4961632e+01 1.5029181e+01 1.5085613e+01 1.5129711e+01 1.5157388e+01 1.5166393e+01 1.5154526e+01 1.5125321e+01 1.5080557e+01 1.5027631e+01 1.4965173e+01 1.4891694e+01 1.4805921e+01 1.4706078e+01 1.4595117e+01 1.4477270e+01 1.4355227e+01 1.4225003e+01 1.4086031e+01 1.3932564e+01 1.3759504e+01 1.3561909e+01 1.3338595e+01 1.3088045e+01 1.2804622e+01 1.2485101e+01 1.2127495e+01 1.1731984e+01 1.1287742e+01 1.0791409e+01 1.0246945e+01 9.6549115e+00 9.0163999e+00 8.3429915e+00 7.6424577e+00 6.9141289e+00 6.1717359e+00 5.4241955e+00 4.6837760e+00 3.9667305e+00 3.2896928e+00 2.6596951e+00 2.0775446e+00 1.5371471e+00 1.0372276e+00 5.7484077e-01 1.3616634e-01 -2.8654243e-01 -7.0215116e-01 -1.1292070e+00 -1.5858707e+00 -2.0831965e+00 -2.6337776e+00 -3.2353404e+00 -3.8828340e+00 -4.5713700e+00 -5.2886901e+00 -6.0173973e+00 -6.7494351e+00 -7.4793072e+00 -8.1966436e+00 -8.8898102e+00 -9.5531090e+00 -1.0193884e+01 -1.0810508e+01 -1.1405132e+01 -1.1992102e+01 -1.2579459e+01 -1.3165781e+01 -1.3750911e+01 -1.4333015e+01 -1.4901638e+01 -1.5445668e+01 -1.5960884e+01 -1.6448293e+01 -1.6904764e+01 -1.7326901e+01 -1.7717626e+01 -1.8072300e+01 -1.8388066e+01 -1.8668537e+01 -1.8923314e+01 -1.9154289e+01 -1.9361313e+01 -1.9544800e+01 -1.9703067e+01 -1.9833673e+01 -1.9937804e+01 -2.0020333e+01 -2.0083299e+01 -2.0128344e+01 -2.0151465e+01 -2.0151787e+01 -2.0131503e+01 -2.0093775e+01 -2.0038074e+01 -1.9966016e+01 -1.9874850e+01 -1.9759533e+01 -1.9615394e+01 -1.9438614e+01 -1.9227389e+01 -1.8983097e+01 -1.8693873e+01 -1.8350521e+01 -1.7954709e+01 -1.7509351e+01 -1.7011716e+01 -1.6469782e+01 -1.5893233e+01 -1.5291308e+01 -1.4669036e+01 -1.4036637e+01 -1.3403770e+01 -1.2778276e+01 -1.2158494e+01 -1.1542708e+01 -1.0936186e+01 -1.0342553e+01 -9.7589606e+00 -9.1858901e+00 -8.6249707e+00 -8.0683655e+00 -7.5124170e+00 -6.9581995e+00 -6.4079098e+00 -5.8678901e+00 -5.3441347e+00 -4.8405802e+00 -4.3613352e+00 -3.9109624e+00 -3.4933813e+00 -3.1200893e+00 -2.7981569e+00 -2.5248429e+00 -2.2951299e+00 -2.1018328e+00 -1.9345761e+00 -1.7880398e+00 -1.6616552e+00 -1.5524532e+00 -1.4567809e+00 -1.3703375e+00 -1.2912011e+00 -1.2185241e+00 -1.1544924e+00 -1.1006011e+00 -1.0527474e+00 -1.0058318e+00 -9.5830542e-01 -9.0640139e-01 -8.4465500e-01 -7.7568409e-01 -7.0328150e-01 -6.3050790e-01 -5.5918987e-01 -4.9452564e-01 -4.4013945e-01 -3.9476358e-01 -3.5542229e-01 -3.2422018e-01 -3.0306760e-01 -2.8853966e-01 -2.7970261e-01 -2.7655134e-01 -2.7652302e-01 -2.7652236e-01 -2.7646403e-01 -2.7607457e-01 -2.7483384e-01 -2.7200169e-01 -2.6664604e-01 -2.5772839e-01 -2.4437722e-01 -2.2764439e-01 -2.0680735e-01 -1.8006740e-01 -1.4621912e-01 -1.0796642e-01 -6.0822626e-02 0.0000000e+00
diff --git a/data/adhsmm/ADHSMM_test_trans.txt b/data/adhsmm/ADHSMM_test_trans.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3dc03f6279535e84b99b8d8734d87d5fd4bab7ac
--- /dev/null
+++ b/data/adhsmm/ADHSMM_test_trans.txt
@@ -0,0 +1,7 @@
+0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000
+0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000
+0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000
+0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000
+0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000
+0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
+0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
diff --git a/data/adhsmm/ADHSMM_test_varnames.txt b/data/adhsmm/ADHSMM_test_varnames.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7d220dfaecee7c459dab99594033b6fdf4d83979
--- /dev/null
+++ b/data/adhsmm/ADHSMM_test_varnames.txt
@@ -0,0 +1 @@
+x1 x2
\ No newline at end of file
diff --git a/data/data.csv b/data/data.csv
new file mode 100644
index 0000000000000000000000000000000000000000..84d53aace5d576ba3e57a541c313e97e0e7beb22
--- /dev/null
+++ b/data/data.csv
@@ -0,0 +1,200 @@
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068152
+-0.082378,-0.068155
+-0.081687,-0.068319
+-0.080945,-0.068468
+-0.079977,-0.068612
+-0.079434,-0.068601
+-0.078752,-0.068765
+-0.078346,-0.068913
+-0.077863,-0.069059
+-0.077419,-0.069043
+-0.076856,-0.069042
+-0.076383,-0.069042
+-0.075905,-0.069042
+-0.075352,-0.069042
+-0.074704,-0.069042
+-0.073753,-0.069042
+-0.073005,-0.069042
+-0.072234,-0.069042
+-0.071674,-0.069042
+-0.070825,-0.069042
+-0.069984,-0.069042
+-0.069139,-0.069042
+-0.068414,-0.069042
+-0.067672,-0.069042
+-0.066948,-0.069042
+-0.066062,-0.069042
+-0.065174,-0.069042
+-0.064223,-0.069042
+-0.063224,-0.069043
+-0.062107,-0.068989
+-0.060982,-0.068842
+-0.059842,-0.068692
+-0.058749,-0.068598
+-0.057629,-0.068597
+-0.056719,-0.068597
+-0.055622,-0.068597
+-0.054388,-0.068597
+-0.052765,-0.068597
+-0.051501,-0.068597
+-0.050344,-0.068597
+-0.049266,-0.068597
+-0.048116,-0.068597
+-0.046853,-0.068597
+-0.045609,-0.068597
+-0.044176,-0.068597
+-0.042627,-0.068597
+-0.040775,-0.068597
+-0.038567,-0.068597
+-0.03649,-0.068597
+-0.034641,-0.068596
+-0.032726,-0.068645
+-0.030486,-0.068793
+-0.027966,-0.068942
+-0.025945,-0.069041
+-0.02427,-0.069107
+-0.022706,-0.069255
+-0.021146,-0.069404
+-0.019209,-0.069488
+-0.017422,-0.069503
+-0.014873,-0.069356
+-0.012387,-0.069224
+-0.0096192,-0.068913
+-0.0075533,-0.068668
+-0.0055986,-0.068337
+-0.0038396,-0.068058
+-0.0019648,-0.067687
+-0.00029778,-0.067273
+0.0012646,-0.066657
+0.0027773,-0.066071
+0.0041089,-0.065458
+0.0054553,-0.065006
+0.0064648,-0.064555
+0.0075943,-0.064139
+0.0087145,-0.063551
+0.010193,-0.062832
+0.011628,-0.061999
+0.012856,-0.061119
+0.013874,-0.059991
+0.01482,-0.05887
+0.015683,-0.057894
+0.016577,-0.057079
+0.017276,-0.056179
+0.017999,-0.054902
+0.018637,-0.053539
+0.019352,-0.052029
+0.019969,-0.050515
+0.020415,-0.048875
+0.020889,-0.047214
+0.021206,-0.045725
+0.021367,-0.044161
+0.021194,-0.0425
+0.020888,-0.04068
+0.020426,-0.038966
+0.019996,-0.03732
+0.019536,-0.035751
+0.01907,-0.034337
+0.018527,-0.032988
+0.017891,-0.031691
+0.017125,-0.030414
+0.016234,-0.029054
+0.015385,-0.027795
+0.014421,-0.026378
+0.013562,-0.024958
+0.012522,-0.023521
+0.011731,-0.022419
+0.010841,-0.02157
+0.009797,-0.0206
+0.0086814,-0.019597
+0.0076521,-0.018633
+0.0069458,-0.01797
+0.0060823,-0.017282
+0.0050601,-0.016597
+0.0038789,-0.015775
+0.0027138,-0.014959
+0.0012034,-0.013984
+-0.00028179,-0.013176
+-0.0019616,-0.012486
+-0.0035161,-0.011811
+-0.0051711,-0.011096
+-0.0069248,-0.010243
+-0.0088179,-0.0092795
+-0.011021,-0.0080833
+-0.013347,-0.0067082
+-0.015722,-0.0052943
+-0.017779,-0.0040924
+-0.019596,-0.0028025
+-0.021325,-0.0015108
+-0.023298,0.0002239
+-0.025116,0.0019443
+-0.026929,0.0038161
+-0.028583,0.0056918
+-0.030099,0.007467
+-0.031415,0.0093155
+-0.03277,0.011482
+-0.034095,0.013725
+-0.035137,0.015862
+-0.035669,0.017311
+-0.036206,0.019018
+-0.036742,0.020935
+-0.037485,0.022875
+-0.038168,0.024782
+-0.038749,0.026619
+-0.039053,0.028823
+-0.039037,0.030941
+-0.038638,0.033282
+-0.038014,0.035871
+-0.037128,0.038602
+-0.036277,0.040889
+-0.035166,0.042934
+-0.03399,0.044689
+-0.032421,0.046423
+-0.03068,0.04795
+-0.028419,0.049379
+-0.026156,0.05066
+-0.023731,0.051782
+-0.02125,0.052903
+-0.018634,0.053978
+-0.015666,0.055105
+-0.013159,0.055795
+-0.010508,0.056446
+-0.0079871,0.056865
+-0.0048888,0.057307
+-0.0018382,0.057606
+0.0014204,0.057749
+0.00481,0.057909
+0.0086031,0.057902
+0.012409,0.057756
+0.016489,0.057528
+0.020271,0.057084
+0.02406,0.056718
+0.027224,0.056199
+0.030641,0.055525
+0.034283,0.054689
+0.038111,0.053857
+0.042136,0.053344
+0.045952,0.053059
+0.049879,0.053006
+0.053413,0.053007
+0.056465,0.053007
+0.05906,0.053011
+0.061465,0.053174
+0.063735,0.05331
+0.066084,0.053637
+0.068435,0.054192
+0.071132,0.055055
+0.073568,0.056049
+0.075369,0.056731
+0.076145,0.057216
+0.076309,0.057399
+0.076291,0.057038
+0.076291,0.056125
diff --git a/data/data2.csv b/data/data2.csv
new file mode 100644
index 0000000000000000000000000000000000000000..3815c089437faff0ac4724ec10cc342f88594ab2
--- /dev/null
+++ b/data/data2.csv
@@ -0,0 +1,200 @@
+0.4572, 0.11382, 0.16862, 0.14988, 0.1299, -0.72128, 0.66364
+0.45702, 0.11399, 0.16857, 0.14999, 0.12999, -0.72121, 0.66367
+0.4573, 0.11399, 0.16849, 0.14965, 0.12991, -0.72152, 0.66343
+0.45646, 0.11385, 0.16867, 0.15053, 0.12959, -0.72129, 0.66355
+0.45676, 0.11403, 0.16864, 0.14988, 0.12938, -0.72145, 0.66355
+0.45626, 0.11381, 0.16852, 0.15057, 0.1296, -0.72137, 0.66345
+0.45664, 0.11381, 0.16813, 0.15046, 0.1304, -0.72133, 0.66335
+0.45738, 0.11432, 0.16819, 0.14933, 0.12986, -0.72165, 0.66337
+0.45678, 0.1138, 0.16864, 0.15023, 0.12972, -0.72139, 0.66347
+0.45688, 0.11387, 0.1687, 0.1502, 0.12984, -0.72127, 0.66359
+0.45707, 0.11375, 0.16837, 0.14979, 0.12951, -0.72162, 0.66336
+0.45697, 0.11381, 0.16864, 0.15018, 0.12983, -0.72128, 0.66359
+0.45684, 0.11416, 0.16898, 0.15, 0.12963, -0.72111, 0.66385
+0.45703, 0.11372, 0.16932, 0.14976, 0.12914, -0.72107, 0.66404
+0.45705, 0.11392, 0.16907, 0.1501, 0.12984, -0.7209, 0.66402
+0.45586, 0.1135, 0.16839, 0.15008, 0.13468, -0.72134, 0.66257
+0.45568, 0.11178, 0.16424, 0.14264, 0.13343, -0.72627, 0.65907
+0.45078, 0.10713, 0.16643, 0.14619, 0.12927, -0.72542, 0.66006
+0.44348, 0.099129, 0.16808, 0.15063, 0.12382, -0.72499, 0.66058
+0.43799, 0.089524, 0.16935, 0.15013, 0.11725, -0.72495, 0.66194
+0.43676, 0.082458, 0.16985, 0.13791, 0.1048, -0.72646, 0.66502
+0.43992, 0.072334, 0.17322, 0.1333, 0.1064, -0.72345, 0.66898
+0.44161, 0.057072, 0.18072, 0.12999, 0.10229, -0.71811, 0.67599
+0.43777, 0.041894, 0.18931, 0.13098, 0.089916, -0.71293, 0.683
+0.43548, 0.029938, 0.19545, 0.12202, 0.07351, -0.70986, 0.68979
+0.43188, 0.019819, 0.19822, 0.116, 0.059533, -0.7089, 0.69315
+0.42855, 0.0079365, 0.20217, 0.11492, 0.05479, -0.70649, 0.69617
+0.4298, -0.0074189, 0.20954, 0.10882, 0.050741, -0.70135, 0.70263
+0.42785, -0.023613, 0.21664, 0.09227, 0.031143, -0.69741, 0.71003
+0.42297, -0.040153, 0.22432, 0.084046, 0.014229, -0.69255, 0.71631
+0.41642, -0.053571, 0.23079, 0.077223, -0.0064743, -0.6886, 0.72099
+0.41461, -0.063535, 0.23432, 0.06781, -0.018561, -0.68543, 0.72474
+0.41491, -0.073991, 0.23801, 0.063166, -0.020154, -0.68242, 0.72795
+0.41004, -0.085166, 0.23526, 0.062299, -0.027519, -0.68547, 0.72491
+0.42007, -0.095121, 0.23103, 0.050559, -0.01603, -0.69007, 0.7218
+0.42094, -0.10342, 0.23493, 0.0515, -0.006528, -0.68703, 0.72477
+0.41971, -0.11454, 0.2417, 0.049995, -0.0040435, -0.68225, 0.7294
+0.42069, -0.12394, 0.2464, 0.047492, -0.0015047, -0.67972, 0.73193
+0.42208, -0.13055, 0.25468, 0.044277, -2.6483e-05, -0.6752, 0.73631
+0.41831, -0.13536, 0.25624, 0.049308, 0.0013233, -0.67778, 0.73361
+0.419, -0.14338, 0.24364, 0.046709, 0.0072669, -0.69069, 0.72161
+0.41581, -0.14825, 0.24571, 0.047654, 0.0086141, -0.68913, 0.72302
+0.41886, -0.15516, 0.25216, 0.043537, 0.010582, -0.68462, 0.72753
+0.41832, -0.16375, 0.26005, 0.040562, 0.0057741, -0.67969, 0.73235
+0.41613, -0.17106, 0.26621, 0.042369, 0.0065919, -0.67566, 0.73596
+0.41377, -0.17845, 0.26693, 0.040562, 0.0032748, -0.67636, 0.73544
+0.413, -0.1862, 0.27068, 0.03839, 0.0027046, -0.67407, 0.73766
+0.41429, -0.19317, 0.26689, 0.034538, 0.0029203, -0.67856, 0.73373
+0.41757, -0.19938, 0.26848, 0.030604, 0.0055982, -0.67806, 0.73435
+0.41446, -0.20528, 0.27175, 0.030754, 0.0019213, -0.67545, 0.73676
+0.41641, -0.20772, 0.26922, 0.028422, 0.0033884, -0.67757, 0.7349
+0.41583, -0.20785, 0.26962, 0.028713, 0.0029481, -0.67737, 0.73508
+0.41645, -0.20795, 0.27052, 0.027856, 0.0027676, -0.67738, 0.7351
+0.41708, -0.20779, 0.2699, 0.0273, 0.0027223, -0.6778, 0.73473
+0.41779, -0.20752, 0.26726, 0.026839, 0.0036349, -0.67991, 0.7328
+0.41732, -0.20771, 0.26942, 0.027118, 0.003051, -0.67884, 0.73378
+0.41786, -0.20704, 0.26862, 0.026759, 0.0034835, -0.67923, 0.73343
+0.41746, -0.2066, 0.268, 0.027169, 0.0038735, -0.67958, 0.73309
+0.418, -0.20653, 0.26499, 0.027079, 0.004656, -0.68167, 0.73114
+0.41764, -0.20651, 0.26458, 0.026878, 0.0044331, -0.6822, 0.73066
+0.4183, -0.20625, 0.26447, 0.02619, 0.0044816, -0.6824, 0.73049
+0.41795, -0.20658, 0.26473, 0.02632, 0.0039383, -0.68188, 0.73098
+0.41686, -0.2069, 0.26226, 0.0204, -0.0029815, -0.68364, 0.72952
+0.4172, -0.20649, 0.25713, 0.013614, -0.01023, -0.68514, 0.72822
+0.41678, -0.20689, 0.25061, 0.0035748, -0.024575, -0.68559, 0.72757
+0.41637, -0.2066, 0.2419, -0.0092708, -0.044659, -0.68368, 0.72836
+0.41686, -0.2063, 0.23032, -0.019526, -0.064949, -0.68361, 0.72669
+0.41748, -0.20557, 0.2182, -0.029964, -0.088781, -0.68401, 0.72343
+0.41833, -0.2052, 0.20698, -0.037426, -0.11217, -0.68225, 0.72149
+0.42092, -0.20461, 0.19574, -0.048918, -0.13596, -0.67955, 0.71926
+0.42522, -0.20388, 0.18471, -0.062004, -0.159, -0.67634, 0.71655
+0.42741, -0.20302, 0.1761, -0.070588, -0.18413, -0.67119, 0.71457
+0.4322, -0.20177, 0.16637, -0.084083, -0.20469, -0.66667, 0.71175
+0.43898, -0.20127, 0.1562, -0.10311, -0.22688, -0.66122, 0.70759
+0.44647, -0.20163, 0.14714, -0.12509, -0.25282, -0.65448, 0.70149
+0.45402, -0.20145, 0.13739, -0.14624, -0.27688, -0.64941, 0.69298
+0.4656, -0.20151, 0.12539, -0.1778, -0.30858, -0.64253, 0.67847
+0.47838, -0.20086, 0.11528, -0.20278, -0.33067, -0.63601, 0.6671
+0.4902, -0.20098, 0.10583, -0.23051, -0.35767, -0.6265, 0.65302
+0.50247, -0.2016, 0.099317, -0.25425, -0.37795, -0.6191, 0.63971
+0.51677, -0.20246, 0.092642, -0.28181, -0.39918, -0.61109, 0.62274
+0.53104, -0.20151, 0.086259, -0.30164, -0.41563, -0.60576, 0.60772
+0.54177, -0.20211, 0.082467, -0.32131, -0.43138, -0.59651, 0.59569
+0.5533, -0.20234, 0.079185, -0.34348, -0.45786, -0.5834, 0.57622
+0.56674, -0.20368, 0.07637, -0.3652, -0.4817, -0.57175, 0.55471
+0.57485, -0.2033, 0.074383, -0.38096, -0.50093, -0.5605, 0.53832
+0.57607, -0.2041, 0.074519, -0.38776, -0.50527, -0.55616, 0.53388
+0.57442, -0.20466, 0.074794, -0.38711, -0.50567, -0.55566, 0.5345
+0.5742, -0.20472, 0.074399, -0.38641, -0.50531, -0.55631, 0.53466
+0.57109, -0.20458, 0.07296, -0.38389, -0.50506, -0.5569, 0.53611
+0.5714, -0.20463, 0.073994, -0.38397, -0.50366, -0.55701, 0.53724
+0.57289, -0.20426, 0.074928, -0.38469, -0.50368, -0.55695, 0.53677
+0.57259, -0.20465, 0.075494, -0.38835, -0.50674, -0.55339, 0.53494
+0.57371, -0.2043, 0.074365, -0.3972, -0.5129, -0.54708, 0.52904
+0.57477, -0.20488, 0.075348, -0.40108, -0.51365, -0.54461, 0.52792
+0.57635, -0.20407, 0.075015, -0.4045, -0.51543, -0.54278, 0.52545
+0.58012, -0.20453, 0.075817, -0.4133, -0.52236, -0.53661, 0.51805
+0.58615, -0.20544, 0.076722, -0.42991, -0.53266, -0.5245, 0.50631
+0.6015, -0.20516, 0.077649, -0.45121, -0.54599, -0.51067, 0.48736
+0.61743, -0.20556, 0.079781, -0.47641, -0.5634, -0.49179, 0.46234
+0.6363, -0.20473, 0.083357, -0.50281, -0.58287, -0.47164, 0.43012
+0.65308, -0.20231, 0.091205, -0.52217, -0.59257, -0.45664, 0.40947
+0.6633, -0.19969, 0.097655, -0.53799, -0.6091, -0.43473, 0.38803
+0.67383, -0.19957, 0.10231, -0.55332, -0.61846, -0.41938, 0.36804
+0.68089, -0.20207, 0.10734, -0.56156, -0.6221, -0.41247, 0.3571
+0.68766, -0.2027, 0.11051, -0.57471, -0.63498, -0.39319, 0.33454
+0.69404, -0.20723, 0.1141, -0.58363, -0.6412, -0.38347, 0.3181
+0.69753, -0.20834, 0.11784, -0.58956, -0.64857, -0.37156, 0.30613
+0.70646, -0.20778, 0.12443, -0.60204, -0.65635, -0.3539, 0.28551
+0.71318, -0.20797, 0.1302, -0.60771, -0.66177, -0.34355, 0.27333
+0.71971, -0.20838, 0.13796, -0.61497, -0.67155, -0.32495, 0.25544
+0.72638, -0.20772, 0.14404, -0.62623, -0.67953, -0.30326, 0.23261
+0.73411, -0.20797, 0.15496, -0.63638, -0.68384, -0.28555, 0.21409
+0.73791, -0.20902, 0.16169, -0.64306, -0.69144, -0.26591, 0.19413
+0.74339, -0.20849, 0.17008, -0.65184, -0.69622, -0.24527, 0.17386
+0.74719, -0.20939, 0.17913, -0.6588, -0.69918, -0.22814, 0.15836
+0.75102, -0.20966, 0.18725, -0.66429, -0.70352, -0.21066, 0.13931
+0.75532, -0.20988, 0.1961, -0.66941, -0.70693, -0.19397, 0.12045
+0.75833, -0.2101, 0.20305, -0.67414, -0.71169, -0.17194, 0.097348
+0.76096, -0.21037, 0.20923, -0.67703, -0.71326, -0.16043, 0.084639
+0.76159, -0.21088, 0.2141, -0.67919, -0.7134, -0.15343, 0.078872
+0.76321, -0.21101, 0.21982, -0.67994, -0.7169, -0.1385, 0.067497
+0.76402, -0.21132, 0.22605, -0.6832, -0.71716, -0.12526, 0.056829
+0.76558, -0.21023, 0.22946, -0.68427, -0.71902, -0.11306, 0.044778
+0.76641, -0.21086, 0.23856, -0.68776, -0.71892, -0.096219, 0.029695
+0.77012, -0.20961, 0.24657, -0.69274, -0.71572, -0.086862, 0.01768
+0.77913, -0.20884, 0.25292, -0.69404, -0.71532, -0.081308, 0.0031653
+0.7808, -0.2095, 0.25715, -0.69569, -0.71378, -0.080883, 0.0012859
+0.77929, -0.2087, 0.25978, -0.69721, -0.71267, -0.077439, 0.0019853
+0.77495, -0.21064, 0.26187, -0.69871, -0.71265, -0.062228, -0.0071358
+0.77148, -0.21227, 0.26353, -0.70072, -0.71163, -0.047598, -0.017688
+0.77086, -0.21315, 0.26499, -0.70057, -0.71192, -0.044439, -0.019812
+0.77054, -0.2133, 0.26708, -0.70114, -0.71156, -0.037958, -0.025335
+0.7705, -0.21324, 0.26904, -0.7013, -0.71148, -0.033951, -0.028568
+0.77114, -0.21334, 0.27027, -0.70132, -0.71142, -0.034136, -0.029436
+0.77068, -0.21273, 0.26917, -0.70102, -0.71174, -0.034515, -0.028386
+0.77103, -0.21246, 0.26927, -0.7009, -0.71185, -0.034273, -0.028888
+0.77054, -0.21305, 0.26929, -0.70096, -0.71179, -0.034304, -0.028782
+0.77036, -0.21306, 0.26927, -0.70101, -0.71176, -0.034234, -0.028562
+0.77001, -0.21347, 0.26908, -0.70113, -0.71164, -0.034643, -0.028083
+0.77175, -0.21231, 0.26849, -0.70049, -0.71218, -0.035591, -0.028985
+0.7708, -0.212, 0.26582, -0.69975, -0.71292, -0.036589, -0.027458
+0.77051, -0.21191, 0.26781, -0.70075, -0.71199, -0.036066, -0.026908
+0.77167, -0.21101, 0.26619, -0.69986, -0.71279, -0.036658, -0.027796
+0.77361, -0.20843, 0.26646, -0.69985, -0.71275, -0.037446, -0.028259
+0.77345, -0.2039, 0.26124, -0.6987, -0.71382, -0.041351, -0.023852
+0.7692, -0.19667, 0.26284, -0.70244, -0.71059, -0.037198, -0.015968
+0.77085, -0.18973, 0.26509, -0.7045, -0.70861, -0.036989, -0.013381
+0.77772, -0.18235, 0.26551, -0.70483, -0.7081, -0.039485, -0.015892
+0.77995, -0.17288, 0.26205, -0.70234, -0.71068, -0.037571, -0.015322
+0.77553, -0.1619, 0.2608, -0.70454, -0.70878, -0.034988, -0.0055403
+0.77683, -0.15021, 0.26245, -0.70592, -0.70733, -0.036737, -0.0010456
+0.77758, -0.14028, 0.26452, -0.70614, -0.7067, -0.043669, 0.0049285
+0.77632, -0.13203, 0.26488, -0.70772, -0.70403, -0.056916, 0.015334
+0.77872, -0.12161, 0.26404, -0.70738, -0.70299, -0.070381, 0.021702
+0.78252, -0.1103, 0.26206, -0.70613, -0.70247, -0.085092, 0.026227
+0.78997, -0.099636, 0.26235, -0.70919, -0.69605, -0.10754, 0.031681
+0.79859, -0.085097, 0.26252, -0.71052, -0.6903, -0.13142, 0.037136
+0.80563, -0.066733, 0.26309, -0.7126, -0.67637, -0.17551, 0.062592
+0.80232, -0.047037, 0.26225, -0.71637, -0.66481, -0.19608, 0.07998
+0.79731, -0.02673, 0.25997, -0.71851, -0.65398, -0.21613, 0.096647
+0.79374, -0.0087205, 0.24746, -0.71049, -0.64879, -0.24788, 0.11325
+0.79375, 0.0041887, 0.24296, -0.70714, -0.63761, -0.27828, 0.1264
+0.79983, 0.012978, 0.24988, -0.70603, -0.61129, -0.32237, 0.15466
+0.80811, 0.020666, 0.2546, -0.70543, -0.59137, -0.35132, 0.17093
+0.82117, 0.030488, 0.25985, -0.70607, -0.56606, -0.38278, 0.18581
+0.83498, 0.043246, 0.26271, -0.70892, -0.54419, -0.40865, 0.18521
+0.8404, 0.053808, 0.25123, -0.70089, -0.53131, -0.43707, 0.18826
+0.84575, 0.063273, 0.2391, -0.68626, -0.51223, -0.47571, 0.20095
+0.85567, 0.075332, 0.23415, -0.67858, -0.4915, -0.50618, 0.2043
+0.86395, 0.089946, 0.23229, -0.67731, -0.47295, -0.52589, 0.20251
+0.87448, 0.11042, 0.22834, -0.67488, -0.44446, -0.55206, 0.20546
+0.88421, 0.12822, 0.22768, -0.67135, -0.41746, -0.57333, 0.2152
+0.89309, 0.14392, 0.23375, -0.6687, -0.38495, -0.59499, 0.22502
+0.89469, 0.15786, 0.23454, -0.66643, -0.36932, -0.60827, 0.22244
+0.89495, 0.17136, 0.22672, -0.6618, -0.35672, -0.62144, 0.22041
+0.89451, 0.18044, 0.22036, -0.65838, -0.35193, -0.62841, 0.2186
+0.9011, 0.18966, 0.22499, -0.66041, -0.34365, -0.63077, 0.21884
+0.90299, 0.194, 0.22678, -0.65998, -0.33953, -0.63314, 0.21974
+0.90334, 0.1946, 0.22659, -0.6588, -0.34066, -0.63437, 0.21797
+0.90292, 0.19425, 0.22617, -0.65822, -0.34081, -0.63487, 0.21803
+0.90192, 0.19296, 0.22528, -0.65751, -0.34125, -0.63515, 0.21865
+0.89915, 0.18956, 0.22385, -0.65627, -0.34236, -0.63512, 0.22074
+0.89805, 0.18715, 0.22451, -0.65629, -0.34256, -0.63451, 0.22212
+0.89606, 0.18425, 0.22382, -0.65558, -0.34344, -0.6342, 0.22373
+0.89526, 0.18264, 0.22276, -0.65486, -0.34417, -0.63435, 0.2243
+0.89556, 0.18299, 0.22311, -0.65512, -0.34386, -0.63429, 0.22417
+0.89604, 0.18345, 0.22261, -0.65548, -0.3434, -0.6342, 0.22408
+0.89694, 0.1852, 0.22262, -0.65578, -0.34262, -0.63445, 0.2237
+0.89702, 0.18575, 0.22186, -0.65565, -0.34255, -0.6347, 0.22345
+0.89843, 0.1854, 0.22446, -0.65737, -0.34213, -0.63315, 0.22343
+0.89973, 0.18711, 0.22462, -0.65782, -0.34162, -0.63328, 0.22253
+0.90043, 0.18785, 0.22516, -0.65837, -0.34168, -0.6329, 0.2219
+0.90017, 0.18765, 0.22483, -0.65817, -0.34185, -0.63294, 0.22211
+0.9011, 0.189, 0.22453, -0.6582, -0.34138, -0.63347, 0.22122
+0.90094, 0.18902, 0.22755, -0.66054, -0.34087, -0.631, 0.22209
+0.90116, 0.19002, 0.22663, -0.66012, -0.34014, -0.6319, 0.22194
+0.90106, 0.18968, 0.22693, -0.66016, -0.34059, -0.63164, 0.22184
+0.90114, 0.19037, 0.2265, -0.66009, -0.34061, -0.63192, 0.22124
+0.90144, 0.18984, 0.22706, -0.66038, -0.341, -0.63135, 0.22139
diff --git a/data/hsmm/HSMM_test2_durMu.txt b/data/hsmm/HSMM_test2_durMu.txt
new file mode 100644
index 0000000000000000000000000000000000000000..25d0cfac99d80c6ef8c043b687041605895f80a1
--- /dev/null
+++ b/data/hsmm/HSMM_test2_durMu.txt
@@ -0,0 +1 @@
+49 39.667 39 34.667 38
diff --git a/data/hsmm/HSMM_test2_durSigma.txt b/data/hsmm/HSMM_test2_durSigma.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ff3462087de2ac06543985af772917eccec329ca
--- /dev/null
+++ b/data/hsmm/HSMM_test2_durSigma.txt
@@ -0,0 +1 @@
+29 12.333 22 14.333 17
diff --git a/data/hsmm/HSMM_test2_mu.txt b/data/hsmm/HSMM_test2_mu.txt
new file mode 100644
index 0000000000000000000000000000000000000000..25ec8efc7d15688a6a3092bb3c6c3fd2b737203a
--- /dev/null
+++ b/data/hsmm/HSMM_test2_mu.txt
@@ -0,0 +1,2 @@
+-0.73986 -21.312 -2.6725 16.562 6.3776
+16.538 -3.5265 -21.63 -8.3458 0.37231
diff --git a/data/hsmm/HSMM_test2_priors.txt b/data/hsmm/HSMM_test2_priors.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3900d209870a7f8273d8c0095f7923f0816e5f61
--- /dev/null
+++ b/data/hsmm/HSMM_test2_priors.txt
@@ -0,0 +1 @@
+1 0 0 0 0
diff --git a/data/hsmm/HSMM_test2_sigma.txt b/data/hsmm/HSMM_test2_sigma.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9df6ee21475c4dbcaeb8f50cf65f7afb4514ed2f
--- /dev/null
+++ b/data/hsmm/HSMM_test2_sigma.txt
@@ -0,0 +1,2 @@
+93.815 7.5348 7.2585 0.11544 85.661 -3.9117 13.396 3.602 23.068 1.2001
+7.5348 6.8704 0.11544 76.008 -3.9117 5.5716 3.602 36.467 1.2001 0.76714
diff --git a/data/hsmm/HSMM_test2_testData.txt b/data/hsmm/HSMM_test2_testData.txt
new file mode 100644
index 0000000000000000000000000000000000000000..fb9542f09571d13616162874dd371cd85e83c90f
--- /dev/null
+++ b/data/hsmm/HSMM_test2_testData.txt
@@ -0,0 +1,2 @@
+11.89 11.88 11.851 11.812 11.767 11.71 11.637 11.545 11.426 11.267 11.058 10.791 10.462 10.064 9.5977 9.0722 8.4899 7.8533 7.1702 6.4516 5.7085 4.9471 4.1748 3.4004 2.6189 1.8207 1.0127 0.20069 -0.61444 -1.4232 -2.2194 -2.9993 -3.7613 -4.504 -5.2216 -5.9061 -6.5623 -7.1953 -7.8057 -8.4012 -8.9916 -9.583 -10.175 -10.769 -11.371 -11.979 -12.588 -13.194 -13.799 -14.4 -14.989 -15.56 -16.109 -16.626 -17.101 -17.531 -17.918 -18.261 -18.557 -18.807 -19.012 -19.175 -19.305 -19.41 -19.496 -19.57 -19.632 -19.683 -19.725 -19.761 -19.793 -19.818 -19.834 -19.84 -19.83 -19.801 -19.753 -19.687 -19.595 -19.478 -19.337 -19.167 -18.964 -18.729 -18.458 -18.146 -17.79 -17.387 -16.942 -16.45 -15.923 -15.367 -14.78 -14.171 -13.552 -12.92 -12.273 -11.619 -10.957 -10.282 -9.5857 -8.8725 -8.1501 -7.4151 -6.6637 -5.9123 -5.1686 -4.4186 -3.673 -2.9423 -2.2281 -1.5244 -0.83779 -0.16886 0.4818 1.123 1.7608 2.3885 3.0124 3.6413 4.2702 4.8915 5.5179 6.1481 6.7742 7.3963 8.0058 8.5967 9.162 9.7016 10.218 10.708 11.168 11.598 11.996 12.357 12.684 12.974 13.229 13.445 13.624 13.769 13.882 13.964 14.018 14.039 14.029 13.995 13.939 13.863 13.768 13.652 13.51 13.342 13.154 12.953 12.744 12.526 12.3 12.063 11.818 11.563 11.298 11.023 10.736 10.433 10.117 9.7878 9.4412 9.0734 8.6792 8.2614 7.8253 7.3748 6.913 6.4459 5.9678 5.4787 4.9837 4.4882 3.998 3.5186 3.0521 2.6024 2.1719 1.7616 1.3798 1.034 0.72815 0.46829 0.2593 0.098194 -0.0198 -0.10251 -0.15107 -0.17088 -0.16396 -0.13779 -0.088331 0
+14.103 14.103 14.103 14.103 14.103 14.107 14.116 14.131 14.155 14.189 14.233 14.287 14.352 14.429 14.516 14.61 14.704 14.798 14.885 14.962 15.029 15.086 15.13 15.157 15.166 15.155 15.125 15.081 15.028 14.965 14.892 14.806 14.706 14.595 14.477 14.355 14.225 14.086 13.933 13.76 13.562 13.339 13.088 12.805 12.485 12.127 11.732 11.288 10.791 10.247 9.6549 9.0164 8.343 7.6425 6.9141 6.1717 5.4242 4.6838 3.9667 3.2897 2.6597 2.0775 1.5371 1.0372 0.57484 0.13617 -0.28654 -0.70215 -1.1292 -1.5859 -2.0832 -2.6338 -3.2353 -3.8828 -4.5714 -5.2887 -6.0174 -6.7494 -7.4793 -8.1966 -8.8898 -9.5531 -10.194 -10.811 -11.405 -11.992 -12.579 -13.166 -13.751 -14.333 -14.902 -15.446 -15.961 -16.448 -16.905 -17.327 -17.718 -18.072 -18.388 -18.669 -18.923 -19.154 -19.361 -19.545 -19.703 -19.834 -19.938 -20.02 -20.083 -20.128 -20.151 -20.152 -20.132 -20.094 -20.038 -19.966 -19.875 -19.76 -19.615 -19.439 -19.227 -18.983 -18.694 -18.351 -17.955 -17.509 -17.012 -16.47 -15.893 -15.291 -14.669 -14.037 -13.404 -12.778 -12.158 -11.543 -10.936 -10.343 -9.759 -9.1859 -8.625 -8.0684 -7.5124 -6.9582 -6.4079 -5.8679 -5.3441 -4.8406 -4.3613 -3.911 -3.4934 -3.1201 -2.7982 -2.5248 -2.2951 -2.1018 -1.9346 -1.788 -1.6617 -1.5525 -1.4568 -1.3703 -1.2912 -1.2185 -1.1545 -1.1006 -1.0527 -1.0058 -0.95831 -0.9064 -0.84465 -0.77568 -0.70328 -0.63051 -0.55919 -0.49453 -0.44014 -0.39476 -0.35542 -0.32422 -0.30307 -0.28854 -0.2797 -0.27655 -0.27652 -0.27652 -0.27646 -0.27607 -0.27483 -0.272 -0.26665 -0.25773 -0.24438 -0.22764 -0.20681 -0.18007 -0.14622 -0.10797 -0.060823 0
diff --git a/data/hsmm/HSMM_test2_trans.txt b/data/hsmm/HSMM_test2_trans.txt
new file mode 100644
index 0000000000000000000000000000000000000000..4d40147087ee1678a9fab9cc42da755d113a8e9b
--- /dev/null
+++ b/data/hsmm/HSMM_test2_trans.txt
@@ -0,0 +1,5 @@
+1.0807e-306 1 0 0 0
+0 8.8082e-307 1 0 0
+0 0 8.7071e-307 1 0
+0 0 0 7.7463e-307 1
+0 0 0 0 1
diff --git a/data/hsmm/HSMM_test2_varnames.txt b/data/hsmm/HSMM_test2_varnames.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7d220dfaecee7c459dab99594033b6fdf4d83979
--- /dev/null
+++ b/data/hsmm/HSMM_test2_varnames.txt
@@ -0,0 +1 @@
+x1 x2
\ No newline at end of file
diff --git a/data/hsmm/HSMM_test_durMu.txt b/data/hsmm/HSMM_test_durMu.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a3579fb62119aad84f16ebe5c9e0a7d31dc1e319
--- /dev/null
+++ b/data/hsmm/HSMM_test_durMu.txt
@@ -0,0 +1 @@
+32.5 34 33.75
diff --git a/data/hsmm/HSMM_test_durSigma.txt b/data/hsmm/HSMM_test_durSigma.txt
new file mode 100644
index 0000000000000000000000000000000000000000..2bd29ebc9006c32469db047bb5bab022a96df13f
--- /dev/null
+++ b/data/hsmm/HSMM_test_durSigma.txt
@@ -0,0 +1 @@
+1.3333 3 4.5833
diff --git a/data/hsmm/HSMM_test_mu.txt b/data/hsmm/HSMM_test_mu.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8b2fc22f387a772f2d94e527f9e25842382c66f1
--- /dev/null
+++ b/data/hsmm/HSMM_test_mu.txt
@@ -0,0 +1,4 @@
+-17.283 -24.539 -17.462
+40.324 18.767 -0.6093
+113.93 -104.75 112.44
+-10.878 -102.07 -5.8334
diff --git a/data/hsmm/HSMM_test_priors.txt b/data/hsmm/HSMM_test_priors.txt
new file mode 100644
index 0000000000000000000000000000000000000000..427172728db3b671b77957ff24c2ecfca154e0ae
--- /dev/null
+++ b/data/hsmm/HSMM_test_priors.txt
@@ -0,0 +1 @@
+1 0 0
diff --git a/data/hsmm/HSMM_test_sigma.txt b/data/hsmm/HSMM_test_sigma.txt
new file mode 100644
index 0000000000000000000000000000000000000000..251d99797217b5bd37459422fb35aa2e8c415b4b
--- /dev/null
+++ b/data/hsmm/HSMM_test_sigma.txt
@@ -0,0 +1,4 @@
+179.07 -2.9226 -316.98 -256.51 141.99 116.77 -384.41 20.704 158.3 2.0917 -101.99 206.92
+-2.9226 7.5428 58.59 17.033 116.77 107.01 -409.21 -2.0213 2.0917 0.77987 -21.862 -6.8527
+-316.98 58.59 5311.3 1290.9 -384.41 -409.21 2961.5 247.41 -101.99 -21.862 1623 358.92
+-256.51 17.033 1290.9 716.09 20.704 -2.0213 247.41 270.36 206.92 -6.8527 358.92 530.29
diff --git a/data/hsmm/HSMM_test_trans.txt b/data/hsmm/HSMM_test_trans.txt
new file mode 100644
index 0000000000000000000000000000000000000000..93b786e0f50e2589accdd0ded0050831798c5f82
--- /dev/null
+++ b/data/hsmm/HSMM_test_trans.txt
@@ -0,0 +1,3 @@
+3.231e-11 1 0
+0 3.3928e-11 1
+0 0 1
diff --git a/data/hsmm/HSMM_test_varnames.txt b/data/hsmm/HSMM_test_varnames.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b7668bd809ee215aa59059a2d7ce26895c1419c1
--- /dev/null
+++ b/data/hsmm/HSMM_test_varnames.txt
@@ -0,0 +1 @@
+x1 x2 x1d x2d
diff --git a/data/pgmm/Data01.txt b/data/pgmm/Data01.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/Data02.txt b/data/pgmm/Data02.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/Data03.txt b/data/pgmm/Data03.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/Data04.txt b/data/pgmm/Data04.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/Mu02.txt b/data/pgmm/Mu02.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/Param01.txt b/data/pgmm/Param01.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/Param02.txt b/data/pgmm/Param02.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/Param03.txt b/data/pgmm/Param03.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/Param04.txt b/data/pgmm/Param04.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/ParamRepro01.txt b/data/pgmm/ParamRepro01.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/Priors02.txt b/data/pgmm/Priors02.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/Sigma02.txt b/data/pgmm/Sigma02.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/quaternionModel/Zmu_quaternions_P1.txt b/data/pgmm/quaternionModel/Zmu_quaternions_P1.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6bab7d7024801509a2be00c417799783b0ddee11
--- /dev/null
+++ b/data/pgmm/quaternionModel/Zmu_quaternions_P1.txt
@@ -0,0 +1,5 @@
+ 1.0826759e+00 2.1437427e+00 3.9976223e+00 5.7188339e+00 7.1467884e+00
+ 6.1258978e-01 3.1929807e-01 1.3637907e-01 4.5738128e-01 6.5068268e-01
+ -2.3925204e-01 -3.0859294e-01 -3.3499393e-01 -2.9172409e-01 -2.2733169e-01
+ 6.7494557e-01 8.7489337e-01 9.2412980e-01 7.8708710e-01 6.3979579e-01
+ -3.1945393e-01 -1.4603353e-01 -9.1636504e-02 -2.6502574e-01 -3.3705515e-01
diff --git a/data/pgmm/quaternionModel/Zsigma_quaternions_P1.txt b/data/pgmm/quaternionModel/Zsigma_quaternions_P1.txt
new file mode 100644
index 0000000000000000000000000000000000000000..2ea4b82526a59fe981458574e5037dc45ae3e2ab
--- /dev/null
+++ b/data/pgmm/quaternionModel/Zsigma_quaternions_P1.txt
@@ -0,0 +1,5 @@
+ 6.3201050e-01 -3.7873626e-02 -6.3948641e-03 3.6608104e-02 2.2128139e-02 1.7787496e-01 -3.5858157e-02 -6.0805784e-03 1.4818931e-02 2.0703031e-02 6.9222069e-01 2.2089937e-02 2.3709702e-03 -3.0567595e-03 -3.8854456e-03 5.7862043e-01 4.5391469e-02 2.1821251e-02 -2.0170213e-02 -8.5663402e-03 5.9255670e-01 6.5784883e-03 5.1697769e-03 -5.5991678e-03 -1.6965199e-03
+ -3.7873626e-02 4.2449302e-03 1.0215201e-03 -3.9352402e-03 -2.4510863e-03 -3.5858157e-02 1.0450125e-02 1.8462050e-03 -4.0681531e-03 -4.6523603e-03 2.2089937e-02 5.3031192e-03 1.1638228e-03 -7.0486665e-04 -1.2759996e-03 4.5391469e-02 9.3477583e-03 2.8737818e-03 -5.3008628e-03 -3.6966357e-03 6.5784883e-03 5.1507467e-04 1.1798466e-04 -5.3104105e-04 -3.1191709e-04
+ -6.3948641e-03 1.0215201e-03 5.5240803e-04 -9.5902452e-04 -6.0011135e-04 -6.0805784e-03 1.8462050e-03 1.1893017e-03 -3.8684316e-04 -4.7018039e-04 2.3709702e-03 1.1638228e-03 8.4473060e-04 3.5447366e-05 -3.4462696e-04 2.1821251e-02 2.8737818e-03 1.5962858e-03 -1.3382453e-03 -8.9974853e-04 5.1697769e-03 1.1798466e-04 2.8063637e-04 -2.1592306e-05 4.5694961e-05
+ 3.6608104e-02 -3.9352402e-03 -9.5902452e-04 3.8968833e-03 2.3598633e-03 1.4818931e-02 -4.0681531e-03 -3.8684316e-04 1.9438925e-03 2.1351385e-03 -3.0567595e-03 -7.0486665e-04 3.5447366e-05 2.9480737e-04 2.0555379e-04 -2.0170213e-02 -5.3008628e-03 -1.3382453e-03 3.3745201e-03 2.3032650e-03 -5.5991678e-03 -5.3104105e-04 -2.1592306e-05 9.5805629e-04 6.2314983e-04
+ 2.2128139e-02 -2.4510863e-03 -6.0011135e-04 2.3598633e-03 1.5956654e-03 2.0703031e-02 -4.6523603e-03 -4.7018039e-04 2.1351385e-03 2.8716330e-03 -3.8854456e-03 -1.2759996e-03 -3.4462696e-04 2.0555379e-04 7.2401520e-04 -8.5663402e-03 -3.6966357e-03 -8.9974853e-04 2.3032650e-03 2.0361046e-03 -1.6965199e-03 -3.1191709e-04 4.5694961e-05 6.2314983e-04 6.3374338e-04
diff --git a/data/pgmm/quaternionModel/priors_quaternions.txt b/data/pgmm/quaternionModel/priors_quaternions.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9aa25ba1f8c1452a174caf671ad9ebf9ff2f3faf
--- /dev/null
+++ b/data/pgmm/quaternionModel/priors_quaternions.txt
@@ -0,0 +1 @@
+ 2.3681362e-01 1.1904783e-01 2.7637504e-01 1.6674981e-01 2.0101370e-01
diff --git a/data/pgmm/quaternionModel/tpgmm_quaternions.txt b/data/pgmm/quaternionModel/tpgmm_quaternions.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6500570f7a1931bcb8a78696ab5214187cba6940
--- /dev/null
+++ b/data/pgmm/quaternionModel/tpgmm_quaternions.txt
@@ -0,0 +1,3 @@
+ 5.0000000e+00
+ 1.0000000e+00
+ 5.0000000e+00
diff --git a/data/pgmm/quaternionModel/vars.txt b/data/pgmm/quaternionModel/vars.txt
new file mode 100755
index 0000000000000000000000000000000000000000..606cc0cc92e94df994e4a22a866213f89cdd6a4a
--- /dev/null
+++ b/data/pgmm/quaternionModel/vars.txt
@@ -0,0 +1 @@
+t q0 q1 q2 q3
\ No newline at end of file
diff --git a/data/pgmm/quaternionModel/vars_quaternions.txt b/data/pgmm/quaternionModel/vars_quaternions.txt
new file mode 100755
index 0000000000000000000000000000000000000000..606cc0cc92e94df994e4a22a866213f89cdd6a4a
--- /dev/null
+++ b/data/pgmm/quaternionModel/vars_quaternions.txt
@@ -0,0 +1 @@
+t q0 q1 q2 q3
\ No newline at end of file
diff --git a/data/pgmm/timeInvModel/Zmu_timeInv.txt b/data/pgmm/timeInvModel/Zmu_timeInv.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/timeInvModel/Zmu_timeInv_P1.txt b/data/pgmm/timeInvModel/Zmu_timeInv_P1.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/timeInvModel/Zmu_timeInv_P2.txt b/data/pgmm/timeInvModel/Zmu_timeInv_P2.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/timeInvModel/Zsigma_timeInv.txt b/data/pgmm/timeInvModel/Zsigma_timeInv.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/timeInvModel/Zsigma_timeInv_P1.txt b/data/pgmm/timeInvModel/Zsigma_timeInv_P1.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/timeInvModel/Zsigma_timeInv_P2.txt b/data/pgmm/timeInvModel/Zsigma_timeInv_P2.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/timeInvModel/pgmm_timeInv.txt b/data/pgmm/timeInvModel/pgmm_timeInv.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/timeInvModel/priors_timeInv.txt b/data/pgmm/timeInvModel/priors_timeInv.txt
old mode 100644
new mode 100755
diff --git a/data/pgmm/timeInvModel/vars_timeInv.txt b/data/pgmm/timeInvModel/vars_timeInv.txt
old mode 100644
new mode 100755
diff --git a/data/trajgmm/TrajGMM_posData_Sshape.txt b/data/trajgmm/TrajGMM_posData_Sshape.txt
new file mode 100644
index 0000000000000000000000000000000000000000..43c9ccbde551e2a64510aae8818e23af1055382c
--- /dev/null
+++ b/data/trajgmm/TrajGMM_posData_Sshape.txt
@@ -0,0 +1,2 @@
+ 3.6715065e+01 3.6698180e+01 3.6654433e+01 3.6555972e+01 3.6304132e+01 3.5811352e+01 3.5046292e+01 3.4003224e+01 3.2726588e+01 3.1314475e+01 2.9850986e+01 2.8373505e+01 2.6918949e+01 2.5485001e+01 2.4046275e+01 2.2567617e+01 2.1046567e+01 1.9477440e+01 1.7836034e+01 1.6122311e+01 1.4380153e+01 1.2591647e+01 1.0754291e+01 8.8913439e+00 7.0631197e+00 5.2530874e+00 3.5170695e+00 1.8755615e+00 3.7090234e-01 -9.5065443e-01 -2.0014239e+00 -2.7621772e+00 -3.1950223e+00 -3.3052742e+00 -3.1345822e+00 -2.7472677e+00 -2.1505566e+00 -1.3664161e+00 -4.0788274e-01 7.0098565e-01 1.9738930e+00 3.4024593e+00 4.9935144e+00 6.7370522e+00 8.6508323e+00 1.0677479e+01 1.2784906e+01 1.4914354e+01 1.7056209e+01 1.9167473e+01 2.1243477e+01 2.3297906e+01 2.5305533e+01 2.7230477e+01 2.9025378e+01 3.0682004e+01 3.2127729e+01 3.3359093e+01 3.4379777e+01 3.5223574e+01 3.5899907e+01 3.6444796e+01 3.6893044e+01 3.7246264e+01 3.7463281e+01 3.7509889e+01 3.7369326e+01 3.7040898e+01 3.6551347e+01 3.5905935e+01 3.5124513e+01 3.4240302e+01 3.3289642e+01 3.2262604e+01 3.1191948e+01 3.0094861e+01 2.8946130e+01 2.7703782e+01 2.6358519e+01 2.4930973e+01 2.3480222e+01 2.2022908e+01 2.0570137e+01 1.9134711e+01 1.7717283e+01 1.6187765e+01 1.4520343e+01 1.2672124e+01 1.0684076e+01 8.5720367e+00 6.4664650e+00 4.4572289e+00 2.6883469e+00 1.2939679e+00 3.0350821e-01 -2.9156208e-01 -5.3113315e-01 -5.0993052e-01 -3.4063672e-01 0.0000000e+00 3.6715065e+01 3.6903972e+01 3.6827896e+01 3.6562741e+01 3.6083529e+01 3.5308912e+01 3.4246885e+01 3.2955882e+01 3.1496998e+01 2.9930895e+01 2.8277207e+01 2.6549507e+01 2.4762543e+01 2.2882329e+01 2.0889005e+01 1.8867149e+01 1.6865661e+01 1.4873714e+01 1.2917525e+01 1.1062785e+01 9.2610823e+00 7.4997657e+00 5.7711996e+00 4.1230961e+00 2.5721559e+00 1.1646282e+00 -9.4230400e-02 -1.1307389e+00 -1.9115919e+00 -2.4304088e+00 -2.7071878e+00 -2.7386478e+00 -2.5494263e+00 -2.1417319e+00 -1.4886238e+00 -5.6097807e-01 6.3908436e-01 2.0894534e+00 3.7866704e+00 5.7252716e+00 7.8610206e+00 1.0039924e+01 1.2133808e+01 1.4086914e+01 1.5897252e+01 1.7564226e+01 1.9222185e+01 2.0952984e+01 2.2678774e+01 2.4343835e+01 2.6021148e+01 2.7705174e+01 2.9348194e+01 3.0990672e+01 3.2592802e+01 3.4055973e+01 3.5267277e+01 3.6238486e+01 3.6965996e+01 3.7509968e+01 3.7881541e+01 3.8144184e+01 3.8313955e+01 3.8369354e+01 3.8283647e+01 3.8054580e+01 3.7669679e+01 3.7137962e+01 3.6469393e+01 3.5628422e+01 3.4605394e+01 3.3435078e+01 3.2134436e+01 3.0702071e+01 2.9155761e+01 2.7526914e+01 2.5875045e+01 2.4214904e+01 2.2495337e+01 2.0652936e+01 1.8710918e+01 1.6762201e+01 1.4905196e+01 1.3280915e+01 1.1955447e+01 1.0873601e+01 9.9176940e+00 9.0029439e+00 8.0757154e+00 7.0902469e+00 6.0074897e+00 4.7597672e+00 3.4329709e+00 2.1584091e+00 1.0939618e+00 3.4169401e-01 -6.3558232e-02 -2.0954227e-01 -1.8306636e-01 0.0000000e+00 3.4555356e+01 3.4752410e+01 3.4771036e+01 3.4614055e+01 3.4207218e+01 3.3503157e+01 3.2466647e+01 3.1085264e+01 2.9449410e+01 2.7668103e+01 2.5707248e+01 2.3594865e+01 2.1398836e+01 1.9176297e+01 1.6941304e+01 1.4719024e+01 1.2505787e+01 1.0273301e+01 8.0104096e+00 5.7945109e+00 3.7017806e+00 1.7587052e+00 -2.9559924e-02 -1.6680467e+00 -3.1475942e+00 -4.4173962e+00 -5.4080349e+00 -6.1031768e+00 -6.5094705e+00 -6.6576789e+00 -6.5786739e+00 -6.2899994e+00 -5.7807221e+00 -5.0053769e+00 -3.9805887e+00 -2.7016962e+00 -1.2195771e+00 4.5049444e-01 2.2685323e+00 4.2405504e+00 6.3523074e+00 8.5472523e+00 1.0773535e+01 1.2959805e+01 1.5090058e+01 1.7143269e+01 1.9216130e+01 2.1307772e+01 2.3311627e+01 2.5181634e+01 2.6887384e+01 2.8419490e+01 2.9738706e+01 3.0947883e+01 3.2085804e+01 3.3165700e+01 3.4158583e+01 3.5107376e+01 3.5991580e+01 3.6743988e+01 3.7324934e+01 3.7759900e+01 3.8053747e+01 3.8237748e+01 3.8340862e+01 3.8368760e+01 3.8268888e+01 3.8018467e+01 3.7609689e+01 3.7054398e+01 3.6362020e+01 3.5610243e+01 3.4806285e+01 3.3924822e+01 3.2945779e+01 3.1862469e+01 3.0607105e+01 2.9168297e+01 2.7574801e+01 2.5901908e+01 2.4141359e+01 2.2315833e+01 2.0477287e+01 1.8580400e+01 1.6606426e+01 1.4610033e+01 1.2658001e+01 1.0787923e+01 9.0497280e+00 7.4402830e+00 5.9678547e+00 4.6328196e+00 3.4328760e+00 2.4259431e+00 1.6206616e+00 9.9046701e-01 5.3310970e-01 2.2060530e-01 3.3815408e-02 0.0000000e+00 3.4915307e+01 3.4830371e+01 3.4595069e+01 3.4115605e+01 3.3309745e+01 3.2164473e+01 3.0703322e+01 2.8967123e+01 2.7051416e+01 2.4975366e+01 2.2828294e+01 2.0678996e+01 1.8511852e+01 1.6313816e+01 1.4222231e+01 1.2149745e+01 9.9571333e+00 7.7221002e+00 5.6093221e+00 3.6725731e+00 1.9330532e+00 4.5808646e-01 -7.7483309e-01 -1.7749031e+00 -2.6372041e+00 -3.3210227e+00 -3.8008343e+00 -4.0915329e+00 -4.2934035e+00 -4.4411458e+00 -4.4680773e+00 -4.2333420e+00 -3.6855799e+00 -2.8496387e+00 -1.7002629e+00 -3.2007622e-01 1.1220368e+00 2.5981597e+00 4.1310467e+00 5.6852883e+00 7.2230178e+00 8.8696487e+00 1.0677460e+01 1.2656167e+01 1.4781622e+01 1.7046396e+01 1.9287090e+01 2.1391599e+01 2.3414695e+01 2.5397529e+01 2.7296901e+01 2.9154444e+01 3.0958551e+01 3.2650804e+01 3.4122169e+01 3.5345870e+01 3.6333544e+01 3.7087919e+01 3.7644970e+01 3.8098612e+01 3.8519229e+01 3.8920198e+01 3.9283044e+01 3.9569378e+01 3.9767190e+01 3.9885885e+01 3.9907029e+01 3.9804575e+01 3.9527406e+01 3.9061337e+01 3.8401000e+01 3.7579875e+01 3.6572930e+01 3.5408243e+01 3.4123673e+01 3.2765475e+01 3.1266771e+01 2.9654942e+01 2.8065267e+01 2.6386321e+01 2.4550071e+01 2.2656458e+01 2.0727375e+01 1.8692358e+01 1.6683417e+01 1.4807842e+01 1.3024084e+01 1.1348137e+01 9.8611807e+00 8.4983355e+00 7.2218512e+00 6.0034679e+00 4.8696140e+00 3.7675787e+00 2.7323863e+00 1.8230125e+00 1.1008859e+00 5.4398392e-01 1.6189351e-01 0.0000000e+00
+ 4.1034485e+01 4.1034485e+01 4.1034485e+01 4.1034485e+01 4.1034485e+01 4.1034485e+01 4.1034508e+01 4.1047863e+01 4.1090950e+01 4.1170986e+01 4.1284802e+01 4.1426301e+01 4.1557062e+01 4.1657866e+01 4.1720253e+01 4.1750379e+01 4.1754388e+01 4.1754388e+01 4.1754388e+01 4.1754388e+01 4.1753735e+01 4.1729923e+01 4.1647723e+01 4.1457782e+01 4.1089644e+01 4.0487308e+01 3.9640350e+01 3.8526599e+01 3.7155032e+01 3.5604429e+01 3.3972072e+01 3.2326253e+01 3.0734233e+01 2.9266995e+01 2.7920213e+01 2.6672995e+01 2.5504972e+01 2.4457682e+01 2.3514210e+01 2.2685611e+01 2.1981358e+01 2.1411648e+01 2.0947509e+01 2.0569903e+01 2.0263890e+01 2.0009375e+01 1.9797329e+01 1.9633773e+01 1.9523242e+01 1.9438556e+01 1.9358941e+01 1.9276101e+01 1.9141174e+01 1.8918759e+01 1.8600981e+01 1.8171239e+01 1.7607322e+01 1.6945335e+01 1.6215980e+01 1.5432433e+01 1.4588902e+01 1.3664115e+01 1.2673082e+01 1.1634966e+01 1.0534295e+01 9.3923223e+00 8.2658815e+00 7.1571930e+00 6.0807464e+00 5.0962369e+00 4.2358386e+00 3.4898832e+00 2.8525672e+00 2.3041273e+00 1.8501771e+00 1.4693127e+00 1.1108545e+00 7.7473248e-01 4.9774510e-01 2.6735290e-01 6.5352281e-02 -8.2087902e-02 -1.7564954e-01 -2.4917424e-01 -2.9357202e-01 -2.6943502e-01 -1.6570780e-01 -2.3979521e-02 1.2219724e-01 2.4544974e-01 3.2492417e-01 3.5595775e-01 3.5992266e-01 3.5860827e-01 3.5106899e-01 3.2914213e-01 2.8684887e-01 2.2239219e-01 1.2990600e-01 0.0000000e+00 4.1394436e+01 4.1821455e+01 4.2014579e+01 4.2150392e+01 4.2306835e+01 4.2458359e+01 4.2611201e+01 4.2772258e+01 4.2910991e+01 4.3008048e+01 4.3083194e+01 4.3149058e+01 4.3186045e+01 4.3194195e+01 4.3194194e+01 4.3194193e+01 4.3183612e+01 4.3119848e+01 4.2980379e+01 4.2749427e+01 4.2409419e+01 4.1944919e+01 4.1369406e+01 4.0679697e+01 3.9865011e+01 3.8906505e+01 3.7767458e+01 3.6509339e+01 3.5151373e+01 3.3720691e+01 3.2271393e+01 3.0938095e+01 2.9686033e+01 2.8526706e+01 2.7470620e+01 2.6515865e+01 2.5621473e+01 2.4823869e+01 2.4155509e+01 2.3623106e+01 2.3216175e+01 2.2912496e+01 2.2673071e+01 2.2459802e+01 2.2241563e+01 2.2032674e+01 2.1819378e+01 2.1585919e+01 2.1326958e+01 2.1057318e+01 2.0735817e+01 2.0361592e+01 1.9939402e+01 1.9452664e+01 1.8871469e+01 1.8202535e+01 1.7443056e+01 1.6597156e+01 1.5703961e+01 1.4800949e+01 1.3894942e+01 1.3014911e+01 1.2197343e+01 1.1444258e+01 1.0719442e+01 1.0000769e+01 9.2687822e+00 8.5173176e+00 7.7430212e+00 6.9597575e+00 6.2197888e+00 5.5360469e+00 4.9051358e+00 4.3414867e+00 3.8653090e+00 3.4474627e+00 3.0675276e+00 2.7257224e+00 2.4133159e+00 2.1435317e+00 1.9162507e+00 1.7355603e+00 1.6082592e+00 1.4966817e+00 1.3578005e+00 1.1962554e+00 1.0452517e+00 8.8808832e-01 7.3422995e-01 6.1081950e-01 5.2891459e-01 4.5464677e-01 3.8457378e-01 3.1661916e-01 2.4758852e-01 1.7355186e-01 9.6727756e-02 3.4493704e-02 6.5055683e-04 0.0000000e+00 4.4993953e+01 4.5398725e+01 4.5582601e+01 4.5668582e+01 4.5742355e+01 4.5811313e+01 4.5885385e+01 4.5959512e+01 4.6025902e+01 4.6064219e+01 4.6073798e+01 4.6073807e+01 4.6073552e+01 4.6055591e+01 4.5971909e+01 4.5803078e+01 4.5542928e+01 4.5186205e+01 4.4706703e+01 4.4133817e+01 4.3435769e+01 4.2581375e+01 4.1540215e+01 4.0320955e+01 3.8948201e+01 3.7442891e+01 3.5862038e+01 3.4277417e+01 3.2775137e+01 3.1383218e+01 3.0148164e+01 2.9053262e+01 2.8045721e+01 2.7104087e+01 2.6262120e+01 2.5510754e+01 2.4868423e+01 2.4331209e+01 2.3887105e+01 2.3493018e+01 2.3146290e+01 2.2851524e+01 2.2610297e+01 2.2409945e+01 2.2213198e+01 2.2012731e+01 2.1799748e+01 2.1571751e+01 2.1302043e+01 2.0996117e+01 2.0661114e+01 2.0276085e+01 1.9840044e+01 1.9387857e+01 1.8933045e+01 1.8472620e+01 1.7975036e+01 1.7379074e+01 1.6672280e+01 1.5865648e+01 1.4943403e+01 1.3974472e+01 1.3099056e+01 1.2330542e+01 1.1650850e+01 1.1074912e+01 1.0524010e+01 9.8816633e+00 9.1530707e+00 8.3520904e+00 7.4978478e+00 6.6910671e+00 5.9914799e+00 5.3715486e+00 4.8336934e+00 4.3830703e+00 3.9977351e+00 3.6592327e+00 3.3857450e+00 3.1570578e+00 2.9453922e+00 2.7249393e+00 2.5301891e+00 2.3608655e+00 2.2042617e+00 2.0549181e+00 1.9070521e+00 1.7267152e+00 1.5258113e+00 1.3427035e+00 1.2070189e+00 1.0981924e+00 1.0063243e+00 9.2423829e-01 8.4951002e-01 7.6685134e-01 6.7229520e-01 5.6511495e-01 3.7682130e-01 0.0000000e+00 4.8593469e+01 4.8593469e+01 4.8593469e+01 4.8593469e+01 4.8593469e+01 4.8593469e+01 4.8593469e+01 4.8593469e+01 4.8593221e+01 4.8578661e+01 4.8531865e+01 4.8432172e+01 4.8268145e+01 4.8019821e+01 4.7689120e+01 4.7234270e+01 4.6641286e+01 4.5904612e+01 4.5054639e+01 4.4122900e+01 4.3122722e+01 4.2059856e+01 4.0869328e+01 3.9572584e+01 3.8197859e+01 3.6873681e+01 3.5661536e+01 3.4626927e+01 3.3704468e+01 3.2770874e+01 3.1641189e+01 3.0287293e+01 2.8821391e+01 2.7427686e+01 2.6217654e+01 2.5309499e+01 2.4671530e+01 2.4211591e+01 2.3824640e+01 2.3482305e+01 2.3199300e+01 2.2974370e+01 2.2785896e+01 2.2598934e+01 2.2421034e+01 2.2225240e+01 2.2024867e+01 2.1785303e+01 2.1497087e+01 2.1133732e+01 2.0721945e+01 2.0253322e+01 1.9725574e+01 1.9121873e+01 1.8460914e+01 1.7749310e+01 1.6998647e+01 1.6263959e+01 1.5553651e+01 1.4860046e+01 1.4173570e+01 1.3451156e+01 1.2660664e+01 1.1814216e+01 1.0899141e+01 9.9321297e+00 8.9578942e+00 8.0180608e+00 7.1101689e+00 6.2777584e+00 5.5143605e+00 4.8223547e+00 4.2030583e+00 3.6725072e+00 3.2347176e+00 2.8923909e+00 2.6214606e+00 2.3971390e+00 2.2160131e+00 2.0558757e+00 1.8941541e+00 1.7404784e+00 1.6261378e+00 1.5275892e+00 1.4209143e+00 1.3138944e+00 1.2061371e+00 1.0753528e+00 9.4637675e-01 8.4673338e-01 7.7157992e-01 6.9775615e-01 6.2341039e-01 5.4902187e-01 4.7245270e-01 3.9218895e-01 3.1973082e-01 2.5357625e-01 1.5835146e-01 0.0000000e+00
diff --git a/data/trajgmm/TrajGMM_posData_Sshape1.txt b/data/trajgmm/TrajGMM_posData_Sshape1.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1408fee8edeea14f5beb6702486b19f8348a76f8
--- /dev/null
+++ b/data/trajgmm/TrajGMM_posData_Sshape1.txt
@@ -0,0 +1,2 @@
+ 3.6715065e+01 3.6698180e+01 3.6654433e+01 3.6555972e+01 3.6304132e+01 3.5811352e+01 3.5046292e+01 3.4003224e+01 3.2726588e+01 3.1314475e+01 2.9850986e+01 2.8373505e+01 2.6918949e+01 2.5485001e+01 2.4046275e+01 2.2567617e+01 2.1046567e+01 1.9477440e+01 1.7836034e+01 1.6122311e+01 1.4380153e+01 1.2591647e+01 1.0754291e+01 8.8913439e+00 7.0631197e+00 5.2530874e+00 3.5170695e+00 1.8755615e+00 3.7090234e-01 -9.5065443e-01 -2.0014239e+00 -2.7621772e+00 -3.1950223e+00 -3.3052742e+00 -3.1345822e+00 -2.7472677e+00 -2.1505566e+00 -1.3664161e+00 -4.0788274e-01 7.0098565e-01 1.9738930e+00 3.4024593e+00 4.9935144e+00 6.7370522e+00 8.6508323e+00 1.0677479e+01 1.2784906e+01 1.4914354e+01 1.7056209e+01 1.9167473e+01 2.1243477e+01 2.3297906e+01 2.5305533e+01 2.7230477e+01 2.9025378e+01 3.0682004e+01 3.2127729e+01 3.3359093e+01 3.4379777e+01 3.5223574e+01 3.5899907e+01 3.6444796e+01 3.6893044e+01 3.7246264e+01 3.7463281e+01 3.7509889e+01 3.7369326e+01 3.7040898e+01 3.6551347e+01 3.5905935e+01 3.5124513e+01 3.4240302e+01 3.3289642e+01 3.2262604e+01 3.1191948e+01 3.0094861e+01 2.8946130e+01 2.7703782e+01 2.6358519e+01 2.4930973e+01 2.3480222e+01 2.2022908e+01 2.0570137e+01 1.9134711e+01 1.7717283e+01 1.6187765e+01 1.4520343e+01 1.2672124e+01 1.0684076e+01 8.5720367e+00 6.4664650e+00 4.4572289e+00 2.6883469e+00 1.2939679e+00 3.0350821e-01 -2.9156208e-01 -5.3113315e-01 -5.0993052e-01 -3.4063672e-01 0.0000000e+00
+ 4.1034485e+01 4.1034485e+01 4.1034485e+01 4.1034485e+01 4.1034485e+01 4.1034485e+01 4.1034508e+01 4.1047863e+01 4.1090950e+01 4.1170986e+01 4.1284802e+01 4.1426301e+01 4.1557062e+01 4.1657866e+01 4.1720253e+01 4.1750379e+01 4.1754388e+01 4.1754388e+01 4.1754388e+01 4.1754388e+01 4.1753735e+01 4.1729923e+01 4.1647723e+01 4.1457782e+01 4.1089644e+01 4.0487308e+01 3.9640350e+01 3.8526599e+01 3.7155032e+01 3.5604429e+01 3.3972072e+01 3.2326253e+01 3.0734233e+01 2.9266995e+01 2.7920213e+01 2.6672995e+01 2.5504972e+01 2.4457682e+01 2.3514210e+01 2.2685611e+01 2.1981358e+01 2.1411648e+01 2.0947509e+01 2.0569903e+01 2.0263890e+01 2.0009375e+01 1.9797329e+01 1.9633773e+01 1.9523242e+01 1.9438556e+01 1.9358941e+01 1.9276101e+01 1.9141174e+01 1.8918759e+01 1.8600981e+01 1.8171239e+01 1.7607322e+01 1.6945335e+01 1.6215980e+01 1.5432433e+01 1.4588902e+01 1.3664115e+01 1.2673082e+01 1.1634966e+01 1.0534295e+01 9.3923223e+00 8.2658815e+00 7.1571930e+00 6.0807464e+00 5.0962369e+00 4.2358386e+00 3.4898832e+00 2.8525672e+00 2.3041273e+00 1.8501771e+00 1.4693127e+00 1.1108545e+00 7.7473248e-01 4.9774510e-01 2.6735290e-01 6.5352281e-02 -8.2087902e-02 -1.7564954e-01 -2.4917424e-01 -2.9357202e-01 -2.6943502e-01 -1.6570780e-01 -2.3979521e-02 1.2219724e-01 2.4544974e-01 3.2492417e-01 3.5595775e-01 3.5992266e-01 3.5860827e-01 3.5106899e-01 3.2914213e-01 2.8684887e-01 2.2239219e-01 1.2990600e-01 0.0000000e+00
diff --git a/data/trajgmm/TrajGMM_posData_Sshape2.txt b/data/trajgmm/TrajGMM_posData_Sshape2.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f001e9794d1b6a51ec3557b2d6d94e09035a6176
--- /dev/null
+++ b/data/trajgmm/TrajGMM_posData_Sshape2.txt
@@ -0,0 +1,2 @@
+ 3.6715065e+01 3.6903972e+01 3.6827896e+01 3.6562741e+01 3.6083529e+01 3.5308912e+01 3.4246885e+01 3.2955882e+01 3.1496998e+01 2.9930895e+01 2.8277207e+01 2.6549507e+01 2.4762543e+01 2.2882329e+01 2.0889005e+01 1.8867149e+01 1.6865661e+01 1.4873714e+01 1.2917525e+01 1.1062785e+01 9.2610823e+00 7.4997657e+00 5.7711996e+00 4.1230961e+00 2.5721559e+00 1.1646282e+00 -9.4230400e-02 -1.1307389e+00 -1.9115919e+00 -2.4304088e+00 -2.7071878e+00 -2.7386478e+00 -2.5494263e+00 -2.1417319e+00 -1.4886238e+00 -5.6097807e-01 6.3908436e-01 2.0894534e+00 3.7866704e+00 5.7252716e+00 7.8610206e+00 1.0039924e+01 1.2133808e+01 1.4086914e+01 1.5897252e+01 1.7564226e+01 1.9222185e+01 2.0952984e+01 2.2678774e+01 2.4343835e+01 2.6021148e+01 2.7705174e+01 2.9348194e+01 3.0990672e+01 3.2592802e+01 3.4055973e+01 3.5267277e+01 3.6238486e+01 3.6965996e+01 3.7509968e+01 3.7881541e+01 3.8144184e+01 3.8313955e+01 3.8369354e+01 3.8283647e+01 3.8054580e+01 3.7669679e+01 3.7137962e+01 3.6469393e+01 3.5628422e+01 3.4605394e+01 3.3435078e+01 3.2134436e+01 3.0702071e+01 2.9155761e+01 2.7526914e+01 2.5875045e+01 2.4214904e+01 2.2495337e+01 2.0652936e+01 1.8710918e+01 1.6762201e+01 1.4905196e+01 1.3280915e+01 1.1955447e+01 1.0873601e+01 9.9176940e+00 9.0029439e+00 8.0757154e+00 7.0902469e+00 6.0074897e+00 4.7597672e+00 3.4329709e+00 2.1584091e+00 1.0939618e+00 3.4169401e-01 -6.3558232e-02 -2.0954227e-01 -1.8306636e-01 0.0000000e+00
+ 4.1394436e+01 4.1821455e+01 4.2014579e+01 4.2150392e+01 4.2306835e+01 4.2458359e+01 4.2611201e+01 4.2772258e+01 4.2910991e+01 4.3008048e+01 4.3083194e+01 4.3149058e+01 4.3186045e+01 4.3194195e+01 4.3194194e+01 4.3194193e+01 4.3183612e+01 4.3119848e+01 4.2980379e+01 4.2749427e+01 4.2409419e+01 4.1944919e+01 4.1369406e+01 4.0679697e+01 3.9865011e+01 3.8906505e+01 3.7767458e+01 3.6509339e+01 3.5151373e+01 3.3720691e+01 3.2271393e+01 3.0938095e+01 2.9686033e+01 2.8526706e+01 2.7470620e+01 2.6515865e+01 2.5621473e+01 2.4823869e+01 2.4155509e+01 2.3623106e+01 2.3216175e+01 2.2912496e+01 2.2673071e+01 2.2459802e+01 2.2241563e+01 2.2032674e+01 2.1819378e+01 2.1585919e+01 2.1326958e+01 2.1057318e+01 2.0735817e+01 2.0361592e+01 1.9939402e+01 1.9452664e+01 1.8871469e+01 1.8202535e+01 1.7443056e+01 1.6597156e+01 1.5703961e+01 1.4800949e+01 1.3894942e+01 1.3014911e+01 1.2197343e+01 1.1444258e+01 1.0719442e+01 1.0000769e+01 9.2687822e+00 8.5173176e+00 7.7430212e+00 6.9597575e+00 6.2197888e+00 5.5360469e+00 4.9051358e+00 4.3414867e+00 3.8653090e+00 3.4474627e+00 3.0675276e+00 2.7257224e+00 2.4133159e+00 2.1435317e+00 1.9162507e+00 1.7355603e+00 1.6082592e+00 1.4966817e+00 1.3578005e+00 1.1962554e+00 1.0452517e+00 8.8808832e-01 7.3422995e-01 6.1081950e-01 5.2891459e-01 4.5464677e-01 3.8457378e-01 3.1661916e-01 2.4758852e-01 1.7355186e-01 9.6727756e-02 3.4493704e-02 6.5055683e-04 0.0000000e+00
diff --git a/data/trajgmm/TrajGMM_posData_Sshape3.txt b/data/trajgmm/TrajGMM_posData_Sshape3.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7b985d894415956ef21d45595e0181b3a5518bba
--- /dev/null
+++ b/data/trajgmm/TrajGMM_posData_Sshape3.txt
@@ -0,0 +1,2 @@
+ 3.4555356e+01 3.4752410e+01 3.4771036e+01 3.4614055e+01 3.4207218e+01 3.3503157e+01 3.2466647e+01 3.1085264e+01 2.9449410e+01 2.7668103e+01 2.5707248e+01 2.3594865e+01 2.1398836e+01 1.9176297e+01 1.6941304e+01 1.4719024e+01 1.2505787e+01 1.0273301e+01 8.0104096e+00 5.7945109e+00 3.7017806e+00 1.7587052e+00 -2.9559924e-02 -1.6680467e+00 -3.1475942e+00 -4.4173962e+00 -5.4080349e+00 -6.1031768e+00 -6.5094705e+00 -6.6576789e+00 -6.5786739e+00 -6.2899994e+00 -5.7807221e+00 -5.0053769e+00 -3.9805887e+00 -2.7016962e+00 -1.2195771e+00 4.5049444e-01 2.2685323e+00 4.2405504e+00 6.3523074e+00 8.5472523e+00 1.0773535e+01 1.2959805e+01 1.5090058e+01 1.7143269e+01 1.9216130e+01 2.1307772e+01 2.3311627e+01 2.5181634e+01 2.6887384e+01 2.8419490e+01 2.9738706e+01 3.0947883e+01 3.2085804e+01 3.3165700e+01 3.4158583e+01 3.5107376e+01 3.5991580e+01 3.6743988e+01 3.7324934e+01 3.7759900e+01 3.8053747e+01 3.8237748e+01 3.8340862e+01 3.8368760e+01 3.8268888e+01 3.8018467e+01 3.7609689e+01 3.7054398e+01 3.6362020e+01 3.5610243e+01 3.4806285e+01 3.3924822e+01 3.2945779e+01 3.1862469e+01 3.0607105e+01 2.9168297e+01 2.7574801e+01 2.5901908e+01 2.4141359e+01 2.2315833e+01 2.0477287e+01 1.8580400e+01 1.6606426e+01 1.4610033e+01 1.2658001e+01 1.0787923e+01 9.0497280e+00 7.4402830e+00 5.9678547e+00 4.6328196e+00 3.4328760e+00 2.4259431e+00 1.6206616e+00 9.9046701e-01 5.3310970e-01 2.2060530e-01 3.3815408e-02 0.0000000e+00
+ 4.4993953e+01 4.5398725e+01 4.5582601e+01 4.5668582e+01 4.5742355e+01 4.5811313e+01 4.5885385e+01 4.5959512e+01 4.6025902e+01 4.6064219e+01 4.6073798e+01 4.6073807e+01 4.6073552e+01 4.6055591e+01 4.5971909e+01 4.5803078e+01 4.5542928e+01 4.5186205e+01 4.4706703e+01 4.4133817e+01 4.3435769e+01 4.2581375e+01 4.1540215e+01 4.0320955e+01 3.8948201e+01 3.7442891e+01 3.5862038e+01 3.4277417e+01 3.2775137e+01 3.1383218e+01 3.0148164e+01 2.9053262e+01 2.8045721e+01 2.7104087e+01 2.6262120e+01 2.5510754e+01 2.4868423e+01 2.4331209e+01 2.3887105e+01 2.3493018e+01 2.3146290e+01 2.2851524e+01 2.2610297e+01 2.2409945e+01 2.2213198e+01 2.2012731e+01 2.1799748e+01 2.1571751e+01 2.1302043e+01 2.0996117e+01 2.0661114e+01 2.0276085e+01 1.9840044e+01 1.9387857e+01 1.8933045e+01 1.8472620e+01 1.7975036e+01 1.7379074e+01 1.6672280e+01 1.5865648e+01 1.4943403e+01 1.3974472e+01 1.3099056e+01 1.2330542e+01 1.1650850e+01 1.1074912e+01 1.0524010e+01 9.8816633e+00 9.1530707e+00 8.3520904e+00 7.4978478e+00 6.6910671e+00 5.9914799e+00 5.3715486e+00 4.8336934e+00 4.3830703e+00 3.9977351e+00 3.6592327e+00 3.3857450e+00 3.1570578e+00 2.9453922e+00 2.7249393e+00 2.5301891e+00 2.3608655e+00 2.2042617e+00 2.0549181e+00 1.9070521e+00 1.7267152e+00 1.5258113e+00 1.3427035e+00 1.2070189e+00 1.0981924e+00 1.0063243e+00 9.2423829e-01 8.4951002e-01 7.6685134e-01 6.7229520e-01 5.6511495e-01 3.7682130e-01 0.0000000e+00
diff --git a/data/trajgmm/TrajGMM_posData_Sshape4.txt b/data/trajgmm/TrajGMM_posData_Sshape4.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9e89acc6afcec24ba49143c1c267b109e8def823
--- /dev/null
+++ b/data/trajgmm/TrajGMM_posData_Sshape4.txt
@@ -0,0 +1,2 @@
+ 3.4915307e+01 3.4830371e+01 3.4595069e+01 3.4115605e+01 3.3309745e+01 3.2164473e+01 3.0703322e+01 2.8967123e+01 2.7051416e+01 2.4975366e+01 2.2828294e+01 2.0678996e+01 1.8511852e+01 1.6313816e+01 1.4222231e+01 1.2149745e+01 9.9571333e+00 7.7221002e+00 5.6093221e+00 3.6725731e+00 1.9330532e+00 4.5808646e-01 -7.7483309e-01 -1.7749031e+00 -2.6372041e+00 -3.3210227e+00 -3.8008343e+00 -4.0915329e+00 -4.2934035e+00 -4.4411458e+00 -4.4680773e+00 -4.2333420e+00 -3.6855799e+00 -2.8496387e+00 -1.7002629e+00 -3.2007622e-01 1.1220368e+00 2.5981597e+00 4.1310467e+00 5.6852883e+00 7.2230178e+00 8.8696487e+00 1.0677460e+01 1.2656167e+01 1.4781622e+01 1.7046396e+01 1.9287090e+01 2.1391599e+01 2.3414695e+01 2.5397529e+01 2.7296901e+01 2.9154444e+01 3.0958551e+01 3.2650804e+01 3.4122169e+01 3.5345870e+01 3.6333544e+01 3.7087919e+01 3.7644970e+01 3.8098612e+01 3.8519229e+01 3.8920198e+01 3.9283044e+01 3.9569378e+01 3.9767190e+01 3.9885885e+01 3.9907029e+01 3.9804575e+01 3.9527406e+01 3.9061337e+01 3.8401000e+01 3.7579875e+01 3.6572930e+01 3.5408243e+01 3.4123673e+01 3.2765475e+01 3.1266771e+01 2.9654942e+01 2.8065267e+01 2.6386321e+01 2.4550071e+01 2.2656458e+01 2.0727375e+01 1.8692358e+01 1.6683417e+01 1.4807842e+01 1.3024084e+01 1.1348137e+01 9.8611807e+00 8.4983355e+00 7.2218512e+00 6.0034679e+00 4.8696140e+00 3.7675787e+00 2.7323863e+00 1.8230125e+00 1.1008859e+00 5.4398392e-01 1.6189351e-01 0.0000000e+00
+ 4.8593469e+01 4.8593469e+01 4.8593469e+01 4.8593469e+01 4.8593469e+01 4.8593469e+01 4.8593469e+01 4.8593469e+01 4.8593221e+01 4.8578661e+01 4.8531865e+01 4.8432172e+01 4.8268145e+01 4.8019821e+01 4.7689120e+01 4.7234270e+01 4.6641286e+01 4.5904612e+01 4.5054639e+01 4.4122900e+01 4.3122722e+01 4.2059856e+01 4.0869328e+01 3.9572584e+01 3.8197859e+01 3.6873681e+01 3.5661536e+01 3.4626927e+01 3.3704468e+01 3.2770874e+01 3.1641189e+01 3.0287293e+01 2.8821391e+01 2.7427686e+01 2.6217654e+01 2.5309499e+01 2.4671530e+01 2.4211591e+01 2.3824640e+01 2.3482305e+01 2.3199300e+01 2.2974370e+01 2.2785896e+01 2.2598934e+01 2.2421034e+01 2.2225240e+01 2.2024867e+01 2.1785303e+01 2.1497087e+01 2.1133732e+01 2.0721945e+01 2.0253322e+01 1.9725574e+01 1.9121873e+01 1.8460914e+01 1.7749310e+01 1.6998647e+01 1.6263959e+01 1.5553651e+01 1.4860046e+01 1.4173570e+01 1.3451156e+01 1.2660664e+01 1.1814216e+01 1.0899141e+01 9.9321297e+00 8.9578942e+00 8.0180608e+00 7.1101689e+00 6.2777584e+00 5.5143605e+00 4.8223547e+00 4.2030583e+00 3.6725072e+00 3.2347176e+00 2.8923909e+00 2.6214606e+00 2.3971390e+00 2.2160131e+00 2.0558757e+00 1.8941541e+00 1.7404784e+00 1.6261378e+00 1.5275892e+00 1.4209143e+00 1.3138944e+00 1.2061371e+00 1.0753528e+00 9.4637675e-01 8.4673338e-01 7.7157992e-01 6.9775615e-01 6.2341039e-01 5.4902187e-01 4.7245270e-01 3.9218895e-01 3.1973082e-01 2.5357625e-01 1.5835146e-01 0.0000000e+00
diff --git a/data/trajgmm/TrajGMM_test_Gamma.txt b/data/trajgmm/TrajGMM_test_Gamma.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7bce983d88005235c63494aa1b850ab2111ef9d2
--- /dev/null
+++ b/data/trajgmm/TrajGMM_test_Gamma.txt
@@ -0,0 +1 @@
+1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5
diff --git a/data/trajgmm/TrajGMM_test_info.txt b/data/trajgmm/TrajGMM_test_info.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c1f7281da1b3a374492d2983f477d3c2048eef76
--- /dev/null
+++ b/data/trajgmm/TrajGMM_test_info.txt
@@ -0,0 +1,3 @@
+2
+3
+0.01
diff --git a/data/trajgmm/TrajGMM_test_mu.txt b/data/trajgmm/TrajGMM_test_mu.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9abc4a12da0825ee7881f5fb9d42cd6f2eabbf80
--- /dev/null
+++ b/data/trajgmm/TrajGMM_test_mu.txt
@@ -0,0 +1,6 @@
+2705.9 -15.69 2088 3416.8 845.14
+4430.9 3438.8 2052.8 720.48 92.941
+-12705 -5638.8 17010 -5638.1 -12303
+70.954 -10315 -3909.9 -6520.1 -1054.5
+-1.1034e+05 1.7525e+05 -28614 -1.1261e+05 86543
+-25984 -21702 -3294.5 25880 2466.8
diff --git a/data/trajgmm/TrajGMM_test_priors.txt b/data/trajgmm/TrajGMM_test_priors.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c11780523c5d59775ada6605801b0370a95c3099
--- /dev/null
+++ b/data/trajgmm/TrajGMM_test_priors.txt
@@ -0,0 +1 @@
+0.18806 0.19367 0.20767 0.21428 0.19632
diff --git a/data/trajgmm/TrajGMM_test_sigma.txt b/data/trajgmm/TrajGMM_test_sigma.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d194c6884964e8c79ca7c4dcb11216dd60cb779f
--- /dev/null
+++ b/data/trajgmm/TrajGMM_test_sigma.txt
@@ -0,0 +1,6 @@
+6.5951e+05 -36308 5.3567e+06 8.3104e+05 -5.0128e+07 1.369e+07 2.0567e+05 1.7805e+05 -2.9084e+06 1.4102e+06 -3.5251e+07 -2.324e+07 1.1857e+06 -2.1024e+05 -2.4341e+06 -9.8961e+05 -1.1547e+08 -6.4257e+07 2.743e+05 1.9035e+05 3.037e+06 -1.2514e+06 -1.4441e+07 -9.2701e+06 6.106e+05 51883 -3.9052e+06 -3.2714e+05 -7.2861e+07 1.2307e+07
+-36308 73346 -4.931e+05 -1.3712e+05 -5.0765e+06 -2.443e+06 1.7805e+05 4.4673e+05 -7.4378e+06 7.8094e+05 -3.9038e+07 -6.5663e+07 -2.1024e+05 50355 5.8716e+05 2.4716e+05 2.0403e+07 1.0351e+07 1.9035e+05 2.1639e+05 3.2658e+06 -9.2044e+05 -7.8488e+06 -1.5476e+07 51883 6501.3 -3.232e+05 -54874 -4.2507e+06 8.7444e+05
+5.3567e+06 -4.931e+05 6.1081e+07 6.0765e+06 -1.9792e+08 4.6349e+07 -2.9084e+06 -7.4378e+06 1.3061e+08 -1.015e+07 6.4626e+08 1.1856e+09 -2.4341e+06 5.8716e+05 1.5887e+07 6.4734e+06 2.5971e+08 8.6389e+07 3.037e+06 3.2658e+06 5.4757e+07 -1.5776e+07 -9.2961e+07 -2.4375e+08 -3.9052e+06 -3.232e+05 4.3795e+07 2.024e+05 5.4494e+08 -8.7523e+07
+8.3104e+05 -1.3712e+05 6.0765e+06 2.227e+06 -5.32e+07 2.6998e+07 1.4102e+06 7.8094e+05 -1.015e+07 1.4302e+07 -2.9262e+08 -3.6425e+07 -9.8961e+05 2.4716e+05 6.4734e+06 3.9838e+06 9.5975e+07 3.3778e+07 -1.2514e+06 -9.2044e+05 -1.5776e+07 7.6561e+06 7.7672e+07 6.4287e+07 -3.2714e+05 -54874 2.024e+05 9.6993e+05 1.072e+07 4.0353e+06
+-5.0128e+07 -5.0765e+06 -1.9792e+08 -5.32e+07 1.3837e+10 -9.8031e+08 -3.5251e+07 -3.9038e+07 6.4626e+08 -2.9262e+08 9.6912e+09 5.326e+09 -1.1547e+08 2.0403e+07 2.5971e+08 9.5975e+07 1.6844e+10 6.3361e+09 -1.4441e+07 -7.8488e+06 -9.2961e+07 7.7672e+07 2.8761e+09 3.5742e+08 -7.2861e+07 -4.2507e+06 5.4494e+08 1.072e+07 2.0348e+10 -1.9149e+09
+1.369e+07 -2.443e+06 4.6349e+07 2.6998e+07 -9.8031e+08 2.3729e+09 -2.324e+07 -6.5663e+07 1.1856e+09 -3.6425e+07 5.326e+09 1.7476e+10 -6.4257e+07 1.0351e+07 8.6389e+07 3.3778e+07 6.3361e+09 4.4779e+09 -9.2701e+06 -1.5476e+07 -2.4375e+08 6.4287e+07 3.5742e+08 3.5256e+09 1.2307e+07 8.7444e+05 -8.7523e+07 4.0353e+06 -1.9149e+09 1.262e+09
diff --git a/data/trajgmm/TrajGMM_test_varnames.txt b/data/trajgmm/TrajGMM_test_varnames.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f835f1aa56b304d54e67f578b6ac21adb929d951
--- /dev/null
+++ b/data/trajgmm/TrajGMM_test_varnames.txt
@@ -0,0 +1 @@
+x1 x2 v1 v2 a1 a2
\ No newline at end of file
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index dfefe4c7417587d34f96980f60f7ab95641aa7f3..63e3b77848cec6b95eeaba4a5850943d41ed3b15 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -10,12 +10,18 @@ target_link_libraries(test_gmr pbd)
add_executable(test_gmr2 test_gmr2.cpp)
target_link_libraries(test_gmr2 pbd)
+add_executable(test_gmr3 test_gmr3.cpp)
+target_link_libraries(test_gmr3 pbd)
+
add_executable(test_tpgmm_timeInvariant test_tpgmm_timeInvariant.cpp)
target_link_libraries(test_tpgmm_timeInvariant pbd)
add_executable(test_lqr test_lqr.cpp)
target_link_libraries(test_lqr pbd)
+add_executable(test_lqr_discrete test_lqr_discrete.cpp)
+target_link_libraries(test_lqr_discrete pbd)
+
add_executable(test_onlineDP test_onlineDP.cpp)
target_link_libraries(test_onlineDP pbd)
@@ -36,3 +42,39 @@ target_link_libraries(test_mvn pbd)
add_executable(test_HSMM test_hsmm.cpp)
target_link_libraries(test_HSMM pbd)
+
+add_executable(test_HSMM2 test_hsmm2.cpp)
+target_link_libraries(test_HSMM2 pbd)
+
+add_executable(test_ADHSMM test_adhsmm.cpp)
+target_link_libraries(test_ADHSMM pbd)
+
+add_executable(test_quaternions test_quaternions.cpp)
+target_link_libraries(test_quaternions pbd)
+
+add_executable(test_tpgmm_quaternions test_tpgmm_quaternions.cpp)
+target_link_libraries(test_tpgmm_quaternions pbd)
+
+add_executable(test_tpgmm2 test_tpgmm2.cpp)
+target_link_libraries(test_tpgmm2 pbd)
+
+add_executable(test_tpgmm test_tpgmm.cpp)
+target_link_libraries(test_tpgmm pbd)
+
+add_executable(test_TrajGMM test_trajGMM.cpp)
+target_link_libraries(test_TrajGMM pbd)
+
+add_executable(test_tphsmm test_tphsmm.cpp)
+target_link_libraries(test_tphsmm pbd)
+
+add_executable(test_mpc test_mpc.cpp)
+target_link_libraries(test_mpc pbd)
+
+add_executable(test_trajMPC test_trajMPC.cpp)
+target_link_libraries(test_trajMPC pbd)
+
+add_executable(test_trajMPC2 test_trajMPC2.cpp)
+target_link_libraries(test_trajMPC2 pbd)
+
+add_executable(test_trajdist test_trajdist.cpp)
+target_link_libraries(test_trajdist pbd)
diff --git a/examples/test_adhsmm.cpp b/examples/test_adhsmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..329a5015f86bf06579a13dee56222ed05381b0c8
--- /dev/null
+++ b/examples/test_adhsmm.cpp
@@ -0,0 +1,155 @@
+/**
+Copyright (C) 2015, Leonel Rozo, Martijn Zeestraten, Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+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 Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file test_adhsmm.cpp
+\brief Learning ADHSMM model
+Example showing the computation of the forward variable of an ADHSMM for a given external input that modifies the duration probabilities of the model.
+
+\author Leonel Rozo
+\bug No known bugs.
+*/
+
+#include "pbdlib/adhsmm.h"
+#include "armadillo"
+
+using namespace arma;
+using namespace pbdlib;
+using namespace std;
+
+int main()
+{
+
+ // ---> Create Object
+ ADHSMM* myADHSMM;
+
+ // ---> Load data from file
+ string muPath = "data/adhsmm/ADHSMM_test_mu.txt";
+ string sigmaPath = "data/adhsmm/ADHSMM_test_sigma.txt";
+ string priorsPath = "data/adhsmm/ADHSMM_test_priors.txt";
+ string transPath = "data/adhsmm/ADHSMM_test_trans.txt";
+ string durPriors = "data/adhsmm/ADHSMM_test_durPriors.txt";
+ string durMu = "data/adhsmm/ADHSMM_test_durMu.txt";
+ string durSigma = "data/adhsmm/ADHSMM_test_durSigma.txt";
+ string testData = "data/adhsmm/ADHSMM_test_testData.txt";
+
+ // ---> Loading test data
+ mat test;
+ test.load(testData, raw_ascii);
+ uint T = test.n_cols;
+
+ // ---> Setting artificial external input
+ //mat u = zeros<mat>(1,T);
+ mat u1 = zeros<mat>(1,45);
+ mat u2 = ones<mat>(1,45);
+ mat u3 = zeros<mat>(1,110);
+ mat u = arma::join_horiz(u1,u2);
+ u = arma::join_horiz(u,u3);
+ urowvec varIn, varOut;
+ varIn << 0; // The input variables for regression of the duration probability. (external input)
+ varOut << 1; // The output variables for regression of the duration probability. (duration)
+
+ myADHSMM = new ADHSMM(priorsPath,muPath,sigmaPath,transPath,durPriors,durMu,durSigma,T);
+
+
+ // ---> Display Data
+ cout << "--- ADHSMM components: " << endl;
+ cout << "nStates " << myADHSMM->getNumSTATES() <<endl;
+ for (uint i=0;i<myADHSMM->getNumSTATES();i++)
+ {
+ cout << "Priors" << endl;
+ cout << myADHSMM->getPRIORS(i) << endl;
+ cout << "Mu" << endl;
+ cout << myADHSMM->getMU(i) << endl;
+ cout << "Sigma" << endl;
+ cout << myADHSMM->getSIGMA(i) << endl;
+
+ // Duration GMM
+ cout << "Duration prob. [" << i << "]" << endl;
+ myADHSMM->getDurationGMMs(i).getPRIORS().print("durPriors");
+ for(uint j=0 ; j<myADHSMM->getNumSTATESDUR() ; j++)
+ {
+ myADHSMM->getDurationGMMs(i).getMU(j).print("durMu:");
+ myADHSMM->getDurationGMMs(i).getSIGMA(j).print("durSigma:");
+ }
+ }
+ cout << "Transition matrix = " << endl;
+ myADHSMM->getTRANSITION().print();
+
+
+ // ---> Testing duration-based forward variable
+ cout << "Press [ENTER] to test duration-based forward variable...";
+ cin.get();
+
+ // Loop for T samples
+ for(uint t=0 ; t<T ; t++)
+ myADHSMM->stepRecursiveForwardVariable(u.col(t), varIn, varOut, t);
+
+ // Getting alpha variable and normalizing it
+ mat alpha = myADHSMM->getRecursiveForwardVariable(); // Getting forward variable
+ alpha = alpha / repmat(sum(alpha,0),myADHSMM->getNumSTATES(),1); // Normalizing forward variable
+
+ // Printing results
+ for(uint t=0 ; t<T ; t++)
+ {
+ if (t%4 == 0)
+ {
+ cout << "alpha(:," << t << ") = " << endl;
+ alpha.col(t).print();
+ }
+ //cin.get();
+ }
+ // ---> Saving alpha for comparison purposes (in MATLAB)
+ //alpha.save("Alpha1.mat", raw_ascii);
+
+
+ // ---> Testing forward variable computation with observation+duration
+ cout << "Press [ENTER] to test forward variable using full observations";
+ cin.get();
+
+ myADHSMM->resetRecursiveForwardVariable();
+ mat tmpObs;
+ // Loop for the observations saved in matrix "test"
+ tmpObs = test.col(0);
+ for(uint t=0 ; t<T ; t++)
+ {
+ myADHSMM->stepRecursiveForwardVariable(u.col(t), varIn, varOut, t,tmpObs);
+
+ // Simulating the effects of the external input on the system.
+ // If the external input is "1", then the systems entirely stops
+ if(!u.col(t)(0))
+ tmpObs = arma::join_horiz(tmpObs,test.col(min(t+1,T-1)));
+ else
+ tmpObs = arma::join_horiz(tmpObs,tmpObs.col(tmpObs.n_cols-1));
+ }
+ alpha = myADHSMM->getRecursiveForwardVariable(); // Getting forward variable
+ alpha = alpha / repmat(sum(alpha,0),myADHSMM->getNumSTATES(),1); // Normalizing forward variable
+
+ for(uint t=0 ; t<T ; t++)
+ {
+ if (t%4 == 0)
+ {
+ cout << "alpha(:," << t << ") = " << endl;
+ alpha.col(t).print();
+ }
+ //cin.get();
+ }
+
+ // ---> Saving alpha for comparison purposes (in MATLAB)
+ //alpha.save("Alpha2.mat", raw_ascii);
+}
diff --git a/examples/test_datapoints.cpp b/examples/test_datapoints.cpp
index d3365962b83c60ad6798a77e04afe4fb2ecde606..6e03157d369cceb87915814e5748485a70197cbc 100644
--- a/examples/test_datapoints.cpp
+++ b/examples/test_datapoints.cpp
@@ -39,9 +39,9 @@ int main(int argc, char **argv)
datapoint.setData(A);
cout << "\n Data \n " << datapoint.getData();
- datapoint.saveInFile("data01.txt");
+ datapoint.saveInFile("data/data01.txt");
- datapoint.loadFromFile("data01.txt");
+ datapoint.loadFromFile("data/data01.txt");
cout << "\n Data \n " << datapoint.getData();
return 0;
diff --git a/examples/test_demonstration.cpp b/examples/test_demonstration.cpp
index 42907415700bc0997e811e395740088e51ec300e..bb69d6279d98cbe84242df2088d73e4f671d863b 100644
--- a/examples/test_demonstration.cpp
+++ b/examples/test_demonstration.cpp
@@ -39,7 +39,7 @@ int main(int argc, char **argv)
cout << "\n Data \n " << datapoint.getData();
- datapoint.saveInFile("data02.txt");
+ datapoint.saveInFile("data/data02.txt");
Demonstration demo = Demonstration(4,200);
std::vector<Demonstration> demos;
@@ -52,7 +52,7 @@ int main(int argc, char **argv)
demo.getDatapoints().setVarNames(vars);
- demo.getDatapoints().loadFromFile("data02.txt");
+ demo.getDatapoints().loadFromFile("data/data02.txt");
cout << endl << "Data is: " << demo.getDatapoints().getData() << endl;
cout << endl << "nbVar is: " << demo.getDatapoints().getNumVARS() << endl;
@@ -60,7 +60,7 @@ int main(int argc, char **argv)
demos.push_back(demo);
- demo.saveInFile("demo01.txt");
+ demo.saveInFile("data/demo01.txt");
return 0;
}
diff --git a/examples/test_gmm.cpp b/examples/test_gmm.cpp
index 279b36ecf91fae0636633abf26b2c573467f47b8..659d8ec506638b62310c9f3b5925343555730ea6 100644
--- a/examples/test_gmm.cpp
+++ b/examples/test_gmm.cpp
@@ -45,7 +45,7 @@ int main(int argc, char **argv)
vars.push_back("y");
vars.push_back("z");
- demo.getDatapoints().loadFromFile("../../data/data_txyz.txt");
+ demo.getDatapoints().loadFromFile("data/data_txyz.txt");
demo.getDatapoints().setVarNames(vars);
cout << endl << "Data is: " << demo.getDatapoints().getData() << endl;
@@ -69,7 +69,10 @@ int main(int argc, char **argv)
cout << "\nPress enter to continue..." << endl; getchar();
//Loading the GMM model from files...
- GMM_Model *gmm2 = new GMM_Model("GMM_priors.txt", "GMM_mu.txt", "GMM_sigma.txt", "GMM_vars.txt");
+ GMM_Model *gmm2 = new GMM_Model("data/gmm/GMM_priors.txt",
+ "data/gmm/GMM_mu.txt",
+ "data/gmm/GMM_sigma.txt",
+ "data/gmm/GMM_vars.txt");
for (int i=0;i<nbStates;i++)
gmm2->setMU(i, gmm2->getMU(i)*10.0);
diff --git a/examples/test_gmr.cpp b/examples/test_gmr.cpp
index f0616f50d1e0c9f6e8ad8060c2d01b4a57c41e76..04e8c01430ea2b5aee426219c2f114ce2eb97cde 100644
--- a/examples/test_gmr.cpp
+++ b/examples/test_gmr.cpp
@@ -45,7 +45,7 @@ int main(int argc, char **argv)
vars.push_back("y");
vars.push_back("z");
- demo.getDatapoints().loadFromFile("../../data/data_txyz.txt");
+ demo.getDatapoints().loadFromFile("data/data_txyz.txt");
demo.getDatapoints().setVarNames(vars);
cout << endl << "Data is: " << demo.getDatapoints().getData() << endl;
diff --git a/examples/test_gmr2.cpp b/examples/test_gmr2.cpp
index b45aa0e5b295ef058c7707d0ea0dad9731290331..6d1dd6fb34f10ae975899b666fc876c4e488c697 100644
--- a/examples/test_gmr2.cpp
+++ b/examples/test_gmr2.cpp
@@ -53,7 +53,7 @@ int main(int argc, char **argv)
vars.push_back("y");
vars.push_back("z");
- demo.getDatapoints().loadFromFile("../../data/data_txyz.txt");
+ demo.getDatapoints().loadFromFile("data/data_txyz.txt");
demo.getDatapoints().setVarNames(vars);
cout << endl << "Data is: " << demo.getDatapoints().getData() << endl;
diff --git a/examples/test_gmr3.cpp b/examples/test_gmr3.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c48558a4459827ca1806b7b484ed970cf98ea034
--- /dev/null
+++ b/examples/test_gmr3.cpp
@@ -0,0 +1,216 @@
+// Ajay Tanwani, 13 september 2016
+
+#include "pbdlib/gmm.h"
+#include "pbdlib/gmr.h"
+#include <pbdlib/lqr.h>
+#include <pbdlib/quaternion.h>
+#include <pbdlib/tpgmm.h>
+#include <pbdlib/tphsmm.h>
+
+#include <sstream>
+#include <string>
+#include "armadillo"
+#include <stdio.h>
+
+using namespace pbdlib;
+using namespace arma;
+using namespace std;
+
+void print_matrix(arma::mat matrix){ cout << matrix << endl;}
+
+int main(int argc, char **argv)
+{
+ //learning model initializations
+ uint nbFrames, nbStates, nbVarPos, nbD, nbVar;
+ int nbSamples, demo_id, dataset_num;
+ double dt, rf, dt_sm;
+ mat A, B, R, A1d, B1d, R_lqr_FH;
+ float iFactor, rFactor, lqrFH_factor, g_factor;
+ mat AlphaHSMM;
+ uint nbVarIn, nbVarOut;
+
+ nbFrames = 2;
+ nbStates = 7;
+ nbVarPos = 7;
+ nbVarIn = 7;
+ nbVarOut = 7;
+ nbVar = 14; //X,Q, Xo, Qo
+
+ TPHSMM *tphsmm;
+ mat rData;
+ int pred_length, sampling_method, horizon_method, sim_method;
+ uint auto_mode_counter;
+
+ //shared teleop initializations
+ TaskParameters *TPs;
+ GMM_Model *ProdGauss, *gmmOut, *gmmP;
+ std::vector<TaskParameter> cTaskParam, refTaskParam;
+ GMR *gmr;
+ urowvec in, out;
+ rowvec pg_priors;
+ mat teleop_sigma;
+ float scale_teleop_sigma;
+ float offset_hands[3];
+
+ mat Data;
+
+ string loadPath = "data/valve_data_traj.txt";
+ Data.load(loadPath, raw_ascii);
+ Data = Data.t();
+// cout << "Data: " << Data.t() << endl;
+
+ string saveDir;
+
+ saveDir = "/home/ajay/ros_ws/src/baxter_rli/teleop_ctrl/models/tphsmm/ValveICRA2/";
+
+ string priorsPath = saveDir + "HSMM__priors.txt";
+ string varNamesPath = saveDir + "HSMM__varnames.txt";
+ string muPrefix = saveDir + "HSMM__mu";
+ string SigmaPrefix = saveDir + "HSMM__sigma";
+
+ string transPath = saveDir + "HSMM__trans.txt";
+ string durMu = saveDir + "HSMM__durMu.txt";
+ string durSigma = saveDir + "HSMM__durSigma.txt";
+
+ stringstream frameFile, index;
+ demo_id = 1;
+ frameFile.str("");
+ index.str("");
+ index<<(demo_id);
+
+ frameFile << saveDir << "HSMM__Frame_" << index.str() << ".txt";
+
+ TPs = new TaskParameters(nbVar, nbFrames);
+ TPs->loadFromFile(frameFile.str());
+ cTaskParam = TPs->getTaskParameters();
+ refTaskParam = cTaskParam;
+
+// for (int j=0;j < nbFrames; j++){
+// for (int k=0; k < nbVar; k++){
+// cout << cTaskParam[j].b[k] << ",\t";
+// }
+// cout << endl;
+// }
+
+// cout << "Task Param.A" << cTaskParam[0].A << endl;
+// cout << "cTaskParam[0].b" << cTaskParam[0].b << endl;
+
+ // initialize the model
+ tphsmm = new TPHSMM(nbVar, nbStates, nbFrames);
+
+ tphsmm->setSamplingMethod(1);
+ tphsmm->loadTPHSMMfromFiles(priorsPath,varNamesPath,muPrefix, SigmaPrefix, transPath, durMu, durSigma);
+
+// cout << tphsmm->getTRANSITION() << endl;
+//
+// cout << tphsmm->getDurMU(0) << endl;
+// cout << tphsmm->getDurSIGMA(0) << endl;
+//
+// cout << tphsmm->getDurMU(1) << endl;
+// cout << tphsmm->getDurSIGMA(1) << endl;
+
+// cout << tphsmm->getPRIORS() << endl;
+ tphsmm->setCurrentTaskFrame(cTaskParam);
+ cTaskParam = tphsmm->getTaskParameters();
+
+ tphsmm->setScalingR((double)iFactor);
+ tphsmm->setPredictionLength(pred_length);
+ tphsmm->setRegularizationConstLQR((double)rFactor);
+ rData = zeros(nbVarIn, pred_length);
+ AlphaHSMM = zeros(tphsmm->getNumSTATES(), pred_length);
+
+
+ ProdGauss = new GMM_Model(nbStates, nbVar);
+
+ for (int i=0; i < nbStates ; i++){
+ ProdGauss->setMU(i,cTaskParam[0].A * tphsmm->get_TPGMM()->getGMMS(0).getMU(i) + cTaskParam[0].b );
+ ProdGauss->setSIGMA(i,cTaskParam[0].A * tphsmm->get_TPGMM()->getGMMS(0).getSIGMA(i) * cTaskParam[0].A.t());
+ }
+
+ pg_priors = rowvec({0.1462, 0.1097, 0.1619, 0.1391, 0.1988, 0.0898, 0.1545});
+ ProdGauss->setPRIORS(pg_priors);
+// cout << "tphsmm Priors: " << tphsmm->getPRIORS() << endl;
+// cout << "ProdGauss Priors" << ProdGauss->getPRIORS() << endl;
+// cout << "ProdGauss Number of States " << ProdGauss->getNumSTATES() << endl;
+
+ out = urowvec({7,8,9,10,11,12,13});
+ in = urowvec({0,1,2,3,4,5,6});
+
+ scale_teleop_sigma = 0.03;
+ teleop_sigma = arma::eye(nbVarIn, nbVarIn)*scale_teleop_sigma;
+
+ gmr = new GMR(ProdGauss);
+ gmmOut = new GMM_Model(1, nbVarIn);
+
+ //testing GMR out
+ gmmOut = gmr->regression(ProdGauss->getMU(0).rows(0,nbVarIn - 1), in, out);
+// cout << "ProdGaussian - Mu 0 " << ProdGauss->getMU(0).t() << endl;
+// cout << "gmmOut - Mu oUT" << gmmOut->getMU(0).t() << endl;
+//
+// cout << "ProdGaussian - Sigma 0 " << ProdGauss->getSIGMA(0) << endl;
+// cout << "gmmOut Sigma 0" << gmmOut->getSIGMA(0) << endl;
+
+ gmmOut = gmr->regression(ProdGauss->getMU(1).rows(0,nbVarIn - 1), in, out);
+// cout << "ProdGaussian - Mu 1" << ProdGauss->getMU(1).t() << endl;
+// cout << "gmmOut - Mu oUT" << gmmOut->getMU(0).t() << endl;
+//
+// cout << "ProdGaussian - Sigma 1 " << ProdGauss->getSIGMA(1) << endl;
+// cout << "gmmOut Sigma OuT" << gmmOut->getSIGMA(0) << endl;
+
+ gmmP = new GMM_Model(1,nbVarIn);
+
+// ProdGauss = tphsmm->get_ProdGMM(cTaskParam);
+ ProdGauss->setPRIORS(pg_priors);
+// for (int i=0; i < nbStates; i++){
+// ProdGauss->setSIGMA(i,ProdGauss->getSIGMA(i) + eye(nbVar,nbVar)*1E-2);
+// }
+// gmr->setGMMModel(ProdGauss);
+
+//// quat_teleoperator->changeSortConvention(SCALAR_VEC);
+// colvec test_vec;
+// test_vec = colvec({0.4572, 0.1138, 0.1686, 0.1499, 0.1299, -0.7213, 0.6636});
+// gmr->regression(gmmOut, test_vec, in, out);
+//// gmr->regression(join_vert(teleoperator_arm_pose.rows(0,2),quat_teleoperator->getAllCoeffs()), in, out);
+//
+// // Take product of Gaussians with the teleoperator arm
+// mat SigmaP = (teleop_sigma.i() + gmmOut->getSIGMA(0).i()).i();
+// gmmP->setSIGMA(0,SigmaP);
+// gmmP->setMU(0,SigmaP*(teleop_sigma.i()*test_vec + gmmOut->getSIGMA(0).i()*gmmOut->getMU(0)));
+//// quat_teleoperator->changeSortConvention(VEC_SCALAR);
+// vec MuOffset = gmmP->getMU(0);
+//
+//// cout << "test vector: " << test_vec << endl;
+ double regF = 1E-2;
+
+ for (int t=0; t < Data.n_cols; t++){
+ mat SigmaTmp = zeros(nbVarOut, nbVarOut);
+ colvec MuTmp = zeros(nbVarOut,1);
+ for (int m=0; m < nbFrames; m++){
+ for (int i=0; i < nbStates ; i++){
+// ProdGauss->setMU(i,cTaskParam[m].A * tphsmm->get_TPGMM()->getGMMS(m).getMU(i) + cTaskParam[m].b );
+// ProdGauss->setSIGMA(i,cTaskParam[m].A * tphsmm->get_TPGMM()->getGMMS(m).getSIGMA(i) * cTaskParam[m].A.t());
+ ProdGauss->setMU(i,tphsmm->get_TPGMM()->getGMMS(m).getMU(i));
+ ProdGauss->setSIGMA(i,tphsmm->get_TPGMM()->getGMMS(m).getSIGMA(i));
+ }
+ gmr->setGMMModel(ProdGauss);
+ gmr->regression2(gmmOut, Data.col(t), in, out, regF);
+ gmmOut->setMU(0,cTaskParam[m].A.submat(nbVarIn,nbVarIn,nbVar-1,nbVar-1)*gmmOut->getMU(0) + cTaskParam[m].b.subvec(nbVarIn,nbVar-1));
+ gmmOut->setSIGMA(0, cTaskParam[m].A.submat(nbVarIn,nbVarIn,nbVar-1,nbVar-1)*gmmOut->getSIGMA(0)*cTaskParam[m].A.submat(nbVarIn,nbVarIn,nbVar-1,nbVar-1).t());
+
+ SigmaTmp += gmmOut->getSIGMA(0).i();
+ MuTmp += gmmOut->getSIGMA(0).i()*gmmOut->getMU(0);
+// cout << gmmOut->getMU(0).t();
+ }
+ gmmOut->setMU(0,SigmaTmp.i()*MuTmp);
+ gmmOut->setSIGMA(0,SigmaTmp.i());
+
+ // Take product of Gaussians with the teleoperator arm
+ mat SigmaP = (teleop_sigma.i() + gmmOut->getSIGMA(0).i()).i();
+ gmmP->setSIGMA(0,SigmaP);
+ gmmP->setMU(0,SigmaP*(teleop_sigma.i()*Data.col(t) + gmmOut->getSIGMA(0).i()*gmmOut->getMU(0)));
+
+ cout << gmmOut->getMU(0).t() << endl;
+ }
+
+ return 0;
+}
diff --git a/examples/test_hsmm.cpp b/examples/test_hsmm.cpp
index 19961634f4f0fe51b2c2bf40214e82f654fde10d..66ea29632fd988a40ffc1f82ec4b656b59c61c48 100644
--- a/examples/test_hsmm.cpp
+++ b/examples/test_hsmm.cpp
@@ -39,12 +39,12 @@ int main()
HSMM* myHSMM;
// Load data from file
- string muPath = "./data/HSMM_test_mu.txt";
- string sigmaPath = "./data/HSMM_test_sigma.txt";
- string priorsPath = "./data/HSMM_test_priors.txt";
- string transPath = "./data/HSMM_test_trans.txt";
- string durMu = "./data/HSMM_test_durMu.txt";
- string durSigma = "./data/HSMM_test_durSigma.txt";
+ string muPath = "data/hsmm/HSMM_test_mu.txt";
+ string sigmaPath = "data/hsmm/HSMM_test_sigma.txt";
+ string priorsPath = "data/hsmm/HSMM_test_priors.txt";
+ string transPath = "data/hsmm/HSMM_test_trans.txt";
+ string durMu = "data/hsmm/HSMM_test_durMu.txt";
+ string durSigma = "data/hsmm/HSMM_test_durSigma.txt";
myHSMM = new HSMM(priorsPath,muPath,sigmaPath,transPath,durMu,durSigma);
diff --git a/examples/test_hsmm2.cpp b/examples/test_hsmm2.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..86151cad9fa3860b5c8c2700c7bf6c62f17da7d0
--- /dev/null
+++ b/examples/test_hsmm2.cpp
@@ -0,0 +1,150 @@
+/**
+Copyright (C) 2015, Leonel Rozo, Martijn Zeestraten, Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+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 Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file test_hsmm2.cpp
+\brief Learning HSMM model
+Testing the computation of the forward variable of an HSMM learned from demonstrations. Forward variable is computed using the classical (readable) formulation.
+
+\author Leonel Rozo
+\bug No known bugs.
+*/
+
+#include "pbdlib/hsmm.h"
+#include "armadillo"
+
+using namespace arma;
+using namespace pbdlib;
+using namespace std;
+
+int main()
+{
+
+ // Create HSMM object
+ HSMM* myHSMM;
+
+ // Load data from files
+ string muPath = "data/hsmm/HSMM_test2_mu.txt";
+ string sigmaPath = "data/hsmm/HSMM_test2_sigma.txt";
+ string priorsPath = "data/hsmm/HSMM_test2_priors.txt";
+ string transPath = "data/hsmm/HSMM_test2_trans.txt";
+ string durMu = "data/hsmm/HSMM_test2_durMu.txt";
+ string durSigma = "data/hsmm/HSMM_test2_durSigma.txt";
+ string testData = "data/hsmm/HSMM_test2_testData.txt";
+
+ // Loading test data
+ mat test;
+ test.load(testData, raw_ascii);
+
+ // Creating HSMM
+ myHSMM = new HSMM(priorsPath,muPath,sigmaPath,transPath,durMu,durSigma,test.n_cols);
+ myHSMM->setMaxDuration(80);
+
+ // Display HSMM parameters
+ cout << "--- HSMM components: " << endl;
+ cout << "nStates " << myHSMM->getNumSTATES() <<endl;
+ for (uint i=0 ; i<myHSMM->getNumSTATES() ; i++)
+ {
+ cout << "Priors" << endl;
+ cout << myHSMM->getPRIORS(i) << endl;
+ cout << "Mu" << endl;
+ cout << myHSMM->getMU(i) << endl;
+ cout << "Sigma" << endl;
+ cout << myHSMM->getSIGMA(i) << endl;
+ cout << "DurMu" << endl;
+ cout << myHSMM->getDurMU(i) << endl;
+ cout << "DurSigma" << endl;
+ cout << myHSMM->getDurSIGMA(i) << endl;
+ }
+
+ // --> Testing duration-based forward variable
+ cout << "Press [ENTER] to test duration-based forward variable...";
+ cin.get();
+
+ uint T = test.n_cols;
+ // Loop for T samples
+ for(uint t=0 ; t<T ; t++)
+ myHSMM->stepRecursiveForwardVariable(t);
+
+ // Getting alpha variable and normalizing it
+ mat alpha = myHSMM->getRecursiveForwardVariable(); // Getting forward variable
+ alpha = alpha / repmat(sum(alpha,0),myHSMM->getNumSTATES(),1); // Normalizing forward variable
+
+ // Printing results
+ for(uint t=0 ; t<T ; t++)
+ {
+ if (t%10 == 0)
+ {
+ cout << "alpha(:," << t << ") = " << endl;
+ alpha.col(t).print();
+ }
+ }
+ // ---> Saving alpha for comparison purposes (in MATLAB)
+ //alpha.save("Alpha1.mat", raw_ascii);
+
+ // --> Testing forward variable computation with observation+duration
+ cout << "Press [ENTER] to test forward variable using full observations";
+ cin.get();
+
+ myHSMM->resetRecursiveForwardVariable();
+ mat tmpObs;
+ // Loop for the observations saved in matrix "test"
+ for(uint t=0 ; t<T ; t++)
+ {
+ tmpObs = test.cols(0,t);
+ myHSMM->stepRecursiveForwardVariable(t,tmpObs);
+ }
+ alpha = myHSMM->getRecursiveForwardVariable(); // Getting forward variable
+ alpha = alpha / repmat(sum(alpha,0),myHSMM->getNumSTATES(),1); // Normalizing forward variable
+
+ for(uint t=0 ; t<T ; t++)
+ {
+ if (t%10 == 0)
+ {
+ cout << "alpha(:," << t << ") = " << endl;
+ alpha.col(t).print();
+ }
+ }
+ // ---> Saving alpha for comparison purposes (in MATLAB)
+ //alpha.save("Alpha2.mat", raw_ascii);
+
+ // --> Testing forward variable computation with partial observation + duration
+ cout << "Press [ENTER] to test forward variable using partial observations";
+ cin.get();
+ myHSMM->resetRecursiveForwardVariable();
+ urowvec index;
+ index << 0 ;
+ // Loop for the observations saved in matrix "test"
+ for(uint t=0 ; t<T ; t++)
+ {
+ tmpObs = test.cols(0,t);
+ myHSMM->stepRecursiveForwardVariable(t,tmpObs,index);
+ }
+ alpha = myHSMM->getRecursiveForwardVariable(); // Getting forward variable
+ alpha = alpha / repmat(sum(alpha,0),myHSMM->getNumSTATES(),1); // Normalizing forward variable
+
+ for(uint t=0 ; t<T ; t++)
+ {
+ if (t%10 == 0)
+ {
+ cout << "alpha(:," << t << ") = " << endl;
+ alpha.col(t).print();
+ }
+ }
+}
+
diff --git a/examples/test_lqr_discrete.cpp b/examples/test_lqr_discrete.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..893d362e36e59576f5ac50d4e65c5e6346d74979
--- /dev/null
+++ b/examples/test_lqr_discrete.cpp
@@ -0,0 +1,102 @@
+// Author: Ajay Tanwani, 2016
+
+//Discrete lqr tests for finite and infinite horizon
+#include <iostream>
+#include <sstream>
+#include "pbdlib/lqr.h"
+#include "armadillo"
+#include <vector>
+
+
+using namespace pbdlib;
+using namespace arma;
+
+void print_matrix(arma::mat matrix){ cout << matrix << endl;}
+
+
+int Factorial(int x, int result = 1) {
+
+ if (x == 1 || x == 0){
+ return result;
+ }else{
+ return Factorial(x - 1, x * result);
+ }
+}
+
+mat diagMat(vec d, int k){
+
+ mat dout = zeros(d.n_rows + abs(k),d.n_rows + abs(k));
+
+ for (int i=0; i < d.n_rows; i++){
+ dout(i,i+abs(k)) = d.at(i);
+ }
+ return dout;
+}
+
+int main(int argc, char **argv)
+{
+ int nbVar = 2;
+ int nbDeriv = 2; //for [x,dx];
+ mat A,B;
+ float dt,rFactor;
+ int nbData = 10;
+ mat R,Target;
+ std::vector<float> DataIn;
+ std::vector<mat> Q;
+
+ dt = 0.01;
+ rFactor = 1E-5;
+
+ mat A1d = zeros(nbDeriv,nbDeriv);
+ mat B1d = zeros(nbDeriv,1);
+
+ for (int i=0; i < nbDeriv; i++){
+ A1d = A1d + diagMat(ones(nbDeriv-i,1),i)* pow(dt,i) * 1/Factorial(i);
+ }
+
+ for (int i=0; i < nbDeriv; i++){
+ B1d.at(nbDeriv - 1 - i) = pow(dt,i+1) * 1/Factorial(i+1);
+ }
+
+ A = kron(A1d, eye(nbVar,nbVar));
+ B = kron(B1d, eye(nbVar,nbVar));
+
+ R = eye(nbVar,nbVar)*rFactor;
+
+ Target = zeros(nbVar*2,nbData);
+
+ Target.row(0) = 100*ones(1,nbData);
+ Target.row(1) = 50*ones(1,nbData);
+
+ for (int t=0;t<nbData;t++){
+ DataIn.push_back(t*dt);
+ mat QTmp = zeros(nbVar*2,nbVar*2);
+// QTmp(0,0) = 1;
+ QTmp.submat(0,0,nbVar-1,nbVar-1) = eye(nbVar,nbVar);
+ Q.push_back(QTmp);
+ }
+
+ LQR test_lqr(A,B,dt);
+
+ test_lqr.setProblem(R,Q,Target);
+
+ //Test infinite horizon
+ test_lqr.evaluate_gains_infiniteHorizon_Discrete();
+
+ //Test finite horizon
+// mat S;
+// colvec d;
+//
+// S = zeros(nbVar*2,nbVar*2);
+// d = zeros(nbVar*2,1);
+//
+// test_lqr.evaluate_gains_finiteHorizon(S,d);
+
+ //Output data
+ for (int t=0;t<nbData;t++){
+ std::cout << t << " , " << test_lqr.getGains().at(t) << std::endl;
+ }
+ std::cout << test_lqr.getFF() << std::endl;
+
+ return 0;
+}
diff --git a/examples/test_mpc.cpp b/examples/test_mpc.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b7b712f5d02a062c184eb1a5582ebbdffeb36ca4
--- /dev/null
+++ b/examples/test_mpc.cpp
@@ -0,0 +1,115 @@
+
+/**
+Copyright (C) 2015, Martijn Zeestraten
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+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 Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file test_mpc_
+\brief Testing mpc
+
+\author Martijn Zeestraten
+\bug No known bugs.
+*/
+
+#include <iostream>
+#include <sstream>
+#include "pbdlib/mpc.h"
+#include "armadillo"
+#include <vector>
+
+
+using namespace pbdlib;
+using namespace arma;
+using namespace std;
+
+int main(int argc, char **argv)
+{
+ // Settings
+ int Np = 20; // Prediction horizon
+ int Nc = 10; // Control horizon
+ int nbVarPos= 1; // Use 1D system
+ int nbDeriv = 2; // Number of derivatives (position and velocity)
+ float dt = 0.1; // time step
+ float alpha = 0.05; // control cost factor
+ int nbData = 250; // Number of data points
+ colvec Xinit; // Initial state of the system
+ Xinit << 0 << 0;
+ colvec Target; // Final desired final value for the system
+ Target << 10 << 0;
+
+
+ //----------- Create System Matrices
+ // Assistive Matrices for System construction:
+
+ cout << "Creating system matrices:"<< endl;
+ mat A,B,C,Ad,Bd;
+
+ // A matrix:
+ A << 0 << 1 << endr << 0 << 0;
+
+ // B Matrix:
+ B << 0 << endr <<1<< endr;
+
+ // C Matrix:
+ C << 1 << 0;
+
+ // Discretize system matrices (Euler discretization):
+ Ad = A*dt+eye<mat>(2,2);
+ Bd = B*dt;
+
+ cout <<"Ad: "<< endl << Ad << endl;
+ cout <<"Bd: "<< endl << Bd << endl;
+
+ // ----------- Create Target:
+ // We create a simple target, to let the system go to zero
+ colvec MuQ= repmat(Target,Np,1); // Np predictions of dimension 2 (position and velocity)
+ cout << "Size MuQ: " << MuQ.n_rows<< "x" << MuQ.n_cols << endl;
+
+ // Create tracking cost matrix:
+ mat QTmp = zeros(2,2);
+ QTmp(0,0) = 1;
+ mat Q = kron(QTmp,eye<mat>(Np,Np));
+ cout << "Size Q: " << Q.n_rows << "x" << Q.n_cols << endl;
+
+ mat R = eye(nbVarPos*Nc,nbVarPos*Nc)*(alpha*alpha);
+ cout << "Size R: " << R.n_rows << "x" << R.n_cols << endl;
+
+ // ----- Perform MPC:
+ cout << "Performing MPC" << endl;
+
+ // Create MPC object:
+ MPC myMPC(Ad,Bd,C,Np,Nc);
+
+ //Output data
+ colvec Xtmp = Xinit;
+ colvec U;
+ for (int t=0;t<nbData;t++){
+ // Compute control command:
+ U = myMPC.computeControlCommand(Xtmp,MuQ,Q,R);
+ // Perform iteration:
+ Xtmp = Ad*Xtmp+Bd*U;
+
+ // Display Result:
+ if (t%10==0 || t==0 || t==nbData)
+ {
+ std::cout << "X("<< t << "): [" << Xtmp(0) << ", "<< Xtmp(1) <<
+ "]\t U(" << t << "): " << U(0) << endl;
+ }
+ }
+
+ return 0;
+}
diff --git a/examples/test_mvn.cpp b/examples/test_mvn.cpp
index 0084179141d869124765e4735f2cf4ac6214246e..808052104a6288d0f8e02ee6af486ff38dbbf050 100644
--- a/examples/test_mvn.cpp
+++ b/examples/test_mvn.cpp
@@ -62,5 +62,11 @@ int main(int argc, char **argv)
cout << "Probs of samples" << endl;
for (int i = 0;i<20;i++)
cout << G1.getPDFValue(samps.col(i)) << endl;
+
+ // Product of Two Distributions:
+ GaussianDistribution Gprod = G1*G1;
+ cout << "Product \n Mu: \n" << Gprod.getMU() << "\nSigma:\n" << Gprod.getSIGMA() << endl;
+
+
}
diff --git a/examples/test_onlineDP.cpp b/examples/test_onlineDP.cpp
index 91bc1baa09a4fa3203a1982defb86f56ed3c7f36..baa36505c93b1afc488df591846b5e11369d8dd6 100644
--- a/examples/test_onlineDP.cpp
+++ b/examples/test_onlineDP.cpp
@@ -42,7 +42,7 @@ int main(int argc, char **argv)
//load data
- std::ifstream dataFile("data.csv");
+ std::ifstream dataFile("data/data.csv");
std::string line;
diff --git a/examples/test_quaternions.cpp b/examples/test_quaternions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c30e360cc3a93a175ef4f95aceaa68fdd995f533
--- /dev/null
+++ b/examples/test_quaternions.cpp
@@ -0,0 +1,168 @@
+/**
+Copyright (C) 2015, João Silvério
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ 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 Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file test_quaternions.cpp
+\brief testing Quaternion class
+
+Testing basic features of the Quaternion class.
+
+\author João Silvério
+\bug No known bugs.
+*/
+
+#include "pbdlib/quaternion.h"
+#include <sstream>
+
+using namespace pbdlib;
+using namespace arma;
+
+int main(int argc, char **argv)
+{
+ SortConvention sort = SCALAR_VEC;
+
+ cout << endl;
+ cout << " ** Testing pbdlib::Quaternion class **" << endl;
+ cout << endl;
+
+ // Generate an identity quaternion
+ cout << "-- Creating a quaternion using default constructor... " << endl;
+ Quaternion q1(sort);
+ cout << "q1 = ";
+ q1.print();
+ cout << "norm = " << q1.norm() << endl << endl;
+
+ // Initialize a quaternion from a vec3 and a double
+ cout << "-- Constructing quaternion from a vec3 and a double... " << endl;
+ vec3 vecPart;
+ double scalarPart;
+ vecPart[0] = 0.5332;
+ vecPart[1] = 0.5928;
+ vecPart[2] = 0.0831;
+ scalarPart = 0.5978;
+ Quaternion q2(vecPart,scalarPart,sort);
+ q2.normalize();
+ cout << "q2 = ";
+ q2.print();
+ cout << "norm = " << q2.norm() << endl << endl;
+
+ // Initialize a quaternion from a vec4
+ cout << "-- Constructing quaternion from a vec4... " << endl ;
+ vec4 quat;
+
+// quat[0] = scalarPart;
+// quat.subvec(1,3) = vecPart;
+ quat.subvec(0,2) = vecPart;
+ quat[3] = scalarPart;
+
+ Quaternion q3(quat, sort); // the order in quat is reverted on purpose, so that it is clear that 'sort' works
+ q3.normalize();
+ cout << "q3 = ";
+ q3.print();
+ cout << "norm = " << q3.norm() << endl << endl;
+
+ // Initialize a quaternion from 4 elements
+ cout << "-- Constructing quaternion from 4 double... " << endl;
+ Quaternion q4(quat[0],quat[1],quat[2],quat[3], sort);
+ q4.normalize();
+ cout << "q4 = ";
+ q4.print();
+ cout << "norm = " << q4.norm() << endl << endl;
+
+ // Initialize a quaternion from an angle and an axis
+ cout << "-- Constructing quaternion from axis-angle pair " << endl;
+ double angle = M_PI/2.0;
+ vec3 axis = zeros(3);
+ axis[0] = 1.0;
+ cout << "Angle = " << angle << "rad, " << " Axis = "; axis.t().print();
+ Quaternion qAxisAngle(angle, axis, sort);
+// qAxisAngle.normalize();
+ cout << "\nqAxisAngle = ";
+ qAxisAngle.print();
+ cout << "norm = " << qAxisAngle.norm() << endl << endl;
+
+ // Initialize a quaternion from an vec3
+// cout << "-- Constructing quaternion from rotation vector (norm gives the angle, direction gives the axis) " << endl;
+// vec3 rotVec;
+// rotVec << -0.8626 << -1.8820 << -2.0997;
+// cout << "Rotation vector = " << rotVec.t().print();
+// Quaternion qRotVec(rotVec, sort);
+// cout << "\nqRotVec = ";
+// qRotVec.print();
+// cout << "norm = " << qRotVec.norm() << endl << endl;
+
+ // Generate matrix representation of quaternion
+ cout << "-- Generating matrix representation of quaternion... " << endl;
+ mat44 quatMatrix = q4.matrix();
+ cout << "Q4 = " << endl;
+ quatMatrix.print();
+ cout << endl;
+
+ // Test quaternion product computation
+ cout << "-- Computing quaternion product q3*q2... " << endl;
+ Quaternion q5 = q3*q2;
+ cout << "q5 = ";
+ q5.print();
+ cout << "norm = " << q5.norm() << endl << endl;
+
+ // Compute quaternion product using matrix reverse
+ cout << "-- Computing quaternion product using matrix reverse... " << endl;
+ vec4 quatProdOut;
+ quatProdOut = q2.matrixReverse()*q3.getAllCoeffs();
+ cout << "q5 = ";
+ quatProdOut.t().print();
+ cout << endl;
+
+ // Getting the 4-elements of a quaternion into a vec4/colvec4
+ cout << "-- Retrieving the 4 elements of the quaternion as a vec4... " << endl;
+ vec4 coeffs;
+ coeffs = q5.getAllCoeffs();
+ cout << "q5 = ";
+ coeffs.t().print();
+ cout << endl;
+
+ // Computing the logarithm of a quaternion
+ cout << "-- Log of quaternion q5... " << endl;
+ cout << "log(q5) = ";
+ qLog(q5).t().print();
+ cout << endl;
+
+ // Computing the exponential of a
+ cout << "-- Exponential of log(q5) (should be again q5)... " << endl;
+ cout << "exp(log(q5)) = ";
+ qExp(qLog(q5),q5.getSort()).getAllCoeffs().t().print();
+ cout << endl;
+
+ // Angular displacement between q2 and q3
+ cout << "-- Angular displacement between q2 and q3 (= angular velocity that rotates q2 into q3 in the unit time)... " << endl;
+ cout << "omega = ";
+ angDiff(q3,q2).t().print();
+ cout << endl;
+
+ // Rotate a 3D point
+ cout << "-- Rotation of a 3D point by q5... " << endl;
+ vec3 testPoint;
+ testPoint << 0.8147 << 0.9058 << 0.1270;
+ testPoint.print("p = ");
+ cout << endl;
+ vec3 rotatedPoint = q5*testPoint;
+ rotatedPoint.print("Rotated p = ");
+ cout << endl;
+
+ return 0;
+}
diff --git a/examples/test_tpgmm.cpp b/examples/test_tpgmm.cpp
index 07a345f8a1ade848ca0124045af85224874950f7..cc68a756c149c4442e4b4791dba6ee632d415877 100644
--- a/examples/test_tpgmm.cpp
+++ b/examples/test_tpgmm.cpp
@@ -79,8 +79,8 @@ int main(int argc, char **argv)
cout << "Loading the demonstrations and the task parameters ..." << endl;
for (uint m=0; m<nDemos; m++){ //Loading demos in the loop
// Form filenames:
- sprintf(Datafilename, "../../data/pgmm/Data0%d.txt",m+1);
- sprintf(TPfilename, "../../data/pgmm/Param0%d.txt",m+1);
+ sprintf(Datafilename, "../data/pgmm/Data0%d.txt",m+1);
+ sprintf(TPfilename, "../data/pgmm/Param0%d.txt",m+1);
// Load files in demo:
// note we need to transpose the data (last argument)
@@ -106,27 +106,15 @@ int main(int argc, char **argv)
cout << "\n Number of EM iterations: " << tpgmm->estimateTensorGMM() << endl;
// saving the learned model parameters
- mat MuTmp = zeros(nVars, nStates*nFrames);
- mat SigmaTmp = zeros(nVars, nVars*nStates*nFrames);
- mat Priors = zeros(1,nStates);
- std::vector<GMM_Model> GMMSresult;
- GMMSresult = tpgmm->getGMMS();
- for(uint m=0; m<nFrames; m++){
- for (uint i=0;i<nStates; i++){
- MuTmp.col(m*nStates+i) = GMMSresult[m].getMU(i);
- SigmaTmp.cols(m*nVars*nStates+i*nVars, m*nVars*nStates+i*nVars+nVars-1) = GMMSresult[m].getSIGMA(i);
- }
- }
- sprintf(Datafilename, "../../data/pgmm/Mu0%d.txt",2);
- MuTmp.save(Datafilename, raw_ascii);
- sprintf(Datafilename, "../../data/pgmm/Sigma0%d.txt",2);
- SigmaTmp.save(Datafilename, raw_ascii);
- sprintf(Datafilename, "../../data/pgmm/Priors0%d.txt",2);
- Priors = tpgmm->getPRIORS();
- Priors.save(Datafilename, raw_ascii);
+ tpgmm->saveInFiles("../data/pgmm/");
+
cout<<"the learned model parameters are written into text files succesfully."<<endl; cout<<"Press any key to continue..."<<endl; getchar();
-
+ tpgmm = new TPGMM("../data/pgmm/",nFrames);
+
+ cout <<"Loaded files" << endl;
+
+
// Reproduction variables (GMR)
GMR *gmr = new GMR();
Datapoints *repro = new Datapoints(1,1); // Only one datapoint passed at each time step
@@ -138,7 +126,7 @@ int main(int argc, char **argv)
// loading new parameters for the reproduction from the text file
// In this case we have considered that the parameters are the same all along the reproduction. They can be varying.
TaskParameters ReproTPs(nVars,nFrames);
- sprintf(TPfilename, "../../data/pgmm/ParamRepro0%d.txt",1);
+ sprintf(TPfilename, "../data/pgmm/ParamRepro0%d.txt",1);
ReproTPs.loadFromFile(TPfilename);
GMM_Model* gmm;
diff --git a/examples/test_tpgmm2.cpp b/examples/test_tpgmm2.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e2ef52fbf28cc41cbaf9f917c8f74dcbc64bc1a2
--- /dev/null
+++ b/examples/test_tpgmm2.cpp
@@ -0,0 +1,89 @@
+// Ajay Tanwnani, 1st April 2016
+
+#include "pbdlib/tpgmm.h"
+#include "pbdlib/gmm.h"
+#include "pbdlib/gmr.h"
+#include "pbdlib/datapoints.h"
+#include "pbdlib/tpdemonstration.h"
+#include "pbdlib/taskparameters.h"
+#include <sstream>
+
+using namespace pbdlib;
+using namespace std;
+
+int main(int argc, char **argv){
+
+ uint nbVarIn = 2;
+ uint nbVarOut = 2;
+ uint nbFrames = 1;
+ uint nbVar;
+ nbVar = nbVarIn + nbVarOut;
+
+ TPGMM *tpgmm;
+ TaskParameters *tp;
+ GMM_Model *ProdGauss, *gmmOut, *gmmP;
+ GMR *gmr;
+ urowvec in, out;
+ mat teleop_sigma;
+ GaussianDistribution Teleop_Gauss(nbVarIn);
+
+ int nbD = 100;
+ colvec M_obj, M_teleop;
+ mat S_obj, S_teleop;
+
+ S_obj << 10000 << 100 << endr
+ << 100 << 10000;
+
+ M_obj << 450 << 370;
+ GaussianDistribution G1(M_obj,S_obj);
+
+ S_teleop << 10000 << 100 << endr
+ << 100 << 10000;
+ M_teleop << 120 << 390;
+
+ GaussianDistribution G_obj(M_obj,S_obj);
+ mat pos_obj = G_obj.stochasticSampling(nbD);
+
+ GaussianDistribution G_teleop(M_teleop,S_teleop);
+ mat pos_teleop = G_teleop.stochasticSampling(nbD/2);
+
+ pos_teleop = join_rows(pos_obj.submat(0,0,1,pos_teleop.n_cols-1) + rand()%10, pos_teleop);
+
+ std::vector<TPDemonstration> tpdemos;
+ std::vector<TaskParameters> vTPs;
+ std::vector<TaskParameter> cTaskParam;
+ vector<mat> demos;
+ mat demo;
+ vector<mat> repros, robotRepros;
+
+ tp = new TaskParameters(nbVar, nbFrames);
+ for (int i=0; i<nbD; i++){
+ TaskParameter tmpTp;
+ for (int j=0; j < nbFrames; j++){
+ tmpTp.A = eye(nbVar, nbVar);
+ tmpTp.b = join_cols(zeros(nbVarIn,1), pos_obj.col(i));
+ tp->setTaskParameters(j,tmpTp);
+ }
+ vTPs.push_back(*tp);
+ }
+
+ for (int i=0; i < nbD; i++){
+// vec tmpPoint = vTPs[i].getTaskParameters(0).A.i() *( join_cols(pos_teleop.col(i), pos_teleop.col(i)) - vTPs[i].getTaskParameters(0).b);
+// cout << tmpPoint << endl;
+// demo.insert_cols(i, tmpPoint);
+ demo.insert_cols(i, join_cols(pos_teleop.col(i), pos_teleop.col(i)));
+ }
+
+ TPDemonstration tmpTpDemo(demo, vTPs);
+ tpdemos.push_back(tmpTpDemo);
+
+ mat tmpData = tpdemos[0].getDataInTaskParameters();
+
+ tpgmm = new TPGMM(tpdemos, 2);
+ tpgmm->initTensorGMM_kmeans();
+
+ tpgmm->EM_tensorGMM();
+
+// tpgmm->estimateTensorGMM();
+ cout << tpgmm->getNumFRAMES() << ",\t" << tpgmm->getNumVARS() << ",\t" << tpgmm->getNumSTATES() << endl;
+}
diff --git a/examples/test_tpgmm_quaternions.cpp b/examples/test_tpgmm_quaternions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2792781fbaef088306214a25c050ce2ebf952141
--- /dev/null
+++ b/examples/test_tpgmm_quaternions.cpp
@@ -0,0 +1,159 @@
+/**
+Copyright (C) 2015, João Silvério
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ 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 Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file test_tpgmm_quaternions.cpp
+\brief An example of TP-GMM using quaternions.
+
+This source code is given for free! In exchange, I would be grateful if you cite
+the following reference in any academic publication that uses this code or part of it:
+
+@inproceedings{Silverio15IROS,
+ author="Silv\'erio, J. and Rozo, L. and Calinon, S. and Caldwell, D. G.",
+ title="Learning bimanual end-effector poses from demonstrations using task-parameterized dynamical systems",
+ booktitle='Proc. {IEEE/RSJ} Intl Conf. on Intelligent Robots and Systems ({IROS})",
+ year="2015",
+ month="Sept.-Oct.",
+ address="Hamburg, Germany",
+ pages=""
+}
+
+\author João Silvério
+\bug No known bugs.
+*/
+
+#include "pbdlib/tpgmm.h"
+#include "pbdlib/gmm.h"
+#include "pbdlib/gmr.h"
+#include "pbdlib/datapoints.h"
+#include "pbdlib/taskparameters.h"
+#include "pbdlib/quaternion.h"
+#include <sstream>
+#include <math.h>
+#include <iostream>
+
+using namespace std;
+using namespace pbdlib;
+
+int main(int argc, char **argv)
+{
+ // Filenames that contain model parameters
+ std::string fParamsTPGMM("data/pgmm/quaternionModel/tpgmm_quaternions.txt");
+ std::string fVarsTPGMM ("data/pgmm/quaternionModel/vars_quaternions.txt");
+ std::string fPriorsTPGMM("data/pgmm/quaternionModel/priors_quaternions.txt");
+ std::string fMuTPGMM ("data/pgmm/quaternionModel/Zmu_quaternions"); // Initialize without file extension -> "_P#" will be added depending on nParams
+ std::string fSigmaTPGMM ("data/pgmm/quaternionModel/Zsigma_quaternions"); // Initialize without file extension -> "_P#" will be added depending on nParams
+
+ // Loading files and setting tpgmm variables
+ vec ModelParams; // The model parameters are loaded from the TPGMM model file
+ ModelParams.load(fParamsTPGMM);
+ uint nVars = ModelParams[0];
+ uint nFrames = ModelParams[1];
+ uint nStates = ModelParams[2];
+
+ // According to GMM class, the name of the variables should also come in a separate .txt file.
+ // I'll use a toy file for now, during object initialization, and keep it like below for the PGMM object construction.
+ std::vector<std::string> varNames;
+ varNames.push_back("t"); // time driven regression
+ varNames.push_back("q0_out"); // quaternion variables - the order depends on the convention with which the quaternion is initialized
+ varNames.push_back("q1_out"); // -> SCALAR_VEC means that q=[u vec], aka [w x y z] (the default is VEC_SCALAR)
+ varNames.push_back("q2_out");
+ varNames.push_back("q3_out");
+
+ TPGMM* tpgmm;
+ tpgmm = new TPGMM(nVars, nStates, nFrames);
+ tpgmm->loadTPGMMfromMATLAB(fPriorsTPGMM, fVarsTPGMM, fMuTPGMM, fSigmaTPGMM);
+
+ // Reproduction variables (GMR)
+ GMR *gmr = new GMR();
+
+ std::vector<std::string> inputVarNames;
+ inputVarNames.push_back("t"); // time-driven movement
+ Datapoints *repro = new Datapoints(1,1); // Only one datapoint passed at each time step (the example uses moving frames, so regression can't be done in batch)
+ repro->setVarNames(inputVarNames);
+
+ mat repDataPt; // stores the value of the regression variable (used by 'repro' object)
+ repDataPt = zeros(1, 1); // Keeping it as matrix to remain generic since time-driven movements => size(repDataPt)=1
+ Quaternion q_out(SCALAR_VEC); // a quaternion object to store the output of GMR
+ // -> quatOrder = SCALAR_VEC means that q=[u vec], aka [w x y z] (the default is VEC_SCALAR)
+
+ // Declare Task Parameter objects
+ TaskParameters TPs(nVars, nFrames);
+ TaskParameter auxTP;
+
+ // The frame will encode the orientation of the end-effector with respect to an object
+ // -> the object's orientation will be initialized as the identity but it might change during the task.
+ Quaternion q_frame(SCALAR_VEC); // default constructor -> q_frame=[1 0 0 0]
+
+ // Use these angle-axis variables to define new orientations of the frame during reproductions
+ double angle = M_PI/5;
+ vec3 axis;
+ axis << 0 << 0 << 1 << endr; // [pi/2 , [0 0 1]] -> rotation of 90deg around z-axis
+
+ // First Frame (this example considers one single frame)
+ auxTP.A = eye<mat>(nVars,nVars);
+ auxTP.A.submat(1,1,4,4) = q_frame.matrix();
+ auxTP.b = zeros(nVars,1);
+ TPs.setTaskParameters(0,auxTP);
+
+ for(int tn = 0 ; tn <= 80 ; tn++){ // the model encodes an 8s movement
+
+ // Adjust angle, axis according to new frame orientations
+ q_frame = Quaternion(angle,axis,SCALAR_VEC);
+
+ //Update task-parameters
+ auxTP.A.submat(1,1,4,4) = q_frame.matrix();
+ TPs.setTaskParameters(0,auxTP);
+
+ // Computing the resulting gmm given the set of parameters {A,b}
+ GMM_Model* gmm;
+ gmm = tpgmm->getTransformedGMM(TPs, ORTHONORMAL); // New Implementation of TPGMM
+ gmm->setVARSNames(varNames);
+
+ // Printing model components
+ cout << "Resulting GMM given the set of parameters 'A' and 'b'" << endl;
+ for(uint i = 0 ; i < nStates ; i++){
+ cout << "State #" << i << ":"<< endl;
+ gmm->getCOMPONENTS(i).getMU().print("Mu = ");
+ gmm->getCOMPONENTS(i).getSIGMA().print("Sigma = ");
+ }
+
+ // Computing GMR
+ gmr->setGMMModel(gmm);
+ repDataPt[0] = tn*0.1;
+ repDataPt.print("t = ");
+ repro->setData(repDataPt);
+ GMM_Model* gmmOut = gmr->regression(repro);
+ q_out = Quaternion(gmmOut->getMU(0),SCALAR_VEC);
+ q_out.normalize();
+
+ cout << "\nFrame orientation: q_frame = " ;
+ q_frame.print();
+
+ cout << "\nNew end-effector orientation: q_new = " ;
+ q_out.print();
+
+ cout << "\nEnd-effector orientation with respect to frame: q_rel = " ;
+ (q_out*q_frame.conjugate()).print();
+
+ cout << "\nPress [ENTER] to continue or [CTRL+C] to finish" << endl;
+ std::cin.ignore(1);
+
+ }
+ return 0;
+}
diff --git a/examples/test_tpgmm_timeInvariant.cpp b/examples/test_tpgmm_timeInvariant.cpp
index ce204451d3a22a0a38205316a3e09cb54f231aad..0b9b84872999cb715e411e68a5b93e5573be59e8 100644
--- a/examples/test_tpgmm_timeInvariant.cpp
+++ b/examples/test_tpgmm_timeInvariant.cpp
@@ -40,17 +40,21 @@
#include <math.h>
#include <iostream>
+#ifdef __APPLE__
+ #include <sys/time.h>
+#endif
+
using namespace std;
using namespace pbdlib;
int main(int argc, char **argv)
{
// Filenames that contain model parameters
- std::string fParamsTPGMM("../../data/pgmm/timeInvModel/pgmm_timeInv.txt");
- std::string fVarsTPGMM ("../../data/pgmm/timeInvModel/vars_timeInv.txt");
- std::string fPriorsTPGMM("../../data/pgmm/timeInvModel/priors_timeInv.txt");
- std::string fMuTPGMM ("../../data/pgmm/timeInvModel/Zmu_timeInv"); // Initialize without file extension -> "_P#" will be added depending on nParams
- std::string fSigmaTPGMM ("../../data/pgmm/timeInvModel/Zsigma_timeInv"); // Initialize without file extension -> "_P#" will be added depending on nParams
+ std::string fParamsTPGMM("data/pgmm/timeInvModel/pgmm_timeInv.txt");
+ std::string fVarsTPGMM ("data/pgmm/timeInvModel/vars_timeInv.txt");
+ std::string fPriorsTPGMM("data/pgmm/timeInvModel/priors_timeInv.txt");
+ std::string fMuTPGMM ("data/pgmm/timeInvModel/Zmu_timeInv"); // Initialize without file extension -> "_P#" will be added depending on nParams
+ std::string fSigmaTPGMM ("data/pgmm/timeInvModel/Zsigma_timeInv"); // Initialize without file extension -> "_P#" will be added depending on nParams
// Loading files and setting pgmm variables
vec ModelParams;// The model parameters are loaded from the PGMM model file
diff --git a/examples/test_tphsmm.cpp b/examples/test_tphsmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cafc006af9e1ca3402aedc06f705e7bfd7d6f6ad
--- /dev/null
+++ b/examples/test_tphsmm.cpp
@@ -0,0 +1,221 @@
+/*
+ * test_tphsmm.cpp
+ *
+ * Created on: Sep 17, 2015
+ * Author: Ajay Tanwani
+ */
+
+#include <pbdlib/tphsmm.h>
+
+using namespace arma;
+using namespace pbdlib;
+using namespace std;
+
+int main(int argc, char **argv)
+{
+ // Model variables
+ uint nbFrames = 2;
+ uint nbStates = 7;
+ uint nbVarPos = 7;
+ uint nbVar = 14; //x, y, z, qw, qx, qy, qz, \dot{x}, \dot{y}, \dot{z}, \dot{qw}, \dot{qx}, \dot{qy}, \dot{qz}
+ uint nbD = 200; //Number of datapoints in a demonstration
+ uint nbSamples = 8;
+ double dt = 0.1;
+ uint train_samples_ids[4] = {1, 2, 3, 4};
+ uint test_samples_ids[4] = {5,6,7,8};
+
+ const char* varNames[] = {"x", "y", "z", "qw", "qx", "qy", "qz", "x_dot", "y_dot", "z_dot", "qw_dot", "qx_dot", "qy_dot", "qz_dot"};
+// varNames.push_back("t");
+
+ TPHSMM *myTphsmm;
+// myTphsmm = new TPHSMM(nbVar, nbStates, nbFrames);
+ myTphsmm = new TPHSMM(nbVar, nbStates, nbFrames, nbVarPos, dt, 1E1);
+
+
+ string priorsPath = "./data/tphsmm/HSMM_check_priors.txt";
+ string varNamesPath = "./data/tphsmm/HSMM_check_varnames.txt";
+ string muPrefix = "./data/tphsmm/HSMM_check_mu";
+ string SigmaPrefix = "./data/tphsmm/HSMM_check_sigma";
+
+ string transPath = "./data/tphsmm/HSMM_check_trans.txt";
+ string durMu = "./data/tphsmm/HSMM_check_durMu.txt";
+ string durSigma = "./data/tphsmm/HSMM_check_durSigma.txt";
+
+ // Load Var names from file.
+ std::ifstream varsfile(varNamesPath.c_str()); // Open file stream
+// std::vector<std::string> vars; // Vector to hold variable names
+ std::string varsnames; // tmp string to read line from file
+// std::string buf; // buffer string
+ // We assume that the variable names are on the first line, separated by spaces
+ getline(varsfile,varsnames);
+ cout << varsnames << endl;
+
+ myTphsmm->loadTPHSMMfromFiles(priorsPath, varNamesPath, muPrefix, SigmaPrefix, transPath, durMu, durSigma);
+
+ cout << "Number of States: " << myTphsmm->getNumSTATES() << endl;
+ //..................................................................................
+
+
+ //......... loading the task parameters from the txt files ......
+ string framePrefix = "./data/tphsmm/HSMM_check_Frame_";
+ stringstream frameFile, index;
+
+ std::vector<TaskParameter> cTaskParam;
+ mat rData;
+ colvec init_state;
+
+ //give me the training sample frames
+ for (int i=0; i < 1; i++){
+ frameFile.str("");
+ index.str("");
+ index<<(train_samples_ids[i]);
+
+ frameFile << framePrefix << index.str() << ".txt";
+ cout << "Frame 1" << endl;
+ myTphsmm->setCurrentTaskFrameFromFile(frameFile.str());
+ cout << "Frame 2" << endl;
+ cTaskParam = myTphsmm->getTaskParameters();
+ cout << "Frame 3" << endl;
+ rData = myTphsmm->TPHSMMController();
+ for (int j=0; j < rData.n_rows; j++){
+ for (int k=0; k < rData.n_cols; k++){
+ cout << rData.at(j,k) << ",\t";
+ }
+ cout << endl;
+ }
+// for (int j=0;j < nbFrames; j++){
+// for (int k=0; k < nbVar; k++){
+// cout << cTaskParam[j].b[k] << ",\t";
+// }
+// cout << endl;
+// }
+
+// myTphsmm->TPHSMMController();
+
+
+ }
+
+ //give me the test sample frames
+
+ //generate random frames by interpolation or some other method
+
+ //take product of Gaussians for the current frame
+
+ //generate sample sequence by deterministic and/or stochastic interpolation from the current state for the current frame
+
+ //Update current state
+
+ //Add noise to the current state - mean 0, std, n1
+
+ //Add noist to every state - mean 0, std n2
+
+// std::vector<TPDemonstration> demos; // Vector to hold demonstrations
+// TPDemonstration tmpDemo; // Temp variable to load individual demonstrations
+//
+// char Datafilename[256];
+// char TPfilename[256];
+// cout << "Loading the demonstrations and the task parameters ..." << endl;
+// for (uint m=0; m<nDemos; m++){ //Loading demos in the loop
+// // Form filenames:
+// sprintf(Datafilename, "../data/pgmm/Data0%d.txt",m+1);
+// sprintf(TPfilename, "../data/pgmm/Param0%d.txt",m+1);
+//
+// // Load files in demo:
+// // note we need to transpose the data (last argument)
+// // because the data in the file a row for each data point
+// // and a column for each variable. The agreement is
+// // the other way around
+// tmpDemo.loadFromFiles(Datafilename,TPfilename, true);
+//
+// // push back demonstration:
+// demos.push_back(tmpDemo);
+// }
+//
+// cout << "Demonstrations Loaded succesfully." << endl;
+// cout<<"Press any key to continue..."<<endl; getchar();
+//
+// //..................................................................................
+// //......... Learning the PGMM model parameters using the EM algorithm ......
+// cout<<"Learning model parameters using EM..." << endl;
+//
+// TPGMM *tpgmm;
+// tpgmm = new TPGMM(demos, nStates);
+//
+// cout << "\n Number of EM iterations: " << tpgmm->estimateTensorGMM() << endl;
+//
+// // saving the learned model parameters
+// mat MuTmp = zeros(nVars, nStates*nFrames);
+// mat SigmaTmp = zeros(nVars, nVars*nStates*nFrames);
+// mat Priors = zeros(1,nStates);
+// std::vector<GMM_Model> GMMSresult;
+// GMMSresult = tpgmm->getGMMS();
+// for(uint m=0; m<nFrames; m++){
+// for (uint i=0;i<nStates; i++){
+// MuTmp.col(m*nStates+i) = GMMSresult[m].getMU(i);
+// SigmaTmp.cols(m*nVars*nStates+i*nVars, m*nVars*nStates+i*nVars+nVars-1) = GMMSresult[m].getSIGMA(i);
+// }
+// }
+// sprintf(Datafilename, "../../data/pgmm/Mu0%d.txt",2);
+// MuTmp.save(Datafilename, raw_ascii);
+// sprintf(Datafilename, "../../data/pgmm/Sigma0%d.txt",2);
+// SigmaTmp.save(Datafilename, raw_ascii);
+// sprintf(Datafilename, "../../data/pgmm/Priors0%d.txt",2);
+// Priors = tpgmm->getPRIORS();
+// Priors.save(Datafilename, raw_ascii);
+// cout<<"the learned model parameters are written into text files succesfully."<<endl; cout<<"Press any key to continue..."<<endl; getchar();
+//
+//
+// // Reproduction variables (GMR)
+// GMR *gmr = new GMR();
+// Datapoints *repro = new Datapoints(1,1); // Only one datapoint passed at each time step
+// mat reproData, auxYgmr;
+// std::vector<std::string> reproVarNames;
+// reproVarNames.push_back("t"); // time is the query point
+// repro->setVarNames(reproVarNames);
+//
+// // loading new parameters for the reproduction from the text file
+// // In this case we have considered that the parameters are the same all along the reproduction. They can be varying.
+// TaskParameters ReproTPs(nVars,nFrames);
+// sprintf(TPfilename, "../../data/pgmm/ParamRepro0%d.txt",1);
+// ReproTPs.loadFromFile(TPfilename);
+//
+// GMM_Model* gmm;
+// for(uint tn=1; tn<=nData; tn++){
+// // Computing the resulting GMM given the set of parameters {A,b}
+// gmm = tpgmm->getTransformedGMM(ReproTPs);
+// gmm->setVARSNames(varNames);
+//
+// // Printing model components
+// cout << "Resulting GMM given the set of parameters 'A' and 'b'" << endl;
+// cout << "nbStates : " << gmm->getNumSTATES() << endl;
+// cout << "nbVar : " << gmm->getNumVARS() << endl;
+// cout << "VarNames " ;
+// for (uint i=0;i<gmm->getNumVARS();i++)
+// cout << varNames[i] ;
+// cout << endl;
+//
+// cout << "Priors : " << gmm->getPRIORS() << endl;
+// for(uint i=0; i<nStates; i++){
+// cout << "State #" << i << ":"<< endl;
+// gmm->getMU(i).print("Mu = ");
+// gmm->getSIGMA(i).print("Sigma = ");
+// }
+//
+// // Computing GMR
+// gmr->setGMMModel(gmm);
+// mat repDataPt;
+// repDataPt << (tn * 0.01); // time step as input
+// repDataPt.print("tn = ");
+// repro->setData(repDataPt);
+// GMM_Model* gmmOut = gmr->regression(repro);
+// gmmOut->getMU(0).print("MuOut = ");
+//
+// cout << "Please press [ENTER] to continue or [CTRL+C] to finish" << endl;
+// char key;
+// std::cin.ignore(1);
+// }
+ return 0;
+}
+
+
+
diff --git a/examples/test_trajGMM.cpp b/examples/test_trajGMM.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce91321201f28a17812b82eb759b8eb8f1e010ea
--- /dev/null
+++ b/examples/test_trajGMM.cpp
@@ -0,0 +1,141 @@
+/**
+Copyright (C) 2015, Leonel Rozo, Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+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 Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file test_trajGMM.cpp
+\brief Learning trajectory GMM model
+Learning a trajectory GMM model from demonstrations saved in the file data_xy.txt
+
+\author Leonel Rozo
+\bug No known bugs.
+*/
+
+#include "pbdlib/trajgmm.h"
+#include <sstream>
+
+#define nbStates 5
+#define nbVarPos 2
+#define nbDeriv 3
+#define dt 0.01
+#define nbData 100
+#define nbSamples 4
+
+using namespace pbdlib;
+
+int main(int argc, char **argv)
+{
+ /**********************************************************************************************************/
+ /* Test TrajGMM learning from demos saved in txt files */
+ /**********************************************************************************************************/
+ // Demonstrations variables
+ std::vector<Demonstration> demos;
+ Demonstration demo = Demonstration(nbVarPos,nbData*nbSamples);
+ std::vector<std::string> vars;
+ vars.push_back("x1"); vars.push_back("x2");
+ vars.push_back("v1"); vars.push_back("v2");
+ vars.push_back("a1"); vars.push_back("a2");
+
+ // Trajectory model initialization
+ TrajGMM *myTrajGMM1;
+ myTrajGMM1 = new TrajGMM(nbStates, nbVarPos, nbDeriv, dt);
+ myTrajGMM1->setVARSNames(vars);
+ myTrajGMM1->constructPHI(nbData,nbSamples);
+
+ // Files variables
+ mat posData;
+ std::string posDataPath;
+ std::string commonPath = "data/trajgmm/TrajGMM_posData_Sshape";
+ std::stringstream convert; // stringstream used for the conversion
+
+ // Loading and storing demos
+ for(uint i = 1 ; i <= nbSamples ; i++)
+ {
+ // Loading i-th demo
+ convert.str("");
+ convert << i;
+ posDataPath = commonPath + convert.str() + ".txt";
+ posData.load(posDataPath);
+
+ // Reshaping demonstration data for computing static and dynamic features
+ posData.reshape(nbVarPos*nbData,1);
+ mat trainingData = conv_to<mat>::from(myTrajGMM1->getPHI1()) * (posData * 1E2); //1E2 is used to avoid numerical computation problem
+ trainingData.reshape(nbVarPos*myTrajGMM1->getNumDERIV(), nbData);
+
+ // Storing transformed demo
+ demo.getDatapoints().setData(trainingData);
+ demo.getDatapoints().setVarNames(vars);
+ demos.push_back(demo);
+ }
+ // Setting training data
+ myTrajGMM1->setDEMONSTRATIONS(demos);
+
+ // Learning and printing results
+ cout << "\n Number of EM iterations: " << myTrajGMM1->EM_learn(1E-4);
+ for (int i=0;i<nbStates;i++){
+ cout << "\n Mu_" << i << " = \n" << myTrajGMM1->getMU(i);
+ cout << "\n Sigma_" << i << " = \n" << myTrajGMM1->getSIGMA(i);
+ }
+ cout << "\nPress enter to test TrajGMM using a model learned in MATLAB..." << endl; getchar();
+
+
+ /**********************************************************************************************************/
+ /* Test TrajGMM reproduction using a pre-trained model */
+ /**********************************************************************************************************/
+ // ---> Load data from file (TrajGMM trained with Sshape AMARSI data)
+ std::string muPath = "data/trajgmm/TrajGMM_test_mu.txt";
+ std::string sigmaPath = "data/trajgmm/TrajGMM_test_sigma.txt";
+ std::string priorsPath = "data/trajgmm/TrajGMM_test_priors.txt";
+ std::string varNamesPath = "data/trajgmm/TrajGMM_test_varnames.txt";
+ std::string modelInfoPath = "data/trajgmm/TrajGMM_test_info.txt";
+ std::string testGammaPath = "data/trajgmm/TrajGMM_test_Gamma.txt";
+
+ TrajGMM *myTrajGMM2;
+ myTrajGMM2 = new TrajGMM(priorsPath,muPath,sigmaPath,varNamesPath,modelInfoPath);
+
+ // Loading a precomputed state sequence (from MATLAB)
+ mat q;
+ q.load(testGammaPath);
+ q = q - 1; // Changing state format to C++
+
+ // Computing optimal path from TrajGMM given the state sequence
+ mat data1 = myTrajGMM2->leastSquaresOptimalData(arma::conv_to<colvec>::from(q));
+ // Computing optimal path and related covariance stored in a vector of Gaussian distributions
+ std::vector<GaussianDistribution> optimalDist;
+ optimalDist = myTrajGMM2->leastSquaresOptimalProb(arma::conv_to<colvec>::from(q));
+
+ // Printing and saving data for possible comparisons
+ mat tmpMu = zeros<mat>(myTrajGMM2->getNumVARSPOS(), q.n_elem);
+ mat tmpSigma = zeros<mat>(myTrajGMM2->getNumVARSPOS(), myTrajGMM2->getNumVARSPOS()*q.n_elem);
+ for(uint t = 0 ; t < q.n_elem ; t++)
+ {
+ data1.col(t).print("Mu_1 = ");
+
+ tmpMu.col(t) = optimalDist.at(t).getMU();
+ tmpSigma.cols(t*myTrajGMM2->getNumVARSPOS(),(t+1)*myTrajGMM2->getNumVARSPOS()-1) = optimalDist.at(t).getSIGMA();
+
+ tmpMu.col(t).print("Mu_2 = ");
+ tmpSigma.cols(t*myTrajGMM2->getNumVARSPOS(),(t+1)*myTrajGMM2->getNumVARSPOS()-1).print("Sigma = ");
+ //std::cin.get();
+ }
+ // ---> Saving data for comparison purposes (in MATLAB)
+ //data1.save("data1TrajGMM.mat", raw_ascii);
+ //tmpMu.save("data2muTrajGMM.mat", raw_ascii);
+ //tmpSigma.save("data2sigmaTrajGMM.mat", raw_ascii);
+
+ return 0;
+}
diff --git a/examples/test_trajMPC.cpp b/examples/test_trajMPC.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a2e73ea60afbed890088df8087c5087eed42b8bd
--- /dev/null
+++ b/examples/test_trajMPC.cpp
@@ -0,0 +1,119 @@
+#include <time.h>
+#include "armadillo"
+// Pbdlib dependencies:
+#include "pbdlib/hsmm.h"
+#include "pbdlib/taskparameters.h"
+#include "pbdlib/gmm.h"
+#include "pbdlib/trajMPC.h"
+
+
+using namespace arma;
+using namespace pbdlib;
+using namespace std;
+
+
+int main(int argc, char* argv[])
+{
+
+cout << "Setting Variables: ";
+
+// Model Settings:
+uint nVarsPos = 2; // Number of position variables
+uint nDeriv = 2; // Number of derivatives considered
+
+// TrajMPC Settings:
+uint Np = 70; // Number of predictions
+uint Nc = 30; // Number of control predictions
+uint N = 600; // Number of iterations of simulation
+double dt = 0.01; // time step considered
+double alpha = 0.001; // control cost variable
+
+// Initial state of the system:
+colvec Xinit;
+Xinit << -27.0 << 42.0<< 0.0 << 0.0;
+
+string prefix = "HSMM_test";
+
+// Source Files for the HSMM Model:
+string muPath = "data/hsmm/" + prefix + "_mu.txt";
+string sigmaPath = "data/hsmm/" + prefix + "_sigma.txt";
+string priorsPath = "data/hsmm/" + prefix + "_priors.txt";
+string transPath = "data/hsmm/" + prefix + "_trans.txt";
+string durMu = "data/hsmm/" + prefix + "_durMu.txt";
+string durSigma = "data/hsmm/" + prefix + "_durSigma.txt";
+
+// Create pbdlib objects:
+cout << "Create pbdlib objects ";
+HSMM* myHSMM; // HSMM to handle state sequences
+TrajMPC* myTrajMPC;
+cout << "done " << endl;
+
+// Load HSMM:
+cout << "load HSMM ";
+myHSMM = new HSMM(priorsPath,muPath,sigmaPath,transPath,durMu,durSigma);
+cout << "done " << endl;
+cout << "HSMM nbStates: " << myHSMM->getNumSTATES() << endl;
+cout << "HSMM nbVars : " << myHSMM->getNumVARS() << endl;
+
+// Load TrajMPC:
+cout << "load TrajMPC ";
+myTrajMPC = new TrajMPC(myHSMM,dt,nVarsPos,nDeriv,Np,Nc,alpha);
+cout << "done " << endl;
+cout << "TrajMPC nbVars : " << myTrajMPC->getNumVARS() << endl;
+
+// **********************************************************************
+
+// Make sure that the TrajMPC model is correclty initialized by resetting it before starting
+myTrajMPC->reset();
+
+// Simulate reproduction:
+colvec U;
+mat Ad = myTrajMPC->getSystemDynamics();
+mat Bd = myTrajMPC->getInputDynamics();
+
+//colvec curPos = myTrajMPC->getCurrentAttractor();
+colvec curPos = Xinit;
+cout << "Current Attractor: " << endl <<curPos<< endl;
+mat TestData= zeros(myHSMM->getNumVARS(),N);
+mat Alphas = zeros(myHSMM->getNumSTATES(),N);
+mat ControlData = zeros(nVarsPos,N);
+umat qPredict;
+qPredict.set_size(Np,N);
+
+Alphas.col(0) = myHSMM->getForwardVariable();
+TestData.col(0) = curPos;
+
+int start = clock();
+cout << "Simulation of the reproduction: " << endl;
+for (uint it =1;it<N;it++)
+{
+ // Make step:
+ U = myTrajMPC->computeControlCommand(curPos);
+
+ // Simulate movement of attractor:
+ curPos = Ad*curPos + Bd*U;
+
+ // Display current Pos
+// cout << "it: " << it << ": " << curPos.t() << endl;
+
+ // Save Data
+ TestData.col(it) = curPos;
+ Alphas.col(it) = myHSMM->getForwardVariable();
+ ControlData.col(it) = U.subvec(0,nVarsPos-1);
+ qPredict.col(it) = myTrajMPC->getq();
+
+}
+int end = clock();
+std::cout << "it took " << end - start << " ticks, or " << ((float)end - start)/CLOCKS_PER_SEC << " seconds." << std::endl;
+std::cout << "freq: " << float(N)/(((float)end - start)/CLOCKS_PER_SEC ) << endl;
+
+
+cout << "Writing algorithm output to files... " ;
+TestData.save("trajMPC_states.txt",raw_ascii);
+Alphas.save("trajMPC_alpha.txt", raw_ascii);
+qPredict.save("trajMPC_q.txt",raw_ascii);
+ControlData.save("trajMPC_u.txt",raw_ascii);
+cout << "Done" << endl;
+
+}
+
diff --git a/examples/test_trajMPC2.cpp b/examples/test_trajMPC2.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d20840ecad95ddbe77abd0d2a6bb3e79744a1247
--- /dev/null
+++ b/examples/test_trajMPC2.cpp
@@ -0,0 +1,99 @@
+/* Martijn Zeestraten, December 2015
+ *
+ * Demo shows how the TrajMPC class can be used to generate trajectory distributions
+ *
+ */
+
+#include <time.h>
+#include "armadillo"
+// Pbdlib dependencies:
+#include "pbdlib/hsmm.h"
+#include "pbdlib/taskparameters.h"
+#include "pbdlib/gmm.h"
+#include "pbdlib/trajMPC.h"
+#include "pbdlib/trajdist.h"
+
+
+using namespace arma;
+using namespace pbdlib;
+using namespace std;
+
+
+int main(int argc, char* argv[])
+{
+
+cout << "Setting Variables: ";
+
+// Model Settings:
+uint nVarsPos = 2; // Number of position variables
+uint nDeriv = 2; // Number of derivatives considered
+
+// TrajMPC Settings:
+uint Np, Nc; // These values are set later based on the average duration
+double dt = 0.01; // time step considered
+double alpha = 0.001; // control cost variable
+
+// Initial state of the system:
+colvec Xinit;
+Xinit << -27.0 << 42.0<< 0.0 << 0.0;
+
+string prefix = "HSMM_test";
+
+// Source Files for the HSMM Model:
+string muPath = "data/hsmm/" + prefix + "_mu.txt";
+string sigmaPath = "data/hsmm/" + prefix + "_sigma.txt";
+string priorsPath = "data/hsmm/" + prefix + "_priors.txt";
+string transPath = "data/hsmm/" + prefix + "_trans.txt";
+string durMu = "data/hsmm/" + prefix + "_durMu.txt";
+string durSigma = "data/hsmm/" + prefix + "_durSigma.txt";
+
+// Create pbdlib objects:
+cout << "Create pbdlib objects ";
+HSMM* myHSMM; // HSMM to handle state sequences
+TrajMPC* myTrajMPC;
+cout << "done " << endl;
+
+// Load HSMM:
+cout << "load HSMM ";
+myHSMM = new HSMM(priorsPath,muPath,sigmaPath,transPath,durMu,durSigma);
+cout << "done " << endl;
+cout << "HSMM nbStates: " << myHSMM->getNumSTATES() << endl;
+cout << "HSMM nbVars : " << myHSMM->getNumVARS() << endl;
+
+// We want to generate a trajectory distribution for a trajectory of average duration:
+Np = 0;
+for (int i=0;i<myHSMM->getNumSTATES();i++)
+ Np = Np + myHSMM->getDurMU(i)[0];
+Nc = Np;
+
+// Load TrajMPC:
+cout << "load TrajMPC ";
+myTrajMPC = new TrajMPC(myHSMM,dt,nVarsPos,nDeriv,Np,Nc,alpha);
+cout << "done " << endl;
+cout << "TrajMPC nbVars : " << myTrajMPC->getNumVARS() << endl;
+
+// **********************************************************************
+
+// Make sure that the TrajMPC model is correclty initialized by resetting it before starting
+myTrajMPC->reset();
+
+
+//colvec curPos = myTrajMPC->getCurrentAttractor();
+colvec curPos = Xinit;
+TrajDist TD;
+cout << "Current Attractor: " << endl <<curPos<< endl;
+
+int start = clock();
+cout << "Simulation of the reproduction: " << endl;
+TD = myTrajMPC->computeTrajDist(curPos,false);
+int end = clock();
+std::cout << "it took " << end - start << " ticks, or " << ((float)end - start)/CLOCKS_PER_SEC << " seconds." << std::endl;
+
+
+cout << "Writing algorithm output to files... " ;
+TD.Sigma.save("trajDist_Sigma.txt",csv_ascii);
+TD.Mu.save("trajDist_Mu.txt",csv_ascii);
+cout << "Done" << endl;
+
+}
+
diff --git a/examples/test_trajdist.cpp b/examples/test_trajdist.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1d3829b6341e3700e85bb0f73936a80f808ee22f
--- /dev/null
+++ b/examples/test_trajdist.cpp
@@ -0,0 +1,112 @@
+/* Martijn Zeestraten, December 2015
+ *
+ * Demo shows how the TrajMPC class can be used to generate trajectory distributions
+ *
+ */
+
+#include <time.h>
+#include "armadillo"
+// Pbdlib dependencies:
+#include "pbdlib/hsmm.h"
+#include "pbdlib/taskparameters.h"
+#include "pbdlib/gmm.h"
+#include "pbdlib/trajMPC.h"
+#include "pbdlib/trajdist.h"
+
+
+
+using namespace arma;
+using namespace pbdlib;
+using namespace std;
+
+
+int main(int argc, char* argv[])
+{
+
+cout << "Setting Variables: ";
+
+// Model Settings:
+uint nVarsPos = 2; // Number of position variables
+uint nDeriv = 2; // Number of derivatives considered
+
+// TrajMPC Settings:
+uint Np, Nc; // These values are set later based on the average duration
+double dt = 0.01; // time step considered
+double alpha = 0.001; // control cost variable
+
+// Initial state of the system:
+colvec Xinit;
+Xinit << -27.0 << 42.0<< 0.0 << 0.0;
+
+string prefix = "HSMM_test";
+
+// Source Files for the HSMM Model:
+string muPath = "data/hsmm/" + prefix + "_mu.txt";
+string sigmaPath = "data/hsmm/" + prefix + "_sigma.txt";
+string priorsPath = "data/hsmm/" + prefix + "_priors.txt";
+string transPath = "data/hsmm/" + prefix + "_trans.txt";
+string durMu = "data/hsmm/" + prefix + "_durMu.txt";
+string durSigma = "data/hsmm/" + prefix + "_durSigma.txt";
+
+// Create pbdlib objects:
+cout << "Create pbdlib objects ";
+HSMM* myHSMM; // HSMM to handle state sequences
+TrajMPC* myTrajMPC;
+cout << "done " << endl;
+
+// Load HSMM:
+cout << "load HSMM ";
+myHSMM = new HSMM(priorsPath,muPath,sigmaPath,transPath,durMu,durSigma);
+cout << "done " << endl;
+cout << "HSMM nbStates: " << myHSMM->getNumSTATES() << endl;
+cout << "HSMM nbVars : " << myHSMM->getNumVARS() << endl;
+
+// We want to generate a trajectory distribution for a trajectory of average duration:
+Np = 0;
+for (int i=0;i<myHSMM->getNumSTATES();i++)
+ Np = Np + myHSMM->getDurMU(i)[0];
+Nc = Np;
+
+// Load TrajMPC:
+cout << "load TrajMPC ";
+myTrajMPC = new TrajMPC(myHSMM,dt,nVarsPos,nDeriv,Np,Nc,alpha);
+cout << "done " << endl;
+cout << "TrajMPC nbVars : " << myTrajMPC->getNumVARS() << endl;
+
+// **********************************************************************
+
+// Make sure that the TrajMPC model is correclty initialized by resetting it before starting
+myTrajMPC->reset();
+
+
+//colvec curPos = myTrajMPC->getCurrentAttractor();
+colvec curPos = Xinit;
+TrajDist TD;
+cout << "Current Attractor: " << endl <<curPos<< endl;
+
+int start = clock();
+cout << "Simulation of the reproduction: " << endl;
+TD = myTrajMPC->computeTrajDist(curPos,false);
+
+// linear transformation
+mat A = eye(nVarsPos,nVarsPos);
+colvec b = zeros(nVarsPos,1);
+
+cout << "Linear transformation... ";
+TrajDist TDTrans = TD.lintrans(A,b); // Linear transformation
+cout << "Done" << endl;
+cout << "Product... "<< endl;
+TrajDist Prod = TDTrans*TD; // Product of Gaussian
+cout << "Done... " << endl;
+
+int end = clock();
+std::cout << "it took " << end - start << " ticks, or " << ((float)end - start)/CLOCKS_PER_SEC << " seconds." << std::endl;
+
+
+cout << "Writing algorithm output to files... " ;
+TD.Sigma.save("trajDist_Sigma.txt",csv_ascii);
+TD.Mu.save("trajDist_Mu.txt",csv_ascii);
+cout << "Done" << endl;
+
+}
+
diff --git a/include/pbdlib/adhsmm.h b/include/pbdlib/adhsmm.h
new file mode 100644
index 0000000000000000000000000000000000000000..e7ce66c41b6aecea0b8552c47bf50d8a25ad6260
--- /dev/null
+++ b/include/pbdlib/adhsmm.h
@@ -0,0 +1,94 @@
+/**
+
+
+Copyright (C) 2015, Leonel Rozo
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+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 Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file
+\brief ADHSMM class
+The class ADHSMM implements an adaptive duration Hidden semi-Markov model where both the model states and duration probabilities are represented
+by a Gaussian mixture model. The training phase is carried out by a HMM-based approach.
+
+\author Leonel Rozo
+\bugs No bugs known
+\todo Handling duration GMM with different number of components for every ADHSMM state
+\todo Determine if a composition pattern may prevent inheritance for some methods of HSMM (e.g., getDurationCOMPONENTS())
+*/
+
+#ifndef ADHSMM_H
+#define ADHSMM_H
+
+#include "pbdlib/datapoints.h"
+#include "pbdlib/hsmm.h"
+#include "pbdlib/demonstration.h"
+#include "pbdlib/mvn.h"
+#include "pbdlib/gmm.h"
+#include "pbdlib/gmr.h"
+#include "armadillo"
+#include <math.h>
+
+using namespace arma;
+
+namespace pbdlib
+{
+
+class ADHSMM : public HSMM
+{
+ public:
+ // Constructors
+ ADHSMM(const std::string &priors_path,
+ const std::string &mu_path,
+ const std::string &sigma_path,
+ const std::string &transition_path,
+ const std::string &durPriors_path,
+ const std::string &durMu_path,
+ const std::string &durSigma_path,
+ uint T);
+
+ // Destructor
+ ~ADHSMM(){}
+
+ // Set and get functions for GMM encoding duration probabilities
+ void setNumSTATESDUR(uint _nSTATESDUR);
+ uint getNumSTATESDUR(){return nSTATESDUR;};
+ void setNumVARSDUR(uint _nVARSDUR);
+ uint getNumVARSDUR(){return nVARSDUR;};
+ void setDurationGMMs(std::vector<GMM_Model> durGMMs);
+ std::vector<GMM_Model>& getDurationGMMs();
+ GMM_Model& getDurationGMMs(uint idGMM);
+
+ // -> Forward variables for recursive (readable) computation:
+ void stepRecursiveForwardVariable(colvec _u, urowvec _varIn, urowvec _varOut, uint _tn); // Forward variable computation without observation
+ void stepRecursiveForwardVariable(colvec _u, urowvec _varIn, urowvec _varOut, uint _tn, mat& _obs); // Forward variable computation with full observation
+ void stepRecursiveForwardVariable(colvec _u, urowvec _varIn, urowvec _varOut, uint _tn, mat& _obs, urowvec _ind); // Forward variable computation with partial observation
+
+ private:
+ std::vector<GMM_Model> durationGMMs;// Vector of GMM for adaptive duration state probabilities
+ GMR *condDurationProb; // GMR for computing conditional duration probabilities
+ GMM_Model *condPd; // Resulting conditional duration probability obtained from GMR
+ uint nVARSDUR; // Number of variables for duration probabilities
+ uint nSTATESDUR; // Number of states for duration probabilities. It is assumed that every GMM has the same number.
+
+ void computeConditionedDurationProbs(uint _i, colvec u, urowvec _varIn, urowvec _varOut); // Computes the conditional duration probabilities for a given external input
+ void initializeRecursiveForwardVariable(uint T); // Initializes the forward variable
+ void initializeCondDurProb(mat durGMMpriors, mat durGMMmu, mat durGMMsigma); // Initialize duration state probabilities
+
+};
+} // PbdLib namespace
+
+#endif
diff --git a/include/pbdlib/gmm.h b/include/pbdlib/gmm.h
index eb699686d0183761f8faf458ee5293fbd673f442..0b425604f79dd08daa9bb7ce3e8b9b79532230b3 100644
--- a/include/pbdlib/gmm.h
+++ b/include/pbdlib/gmm.h
@@ -23,6 +23,10 @@ The class GMM_model allows to use a Gaussian Mixture Model and to learn the para
\author Davide De Tommaso, Milad Malekzadeh
\bug No known bugs.
+\todo Make variables/methods to be inherited by HMM-related classes protected members (e.g., nVARS, nSTATES, PRIORS, learnKMEANS(), etc).
+\todo EM() may be a virtual function to force re-implementation in child classes such as HMM.
+\todo getProbability may be implemented as a template function so that both "mat" and "colvec" can be passed/returned to/by the function.
+
*/
#ifndef GMM_H
@@ -42,15 +46,19 @@ namespace pbdlib
class GMM_Model
{
private:
- uint nVARS, nSTATES;
- std::vector<GaussianDistribution> COMPONENTS;
- rowvec PRIORS;
- std::vector<Demonstration> DEMONSTRATIONS;
- std::vector<std::string> vars_names;
- void learnKMEANS(double regularization = 0.);
- bool EM_isfinished(double l_old, double l_new);
- uint EM(double likelihood,double regularization);
+ protected:
+ uint nVARS, nSTATES;
+ rowvec PRIORS;
+ mat gamma;
+
+ std::vector<GaussianDistribution> COMPONENTS;
+ std::vector<Demonstration> DEMONSTRATIONS;
+ std::vector<std::string> vars_names;
+
+ void learnKMEANS(double regularization = 0.);
+ bool EM_isfinished(double l_old, double l_new);
+ uint EM(double likelihood,double regularization);
public:
GMM_Model(std::vector<Demonstration> &demos, uint _nSTATES);
@@ -60,7 +68,7 @@ class GMM_Model
~GMM_Model(){}
void onlineEMDP(int N,colvec P,double lambda, double minSigma);
- uint EM_learn(double regularization = 1E-5);
+ uint EM_learn(double regularization = 1E-5);
std::vector<std::string>& getVARSNames();
std::string getVARSNames(int);
uint getIndexOfVARName(const std::string& varname);
@@ -73,11 +81,15 @@ class GMM_Model
colvec& getMU(int);
mat& getSIGMA(int);
mat& getLAMBDA(int);
+ mat& getGamma();
- double getProbability(const colvec& sample);
+ double getProbability(const colvec& sample, bool usePriors = true);
+ colvec getProbability(const mat& samples, bool usePriors = true);
double getLikelihood(const mat& SAMPLES);
bool addDemo(Demonstration &demo);
+ void deleteCOMPONENT(int);
+ void setDEMONSTRATIONS(std::vector<Demonstration> demons);
void setPRIORS(const rowvec& priors);
void setCOMPONENTS(const std::vector<GaussianDistribution>& components);
void setMU(int, const colvec&);
diff --git a/include/pbdlib/gmr.h b/include/pbdlib/gmr.h
index 93f9a7834e7516f35f397c76fe19c786e0ce19d0..1b1726262614d22b345807dcee588f795a2b98d2 100644
--- a/include/pbdlib/gmr.h
+++ b/include/pbdlib/gmr.h
@@ -78,6 +78,7 @@ class GMR
regression(): Gaussian mixture regression
*/
void regression(GMM_Model*, mat Data_in, urowvec Var_In, urowvec Var_Out);
+ void regression2(GMM_Model*, mat Data_in, urowvec Var_In, urowvec Var_Out, double RegF);
GMM_Model* regression(Datapoints* data_in);
GMM_Model* regression(mat Data_in, urowvec Var_In, urowvec Var_Out);
diff --git a/include/pbdlib/hmm.h b/include/pbdlib/hmm.h
index ed3bd35e5f4b0c5e4d7348f07ff8e252ec568079..d4d68a60301761b778a3425f2a8bf4bd21f1f96e 100644
--- a/include/pbdlib/hmm.h
+++ b/include/pbdlib/hmm.h
@@ -19,8 +19,10 @@ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
*/
/*! \file
-\brief HMM_model class
-The class GMM_model allows to use a Gaussian Mixture Model and to learn the parameters from task demonstrations
+\brief HMM class
+The class HMM implements learning for a classical hidden Markov model.
+
+\todo Implement the whole class.
*/
@@ -39,15 +41,13 @@ using namespace arma;
namespace pbdlib{
class HMM: public GMM_Model
{
- private:
- uint nVARS, nSTATES;
- rowvec alpha; // Current value for alpha
- // GMM Components:
- GMM_Model* gmm;
+ protected:
+ colvec alpha; // Forward variable
// HMM Extension:
mat TransitionMatrix;
-
+
+ private:
// EM Functions (overrides of GMM parts)
void learnKMEANS();
uint EM(double likelihood);
@@ -62,12 +62,12 @@ class HMM: public GMM_Model
uint EM_Learn(); // Override function
- double getProbability(const colvec& sample); // override
- double getLikelihood(const mat& SAMPLES); // override
+ //double getProbability(const colvec& sample); // override
+ //double getLikelihood(const mat& SAMPLES); // override
void setTRANSITION(const mat&);
};
+} // End pbdlib namespace
#endif
-} // End pbdlib namespace
diff --git a/include/pbdlib/hsmm.h b/include/pbdlib/hsmm.h
index e1b3a76dce8f696fc727ce11e8f31e03b16ad3db..50c7764f0f127e31cd61df401da10e694c923640 100644
--- a/include/pbdlib/hsmm.h
+++ b/include/pbdlib/hsmm.h
@@ -1,7 +1,7 @@
/**
-Copyright (C) 2015, Martijn Zeestraten
+Copyright (C) 2015, Martijn Zeestraten, Leonel Rozo, Ioannis Havoutis
This file is part of PbDLib.
@@ -20,9 +20,13 @@ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
*/
/*! \file
-\brief HMM_model class
-The class HSMM_model allows to use a Gaussian Mixture Model and to learn the parameters from task demonstrations
+\brief HSMM class
+The class HSMM implements a Hidden semi-Markov model where the model states are represented by a Gaussian mixture model,
+and the training phase is carried out by a HMM-based approach.
+\author Martijn Zeestraten
+\author Leonel Rozo
+\author Ioannis Havoutis
*/
#ifndef HSMM_H
@@ -42,29 +46,50 @@ namespace pbdlib
class HSMM:public HMM
{
- private:
- // HMM Components:
+ protected:
+ // -> Variables for computing forward variable recursively (readable):
+ colvec scalingFtr; // Scaling factor for recursive computations
+ mat recAlpha; // Recursive alpha variable
+
+ // Variables for state duration
+ mat Pd; // Matrix with precomputed probability values for the duration;
+ uint PdSize; // Maximum maximum duration step
+
+ // -> Functions for recursive forward variable
+ void initializeRecursiveForwardVariable(uint T, bool initDur = true);
+
std::vector<GaussianDistribution> DurationCOMPONENTS;
- // Variables for calculating Forward Variable:
+ //For demonstration integration
+ mat hsmm_transition; //= zeros(init_nStates, init_nStates);
+ mat hsmm_transition_ticks; //= zeros(init_nStates, init_nStates);
+ rowvec hsmm_priors; //= zeros(init_nStates);
+ rowvec hsmm_priors_ticks; // = zeros(init_nStates);
+
+ private:
+ // -> Variables for calculating Forward Variable efficiently:
GaussianDistribution* Gtmp;
- mat Atmp1, Atmp2; // Help variables for the calculation of alpha variables
- mat ALPHA; // Variable to keep track of past alphas
- colvec bmx; // variable used to keep track of observation probability P(x|model)
- colvec btmp; // Unnormalized observation probabilities
- colvec S; //
+ mat Atmp1, Atmp2; // Help variables for the calculation of alpha variables
+ mat ALPHA; // Variable to keep track of past alphas
+ colvec bmx; // variable used to keep track of observation probability P(x|model)
+ colvec btmp; // Unnormalized observation probabilities
+ colvec S; // Probability of a state starting at t+1 given a partial observation
- mat Pd; // Matrix with precomputed probabilty values for the duration;
- uint PdSize;
- bool Initialized,tmpInit; // Variables used to check if alpha calculation is initialized
+ // Initialization flags
+ bool Initialized,tmpInit; // Variables used to check if alpha calculation is initialized
// For predictions:
- colvec alphatmp; // Temp variable to hold in the loop alpha
- mat ALPHAtmp; // to store ALPHA in the prediction loop
- mat AlphaPred; // Matrix to hold predictions
- colvec Stmp; // to store S in the prediction loop
+ colvec alphatmp; // Temp variable to hold in the loop alpha
+ mat ALPHAtmp; // to store ALPHA in the prediction loop
+ mat AlphaPred; // Matrix to hold predictions
+ colvec Stmp; // to store S in the prediction loop
- void initializeFwdCalculation();
+
+
+ std::vector <running_stat<double> > hsmm_duration_stats;
+
+
+// void initializeFwdCalculation();
void updateBtmp(colvec& _Btmp,colvec& _obs);
void updateBtmp(colvec& _Btmp,colvec& _obs,urowvec& _ind);
void updateBmx(colvec& _bmx, mat& ALPHA, colvec& _btmp);
@@ -75,18 +100,16 @@ class HSMM:public HMM
void updateAlpha(colvec& _alpha, mat& ALPHA, colvec& btmp);
void updateAlpha(colvec& _alpha, mat& ALPHA);
-
- // Forward variable Functions
+ // -> Forward variable Functions for efficient computation
void initializeForwardVariable(); // initialization without observation
void initializeForwardVariable(colvec& obs); // initialization with observation
- void initializeForwardVariable(colvec& obs, urowvec& ind); // initializaiton with observation
-
+ void initializeForwardVariable(colvec& obs, urowvec& ind); // initialization with observation
void lstepForwardVariable(); // Forward step without observation
void lstepForwardVariable(colvec& obs); // Forward step with observation
void lstepForwardVariable(colvec& obs,urowvec& ind); // Forward step with partial observation
- colvec alpha; // Forward variable
public:
+ // Constructors
HSMM(uint _nSTATES, uint _nVARS);
HSMM(const std::string &priors_path,
const std::string &mu_path,
@@ -94,30 +117,55 @@ class HSMM:public HMM
const std::string &transition_path,
const std::string &durMu_path,
const std::string &durSigma_path);
+ HSMM(const std::string &priors_path,
+ const std::string &mu_path,
+ const std::string &sigma_path,
+ const std::string &transition_path,
+ const std::string &durMu_path,
+ const std::string &durSigma_path,
+ uint T, bool initDur = true);
+ // Destructor
~HSMM(){}
- // Properties:
- std::vector<GaussianDistribution>& getDurationCOMPONENTS();
- GaussianDistribution& getDurationCOMPONENTS(uint);
- colvec& getDurMU(uint);
- mat& getDurSIGMA(uint);
-
- // Forward Variables:
- void stepForwardVariable(); // Forward step without observation
- void stepForwardVariable(colvec& obs); // Forward step with observation
+ // -> Forward Variables for efficient computation:
+ void stepForwardVariable(); // Forward step without observation
+ void stepForwardVariable(colvec& obs); // Forward step with observation
void stepForwardVariable(colvec& obs,urowvec& ind); // Forward step with partial observation
colvec& getForwardVariable(){return alpha;} // Get current forward variable
- void resetForwardVariable(){Initialized=false;}
-
+ void resetForwardVariable(){initializeFwdCalculation();}
mat& predictForwardVariable(uint N); // Predict forward variable without predicted observations
void predictForwardVariable(mat& _AlphaPred); // Predict forward variable without predicted observations (implementation for real-time)
+ void clear();
+ void predictForwardVariableDeterministic(mat& _AlphaPred, int startState);
+ void predictForwardVariableStochastic(mat& _AlphaPred);
+ void predictForwardVariableStochasticStart(mat& _AlphaPred, int startState);
+ void integrateDemonstration(Demonstration demo);
+ void get_state_seq(uword state_seq[], mat pred);
+ int getClosestState(const colvec P);
+ void initializeFwdCalculation();
+ // -> Forward variables for classic (readable) computation:
+ void stepRecursiveForwardVariable(uint tn); // Forward variable computation without observation
+ void stepRecursiveForwardVariable(uint tn, mat& obs); // Forward variable computation with full observation
+ void stepRecursiveForwardVariable(uint tn, mat& obs, urowvec ind); // Forward variable computation with partial observation
+ mat& getRecursiveForwardVariable(){return recAlpha;}; // Returning matrix containing forward variable values
+ void resetRecursiveForwardVariable(); // Set to zero the matrix containing the forward variable values
+
+ // State duration probabilities (set and get)
void setDurationCOMPONENTS(const std::vector<GaussianDistribution>& components);
+ std::vector<GaussianDistribution>& getDurationCOMPONENTS();
+ GaussianDistribution& getDurationCOMPONENTS(uint);
void setDurMU(int, const colvec&);
+ colvec& getDurMU(uint);
void setDurSIGMA(int, const mat&);
+ mat& getDurSIGMA(uint);
+ void setMaxDuration(uint maxPd);
+ uint getMaxDuration(){return PdSize;};
+ void saveInFiles(std::string path);
+ void loadFromFiles(std::string path);
};
} // PbdLib namespace
diff --git a/include/pbdlib/lqr.h b/include/pbdlib/lqr.h
index fcf74a81dd04004d2e1dc761cedca0c6070885e5..793dcc6100caeacaf6d71c524c3f17e9bccdbd8f 100644
--- a/include/pbdlib/lqr.h
+++ b/include/pbdlib/lqr.h
@@ -75,22 +75,37 @@ class LQR
LQR(mat,mat,double);
//LQR(GMM_Model* model);
//mat reproduction_finiteHorizon(mat,mat,colvec);
+ bool evaluate_gains_finiteHorizon_discrete(mat,colvec);
bool evaluate_gains_finiteHorizon(mat,colvec);
bool evaluate_gains_infiniteHorizon();
+
+
+ bool evaluate_gains_infiniteHorizon_Discrete();
//Datapoints* reproduction_finiteHorizon(Datapoints*, Datapoints*, Datapoint);
//Datapoints* reproduction_infiniteHorizon(Datapoints*, Datapoints*, Datapoint);
void setQ(std::vector<mat> _Q){this->Q = _Q;};
- void setR(mat _R){this->R = R;};
+ void setR(mat _R){this->R = _R;};
void setTarget(mat _Target){this->Target = _Target;};
+ double getdt(){return this->dt;};
+ mat getR(){return this->R;};
+ mat getA(){return this->A;};
+ mat getB(){return this->B;};
+ std::vector<mat> getS(){return this->S;};
+ mat getd(){return this->d;};
+
std::vector<mat> getGains(){return this->L;};
mat getFF(){return this->M;};
- bool setProblem(mat,std::vector<mat>,mat);
+ void setProblem(mat,std::vector<mat>,mat);
mat diff(mat);
mat solveAlgebraicRiccati(mat,mat,mat,mat);
+ mat solveAlgebraicRiccati_Discrete(mat,mat,mat,mat);
+
+
+
};
} //end pbdlib namespace
diff --git a/include/pbdlib/mpc.h b/include/pbdlib/mpc.h
new file mode 100644
index 0000000000000000000000000000000000000000..179b7da6da7944a0c80fa4297a64725bcb78eee3
--- /dev/null
+++ b/include/pbdlib/mpc.h
@@ -0,0 +1,106 @@
+/**
+Copyright (C) 2015, Martijn Zeestraten
+
+This file is part of PbDLib (Programming-by-demonstration C++ Library).
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ 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 Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file mpc.h
+\brief lwr class
+ This class is the implementation of a minimal intervention controller with
+ linear quadratic regulation, as described in:
+
+\author Martijn Zeestraten
+\bug No known bugs.
+*/
+
+
+#ifndef MPC_H_
+#define MPC_H_
+
+#include "armadillo"
+#include "pbdlib/trajdist.h"
+
+using namespace arma;
+namespace pbdlib {
+class MPC
+{
+ public:
+
+ // Constructor
+ // A: [nbVar x nbVar] System matrix of discretized system
+ // B: [nbVar x nbContr] Input Matrix of the discretized system
+ // C: [nbVar x nbVarOutput] Output Matrix
+ // Np: Number of state predictions made during MPC
+ // Nc: Number of control actions predicted during MPC
+ MPC(mat& _Ad, mat& _Bd, mat& _C, uint _Np, uint _Nc);
+
+ // Function that computes the control command
+ // X: [nbVar x 1] Input vector size nbVar
+ // MuQ: [nbVar*Np x 1] Vector of state reference on the prediction horizon
+ // Q: [nbVar*Np x nbVar*Np] Matrix representing the tracking costs
+ // R: [nbContr*Nc x nbContr*Nc] Matrix representing the control cost
+ colvec& computeControlCommand(colvec& X, colvec& Muq, mat& Q, mat& R);
+ // compute distribution of control commands
+ // U: [nbVar*Np x 1] Mean of control distribution
+ // SigmaU:[nbVar*Np x nbVar*Np] Covariance matrix of control distribution
+ // X: [nbVar x 1] Input vector size nbVar
+ // MuQ: [nbVar*Np x 1] Vector of state reference on the prediction horizon
+ // Q: [nbVar*Np x nbVar*Np] Matrix representing the tracking costs
+ // R: [nbContr*Nc x nbContr*Nc] Matrix representing the control cost
+ TrajDist& computeTrajDist(colvec& X, colvec& ref, mat& Q, mat& R);
+
+
+ // Function to set the system matrices:
+ // A: [nbVar x nbVar] System matrix of discretized system
+ // B: [nbVar x nbContr] Input Matrix of the discretized system
+ // C: [nbVar x nbVarOutput] Output Matrix
+ void setSystem(mat& _A, mat& _B, mat& _C);
+
+ // Function to set the prediction horizon:
+ void setPredictionHorizon(uint _Np);
+
+ // Function to set the control horizon:
+ void setControlHorizon(uint _Nc);
+
+ // Getters:
+ uint getNp(){return Np;}
+ uint getNc(){return Nc;}
+
+ private:
+ mat F; // dynamic matrix
+ mat Phi; // Input matrix
+ mat CT; // C M
+ mat I; // Output matrix (to select first control command)
+
+ mat PhiQPhiR;// help variables for the least square solver
+ vec PhiQMuqFx;
+ uint nbVar; // Number of tracking variables
+ uint nbContr; // Number of control variables
+ colvec U;
+ TrajDist TD;
+
+ mat A, B, C; // System matrices
+
+ void constructDynamicMatrices(mat& A, mat& B, mat& C,uint _Np, uint _Nc);
+ void prepareForLeastSquares();
+
+ protected:
+ uint Np, Nc; // Prediction and control Horizon
+
+};
+} // end of pbdlib namespace
+
+#endif
diff --git a/include/pbdlib/mvn.h b/include/pbdlib/mvn.h
index fe3a1f295be80c9d63a8033ab2cb9e5c7f103269..c6063cda5f3ac3c53080f9c291bdbbb502eeb7a7 100644
--- a/include/pbdlib/mvn.h
+++ b/include/pbdlib/mvn.h
@@ -48,7 +48,9 @@ private:
uint nVARS;
mat SIGMA; // Covariance Matrix
mat LAMBDA; // Precision Matrix
+ double DET; // determinant
colvec MU;
+ bool DETupdated; // indicates if the determinant has already been computed
public:
GaussianDistribution(uint _nVARS);
@@ -58,9 +60,14 @@ public:
colvec& getMU();
mat& getSIGMA(); // Covariance Matrix
mat& getLAMBDA(); // Precision Matrix
+ double getDET();
+ // Get divergence measure
+ double getKLdiv( GaussianDistribution&);
// Get PDF Values
colvec getPDFValue(const mat& SAMPLES); // simple implementation
+ colvec getPDFValue(const mat& SAMPLES, urowvec ind); // simple implementation with specific indexes to use
void getPDFValue(colvec& prob, mat SAMPLES);// Implementation for real time execution: doesnt require memory allocation:
+ void getPDFValue(colvec& prob, mat SAMPLES, urowvec ind);// Implementation for real time execution: doesnt require memory allocation:
mat stochasticSampling( uint nbS);
mat sqrtm(const mat SIGMA);
@@ -69,11 +76,17 @@ public:
void setParamsFromData(const mat SAMPLES, const rowvec REWARD);
void setParamsFromData(const mat SAMPLES, const rowvec REWARD, const uint nbImportanceSampling);
+ void updateDET();
void setMU(const colvec& _MU);
void setSIGMA(const mat& _SIGMA);
void setLAMBDA(const mat& _LAMBDA);
void setNumVARS(uint numvars);
+ // Define operations for TrajectoryDistributions:
+ GaussianDistribution& operator=(const GaussianDistribution& rhs); // Copying
+ GaussianDistribution& operator*=(const GaussianDistribution& rhs); // Product of Gaussian
+ GaussianDistribution operator*(const GaussianDistribution& rhs); // Product of Gaussian
+
};
} //end of namespace pbdlib
diff --git a/include/pbdlib/quaternion.h b/include/pbdlib/quaternion.h
new file mode 100644
index 0000000000000000000000000000000000000000..04d9e07d958f0b4b245653152acc2c0eac6f6c22
--- /dev/null
+++ b/include/pbdlib/quaternion.h
@@ -0,0 +1,114 @@
+/**
+Copyright (C) 2015, João Silvério
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ 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 Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*! \file quaternion.h
+\brief Defines a class for basic quaternion handling using armadillo functions and types.
+
+It aims to facilitate quaternion modeling in task-parameterized models.
+
+The code is adapted from http://www.cs.stanford.edu/~acoates/quaternion.h to:
+ -> use armadillo data types
+ -> allow the usage of quaternions as [u vec] or [vec u]
+ -> compute the quaternion matrix that permits changing multiplication order
+
+Functions will be implemented gradually on a need-to basis.
+
+\author João Silvério
+\bug No known bugs.
+ */
+
+
+#ifndef QUATERNION_H
+#define QUATERNION_H
+
+#include <iostream>
+#include <math.h>
+#include <sstream>
+#include "armadillo"
+
+using namespace arma;
+
+enum SortConvention { SCALAR_VEC , VEC_SCALAR };
+
+namespace pbdlib{
+
+class Quaternion {
+
+ colvec4 coeffs;
+ SortConvention sort;
+
+public:
+
+ // Constructors
+ Quaternion(SortConvention sortInit = VEC_SCALAR); // Default constructor - initializes identity quaternion
+ Quaternion(const vec3& vec, double scalar, SortConvention sortInit = VEC_SCALAR); // Initialize a quaternion from vec3 and scalar
+ Quaternion(const vec4& vec, SortConvention sortInit = VEC_SCALAR); // Initialize a quaternion from a vec4
+ Quaternion(const double* array, SortConvention sortInit = VEC_SCALAR); // Initialize a quaternion from an array of double
+ Quaternion(double q1, double q2, double q3, double q4, SortConvention sortInit = VEC_SCALAR); // Initialize a quaternion from 4 scalars of type double
+ Quaternion(double angle, vec3 axis, SortConvention sortInit = VEC_SCALAR); // Initialize a quaternion from an angle and an axis
+// Quaternion(const vec3& angleAxis, SortConvention sortInit = VEC_SCALAR); // Initialize a quaternion from a vec3 whose direction is the rotation axis and norm is the angle of rotation
+
+ // Coefficients
+ double x() const; // Return the quaternion elements as x,y,z,w
+ double y() const; // -> using .x(),.y(),.z(),.w() may read more neatly though (it also avoids lots of 'if' clauses in member functions)
+ double z() const;
+ double w() const;
+ vec3 vector() const; // Return the vector part of a quaternion as a vec3 type
+ double scalar() const { return w();} // Return the scalar part of a quaternion
+ colvec4 getAllCoeffs() const { return coeffs;} // Return the quaternion elements as a colvec4
+ SortConvention getSort() const {return sort;} // Return the sorting type
+ void setVector(const vec3& vec);
+ void setScalar(double scalar);
+ void setAllCoeffs(const vec4& vec);
+
+ void changeSortConvention(SortConvention c_sort);
+ // Properties
+ Quaternion conjugate(void) const {return Quaternion(-vector(), scalar(), sort);} // Returns quaternion conjugate
+ double norm() const { return sqrt(dot(coeffs,coeffs));} // Computes quaternion norm
+ Quaternion inverse(void) const {return conjugate() / norm();} // Computes quaternion inverse (same thing as conjugate if norm==1, just slower)
+
+ // Operations
+ void normalize(){coeffs /= norm();} // Normalize the quaternion
+ Quaternion product(const Quaternion& rhs) const; // Computes quaternion product
+ Quaternion operator*(const Quaternion& rhs) const {return product(rhs);} // Quaternion product operator
+ vec3 operator*(const vec3& rhs) const; // Quaternion rotation operator
+ Quaternion operator-() const {return Quaternion(-(this->coeffs),sort);} // Unary negation
+ Quaternion operator/(double s) const; // Quaternion scalar division operator.
+
+ // Quaternion matrices
+ mat44 matrix() const; // Quaternion matrix
+ mat44 matrixReverse() const; // Quaternion matrix for switching quaternion order in product
+
+ // Stdout
+ void print(){ cout << " " << w() << " <" << x() << ", " << y() << ", " << z() << ">" << endl;} // Prints the quaternion
+};
+
+// SO(3) conversions
+double inner(const Quaternion& q1, const Quaternion& q2); // inner product between two quaternions
+vec3 qLog(const Quaternion& q); // Logarithm of a quaternion
+Quaternion qExp(const vec3& aa, SortConvention sortType = VEC_SCALAR); // Exponential of an angular displacement (= a quaternion) [maybe the name of the function may be improved?]
+vec3 angDiff(const Quaternion& qh, const Quaternion& q); // Rotation between two quaternions as a 3D vector (angular displacement)
+
+// SLERP
+Quaternion SLERP(const double& t, Quaternion qFrom, const Quaternion& qTo);
+Quaternion SLERP0(const double& t, Quaternion qFrom, const Quaternion& qTo);
+
+};
+
+#endif /* QUATERNION_H */
diff --git a/include/pbdlib/taskparameters.h b/include/pbdlib/taskparameters.h
index 337e05248ec4a2ec7b0cb64cf0b20f4f07bd2b4f..73677b07df9df1448c4d86946bd37684c2d29f6d 100644
--- a/include/pbdlib/taskparameters.h
+++ b/include/pbdlib/taskparameters.h
@@ -69,7 +69,6 @@ class TaskParameters
uint nVARS; // Number of variables
public:
- TaskParameters(){}
~TaskParameters(){}
TaskParameters(uint nVARS,uint nTaskParameters);
diff --git a/include/pbdlib/tpdpgmm.h b/include/pbdlib/tpdpgmm.h
new file mode 100644
index 0000000000000000000000000000000000000000..cf22560d4790200af25ded02ec88bd663b770440
--- /dev/null
+++ b/include/pbdlib/tpdpgmm.h
@@ -0,0 +1,123 @@
+//
+// Created by pignate on 22.10.15.
+//
+
+#ifndef PBDLIB_TPDPGMM_H
+#define PBDLIB_TPDPGMM_H
+
+#define P_1VAR 0.6065
+
+#include "armadillo"
+#include "pbdlib/gmm.h"
+#include "pbdlib/tpgmm.h"
+#include "pbdlib/taskparameters.h"
+#include "pbdlib/tphsmm.h"
+
+using namespace arma;
+
+namespace pbdlib {
+
+#define REALMIN 2.2251e-200
+#define REALMAX 1.7977e200
+
+class TPDPGMM: public TPGMM{
+
+private:
+ uint nbPoints;// nb of points already added to the model
+ uint prevState;
+ float lambda;
+ float lambda2;
+ float lambda3;
+ float minSigma;
+ uint timeStepCNT; //count the time we stayed in the same states
+
+ mat framePRIORS; // prior on the frame in which the state was selected [nSTATES x NTaskparameters+1]
+ bool framePRIORSProd; // using or not priors on frame to make the product
+ TPHSMM* tpTrans;
+
+public:
+ TPDPGMM(uint nVARS, uint nSTATES, uint _nTPs, uint /* nTPS for transition mat */_nTPsTr,
+ float _lambda=50.f,float _minSigma = 2E2)
+ :TPGMM(nVARS,nSTATES,_nTPs)
+ {
+ lambda = _lambda;
+ lambda3 = _lambda;
+ lambda2 = _lambda;
+ nbPoints = 0;
+ minSigma = _minSigma;
+ prevState = 0;
+
+ timeStepCNT =0;
+ framePRIORSProd = true;
+
+ tpTrans = new TPHSMM(nVARS,_nTPsTr,_lambda*10);
+
+ }
+ TPDPGMM(std::string path,uint nVARS, uint _nTPs, uint _nTPsTr,
+ float _lambda=50.f,float _minSigma = 2E2)
+ :TPGMM(path,_nTPs)
+ {
+ lambda = _lambda;
+ lambda2 = _lambda;
+ lambda3 = _lambda;
+ colvec nbPts;
+ nbPts.load(path + "TPGMM_nbPoints.txt",raw_ascii);
+ nbPoints = nbPts(0);
+ minSigma = _minSigma;
+ prevState = 0;
+
+ timeStepCNT =0;
+ framePRIORSProd = true;
+
+ framePRIORS.load(path + "TPGMM_frame_PRIORS.txt",raw_ascii);
+
+ tpTrans = new TPHSMM(path,nSTATES,nVARS,_nTPsTr,_lambda*10);
+
+ }
+ void addDatapoints(mat _dataPoint,vec _trParam,bool startFlag=false,bool endFlag=false,
+ float _lambda= 0.f,float _minSigma = 0.f);
+ void addDatapoints_d(mat _dataPoint,vec _trParam,bool startFlag=false,bool endFlag=false,
+ float _lambda= 0.f,float _lambda2=-1.f,float _lambda3=-1.f,float _minSigma = 0.f,int type=0,
+ bool sticky = false);
+
+ uint getNumPoints();
+
+ GMM_Model* getProdGMM();
+
+ void useFramePRIORS(bool _use){
+ framePRIORSProd = _use;
+ }
+
+ void updateTransition(vec _trParam);
+
+ bool reduceStates(); // search for similar states
+ bool reduceStates_d(); // search for similar states
+ void mergeStates(uint,uint); // merge states
+
+ mat predictForwardVariable(uint _N,uint startState);
+
+ mat getTransitionMatrix();
+ void setTransitionMatrix(mat tm);
+
+ GMM_Model* getTransformedGMM(std::vector<TaskParameter> _TPs, EMatrixType AType = OTHER );
+ GMM_Model* getTransformedGMM(TaskParameters _TPs, EMatrixType AType = OTHER );// [Calinon, 2012] Eq. (3) - Prod. of linearly transformed Gaussians
+
+ void saveInFiles(std::string path);
+
+ void loadFromFiles(std::string path);
+
+
+ HSMM* getHSMM(){return tpTrans;}
+ void setDurationComponents(const std::vector<GaussianDistribution>& components){
+// tpTrans = new TPHSMM(nVARS,_nTPsTr,_lambda*10);
+ tpTrans = new TPHSMM(this->getNumVARS(), components.size(), this->getNumFRAMES());
+ tpTrans->setDurationCOMPONENTS(components);
+ tpTrans->initializeFwdCalculation();
+ }
+
+
+};
+
+}
+
+#endif //PBDLIB_TPDPGMM_H
diff --git a/include/pbdlib/tpgmm.h b/include/pbdlib/tpgmm.h
index 10caed89bb5b16828a50ff85d15e193e57a7d0e0..084cf1457df131f5aea6fa4d3aeab02391a8cb9a 100644
--- a/include/pbdlib/tpgmm.h
+++ b/include/pbdlib/tpgmm.h
@@ -40,6 +40,7 @@ This class is the implementation of the task parameterized Gaussian mixture mode
#include "pbdlib/gmm.h"
#include "pbdlib/taskparameters.h"
#include "pbdlib/tpdemonstration.h"
+#include "assert.h"
using namespace arma;
@@ -52,7 +53,7 @@ enum EMatrixType {INVERTIBLE, SYMMETRIC, ORTHONORMAL, OTHER};
class TPGMM
{
- private:
+ protected:
/*!
nVARS: Number of variables
nSATAES: Number of Gaussian components of the model (This sould be set by the user)
@@ -74,6 +75,7 @@ class TPGMM
std::vector<mat> invA;
std::vector<mat> invAT;
+ mat GAMMA2;
void setAuxiliaryVarsProdGauss();
public:
@@ -87,6 +89,11 @@ class TPGMM
*/
void initTensorGMM_kmeans();
+ /*!
+ initTensorGMM_kbins(): Initialization of the centers and the covariances of GMMS using kbins clustering
+ */
+ void initTensorGMM_kbins(std::vector<TPDemonstration>& demos);
+
/*!
EM_tensorGMM(): The implementation of the Expectation-Maximization algorithm for pgmm
*/
@@ -99,6 +106,8 @@ class TPGMM
TPGMM(std::vector<TPDemonstration>& demos, uint nSTATES);
TPGMM(uint nVARS, uint nSTATES, uint _nTPs);
+ TPGMM(std::string path, uint _nTPs);
+ TPGMM(std::vector<GMM_Model> GMMS);
~TPGMM(){}
/*!
@@ -110,7 +119,10 @@ class TPGMM
Similarly, in the SigmaFileName*.txt there is matrix containing the covariance matrices like this: [SIGMA_1 SIGMA_2 ... SIGMA_j] ; j=1:nSTATES
*/
void loadTPGMMfromMATLAB(std::string PriorsFileName, std::string VarsFileName, std::string MuFileName, std::string SigmaFileName);
-
+
+ void saveInFiles(std::string path);
+
+ void loadFromFiles(std::string path);
/*!
getNumVARS(): Gives nVARS
*/
@@ -128,19 +140,26 @@ class TPGMM
*/
rowvec& getPRIORS();
+ void setPRIORS(rowvec pr);
double getPRIORS(uint);
/*!
getGMMS(): Gives GMMS
*/
std::vector<GMM_Model>& getGMMS();
+ void setGMMS(std::vector<GMM_Model>& vGMMS);
GMM_Model& getGMMS(uint id);
GMM_Model* getTransformedGMM(std::vector<TaskParameter> _TPs, EMatrixType AType = OTHER );// [Calinon, 2012] Eq. (3) - Prod. of linearly transformed Gaussians
GMM_Model* getTransformedGMM(TaskParameters _TPs, EMatrixType AType = OTHER );// [Calinon, 2012] Eq. (3) - Prod. of linearly transformed Gaussians
-
+
+
void setVARSNames(const std::vector<std::string>& vars);
std::vector<std::string>& getVARSNames();
+
+ void tpOnlineEMDP(TaskParameters _TPs, int N,colvec P,double lambda, double minSigma);
+
+ mat getGAMMA2(){return this->GAMMA2;};
};
} // end of namespace pbdlib
diff --git a/include/pbdlib/tphsmm.h b/include/pbdlib/tphsmm.h
new file mode 100644
index 0000000000000000000000000000000000000000..0cef09341f26be623f51bda345af9b755d085aa2
--- /dev/null
+++ b/include/pbdlib/tphsmm.h
@@ -0,0 +1,194 @@
+/*
+ * tphsmm.h
+ *
+ * Created on: Sep 15, 2015
+ * Author: Ajay Tanwani
+ */
+
+#ifndef TPHSMM_H_
+#define TPHSMM_H_
+
+#include "armadillo"
+#include "pbdlib/tpgmm.h"
+#include "pbdlib/taskparameters.h"
+#include "pbdlib/tpdemonstration.h"
+#include "pbdlib/hsmm.h"
+#include "lqr.h"
+#include "assert.h"
+
+using namespace arma;
+
+namespace pbdlib
+{
+// class representing parametrized transition between states
+class TPTRANSITION
+{
+public:
+ TPTRANSITION(uint _nTPS){
+ nTPS = _nTPS;
+ colvec defMu = zeros(_nTPS);
+ mat defSigma = eye(_nTPS,_nTPS);
+ nTrans =0;
+ transParam = new GaussianDistribution(defMu,defSigma);
+ }
+ TPTRANSITION(uint _nTPS,vec Param,float _minSigma){
+ nTPS = _nTPS;
+ mat minSIGMA = _minSigma * eye(_nTPS,_nTPS);
+ nTrans =1;
+ transParam = new GaussianDistribution(Param,minSIGMA);
+ }
+ /*
+ ~TPTRANSITION(){
+ delete transParam;
+ }
+ */
+ void addTransition(vec _trParam,float _minSigma)
+ {
+ nTrans++;
+
+ colvec MuTmp = 1.0/nTrans * ((nTrans-1.0)*transParam->getMU() + _trParam);
+ mat SigmaTmp;
+ if(nTrans>1){
+ SigmaTmp = 1.0/nTrans * ((nTrans-1.0)*transParam->getSIGMA() +
+ (double)(nTrans/(nTrans-1.0)) * ((_trParam-MuTmp)*(_trParam-MuTmp).t()));
+ }
+ else SigmaTmp = _minSigma * eye(nTPS,nTPS);
+ transParam->setMU(MuTmp);
+ transParam->setSIGMA(SigmaTmp);
+ }
+ GaussianDistribution *transParam; // reprensenting the parameters where the transition is likely to occur
+ uint nTrans; // number of transition detected
+ uint nTPS;
+};
+
+class TPHSMM : public HSMM {
+
+private:
+
+ // myTPs -> current Task Frames of a demonstration myTPs[i].A, myTPs[i].b
+
+ uint nbVARPOS;
+ LQR *lqr;
+ TPGMM *tpgmm;
+ TaskParameters *taskparam;
+ GMM_Model *ProdGauss;
+ colvec CurrentState, InitialState;
+ uword StateSequence[200];
+ std::vector<TaskParameter> cTaskParam;
+
+ mat AlphaHSMM;
+
+ uint pred_length;
+ mat rData;
+ uint SamplingMethod, HorizonMethod, SimMethod;
+
+ double RegularizationConstLQR;
+
+
+ mat TransCount;
+public:
+ uint nTPS;
+ float minSigma;
+// uint nSTATES;
+
+ std::vector< std::vector<TPTRANSITION> > TPTransitionMatrix;
+
+public:
+ //TPHSMM();
+ //TPHSMM(std::vector<TPDemonstration>& demos, uint nSTATES);
+ TPHSMM(uint nVARS, uint nSTATES, uint _nTPs);
+ TPHSMM(uint nVARS, uint nSTATES, uint _nTPs, uint nbVARPOS, double dt, double rf);
+ TPHSMM(std::vector<GMM_Model> GMMS, uint _nVARS, uint _nSTATES, uint _nTPs);
+ TPHSMM(std::vector<GMM_Model> GMMS, mat transition, std::vector<GaussianDistribution> components, uint _nVARS, uint _nSTATES, uint _nTPs);
+
+ // For online learning
+ TPHSMM(uint _nVARS,uint _nTPs, float _minSigma);
+ TPHSMM(std::string path,uint _nSTATES,uint _nVARS,uint _nTPs, float _minSigma);
+
+
+
+// TPHSMM(std::string PriorsFileName,
+// std::string VarsFileName,
+// std::string MuFilePrefix,
+// std::string SigmaFilePrefix,
+// const std::string &transition_path,
+// const std::string &durMu_path,
+// const std::string &durSigma_path);
+ virtual ~TPHSMM();
+
+ void loadTPHSMMfromFiles(std::string PriorsFileName,
+ std::string VarsFileName,
+ std::string MuFilePrefix,
+ std::string SigmaFilePrefix,
+ const std::string &transition_path,
+ const std::string &durMu_path,
+ const std::string &durSigma_path);
+
+ void setCurrentTaskFrameFromFile(std::string FramePath);
+ void setCurrentTaskFrame(std::vector<TaskParameter> TaskParameters);
+
+ void setCurrentState(colvec state);
+ void setInitialState(colvec init_state);
+ void setInitialState(GMM_Model& ProdGauss);
+
+ void setSamplingMethod(uint SamplingMethod);
+ void setLQRHorizonMethod(uint HorizonMethod);
+ void setPredictionLength(uint PredictionLength);
+ void setSimulationMethod(uint SimMethod);
+ void setScalingR(double rf);
+
+ void setRegularizationConstLQR(double RC);
+
+ std::vector<TaskParameter>& getTaskParameters();
+
+// uint getNumSTATES();
+
+ void init_TPHSMM_LQR(double dt, double rf);
+
+ colvec& getInitialState();
+ colvec& getCurrentState();
+
+ mat TPHSMMController();
+ mat TPHSMMController(std::vector<TaskParameter> cTaskParam, uint pred_length);
+ mat TPHSMMController(std::vector<TaskParameter> cTaskParam, colvec init_state, uint pred_length);
+
+ mat HSMMLQR(GMM_Model& GMM, colvec init_state, uint pred_length);
+
+ void printGMM(GMM_Model& GMM);
+ void printGaussian(GaussianDistribution& GaussPr);
+
+// std::vector<GMM_Model>& getGMMS();
+// GMM_Model& getGMMS(uint id);
+
+// GMM_Model* getTransformedGMM(std::vector<TaskParameter> _TPs, EMatrixType AType = OTHER );// [Calinon, 2012] Eq. (3) - Prod. of linearly transformed Gaussians
+// GMM_Model* getTransformedGMM(TaskParameters _TPs, EMatrixType AType = OTHER );// [Calinon, 2012] Eq. (3) - Prod. of linearly transformed Gaussians
+
+// void setVARSNames(const std::vector<std::string>& vars);
+// std::vector<std::string>& getVARSNames();
+
+ TPGMM* get_TPGMM();
+ GMM_Model* get_ProdGMM(std::vector<TaskParameter> cTaskParam);
+ LQR* get_LQR();
+
+ // update the transition matrix given the parametrized trans. matrix and the parameters
+ void updateTransitionMat(vec _trParam);
+
+ void updateDuration(uint _idxSTATE,uint _timeStepCNT);
+
+ void mergeTransition(uint, uint);
+
+ double getTransSimilarity(uint,uint);// get a measure of similarity between two states
+
+ void saveInFiles(std::string path);
+ void saveTPTtransitionMatrix(std::string path);
+
+ void loadFromFiles(std::string path);
+ // to call when a new transition is detected
+ void addTransition(uint _prevSTATE, uint _newSTATE, vec _trParam,bool startFlag, uint _timeStepCNT);
+
+};
+
+}
+
+#endif /* TPHSMM_H_ */
+
diff --git a/include/pbdlib/tptrajMPC.h b/include/pbdlib/tptrajMPC.h
new file mode 100644
index 0000000000000000000000000000000000000000..a6a812c02bd034fe146c9e7719bb7ba7a2c8a2b9
--- /dev/null
+++ b/include/pbdlib/tptrajMPC.h
@@ -0,0 +1,45 @@
+/*
+ * tptrajMPC.h
+ *
+ * Created on: 7 Jan 2016
+ * Author: ihavoutis
+ */
+
+#ifndef RLI_PBDLIB_SANDBOX_SRC_TPTRAJMPC_H_
+#define RLI_PBDLIB_SANDBOX_SRC_TPTRAJMPC_H_
+
+#include "armadillo"
+#include "pbdlib/trajMPC.h"
+#include "pbdlib/tpdpgmm.h"
+#include "pbdlib/taskparameters.h"
+
+namespace pbdlib {
+
+class TpTrajMPC: public TrajMPC {
+
+public:
+
+ TpTrajMPC(TPDPGMM* _tpdpgmm, TaskParameters* _tp,
+ HSMM* _hsmm,
+ double _dt, uint _nbVarPos, uint _nbDeriv,
+ uint _Np, uint _Nc, double _alpha)
+ :TrajMPC(_hsmm, _dt, _nbVarPos, _nbDeriv, _Np, _Nc, _alpha)
+ {
+ Np = _Np; // prediction horizon
+ tpdpgmm = _tpdpgmm;
+ tp = _tp;
+ alpha = _alpha;
+ }
+
+ virtual ~TpTrajMPC();
+
+ colvec& computeControlCommand(colvec& X);
+
+private:
+ uint Np;
+ TPDPGMM* tpdpgmm;
+ TaskParameters* tp;
+ double alpha;
+};
+}
+#endif /* RLI_PBDLIB_SANDBOX_SRC_TPTRAJMPC_H_ */
diff --git a/include/pbdlib/trajMPC.h b/include/pbdlib/trajMPC.h
new file mode 100644
index 0000000000000000000000000000000000000000..e14be1371e4e4068008de6f3d4b1065f435f6ffb
--- /dev/null
+++ b/include/pbdlib/trajMPC.h
@@ -0,0 +1,99 @@
+// May 2015, Martijn Zeestraten
+
+
+#ifndef PTRAJMPC_H_
+#define PTRAJMPC_H_
+
+
+#include "armadillo"
+#include "mpc.h"
+#include "pbdlib/mvn.h"
+#include "pbdlib/hsmm.h"
+#include "pbdlib/trajdist.h"
+
+using namespace arma;
+using namespace std;
+
+namespace pbdlib
+{
+class TrajMPC
+{
+protected: // Variables:
+ // variables:
+ uint nbVarPos; // Number of position variables
+ uint nbDeriv; // Number of derivatives used
+ uint nbVar; // Total number of variables
+ urowvec in; // in values
+
+ MPC* myMPC;
+ HSMM* hsmm;
+
+ // System Matrices:
+ mat A, B, C; // System Matrices
+ mat Ad, Bd; // Discrete system matrices
+
+ // For state sequence:
+ mat AlphaPred; // Matrix to hold alpha predictions
+ mat maxAlpha; // Matrix to hold maximum predicted values:
+ mat tmpMax;
+ mat tmpAlphaCol;
+ uvec q; // State sequence
+
+ mat Q; // Tracking cost Matrix
+ mat Lambda; // Precision Matrices
+ colvec Muq; // Reference
+ mat R; // Control cost matrix
+
+ colvec U; // Mean control commands
+ mat SigmaU; // Covariance of control commands
+
+ colvec muX; //
+ mat SigmaX; //
+
+ public:
+ // Constructor 1:
+ // hsmm : Hiddem semi-markov Model
+ // nbVarPos : Number of position variables
+ // nbDeriv : Number of derivative considered (1:Position, 2: Position+Velocity 3: ...)
+ // Np : Prediction horizon
+ // Nc : Control horizon
+ // Alpha : control cost alpha
+ TrajMPC(HSMM* _hsmm, double dt, uint nbVarPos, uint nbDeriv, uint _Np, uint _Nc,double _alpha);
+
+ // Constructor 2:
+ // hsmm : Hiddem semi-markov Model
+ // A : [nbVar x nbVar] System matrix of disretized system
+ // B : [nbVar x nbVarIn] Input matrix of discretized system
+ // C : [nbVarOut x nbVar] Output matrix
+ TrajMPC(HSMM* _hsmm, mat& Ad, mat& Bd, mat& C, uint _Np, uint _Nc, double _alpha);
+
+ // Function that computes the current control command
+ colvec& computeControlCommand(colvec& X);
+ TrajDist& computeTrajDist(colvec& X, bool stepHSMM);
+
+
+
+ void reset(){hsmm->resetForwardVariable();} // reset HSMM
+ mat getSystemDynamics(){return Ad;}
+ mat getInputDynamics(){return Bd;}
+ uvec getq(){return q;}
+ colvec getCurrentAttractor();
+ uint getNumVARS(){return hsmm->getNumVARS();}
+ uint getNumVARSPos(){return nbVarPos;}
+
+ void setMu(int i, const colvec& m);
+ void setSigma(int i, const mat& sigma);
+
+ protected:
+ // functions:
+ void updateReference(uvec& q);
+ void updateCostMatrix(double alpha);
+ void createSystemMatrices(double dt, uint nbVarPos, uint nbDeriv);
+ void createPrecisionMatrices(HSMM* _hsmm);
+ void allocateReference(uint nbVar, uint nbVarPos, uint Np, uint Nc);
+ void assignHSMM(HSMM* hsmm, uint nbVarPos, uint nbDeriv);
+
+
+};
+} // End namespace
+#endif
diff --git a/include/pbdlib/trajdist.h b/include/pbdlib/trajdist.h
new file mode 100644
index 0000000000000000000000000000000000000000..3ddb92aa88d7130cc739dbeea559f9022f45a609
--- /dev/null
+++ b/include/pbdlib/trajdist.h
@@ -0,0 +1,67 @@
+// December,2015, Martijn Zeestraten
+
+
+#ifndef TRAJDIST_H_
+#define TRAJDIST_H_
+
+#include "armadillo"
+#include "vector"
+
+using namespace arma;
+using namespace std;
+
+namespace pbdlib
+{
+ struct ConditionPoint
+ {
+ uint index;
+ colvec Mu;
+ mat Sigma;
+ ConditionPoint(){}
+ ConditionPoint(uint index, colvec Mu)
+ {
+ this->index = index;
+ this->Mu = Mu;
+ this->Sigma = zeros(Mu.n_elem,Mu.n_elem) ;
+ }
+ ConditionPoint(uint index, colvec Mu, mat Sigma)
+ {
+ this->index = index;
+ this->Mu = Mu;
+ this->Sigma = Sigma;
+ }
+ };
+
+
+ struct TrajDist
+ {
+ /* Class to represent trajectory distributions as a Gaussian.
+ *
+ * The class is still under development. Currently it only supports
+ * the product of distributions, in order to combine multiple Gaussian.
+ * It should also implement Gaussian conditioning in order to allow Gaussian Conditioning
+ * to modify the trajectory*/
+ colvec Mu;
+ mat Sigma;
+ uint nbVars;
+ uint nbData;
+
+ TrajDist(){}
+ TrajDist(const colvec& Mu, const mat& Sigma,const uint& nbVars);
+ TrajDist condition(vector<ConditionPoint> points); // Gaussian Conditioning
+ TrajDist lintrans(const mat& A, const colvec& b);
+
+ // Define operations for TrajectoryDistributions:
+ TrajDist& operator=(const TrajDist& rhs); // Copying
+ TrajDist& operator*=(const TrajDist& rhs); // Product of Gaussian
+ TrajDist operator*(const TrajDist& rhs); // Product of Gaussian
+
+
+ };
+
+
+}
+
+
+
+#endif
diff --git a/include/pbdlib/trajgmm.h b/include/pbdlib/trajgmm.h
new file mode 100644
index 0000000000000000000000000000000000000000..30f24b1c856a537c87298f9ad5a2f7fe81295383
--- /dev/null
+++ b/include/pbdlib/trajgmm.h
@@ -0,0 +1,83 @@
+/**
+Copyright (C) 2015, Leonel Rozo, Davide De Tommaso, Milad Malekzadeh
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+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 Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file trajgmm.h
+\brief TrajGMM class
+The class TrajGMM allows to use a trajectory GMM and to learn the parameters from task demonstrations
+
+\author Leonel Rozo
+\bug No known bugs.
+\todo Function init_TrajGMM_timeBased() should be moved to GMM_Model class
+*/
+
+#ifndef TRAJGMM_H
+#define TRAJGMM_H
+
+#include "pbdlib/gmm.h"
+#include "armadillo"
+#include <math.h>
+
+using namespace arma;
+
+namespace pbdlib
+{
+
+class TrajGMM : public GMM_Model
+{
+ private:
+ uint nVARPOS; // Dimension of position data
+ uint nDERIV; // Number of static and dynamic features
+ double dt; // Time step to be used in matrix PHI
+
+ sp_mat PHI; // Window matrix for all the demonstrations
+ sp_mat PHI1; // Window matrix for only one demonstrations
+
+ void init_TrajGMM_timeBased(double regularization = 1E-5); // Initialization with equal bins splitting
+ template<class armaType>
+ armaType circshift(const armaType &_matrix, int _shX, int _shY); // Circular shift for matrices/vectors
+ colvec getMuQ(vec _q); // Function to get the super vector Mu containing the all the means given a sequence of states
+ mat getSigmaQ(vec _q); // Function to get the super (sparse) matrix Sigma containing the all the covariances given a sequence of states
+
+ public:
+ // Constructors
+ TrajGMM(std::vector<Demonstration> &demos, uint _nSTATES, uint _nVARPOS, uint _nDERIV, double _dt);
+ TrajGMM(uint _nSTATES, uint _nVARPOS, uint _nDERIV, double _dt);
+ TrajGMM(const std::string &priors_path, const std::string &mu_path, const std::string &sigma_path,
+ const std::string &vars_path, const std::string &modelInfo_path);
+ TrajGMM(const std::string &priors_path, const std::string &mu_path, const std::string &sigma_path, const std::string &modelInfo_path);
+ // Destructor
+ ~TrajGMM(){}
+
+ uint EM_learn(double regularization = 1E-5); // Override EM learning that uses equal bins splitting
+ void constructPHI(uint _nData, uint _nSamples); // Function to construct the window matrix PHI
+ mat leastSquaresOptimalData(colvec _q); // Function to carry out the WLS-like optimization for a given sequence of states
+ std::vector<GaussianDistribution> leastSquaresOptimalProb(colvec _q); // Function to carry out the WLS-like optimization for a given sequence of states
+
+ uint getNumVARSPOS(){return nVARPOS;};// Returns dimension of position data
+ uint getNumDERIV(){return nDERIV;}; // Returns number of static and dynamic features
+ double getDt(){return dt;}; // Returns the time step used in the window matrix PHI
+ sp_mat getPHI(){return PHI;}; // Returns window matrix for all the demonstrations
+ sp_mat getPHI1(){return PHI1;}; // Returns window matrix for only one demonstration
+ };
+
+} //end of pbdlib namespace
+
+#endif
+
+
diff --git a/src/adhsmm.cpp b/src/adhsmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0f083866ad6c3f7aa594d499010090158231f427
--- /dev/null
+++ b/src/adhsmm.cpp
@@ -0,0 +1,219 @@
+/**
+Copyright (C) 2015, Leonel Rozo
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+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 Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "pbdlib/adhsmm.h"
+
+namespace pbdlib
+{
+ADHSMM::ADHSMM(const std::string &priors_path,
+ const std::string &mu_path,
+ const std::string &sigma_path,
+ const std::string &transition_path,
+ const std::string &durPriors_path,
+ const std::string &durMu_path,
+ const std::string &durSigma_path,
+ uint T):HSMM(priors_path,mu_path,sigma_path,transition_path,durMu_path,durSigma_path, T, false)
+{
+ // Setting duration state probabilities
+ mat tmpDurPriors, tmpDurMu, tmpDurSigma;
+ // Loading duration probability models
+ tmpDurPriors.load(durPriors_path,raw_ascii);
+ tmpDurMu.load(durMu_path,raw_ascii);
+ tmpDurSigma.load(durSigma_path, raw_ascii);
+ this->nSTATESDUR = tmpDurPriors.n_rows;
+ this->nVARSDUR = tmpDurMu.n_rows;
+
+ // Setting duration GMMs
+ initializeCondDurProb(tmpDurPriors, tmpDurMu, tmpDurSigma);
+ this->condDurationProb = new GMR();
+}
+
+void ADHSMM::initializeRecursiveForwardVariable(uint T)
+{
+
+}
+
+void ADHSMM::initializeCondDurProb(mat tmpDurPriors, mat durGMMmu, mat durGMMsigma)
+{
+ GMM_Model _tmpDurationProb(this->nSTATESDUR, this->nVARSDUR);
+ std::vector<GaussianDistribution> _tmpComponents;
+ uint _indMu, _ind1Sigma, _ind2Sigma;
+ colvec _tmpMu;
+ mat _tmpSigma;
+
+ // Assuming that the means and covariances matrices are ordered like this:
+ // durGMMmu = [ mu_1,1 mu_1,2 ... mu_1,nSTATESDUR mu_2,1 mu_2,2, ... mu_2,nSTATESDUR ... mu_nSTATES,1 mu_nSTATE,2 ... mu_nSTATES,nSTATESDUR ]
+ // durGMMsigma = [ Sigma_1,1 Sigma_1,2 ... Sigma_1,nSTATESDUR Sigma_2,1 Sigma_2,2 ... Sigma_2,nSTATESDUR ... Sigma_nSTATES,1 Sigma_nSTATES,2 ... Sigma_nSTATES,nSTATESDUR]
+ for(uint i=0 ; i<this->nSTATES ; i++)
+ {
+ for(uint j=0 ; j<this->nSTATESDUR ; j++)
+ {
+ // Temporally storing \mu
+ _indMu = j+i*this->nSTATESDUR;
+ _tmpMu = durGMMmu.col(_indMu);
+
+ // Temporally storing \Sigma
+ _ind1Sigma = (j*this->nVARSDUR)+i*this->nVARSDUR*this->nSTATESDUR;
+ _ind2Sigma = (j*this->nVARSDUR+this->nVARSDUR-1)+i*this->nVARSDUR*this->nSTATESDUR;
+ _tmpSigma = durGMMsigma.cols(_ind1Sigma, _ind2Sigma);
+
+ // Saving current Gaussian distribution
+ _tmpComponents.push_back(GaussianDistribution(_tmpMu, _tmpSigma));
+ }
+ // Saving GMM encoding duration probability for state "i"
+ _tmpDurationProb.setCOMPONENTS(_tmpComponents);
+ _tmpDurationProb.setPRIORS(tmpDurPriors.col(i).t());
+
+ // Saving in vector of GMMs
+ this->durationGMMs.push_back(_tmpDurationProb);
+
+ // Clearing temporary saving
+ _tmpComponents.clear();
+ }
+}
+
+void ADHSMM::computeConditionedDurationProbs(uint _i, colvec _u, urowvec _varIn, urowvec _varOut)
+{
+ // Set Pd and PdSize for current external input value
+ this->condDurationProb->setGMMModel(&this->durationGMMs[_i]);
+ // Evaluating GMR for the given external input _u
+ this->condPd = this->condDurationProb->regression(_u, _varIn, _varOut);
+ // Setting maximum duration based on the resulting \mu and \Sigma
+ this->PdSize = round(this->condPd->getMU(0)(0) + 2*this->condPd->getSIGMA(0)(0,0));
+}
+
+void ADHSMM::setNumSTATESDUR(uint _nSTATESDUR)
+{
+ this->nSTATESDUR = _nSTATESDUR;
+}
+
+void ADHSMM::setNumVARSDUR(uint _nVARSDUR)
+{
+ this->nVARSDUR = _nVARSDUR;
+}
+
+void ADHSMM::setDurationGMMs(std::vector<GMM_Model> _durGMMs)
+{
+ if(_durGMMs.size() == this->nSTATES)
+ this->durationGMMs = _durGMMs;
+ else
+ std::cout << "\n [ERROR]::ADHSMM_Model::setDurationGMMs if(_durGMMs.size() == this->nSTATES) ... else .";
+}
+
+std::vector<GMM_Model>& ADHSMM::getDurationGMMs()
+{
+ return this->durationGMMs;
+}
+
+GMM_Model& ADHSMM::getDurationGMMs(uint _idGMM)
+{
+ return this->durationGMMs[_idGMM];
+}
+
+void ADHSMM::stepRecursiveForwardVariable(colvec _u, urowvec _varIn, urowvec _varOut, uint _tn)
+{
+ colvec colTn(1), colD(1);
+ colTn(0) = _tn+1; // This needs to be done for evaluating conditional duration probability. There is no "zero duration"
+
+ for(uint i = 0 ; i < this->getNumSTATES() ; i++)
+ {
+ // Setting PdSize and Pd based on external input (GMR for current GMM encoding duration probability)
+ this->computeConditionedDurationProbs(i, _u, _varIn, _varOut);
+
+ // \alpha_i,t = \Pi_i * Pd_i(t|u) + ...
+ if(_tn < PdSize)
+ recAlpha(i,_tn) = this->getPRIORS()(i) * this->condPd->getCOMPONENTS(0).getPDFValue(colTn)(0);
+
+ // \alpha_i,t = \sum_d \alpha_j,t-d * a_j,i * Pd_i(d|u)
+ for(uint d = 0 ; d < std::min(_tn,PdSize) ; d++)
+ {
+ colD(0) = d+1; // This needs to be done for evaluating conditional duration probability. There is no "zero duration"
+ recAlpha(i,_tn) = recAlpha(i,_tn) + as_scalar(recAlpha.col(_tn-d-1).t() * this->getTRANSITION().col(i)) *
+ this->condPd->getCOMPONENTS(0).getPDFValue(colD)(0);
+ }
+ }
+}
+
+void ADHSMM::stepRecursiveForwardVariable(colvec _u, urowvec _varIn, urowvec _varOut, uint _tn, mat& _obs)
+{
+ double tmpObsProb;
+ colvec colTn(1), colD(1);
+ colTn(0) = _tn+1; // This needs to be done for evaluating conditional duration probability. There is no "zero duration"
+
+ for(uint i = 0 ; i < this->getNumSTATES() ; i++)
+ {
+ // Setting PdSize and Pd (precomputed duration prob.)
+ this->computeConditionedDurationProbs(i, _u, _varIn, _varOut);
+
+ // \alpha_i,t = \Pi_i * Pd_i(t|u) * \prod_s N(\ksi_s) + ...
+ if(_tn < PdSize)
+ {
+ tmpObsProb = arma::prod( scalingFtr.subvec(0, _tn) % this->getCOMPONENTS(i).getPDFValue(_obs) );
+ recAlpha(i,_tn) = this->getPRIORS()(i) * this->condPd->getCOMPONENTS(0).getPDFValue(colTn)(0) * tmpObsProb;
+ }
+
+ // \alpha_i,t = \sum_d \alpha_j,t-d * a_j,i * Pd_i(d|u) * \prod_s N(\ksi_s)
+ for(uint d = 0 ; d < std::min(_tn,PdSize) ; d++)
+ {
+ tmpObsProb = arma::prod( scalingFtr.subvec(_tn-d, _tn) % this->getCOMPONENTS(i).getPDFValue(_obs.cols(_tn-d, _tn)) );
+ colD(0) = d+1; // This needs to be done for evaluating conditional duration probability. There is no "zero duration"
+ recAlpha(i,_tn) = recAlpha(i,_tn) + as_scalar(recAlpha.col(_tn-d-1).t() * this->getTRANSITION().col(i)) *
+ this->condPd->getCOMPONENTS(0).getPDFValue(colD)(0) * tmpObsProb;
+ }
+ }
+
+ // Computing scaling factor
+ if(_tn < scalingFtr.n_rows-1)
+ scalingFtr(_tn+1) = 1/sum(recAlpha.col(_tn));
+}
+
+void ADHSMM::stepRecursiveForwardVariable(colvec _u, urowvec _varIn, urowvec _varOut, uint _tn, mat& _obs, urowvec _ind)
+{
+ double tmpObsProb;
+ colvec colTn(1), colD(1);
+ colTn(0) = _tn+1; // This needs to be done for evaluating conditional duration probability. There is no "zero duration"
+
+ for(uint i = 0 ; i < this->getNumSTATES() ; i++)
+ {
+ // Setting PdSize and Pd (precomputed duration prob.)
+ this->computeConditionedDurationProbs(i, _u, _varIn, _varOut);
+
+ // \alpha_i,t = \Pi_i * Pd_i(t|u) * \prod_s N(\ksi_s) + ...
+ if(_tn < PdSize)
+ {
+ tmpObsProb = arma::prod( scalingFtr.subvec(0, _tn) % this->getCOMPONENTS(i).getPDFValue(_obs, _ind) );
+ recAlpha(i,_tn) = this->getPRIORS()(i) * this->condPd->getCOMPONENTS(0).getPDFValue(colTn)(0) * tmpObsProb;
+ }
+
+ // \alpha_i,t = \sum_d \alpha_j,t-d * a_j,i * Pd_i(d|u) * \prod_s N(\ksi_s)
+ for(uint d = 0 ; d < std::min(_tn,PdSize) ; d++)
+ {
+ tmpObsProb = arma::prod( scalingFtr.subvec(_tn-d, _tn) % this->getCOMPONENTS(i).getPDFValue(_obs.cols(_tn-d, _tn), _ind) );
+ colD(0) = d+1; // This needs to be done for evaluating conditional duration probability. There is no "zero duration"
+ recAlpha(i,_tn) = recAlpha(i,_tn) + as_scalar(recAlpha.col(_tn-d-1).t() * this->getTRANSITION().col(i)) *
+ this->condPd->getCOMPONENTS(0).getPDFValue(colD)(0) * tmpObsProb;
+ }
+ }
+
+ // Computing scaling factor
+ if(_tn < scalingFtr.n_rows-1)
+ scalingFtr(_tn+1) = 1/sum(recAlpha.col(_tn));
+}
+
+} // End pbdlib namespace
diff --git a/src/gmm.cpp b/src/gmm.cpp
index a2cfff5a08cb6677ff335c5e78f8c6322c8d8e73..7c901493696e4d084def8adacf41dbb3d84fcac4 100644
--- a/src/gmm.cpp
+++ b/src/gmm.cpp
@@ -2,6 +2,7 @@
Copyright (C) 2014, Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
This file is part of PbDLib.
PbDLib is free software: you can redistribute it and/or modify
@@ -56,20 +57,23 @@ GMM_Model::GMM_Model(const std::string &priors_path, const std::string &mu_path,
nVARS = mu.n_rows;
nSTATES = priors.n_elem;
- std::ifstream varsfile(vars_path.c_str());
- std::string varsnames;
- std::getline (varsfile,varsnames);
-
- std::vector<std::string> vars;
-
- std::stringstream ss(varsnames); // Insert the string into a stream
- std::string buf;
-
- while (ss >> buf)
+ // Load Var names from file.
+ std::ifstream varsfile(vars_path.c_str()); // Open file stream
+ std::vector<std::string> vars; // Vector to hold variable names
+ std::string varsnames; // tmp string to read line from file
+ std::string buf; // buffer string
+ // We assume that the variable names are on the first line, separated by spaces
+ getline(varsfile,varsnames);
+ std::stringstream ss(varsnames);
+ // Read all strings:
+ while (ss>>buf)
+ {
vars.push_back(buf);
+ }
setVARSNames(vars);
+ // Load Sigma and MU
mat _SIGMA = zeros(nVARS, nVARS);
colvec _MU = zeros(nVARS,1);
rowvec _PRIORS(1, nSTATES);
@@ -125,7 +129,7 @@ void GMM_Model::saveInFiles()
mat priors(1, nSTATES);
mat mu(nVARS, nSTATES);
mat sigma(nVARS, nVARS*nSTATES);
- std::ofstream varsfile ("GMM_vars.txt");
+ std::ofstream varsfile ("data/gmm/GMM_vars.txt");
for(uint i=0; i<nSTATES; i++){
priors(0,i) = getPRIORS()(i);
@@ -142,13 +146,18 @@ void GMM_Model::saveInFiles()
varsfile << " ";
}
- priors.save("GMM_priors.txt", raw_ascii);
- mu.save("GMM_mu.txt", raw_ascii);
- sigma.save("GMM_sigma.txt", raw_ascii);
+ priors.save("data/gmm/GMM_priors.txt", raw_ascii);
+ mu.save("data/gmm/GMM_mu.txt", raw_ascii);
+ sigma.save("data/gmm/GMM_sigma.txt", raw_ascii);
varsfile.close();
}
+void GMM_Model::setDEMONSTRATIONS(std::vector<Demonstration> demons)
+{
+ DEMONSTRATIONS = demons;
+}
+
void GMM_Model::setPRIORS(const rowvec& priors)
{
if(priors.n_elem == nSTATES)
@@ -181,6 +190,14 @@ void GMM_Model::setLAMBDA(int id, const mat& _LAMBDA)
COMPONENTS[id].setLAMBDA(_LAMBDA);
}
+void GMM_Model::deleteCOMPONENT(int id)
+{
+
+ COMPONENTS.erase(COMPONENTS.begin()+id);
+ PRIORS.shed_col(id);
+ nSTATES--;
+}
+
uint GMM_Model::getIndexOfVARName(const std::string& varname)
{
std::vector<std::string>::iterator it;
@@ -232,15 +249,32 @@ mat& GMM_Model::getLAMBDA(int id)
return COMPONENTS[id].getLAMBDA();
}
-double GMM_Model::getProbability(const colvec& sample)
+double GMM_Model::getProbability(const colvec& sample, bool usePriors)
{
double P = 0.0;
// See Eq. (2.0.2) in doc/TechnicalReport.pdf
for(uint k=0; k<nSTATES; k++)
- P += PRIORS[k] * as_scalar( COMPONENTS[k].getPDFValue(sample) ); // See Eq. (2.0.3) for "getPDFValue" in doc/TechnicalReport.pdf
+ {
+ if(usePriors)
+ P += PRIORS[k] * as_scalar( COMPONENTS[k].getPDFValue(sample) ); // See Eq. (2.0.3) for "getPDFValue" in doc/TechnicalReport.pdf
+ else
+ P += as_scalar( COMPONENTS[k].getPDFValue(sample) ); // See Eq. (2.0.3) for "getPDFValue" in doc/TechnicalReport.pdf
+ }
return P;
}
+colvec GMM_Model::getProbability(const mat& samples, bool usePriors)
+{
+ colvec probs(samples.n_cols);
+ colvec s(samples.n_rows);
+
+ // See Eq. (2.0.4) in doc/TechnicalReport.pdf
+ for(uint j=0 ; j<samples.n_cols ; j++){
+ s = samples.col(j);
+ probs(j) = log( getProbability(s, usePriors) );
+ }
+ return probs;
+}
void GMM_Model::onlineEMDP(int N,colvec P,double lambda, double minSigma)
{
@@ -295,10 +329,10 @@ void GMM_Model::onlineEMDP(int N,colvec P,double lambda, double minSigma)
}
}
-static urowvec randperm( int n )
+static uvec randperm( int n )
{
//return sort_index(randn<urowvec>(nbData));
- return sort_index(randu<rowvec>(n)).st(); // was randn, should work equally well?
+ return sort_index(randu(n)); // was randn, should work equally well?
}
void GMM_Model::learnKMEANS(double regularization)
@@ -322,12 +356,15 @@ void GMM_Model::learnKMEANS(double regularization)
//srand (time(NULL));
// random permutation
- urowvec idTmp = randperm(nbData);
+ cout << "rand perm" << endl;
+ uvec idTmp = randperm(nbData);
+ cout << "linspace" << endl;
uvec allrows = linspace<uvec>(0, nbVar-1, nbVar);
// nSTATES means
- mat Mu = Data.submat(allrows, idTmp.cols(0,nSTATES-1));
+ cout << "submat" << endl;
+ mat Mu = Data.submat(allrows, idTmp.subvec(0,nSTATES-1));
//k-means iterations
while(true)
@@ -352,8 +389,6 @@ void GMM_Model::learnKMEANS(double regularization)
double cumdist = sum(vTmp);
//M-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- uvec idTmp;
-
for (uint i=0; i<nSTATES; i++)
{
idTmp = find(idList == i);
@@ -447,6 +482,7 @@ uint GMM_Model::EM(double likelihood,double regularization)
// compute posterior probability p(i|x)
Pix_tmp = repmat(prior, demos.n_cols, 1) % Pxi;
Pix = Pix_tmp / repmat(sum(Pix_tmp, 1), 1, nSTATES);
+ this->gamma = Pix;
// compute cumulated posterior probability
E = sum(Pix);
@@ -509,6 +545,9 @@ void GMM_Model::clear(){
setPRIORS(ones<vec>(1));
}
-
+mat& GMM_Model::getGamma()
+{
+ return this->gamma;
+}
} //end of pbdlib namespace
diff --git a/src/gmr.cpp b/src/gmr.cpp
index fad19c0e4c8311d183c6791844c0b28895373919..d85b9c8d0708960702670a938847fe83453502c6 100644
--- a/src/gmr.cpp
+++ b/src/gmr.cpp
@@ -85,35 +85,96 @@ GMM_Model* GMR::regression( mat data_in, urowvec in, urowvec out)
regression(gmmOut,data_in,in,out);
// Return the model
return gmmOut;
+}
-
+void GMR::regression2(GMM_Model* gmmOut, mat data_in, urowvec in, urowvec out, double RegF){
+ // Ajay Tanwani, 2016
-}
+ double diagRegF = 1E-12;
+ Pxi = zeros(data_in.n_cols,gmm->getNumSTATES());
+ //Compute the influence of each GMM component, given input x
+ // See Eq. (3.0.5) in doc/TechnicalReport.pdf
+ for(i=0; i<gmm->getNumSTATES(); i++){
+ Mu = gmm->getMU(i)(in);
+ Sigma = gmm->getSIGMA(i)(in,in);
+// Mu.print("Mu(i) = "); // DEBUGGING
+// Sigma.print("Sigma(i) = "); // DEBUGGING
+ Gtmp->setMU(Mu);
+ Gtmp->setSIGMA(Sigma);
+ Pxi.col(i) = gmm->getPRIORS(i) * (Gtmp->getPDFValue(data_in)) + diagRegF;
+// cout << "(Gtmp->getPDFValue(data_in) + diagRegF)" << (Gtmp->getPDFValue(data_in) + diagRegF) << endl;
+ }
+ //Priors.print("Priors = "); // DEBUGGING
+ //data_in->getData().print("t = "); // DEBUGGING
+ //Pxi.print("Pxi = "); // DEBUGGING
+ beta = Pxi / repmat(sum(Pxi,1),1,gmm->getNumSTATES()); // See Eq. (3.0.5) in doc/TechnicalReport.pdf
+
+ // cout << "data_in" << data_in << endl;
+ // cout << "in: " << in << endl;
+ // cout << "out: " << out << endl;
+ // cout << "Pxi: " << Pxi << endl;
+ // cout << "Priors: " << gmm->getPRIORS() << endl;
+ // cout << "beta: " << beta << endl;
+
+ for(t=0; t<data_in.n_cols; t++){
+ MuOut.zeros(out.n_elem);
+ SigmaOut.zeros(out.n_elem,out.n_elem);
+ for(i=0; i<gmm->getNumSTATES(); i++){
+ Mu = gmm->getMU(i);
+ Sigma = gmm->getSIGMA(i);
+
+ // Pre compute inverse, use inv_sympd to improve performance
+ InvSigmaInIn = (Sigma(in,in) + eye(out.n_elem, out.n_elem)* RegF).i();
+
+ MuOutTmp = Mu(out) + Sigma(out,in) * InvSigmaInIn * (data_in.col(t)-Mu(in)); // See Eq. (3.0.3) in doc/TechnicalReport.pdf
+ MuOut = MuOut + beta(t,i) * MuOutTmp; // See Eq. (3.0.2) in doc/TechnicalReport.pdf
+
+ SigmaOutTmp = Sigma(out,out) - Sigma(out,in) * InvSigmaInIn * Sigma(in,out); // See Eq. (3.0.4) in doc/TechnicalReport.pdf
+ SigmaOut = SigmaOut + beta(t,i) * (SigmaOutTmp + MuOutTmp*MuOutTmp.t()); // See Eq. (3.0.2) in doc/TechnicalReport.pdf
+// cout << "MuOut: " << MuOut << endl;
+// cout << "SigmaOut: " << SigmaOut << endl;
+ }
+ SigmaOut = SigmaOut - MuOut*MuOut.t();
+
+// COMPONENTS.push_back( GaussianDistribution(MuOut, SigmaOut) );
+ gmmOut->getCOMPONENTS(t).setMU(MuOut);
+ gmmOut->getCOMPONENTS(t).setSIGMA(SigmaOut);
+ }
+}
void GMR::regression(GMM_Model* gmmOut, mat data_in, urowvec in, urowvec out)
{
// Base regression function. Use this one to be as fast as possible.
// One need to supply a gmmOut object of appropriate size (nbStates = data_in.n_cols, nbVar = size(out))
-
-
+
+ double diagRegF = 1E-12;
Pxi = zeros(data_in.n_cols,gmm->getNumSTATES());
//Compute the influence of each GMM component, given input x
// See Eq. (3.0.5) in doc/TechnicalReport.pdf
for(i=0; i<gmm->getNumSTATES(); i++){
Mu = gmm->getMU(i)(in);
Sigma = gmm->getSIGMA(i)(in,in);
- //Mu_tmp.print("Mu(i) = "); // DEBUGGING
- //Sigma_tmp.print("Sigma(i) = "); // DEBUGGING
+// Mu.print("Mu(i) = "); // DEBUGGING
+// Sigma.print("Sigma(i) = "); // DEBUGGING
Gtmp->setMU(Mu);
Gtmp->setSIGMA(Sigma);
- Pxi.col(i) = gmm->getPRIORS(i) * (Gtmp->getPDFValue(data_in));
+ Pxi.col(i) = gmm->getPRIORS(i) * (Gtmp->getPDFValue(data_in)) + diagRegF;
+// cout << "Pxi.col(i)" << Pxi.col(i) << endl;
}
//Priors.print("Priors = "); // DEBUGGING
//data_in->getData().print("t = "); // DEBUGGING
//Pxi.print("Pxi = "); // DEBUGGING
+// beta = Pxi / repmat(sum(Pxi,1) + diagRegF,1,gmm->getNumSTATES()); // See Eq. (3.0.5) in doc/TechnicalReport.pdf
beta = Pxi / repmat(sum(Pxi,1),1,gmm->getNumSTATES()); // See Eq. (3.0.5) in doc/TechnicalReport.pdf
+// cout << "data_in" << data_in << endl;
+// cout << "in: " << in << endl;
+// cout << "out: " << out << endl;
+// cout << "Pxi: " << Pxi << endl;
+// cout << "Priors: " << gmm->getPRIORS() << endl;
+// cout << "beta: " << beta << endl;
+
//beta.print("beta = "); // DEBUGGING
for(t=0; t<data_in.n_cols; t++){
MuOut.zeros(out.n_elem);
@@ -123,16 +184,18 @@ void GMR::regression(GMM_Model* gmmOut, mat data_in, urowvec in, urowvec out)
Sigma = gmm->getSIGMA(i);
// Pre compute inverse, use inv_sympd to improve performance
InvSigmaInIn = (Sigma(in,in).i());
-
+// cout << data_in.col(t)-Mu(in) << endl;
MuOutTmp = Mu(out) + Sigma(out,in) * InvSigmaInIn * (data_in.col(t)-Mu(in)); // See Eq. (3.0.3) in doc/TechnicalReport.pdf
SigmaOutTmp = Sigma(out,out) - Sigma(out,in) * InvSigmaInIn * Sigma(in,out); // See Eq. (3.0.4) in doc/TechnicalReport.pdf
MuOut = MuOut + beta(t,i) * MuOutTmp; // See Eq. (3.0.2) in doc/TechnicalReport.pdf
SigmaOut = SigmaOut + beta(t,i) * SigmaOutTmp; // See Eq. (3.0.2) in doc/TechnicalReport.pdf
+// cout << "MuOut: " << MuOut << endl;
+// cout << "SigmaOut: " << SigmaOut << endl;
}
// COMPONENTS.push_back( GaussianDistribution(MuOut, SigmaOut) );
gmmOut->getCOMPONENTS(t).setMU(MuOut);
gmmOut->getCOMPONENTS(t).setSIGMA(SigmaOut);
-
+
}
// GMM_Model* gmmOut;
diff --git a/src/hmm.cpp b/src/hmm.cpp
index 0a7abd7801857668bd8f5ce2014c60f3fd578494..0423942fa150554059fd10759f89ea0f6300ca89 100644
--- a/src/hmm.cpp
+++ b/src/hmm.cpp
@@ -22,7 +22,8 @@ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
namespace pbdlib
{
-HMM::HMM(uint _nSTATES, uint _nVARS):GMM_Model(_nSTATES,_nVARS)
+HMM::HMM(uint _nSTATES, uint _nVARS):
+ GMM_Model(_nSTATES,_nVARS)
{
}
@@ -61,18 +62,18 @@ void HMM::setTRANSITION(const mat& _Transition)
TransitionMatrix = _Transition;
}
-
+/*
double HMM::getProbability(const colvec& sample)
{
double P = 0.0;
// See Eq. (2.0.2) in doc/TechnicalReport.pdf
for(uint k=0; k<nSTATES; k++)
- P += alpha[k] * as_scalar( this->gmm->getCOMPONENTS(k).getPDFValue(sample) ); // See Eq. (2.0.3) for "getPDFValue" in doc/TechnicalReport.pdf
+ //P += alpha[k] * as_scalar( this->gmm->getCOMPONENTS(k).getPDFValue(sample) ); // See Eq. (2.0.3) for "getPDFValue" in doc/TechnicalReport.pdf
return P;
}
-/*
+
void HMM::learnKMEANS()
{
mat DemosTmp = DEMONSTRATIONS[0].getDatapoints().getData();
@@ -163,7 +164,7 @@ void HMM::learnKMEANS()
//cout << endl << "nbStep " << nbStep << endl;
}
}
-*/
+
double HMM::getLikelihood(const mat &SAMPLES)
{
@@ -177,7 +178,7 @@ double HMM::getLikelihood(const mat &SAMPLES)
}
return L;
}
-
+*/
diff --git a/src/hsmm.cpp b/src/hsmm.cpp
index ac3cb3f6c94439ac8e8d07fab8a674b8281e067f..9a8f65878d624c3a9138ea4cfef036542b63c7a4 100644
--- a/src/hsmm.cpp
+++ b/src/hsmm.cpp
@@ -1,5 +1,5 @@
/**
-Copyright (C) 2015, Martijn Zeestraten
+Copyright (C) 2015, Martijn Zeestraten, Leonel Rozo, Ioannis Havoutis
This file is part of PbDLib.
@@ -18,10 +18,15 @@ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
*/
#include "pbdlib/hsmm.h"
-
+#include <algorithm> // std::max
namespace pbdlib
{
+HSMM::HSMM(uint _nSTATES, uint _nVARS):
+ HMM(_nSTATES, _nVARS)
+{
+}
+
HSMM::HSMM(const std::string &priors_path,
const std::string &mu_path,
const std::string &sigma_path,
@@ -32,11 +37,11 @@ HSMM::HSMM(const std::string &priors_path,
{
mat durMu, durSigma;
- // Load components
+ // Load state duration components
durMu.load(durMu_path, raw_ascii);
durSigma.load(durSigma_path, raw_ascii);
- // Set components:
+ // Set state duration components:
mat _SIGMA = zeros(1,1);
colvec _MU = zeros(1,1);
std::vector<GaussianDistribution> components;
@@ -53,6 +58,45 @@ HSMM::HSMM(const std::string &priors_path,
initializeFwdCalculation();
}
+HSMM::HSMM(const std::string &priors_path,
+ const std::string &mu_path,
+ const std::string &sigma_path,
+ const std::string &transition_path,
+ const std::string &durMu_path,
+ const std::string &durSigma_path,
+ uint T, bool initDur):
+ HMM(priors_path,mu_path,sigma_path,transition_path)
+{
+ mat durMu, durSigma;
+ mat _SIGMA = zeros(1,1);
+ colvec _MU = zeros(1,1);
+ std::vector<GaussianDistribution> components;
+
+ if(initDur)
+ {
+ // Load state duration components
+ durMu.load(durMu_path, raw_ascii);
+ durSigma.load(durSigma_path, raw_ascii);
+
+ // Set state duration components:
+ for(uint i=0; i<this->getNumSTATES(); i++){
+ _MU(0,0) = durMu(0,i);
+ _SIGMA(0,0) = durSigma(0,i);
+ // for debugging
+ components.push_back(GaussianDistribution(_MU, _SIGMA));
+ }
+ }
+ else
+ {
+ // Set state duration components to zero:
+ for(uint i=0; i<this->getNumSTATES(); i++)
+ components.push_back(GaussianDistribution(_MU, _SIGMA));
+ }
+ setDurationCOMPONENTS(components);
+
+ // Forward variable calculation initialization:
+ initializeRecursiveForwardVariable(T,initDur);
+}
void HSMM::setDurationCOMPONENTS(const std::vector<GaussianDistribution>& components)
{
@@ -72,6 +116,20 @@ void HSMM::setDurSIGMA(int id, const mat& _SIGMA)
DurationCOMPONENTS[id].setSIGMA(_SIGMA);
}
+void HSMM::setMaxDuration(uint maxPd)
+{
+ this->PdSize = maxPd;
+
+ // Initializing duration probabilities
+ Pd.set_size(this->getNumSTATES(),PdSize);
+
+ // Duration input vector
+ rowvec dur = linspace<rowvec>(1,PdSize, PdSize);
+
+ // Pre-Calculate Pd (duration probabilities)
+ for (uint i = 0;i<this->getNumSTATES();i++)
+ Pd.row(i) = getDurationCOMPONENTS(i).getPDFValue(dur).t(); // getPDFValue accepts a rowvec of values, but returns a colvec....
+}
std::vector<GaussianDistribution>& HSMM::getDurationCOMPONENTS()
{
@@ -96,27 +154,28 @@ mat& HSMM::getDurSIGMA(uint id)
void HSMM::initializeFwdCalculation()
{
// Calculate PD size:
- cout << "Pd size : ";
+// cout << "Pd size : ";
PdSize=0;
for (uint i = 0;i<this->getNumSTATES();i++)
{
if (PdSize <accu(this->getDurMU(i)))
PdSize = accu(this->getDurMU(i));
}
- PdSize *=1.5; // Enlarge the Size of Pd for 'accuracy'
+ PdSize *=1.2; // Enlarge the Size of Pd for 'accuracy'
//PdSize = 50;
- cout << PdSize << endl;
+// cout << PdSize << endl;
// This function is used to set the forward variable matrices to the appropriate size:
- ALPHA.set_size(this->getNumSTATES(), PdSize);
- Atmp1.set_size(this->getNumSTATES(),PdSize-1);
- Atmp2.set_size(this->getNumSTATES(),PdSize-1);
- alpha.set_size(this->getNumSTATES());
- bmx.set_size(this->getNumSTATES());
- btmp.set_size(this->getNumSTATES());
- S.set_size(this->getNumSTATES());
- Pd.set_size(this->getNumSTATES(),PdSize);
+ ALPHA.zeros(this->getNumSTATES(), PdSize);
+ Atmp1.zeros(this->getNumSTATES(),PdSize-1);
+ Atmp2.zeros(this->getNumSTATES(),PdSize-1);
+ alpha.zeros(this->getNumSTATES());
+ bmx.zeros(this->getNumSTATES());
+ btmp.zeros(this->getNumSTATES());
+ S.zeros(this->getNumSTATES());
+ Pd.zeros(this->getNumSTATES(),PdSize);
+
// Duration input vector
mat dur = linspace(1,PdSize, PdSize);
// Pre-Calculate Pd
@@ -216,6 +275,7 @@ void HSMM::initializeForwardVariable(colvec& _obs, urowvec& _ind)
{
// Calculate the initial forward step
updateBtmp(btmp,_obs, _ind);
+
// ALPHA Variable
ALPHA = Pd;
@@ -261,8 +321,8 @@ void HSMM::lstepForwardVariable(colvec& _obs)
updateBtmp(btmp,_obs);
//cout << "Btmp: " << endl << btmp << endl;
- // ALPHA Variable
- updateALPHA(ALPHA,S);
+ // ALPHA Variable (using current bmx)
+ updateALPHA(ALPHA,S,bmx);
// Update bmx:
updateBmx(bmx, ALPHA, btmp);
@@ -281,8 +341,8 @@ void HSMM::lstepForwardVariable(colvec& _obs, urowvec& _ind)
// update Btmp:
updateBtmp(btmp,_obs,_ind);
- // ALPHA Variable
- updateALPHA(ALPHA,S);
+ // ALPHA Variable (using current bmx)
+ updateALPHA(ALPHA,S,bmx);
// Update bmx:
updateBmx(bmx, ALPHA, btmp);
@@ -302,37 +362,11 @@ mat& HSMM::predictForwardVariable(uint _N)
{
//cout << "Allocating Memory: ";
AlphaPred.set_size(this->getNumSTATES(), _N);
- //cout << AlphaPred.n_rows << " x " << AlphaPred.n_cols << endl;
-
- tmpInit = Initialized;
- // Make copy of the current state of the system:
- ALPHAtmp = ALPHA;
- Stmp = S;
-
-
- for (uint i = 0;i<_N;i++)
- {
- // Alpha variable
- if (tmpInit==false)
- {
- // Initialize:
- ALPHAtmp = Pd;
- ALPHAtmp.each_col() %= this->getPRIORS().t(); // % is the element wise product
- tmpInit = true;
- }
- else
- {
- updateALPHA(ALPHAtmp,Stmp);
- }
-
- // Update S
- updateS(Stmp,ALPHAtmp);
- updateAlpha(alphatmp,ALPHAtmp);
+ // Make Predictions:
+ predictForwardVariable(AlphaPred);
- // Save alpha
- AlphaPred.col(i) = alphatmp;
- }
+ // Return Alpha Variable:
return AlphaPred;
}
@@ -373,6 +407,203 @@ void HSMM::predictForwardVariable(mat& _AlphaPred)
// cout << "Done" << endl;
}
}
+
+void HSMM::clear(){
+ hsmm_transition.zeros();
+ hsmm_transition_ticks.zeros();
+ hsmm_priors.clear();
+ hsmm_priors_ticks.clear();
+ GMM_Model::clear();
+}
+
+void HSMM::integrateDemonstration(Demonstration demo) {
+ //Compute sequence of states
+ int nPoints = demo.getDatapoints().getNumPOINTS();
+ mat h(this->getNumSTATES(), nPoints);
+ for (int i=0; i<(int)this->getNumSTATES(); i++){
+ h.row(i) = trans(this->getCOMPONENTS(i).getPDFValue(demo.getDatapoints().getData()));
+ }
+ uword imax[demo.getDatapoints().getNumPOINTS()];
+ get_state_seq(imax, h);
+
+ int nStates = this->getNumSTATES();
+ int nVars = this->getNumVARS();
+
+ hsmm_transition.resize(nStates, nStates);
+ hsmm_transition_ticks.resize(nStates, nStates);
+ hsmm_priors.resize(nStates);
+ hsmm_priors_ticks.resize(nStates);
+
+ //update hsmm priors for stochastic sampling
+ hsmm_priors_ticks(imax[0]) += 1;
+ hsmm_priors = hsmm_priors_ticks / accu(hsmm_priors_ticks);
+
+ //update duration statistics
+ hsmm_duration_stats.resize(nStates);
+
+ uint s0 = imax[0], currStateDuration = 1;
+ for (int t = 0; t < nPoints; t++) {
+ if ( (imax[t] != s0) || (t+1 == nPoints) ) {
+ if ( (t+1 == nPoints) ) { currStateDuration += 1; }
+ hsmm_duration_stats[s0](currStateDuration);
+ currStateDuration = 0.0;
+ hsmm_transition(s0, imax[t]) += 1.0;
+ hsmm_transition_ticks(s0, imax[t]) += 1.0;
+ s0 = imax[t];
+ }
+ currStateDuration += 1;
+ }
+
+ // normalize the transition matrix
+ for (int i = 0; i < hsmm_transition.n_rows ; i++){
+ double row_sum = accu(hsmm_transition.row(i));
+ if (row_sum > 1){
+ double row_sum_ticks = accu(hsmm_transition_ticks.row(i));
+ hsmm_transition.row(i) = hsmm_transition_ticks.row(i) / row_sum_ticks;
+ }
+ }
+
+ this->setTRANSITION(hsmm_transition);
+
+ // Set hsmmd durations:
+ mat _SIGMA = zeros(1,1);
+ colvec _MU = zeros(1,1);
+ std::vector<GaussianDistribution> hsmm_components;
+
+ for (uint i=0; i<nStates; i++) {
+ _MU(0,0) = hsmm_duration_stats[i].mean();//state_duration_means(i);
+ _SIGMA(0,0) = std::max( hsmm_duration_stats[i].var(), 1.0);
+ hsmm_components.push_back(GaussianDistribution(_MU, _SIGMA));
+ }
+
+ this->setDurationCOMPONENTS(hsmm_components);
+
+}
+
+void HSMM::get_state_seq(uword state_seq[], mat pred){
+ for (int t=0; t<pred.n_cols; t++){
+ vec vTmp = pred.col(t);
+ vTmp.max(state_seq[t]);
+ }
+}
+
+int HSMM::getClosestState(const colvec P){
+ /// distance to closest cluster
+ double d = arma::norm(this->getMU(0)-P,2); //Initialized distance of P from first cluster
+ int minIndex = 0; //Index corresponding to current cluster
+ //Find cluster corresponding to minimum distance
+ for (int k=1; k<this->getNumSTATES(); k++) {
+ double Dist = arma::norm(this->getMU(k) - P, 2);
+ if (Dist < d){
+ d = Dist;
+ minIndex = k; //Index corresponding to minimum distance
+ }
+ }
+ return minIndex;
+}
+
+void HSMM::predictForwardVariableDeterministic(mat& _AlphaPred, int startState) {
+ rowvec tempPriors = this->getPRIORS();
+ rowvec fixed_priors(this->getNumSTATES(), fill::zeros);
+ fixed_priors(startState) = 1.0;
+ this->setPRIORS(fixed_priors);
+ this->initializeFwdCalculation();
+ predictForwardVariable(_AlphaPred);
+ this->setPRIORS(tempPriors);
+}
+
+void HSMM::predictForwardVariableStochasticStart(mat& _AlphaPred, int startState) {
+ rowvec tempPriors = this->getPRIORS();
+ this->setPRIORS(hsmm_priors);
+ this->initializeFwdCalculation();
+ int nbSt = 0;
+ int currTime = 0;
+ rowvec iList(1, fill::zeros);
+ int nbData = _AlphaPred.n_cols;
+ int nStates = this->getNumSTATES();
+ mat h = zeros(nStates, nbData);
+ rowvec h1, h2;
+
+ int nbD = this->Pd.n_cols;
+
+ uword Iminmax;
+ while (currTime < nbData) {
+ if (nbSt==0){
+ iList(0) = startState;
+ h1 = ones<rowvec>(nbData);
+ }else{
+ h1 = join_rows( join_rows(
+ zeros<rowvec>(currTime), cumsum(this->Pd.row(iList(iList.n_elem-2))) ),
+ ones<rowvec>( std::max( (nbData-currTime-nbD),0) ));
+ currTime = currTime + round( this->getDurMU( (uint)iList(iList.n_elem-2))(0,0) );
+ }
+ h2 = join_rows( join_rows(
+ ones<rowvec>(currTime),
+ ones<rowvec>( this->Pd.row(iList(iList.n_elem-1)).n_elem )
+ - cumsum(this->Pd.row(iList(iList.n_elem-1))) ),
+ zeros<rowvec>( std::max( (nbData-currTime-nbD),0) ));
+
+ h.row(iList(iList.n_elem-1)) = h.row(iList(iList.n_elem-1))
+ + min( join_cols(h1.cols(0,nbData-1),h2.cols(0,nbData-1)) );
+ vec tmp= this->getTRANSITION().row(iList(iList.n_elem-1)).t() % randu(nStates);
+ tmp.max(Iminmax);
+ iList.resize(iList.n_elem+1);
+ iList(iList.n_elem-1) = Iminmax;
+
+ nbSt = nbSt+1;
+ }
+ h = h / repmat(sum(h,0) ,nStates,1);
+ _AlphaPred = h;
+ this->setPRIORS(tempPriors);
+}
+
+void HSMM::predictForwardVariableStochastic(mat& _AlphaPred) {
+ rowvec tempPriors = this->getPRIORS();
+ this->setPRIORS(hsmm_priors);
+ this->initializeFwdCalculation();
+ int nbSt = 0;
+ int currTime = 0;
+ rowvec iList(1, fill::zeros);
+ int nbData = _AlphaPred.n_cols;
+ int nStates = this->getNumSTATES();
+ mat h = zeros(nStates, nbData);
+ rowvec h1, h2;
+
+ int nbD = this->Pd.n_cols;
+
+ uword Iminmax;
+ while (currTime < nbData) {
+ if (nbSt==0){
+ vec tmp = this->getPRIORS().t() % randu(nStates);
+ tmp.max(Iminmax);
+ iList(0) = Iminmax;
+ h1 = ones<rowvec>(nbData);
+ }else{
+ h1 = join_rows( join_rows(
+ zeros<rowvec>(currTime), cumsum(this->Pd.row(iList(iList.n_elem-2))) ),
+ ones<rowvec>( std::max( (nbData-currTime-nbD),0) ));
+ currTime = currTime + round( this->getDurMU( (uint)iList(iList.n_elem-2))(0,0) );
+ }
+ h2 = join_rows( join_rows(
+ ones<rowvec>(currTime),
+ ones<rowvec>( this->Pd.row(iList(iList.n_elem-1)).n_elem )
+ - cumsum(this->Pd.row(iList(iList.n_elem-1))) ),
+ zeros<rowvec>( std::max( (nbData-currTime-nbD),0) ));
+
+ h.row(iList(iList.n_elem-1)) = h.row(iList(iList.n_elem-1))
+ + min( join_cols(h1.cols(0,nbData-1),h2.cols(0,nbData-1)) );
+ vec tmp= this->getTRANSITION().row(iList(iList.n_elem-1)).t() % randu(nStates);
+ tmp.max(Iminmax);
+ iList.resize(iList.n_elem+1);
+ iList(iList.n_elem-1) = Iminmax;
+
+ nbSt = nbSt+1;
+ }
+ h = h / repmat(sum(h,0) ,nStates,1);
+ _AlphaPred = h;
+ this->setPRIORS(tempPriors);
+}
+
/* FUNCTIONS FACILITATING FORWARD VARIABLE CALCULATION
*
*
@@ -382,8 +613,9 @@ void HSMM::updateBtmp(colvec& _btmp, colvec& _obs)
// Calculate the initial forward step
for (uint i =0;i<this->getNumSTATES();i++)
{
- _btmp(i) = this->getCOMPONENTS(i).getPDFValue(_obs)(0);
+ _btmp(i) = this->getCOMPONENTS(i).getPDFValue(_obs)(0)+1e-12;
}
+
_btmp = _btmp/accu(_btmp);
}
@@ -398,7 +630,7 @@ void HSMM::updateBtmp(colvec& _btmp, colvec& _obs, urowvec& _ind)
Gtmp->setSIGMA(this->getSIGMA(i)(_ind,_ind));
// Evaluate the gaussian probability:
- _btmp(i) = Gtmp->getPDFValue(_obs.rows(_ind))(0);
+ _btmp(i) = Gtmp->getPDFValue(_obs.rows(_ind))(0)+1e-12;
}
_btmp = _btmp/accu(_btmp);
}
@@ -430,15 +662,12 @@ void HSMM::updateALPHA(mat& _ALPHA, colvec& _S, colvec& _bmx)
_ALPHA.col(PdSize-1) = _S % Pd.col(PdSize-1);
}
-
void HSMM::updateBmx(colvec& _bmx, mat& _ALPHA, colvec& _Btmp)
{
// Equation (2) & (3): bmx update
_bmx = _Btmp/accu(_Btmp.t()*sum(_ALPHA,1));
}
-
-
// Equation (6): Update S
void HSMM::updateS(colvec& _S, mat& _ALPHA)
{
@@ -453,8 +682,6 @@ void HSMM::updateS(colvec& _S, mat& _ALPHA, colvec& _bmx)
_S = this->getTRANSITION().t()*(_bmx%_ALPHA.col(0));
}
-
-
void HSMM::updateAlpha(colvec& _alpha, mat& _ALPHA)
{
// Calculate forward varaible
@@ -471,4 +698,173 @@ void HSMM::updateAlpha(colvec& _alpha, mat& _ALPHA, colvec& _Btmp)
// _alpha = _alpha/accu(_alpha);
}
+void HSMM::saveInFiles(std::string path) {
+ int nSTATES = this->getNumSTATES();
+ int nVARS = this->getNumVARS();
+ mat priors(1, nSTATES);
+ mat mu(nVARS, nSTATES);
+ mat sigma(nVARS, nVARS*nSTATES);
+ mat transition(nSTATES, nSTATES);
+ mat durMu(1, nSTATES);
+ mat durSigma(1, nSTATES);
+
+ for(uint i=0; i<nSTATES; i++){
+ priors(0,i) = getPRIORS()(i);
+ durMu(0,i) = getDurMU(i)(0);
+ durSigma(0,i) = getDurSIGMA(i)(0);
+ for(uint j=0; j<nVARS; j++){
+ mu(j,i) = getMU(i)(j);
+ for(uint k=0; k<nVARS; k++)
+ sigma(j,k + i*(nVARS)) = getSIGMA(i)(j,k);
+ }
+ }
+ transition = this->getTRANSITION();
+
+ cout << "Saving in path: "<< path << endl;
+ priors.save(path + "priors.txt", raw_ascii);
+ mu.save(path + "mu.txt", raw_ascii);
+ sigma.save(path + "sigma.txt", raw_ascii);
+ transition.save(path + "transition.txt", raw_ascii);
+ durMu.save(path + "durMu.txt", raw_ascii);
+ durSigma.save(path + "durSigma.txt", raw_ascii);
+
+ hsmm_transition.save(path + "hsmm_transition.txt", raw_ascii);
+ hsmm_transition_ticks.save(path + "hsmm_transition_ticks.txt", raw_ascii);
+ hsmm_priors.save(path + "hsmm_priors.txt", raw_ascii);
+ hsmm_priors_ticks.save(path + "hsmm_priors_ticks.txt", raw_ascii);
+}
+
+void HSMM::loadFromFiles(std::string path) {
+ this->clear();
+ std::string priors_path(path + "priors.txt");
+ std::string mu_path(path + "mu.txt");
+ std::string sigma_path(path + "sigma.txt");
+ std::string transition_path(path + "transition.txt");
+ std::string durMu_path(path + "durMu.txt");
+ std::string durSigma_path(path + "durSigma.txt");
+ *this = HSMM(priors_path,
+ mu_path,
+ sigma_path,
+ transition_path,
+ durMu_path,
+ durSigma_path);
+ hsmm_transition.load(path + "hsmm_transition.txt", raw_ascii);
+ hsmm_transition_ticks.load(path + "hsmm_transition_ticks.txt", raw_ascii);
+ hsmm_priors.load(path + "hsmm_priors.txt", raw_ascii);
+ hsmm_priors_ticks.load(path + "hsmm_priors_ticks.txt", raw_ascii);
+}
+
+// ---------------------------------------------------------------------
+/* Forward Variable Step calculations - Recursive (readable) approach */
+// ---------------------------------------------------------------------
+void HSMM::initializeRecursiveForwardVariable(uint T, bool initDur)
+{
+ // Initializing scaling factor
+ scalingFtr.set_size(T);
+ scalingFtr.zeros();
+ scalingFtr(0) = 1;
+ // Initializing recursive forward variable
+ recAlpha.set_size(this->getNumSTATES(),T);
+ recAlpha.zeros();
+ // For forward variable:
+ Gtmp = new GaussianDistribution(this->getNumVARS());
+
+ // Initializing d
+ if(initDur)
+ {
+ cout << "Pd size : ";
+ PdSize=0;
+ for (uint i = 0;i<this->getNumSTATES();i++)
+ {
+ if (PdSize <accu(this->getDurMU(i)))
+ PdSize = accu(this->getDurMU(i));
+ }
+ PdSize *=1.2; // Enlarge the Size of Pd for 'accuracy'
+ cout << PdSize << endl;
+
+ // Initializing duration probabilities
+ Pd.set_size(this->getNumSTATES(),PdSize);
+
+ // Duration input vector
+ rowvec dur = linspace<rowvec>(1,PdSize, PdSize);
+
+ // Pre-Calculate Pd (duration probabilities)
+ for (uint i = 0;i<this->getNumSTATES();i++)
+ Pd.row(i) = getDurationCOMPONENTS(i).getPDFValue(dur).t(); // getPDFValue accepts a rowvec of values, but returns a colvec....
+ }
+ else
+ {
+ cout << "[WARNING] PdSize and Pd variables were not initialized. Received flag was set to false." << endl;
+ }
+}
+
+void HSMM::stepRecursiveForwardVariable(uint tn)
+{
+ for(uint i = 0 ; i < this->getNumSTATES() ; i++)
+ {
+ // \alpha_i,t = \Pi_i * Pd_i(t) + ...
+ if(tn < PdSize)
+ recAlpha(i,tn) = this->getPRIORS()(i) * Pd(i,tn);
+
+ // \alpha_i,t = \sum_d \alpha_j,t-d * a_j,i * Pd_i(d)
+ for(uint d = 0 ; d < std::min(tn,PdSize) ; d++)
+ recAlpha(i,tn) = recAlpha(i,tn) + as_scalar(recAlpha.col(tn-d-1).t() * this->getTRANSITION().col(i)) * Pd(i,d);
+ }
+}
+
+void HSMM::stepRecursiveForwardVariable(uint tn, mat& obs)
+{
+ double tmpObsProb;
+
+ for(uint i = 0 ; i < this->getNumSTATES() ; i++)
+ {
+ // \alpha_i,t = \Pi_i * Pd_i(t) * \prod_s N(\ksi_s) + ...
+ if(tn < PdSize)
+ {
+ tmpObsProb = arma::prod( scalingFtr.subvec(0, tn) % this->getCOMPONENTS(i).getPDFValue(obs) );
+ recAlpha(i,tn) = this->getPRIORS()(i) * Pd(i,tn) * tmpObsProb;
+ }
+
+ // \alpha_i,t = \sum_d \alpha_j,t-d * a_j,i * Pd_i(d) * \prod_s N(\ksi_s)
+ for(uint d = 0 ; d < std::min(tn,PdSize) ; d++)
+ {
+ tmpObsProb = arma::prod( scalingFtr.subvec(tn-d, tn) % this->getCOMPONENTS(i).getPDFValue(obs.cols(tn-d,tn)) );
+ recAlpha(i,tn) = recAlpha(i,tn) + as_scalar(recAlpha.col(tn-d-1).t() * this->getTRANSITION().col(i)) * Pd(i,d) * tmpObsProb;
+ }
+ }
+
+ // Computing scaling factor
+ if(tn < scalingFtr.n_rows-1)
+ scalingFtr(tn+1) = 1/sum(recAlpha.col(tn));
+}
+
+void HSMM::stepRecursiveForwardVariable(uint tn, mat& obs, urowvec ind)
+{
+ double tmpObsProb;
+
+ for(uint i = 0 ; i < this->getNumSTATES() ; i++)
+ {
+ // \alpha_i,t = \Pi_i * Pd_i(t) * \prod_s N(\ksi_s) + ...
+ if(tn < PdSize)
+ {
+ tmpObsProb = arma::prod( scalingFtr.subvec(0, tn) % this->getCOMPONENTS(i).getPDFValue(obs,ind) );
+ recAlpha(i,tn) = this->getPRIORS()(i) * Pd(i,tn) * tmpObsProb;
+ }
+
+ // \alpha_i,t = \sum_d \alpha_j,t-d * a_j,i * Pd_i(d) * \prod_s N(\ksi_s)
+ for(uint d = 0 ; d < std::min(tn,PdSize) ; d++)
+ {
+ tmpObsProb = arma::prod( scalingFtr.subvec(tn-d, tn) % this->getCOMPONENTS(i).getPDFValue(obs.cols(tn-d,tn),ind) );
+ recAlpha(i,tn) = recAlpha(i,tn) + as_scalar(recAlpha.col(tn-d-1).t() * this->getTRANSITION().col(i)) * Pd(i,d) * tmpObsProb;
+ }
+ }
+
+ if(tn < scalingFtr.n_rows-1)
+ scalingFtr(tn+1) = 1/sum(recAlpha.col(tn));
+}
+
+void HSMM::resetRecursiveForwardVariable()
+{
+ recAlpha.zeros();
+}
} // End pbdlib namespace
diff --git a/src/lqr.cpp b/src/lqr.cpp
index e94579e00fbd9c877c69a47a4edf8435aee4a40c..87fc42c643043acd6798f89a6185dd6a9b31fb35 100644
--- a/src/lqr.cpp
+++ b/src/lqr.cpp
@@ -36,6 +36,34 @@ LQR::LQR(mat _A, mat _B, double _dt)
}
+
+bool LQR::evaluate_gains_finiteHorizon_discrete(mat SFinal,colvec dFinal){ //
+
+ if (prob_set == 0) {return 1;};
+
+ mat DTarget = LQR::diff(Target);
+
+
+ S[nbData-1] = SFinal;
+// S[nbData-1] = Q[nbData-1];
+ d.col(nbData-1) = dFinal;
+// d.col(nbData-1) = Target.col(nbData-1);
+
+ for (int t=nbData-2;t>=0;t--) {
+ S[t] = Q[t] - A.t() * (S[t + 1] * B * (B.t() * S[t + 1] * B + R).i() * B.t() * S[t + 1] - S[t + 1]) * A;
+
+ d.col(t) = (A.t() - A.t() * S[t + 1] * B *(B.t() * S[t + 1] * B + R).i() * B.t()) *
+ (S[t + 1] * (A * Target.col(t) - Target.col(t + 1)) + d.col(t+1));
+ }
+ for (int t=0;t<nbData;t++){
+ L[t] = (B.t()*S[t]*B + R).i() * B.t() * S[t]*A;
+
+ M.col(t) = -(B.t()*S[t]*B + R).i() * B.t() * (S[t] * (A * Target.col(t) - Target.col(t)) + d.col(t));
+ }
+ return 0;
+};
+
+
bool LQR::evaluate_gains_finiteHorizon(mat SFinal,colvec dFinal){
if (this->prob_set == 0) {return 1;};
@@ -49,7 +77,8 @@ bool LQR::evaluate_gains_finiteHorizon(mat SFinal,colvec dFinal){
for (int t=this->nbData-2;t>=0;t--){
this->S[t] = this->S[t+1] + this->dt * (this->A.t()*this->S[t+1] + this->S[t+1]*this->A - this->S[t+1] * this->B * invR * this->B.t() * this->S[t+1] + this->Q[t+1]); //See Eq. (5.1.11) in doc/TechnicalReport.pdf
- this->d.col(t) = this->d.col(t+1) - this->dt * (this->S[t+1]*this->B*invR * this->B.t() - this->A.t()) * this->d.col(t+1) + this->dt * this->S[t+1] * DTarget.col(t+1) - this->dt * (this->S[t+1] * this->A * this->Target.col(t+1)); //See Eq. (5.1.29) in doc/TechnicalReport.pdf
+// this->d.col(t) = this->d.col(t+1) - this->dt * (this->S[t+1]*this->B*invR * this->B.t() - this->A.t()) * this->d.col(t+1) + this->dt * this->S[t+1] * DTarget.col(t+1) - this->dt * (this->S[t+1] * this->A * this->Target.col(t+1)); //See Eq. (5.1.29) in doc/TechnicalReport.pdf
+ this->d.col(t) = this->d.col(t+1) + this->dt * ((this->A.t() -this->S[t+1]*this->B*invR * this->B.t()) * this->d.col(t+1) + this->S[t+1] * DTarget.col(t+1) - this->S[t+1] * this->A * this->Target.col(t+1)); //See Eq. (5.1.29) in doc/TechnicalReport.pdf
}
for (int t=0;t<nbData;t++){
this->L[t] = invR*this->B.t()*this->S[t]; //See Eq. (5.1.30) in doc/TechnicalReport.pdf
@@ -73,6 +102,19 @@ bool LQR::evaluate_gains_infiniteHorizon(){
return 0;
};
+bool LQR::evaluate_gains_infiniteHorizon_Discrete(){
+
+ //FEEDBACK TERM ONLY
+ if (this->prob_set == 0) {return 1;};
+ mat DTarget = LQR::diff(this->Target);
+ mat invR = this->R.i();
+ for (int t=0;t < this->nbData;t++){
+ this->S[t] = this->solveAlgebraicRiccati_Discrete(this->A,this->B,this->Q[t],this->R);
+ this->L[t] = (this->B.t()*this->S[t]*this->B + this->R).i()*this->B.t()*this->S[t]*this->A;
+ }
+ return 0;
+}
+
mat LQR::diff(mat V){
mat DV(V.n_rows,V.n_cols);
@@ -127,8 +169,48 @@ mat LQR::solveAlgebraicRiccati(mat A, mat B, mat Q, mat R) //See Sec. (5.2) in d
return S;
};
+mat LQR::solveAlgebraicRiccati_Discrete(mat A, mat B, mat Qx, mat R)
+{
+ // Ajay Tanwani, 2016
+
+ int n = A.n_rows;
+ mat Q = (Qx + Qx.t())/2; // mat Q here corresponds to (Q+Q')/2 specified in set_problem
+ mat G = B * R.i() * B.t();
+ mat Z(2*n,2*n);
+ Z(span(0,n-1),span(0,n-1)) = A + G * inv(A.t()) *Q;
+ Z(span(0,n-1),span(n,2*n-1)) = -G * inv(A.t());
+ Z(span(n,2*n-1),span(0,n-1)) = -inv(A.t()) * Q;
+ Z(span(n,2*n-1),span(n,2*n-1)) = inv(A.t());
+
+ // Using schur decomposition
+ /*mat U(2*n,2*n);
+ mat T(2*n,2*n);
+ auxlib::schur_dec(U,T,Z); //Missing ordered schur...
+ //Ordered schur decomposition from lapack to add
+ mat S = U(span(0,n-1),span(0,n-1)).t().i()*U(span(n,2*n-1),span(0,n-1)).t();
+ */
+
+ //Using diagonalization
+ cx_mat V(2*n,2*n), U(2*n,n);
+ cx_vec dd(2*n);
+
+ arma::eig_gen(dd,V,Z);
+
+ int i = 0;
+ for(int j=0; j<2*n; j++){
+ if (norm(dd(j)) < 1) {
+ U.col(i) = V.col(j);
+ i++;
+ }
+ }
+ //cx_mat Sc = U(span(0,n-1),span(0,n-1)).t().i() * U(span(n,2*n-1),span(0,n-1)).t(); //TODO: confirm with reverse order
+ //mat S = real(Sc);
+
+ return real(U(span(n,2*n-1),span(0,n-1)) * U(span(0,n-1),span(0,n-1)).i());
+};
+
-bool LQR::setProblem(mat _R, std::vector<mat> _Q, mat _Target)
+void LQR::setProblem(mat _R, std::vector<mat> _Q, mat _Target)
{
this->R = _R;
this->Q = _Q;
diff --git a/src/mpc.cpp b/src/mpc.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f0f729a163ae10109768b0584f93aab3463c8bee
--- /dev/null
+++ b/src/mpc.cpp
@@ -0,0 +1,182 @@
+// May 2015, Martijn Zeestraten
+//
+//
+
+#include "pbdlib/mpc.h"
+
+/* MPC Constructor:
+ * Ad : Discretized system matrix A
+ * Bd : Discretized input matrix B
+ * C ; Ouput matrix
+ * Np : Prediction horizon
+ * Nc : Control Horizon
+*/
+namespace pbdlib{
+MPC::MPC(mat& _Ad, mat& _Bd, mat& _C, uint _Np, uint _Nc)
+{
+
+ A = _Ad;
+ B = _Bd;
+ C = _C;
+
+ Np = _Np;
+ Nc = _Nc;
+
+ // Construct dynamic matrices required to perform MPC
+ constructDynamicMatrices(_Ad,_Bd,_C,_Np,_Nc);
+
+ // Allocate appropriate size for Least squares:
+ prepareForLeastSquares();
+}
+
+void MPC::constructDynamicMatrices(mat& _A, mat& _B, mat& _C, uint _Np, uint _Nc)
+{
+ // Determine Sizes:
+ int An = _A.n_rows;
+ int Am = _A.n_cols;
+ int Bm = _B.n_cols;
+
+
+ // Construct F:
+ // [ CA ]
+ // [ CA^2 ]
+ //F = [ .. ]
+ // [ CA^{Np-1}]
+ // [ CA^Np ]
+ //
+ F.reshape(An*_Np,Am);
+ mat C1 = mat(An*_Np,Bm);
+
+ F.rows(0,An-1) = _A;
+ C1.rows(0,An-1) = _B;
+ for (uint i = 1;i<_Np;i++)
+ {
+ F.rows(i*An,(i+1)*An-1) =
+ F.rows((i-1)*An,i*An-1)*_A;
+ C1.rows(i*An,(i+1)*An-1) =
+ F.rows((i-1)*An,i*An-1)*_B;
+ }
+// cout << "F created " << endl;
+// cout << F << endl;
+
+ // Construct Phi
+ // [ CB 0 0
+ // [ CAB CB
+ // [ CA^2B CAB
+ //
+ // Phi = [ .. .. .. ..
+ // [ CA^(Np-1)
+ // [
+ Phi.resize(An*_Np,Bm*_Nc);
+ for (uint i=0;i<_Nc;i++)
+ {
+
+ Phi.submat(i*An, i*Bm, _Np*An-1, (i+1)*Bm-1) =
+ C1.rows(0,(_Np-i)*An-1);
+ }
+// cout << "Phi Created" << endl;
+// cout << Phi << endl;
+
+ // Construct I Matrix (to select first control command
+ I.resize(Bm, Bm*Nc);
+ I.cols(0,Bm-1) = eye(Bm,Bm);
+
+ CT = kron(eye(_Np,_Np),_C);
+}
+void MPC::prepareForLeastSquares()
+{
+ // Function is used to set all matrices to the appropriate size
+
+ // Get sizes
+ nbVar = C.n_rows; // Number of tracking variables
+ nbContr = B.n_cols; // Number of control variables
+
+ PhiQPhiR.resize(nbContr*Nc,nbContr*Nc);
+ PhiQMuqFx.resize(nbContr*Nc);
+
+
+}
+
+
+colvec& MPC::computeControlCommand(colvec& X,colvec& ref, mat& Q, mat& R)
+{
+ // Solves a damped least-squares problem
+
+ // Define the two parts:
+ PhiQPhiR= Phi.t()*Q*Phi+R;
+ PhiQMuqFx= Phi.t()*Q*(ref-F*X);
+
+ // Solve the least squares and return the first control command:
+ U =I*solve(PhiQPhiR,PhiQMuqFx);
+ return U;
+}
+
+TrajDist& MPC::computeTrajDist(colvec& X,colvec& ref, mat& Q, mat& R)
+{
+ // Define the two parts PhiQPhiR*U = PhiQMuqFx:
+ PhiQPhiR= Phi.t()*Q*Phi+R;
+ PhiQMuqFx= Phi.t()*Q*(ref-F*X);
+
+ // Solve for U and Sigma:
+ // This implementation is not very efficient.
+ // qr-decomposition should be implemented here
+ mat tmpMu = F*X + Phi*solve(PhiQPhiR,PhiQMuqFx);
+ mat tmpSigma = Phi*inv(PhiQPhiR)*Phi.t();
+
+ cout << "size(tmpMu): " << tmpMu.n_rows << "x" << tmpMu.n_cols << endl;
+ cout << "size(tmpSigma): " << tmpSigma.n_rows << "x" << tmpSigma.n_cols << endl;
+ cout << "Computation Finished"<< endl;
+ TD = TrajDist(tmpMu,tmpSigma,nbContr);
+
+ //TD->Mu = tmpMu;
+ //TD->Sigma = tmpSigma;
+
+
+// TD->Mu=F*X + Phi*solve(PhiQPhiR,PhiQMuqFx);
+// TD->Sigma = Phi*inv(PhiQPhiR)*Phi.t();
+
+ return TD;
+}
+
+
+
+void MPC::setSystem(mat& _A, mat& _B, mat& _C)
+{
+ // Copy matrices to local variables
+ A = _A;
+ B = _B;
+ C = _C;
+
+ // Recompute dynamic matrices
+ constructDynamicMatrices(A,B,C,Np,Nc);
+
+ // Allocate appropriate size for Least squares:
+ prepareForLeastSquares();
+}
+
+void MPC::setPredictionHorizon(uint _Np)
+{
+ // Copy variable
+ Np = _Np;
+
+ // Recompute dynamic matrices
+ constructDynamicMatrices(A,B,C,Np,Nc);
+
+ // Allocate appropriate size for Least squares:
+ prepareForLeastSquares();
+}
+
+void MPC::setControlHorizon(uint _Nc)
+{
+
+ // Copy variable
+ Nc = _Nc;
+
+ // Recompute dynamic matrices
+ constructDynamicMatrices(A,B,C,Np,Nc);
+
+ // Allocate appropriate size for Least squares:
+ prepareForLeastSquares();
+}
+
+}
diff --git a/src/mvn.cpp b/src/mvn.cpp
index 2a2073bc09bd4810dbb3e539756c4537cfa6021a..7c5b7e66b01010ffe9a230254468cf3208df0c60 100644
--- a/src/mvn.cpp
+++ b/src/mvn.cpp
@@ -83,6 +83,19 @@ void GaussianDistribution::setNumVARS(uint numvars)
nVARS = numvars;
}
+double GaussianDistribution::getKLdiv(GaussianDistribution &_B)
+{
+ // computed determinant are kept in object to avoid computing more than one time
+ if (!this->DETupdated)
+ this->updateDET();
+ if (!_B.DETupdated)
+ _B.updateDET();
+
+ mat tmpDist = ((_B.MU - this->MU).t()*_B.LAMBDA*(_B.MU-this->MU));
+ double KLdiv = 0.5 * (trace(_B.LAMBDA * this->SIGMA) + tmpDist(0,0) -nVARS
+ + log(_B.DET/this->DET));
+ return KLdiv;
+}
colvec GaussianDistribution::getPDFValue(const mat &SAMPLES)
{
@@ -96,6 +109,20 @@ colvec GaussianDistribution::getPDFValue(const mat &SAMPLES)
return Probs;
}
+
+colvec GaussianDistribution::getPDFValue(const mat &SAMPLES, urowvec ind)
+{
+ // Allocate space for solution and temp variable:
+ colvec Probs(SAMPLES.n_cols);
+// mat D_Tmp;
+// D_Tmp.set_size(MU.n_rows,SAMPLES.n_cols);
+
+ // Calculate probabilities:
+ getPDFValue(Probs,SAMPLES, ind);
+
+ return Probs;
+}
+
void GaussianDistribution::getPDFValue(colvec& _probs, mat _SAMPLES)
{
// Implementation for real time execution: doesnt require memory allocation:
@@ -104,10 +131,10 @@ void GaussianDistribution::getPDFValue(colvec& _probs, mat _SAMPLES)
// - _SAMPLES: Matrix with samples (nbVar x nbSamples, also used as tmp variable)
// calculate difference (x-mu)
- _SAMPLES= trans(_SAMPLES) - repmat(trans(MU),_SAMPLES.n_cols,1);
+ _SAMPLES= trans(_SAMPLES) - repmat(trans(MU),_SAMPLES.n_cols,1);
// calculate exponential (x-mu)^T*inv(sigma)*(x-mu):
- _probs = sum((_SAMPLES* LAMBDA) % _SAMPLES, 1);
+ _probs = sum((_SAMPLES* LAMBDA) % _SAMPLES, 1);
// calculate Exponential:
_probs = sqrt(fabs(det(LAMBDA))/pow(2*PI,LAMBDA.n_cols))*exp(-0.5*arma::abs(_probs));
@@ -117,6 +144,30 @@ void GaussianDistribution::getPDFValue(colvec& _probs, mat _SAMPLES)
}
+void GaussianDistribution::getPDFValue(colvec& _probs, mat _SAMPLES, urowvec ind)
+{
+ // Implementation for real time execution: doesnt require memory allocation:
+ // The user should supply:
+ // - _probs : colvec of length nbSamples
+ // - _SAMPLES: Matrix with samples (nbVar x nbSamples, also used as tmp variable)
+
+ urowvec colInd = linspace<urowvec>(0,_SAMPLES.n_cols-1,_SAMPLES.n_cols);
+
+ // calculate difference (x-mu)
+ _SAMPLES= trans(_SAMPLES.submat(ind,colInd)) - repmat(trans(MU.elem(ind)),_SAMPLES.n_cols,1);
+
+ // calculate exponential (x-mu)^T*inv(sigma)*(x-mu):
+ mat tmpLAMBDA = SIGMA.submat(ind,ind).i();
+ _probs = sum((_SAMPLES* tmpLAMBDA) % _SAMPLES, 1);
+
+ // calculate Exponential:
+ _probs = sqrt(fabs(det(tmpLAMBDA))/pow(2*PI,ind.n_elem))*exp(-0.5*arma::abs(_probs));
+
+ // Implementation with determinant based on SIGMA instead of Lambda:
+ // Probs = exp(-0.5*arma::abs(Probs)) / sqrt(pow((2*PI),SIGMA.n_cols) * (fabs(det(SIGMA)) + THRESHOLD_MIN));
+
+}
+
mat GaussianDistribution::stochasticSampling(uint nbS)
{
mat noisySamples;
@@ -139,19 +190,30 @@ mat GaussianDistribution::sqrtm(const mat SIGMA)
return sqrtmSigma;
}
+void GaussianDistribution::updateDET()
+{
+ DET = det(SIGMA);
+ DETupdated = true;
+}
+
+double GaussianDistribution::getDET()
+{
+ return DET;
+}
+
void GaussianDistribution::setParamsFromData(const mat SAMPLES, const rowvec REWARD, const uint nbImportanceSampling)
{
mat pTmp, eTmp, rTmpDiag;
rowvec rTmp, rSrt;
// Keep the nbImportanceSampling points with highest rewards
- urowvec idSrt = sort_index(REWARD, 1);
- rSrt = sort(REWARD, 1);
+ urowvec idSrt = sort_index(REWARD, "descend");
+ rSrt = sort(REWARD, "descend");
//cout << endl << "rSrt:\n" << rSrt << endl;
//cout << endl << "indices:\n" << idSrt << endl;
uint nbP = idSrt.n_elem;
- if ((idSrt.n_elem > nbImportanceSampling) && (!nbImportanceSampling == 0))
+ if ((idSrt.n_elem > nbImportanceSampling) && (nbImportanceSampling != 0))
nbP = nbImportanceSampling;
//cout << endl << "nbImportanceSampling: \n" << nbP << endl;
@@ -167,7 +229,7 @@ void GaussianDistribution::setParamsFromData(const mat SAMPLES, const rowvec REW
//Udpate the current best SAMPLES
MU = MU + eTmp * trans(rTmp) /sum(rTmp);
- //cout << endl << "Mu: \n" << MU << endl;
+ //cout << endl << "MU: \n" << MU << endl;
//Update the exploration noise (covariance matrix)
rTmpDiag = zeros(rTmp.n_cols, rTmp.n_cols);
@@ -198,5 +260,42 @@ void GaussianDistribution::setParamsFromData( const mat SAMPLES)
//setParamsFromData(SAMPLES, REWARD, nbImportanceSampling);
}
+GaussianDistribution& GaussianDistribution::operator=(const GaussianDistribution& rhs)
+{
+ // Check for self assignment:
+ if (this != &rhs)
+ {
+ // Copy conents:
+ this->MU = rhs.MU;
+ this->LAMBDA = rhs.LAMBDA;
+ this->SIGMA = rhs.SIGMA;
+ }
+
+ return *this;
+}
+
+GaussianDistribution& GaussianDistribution::operator*=(const GaussianDistribution& rhs)
+{
+ // Compute new Covariance:
+ mat newLambda = this->LAMBDA + rhs.LAMBDA;
+ mat newSigma = inv(newLambda);
+
+ // Compute new mean
+ this->MU = newSigma*(this->LAMBDA*this->MU + rhs.LAMBDA*rhs.MU);
+ this->LAMBDA = newLambda;
+ this->SIGMA = newSigma;
+
+ return *this;
+}
+
+GaussianDistribution GaussianDistribution::operator*(const GaussianDistribution& rhs)
+{
+ // Create new object based on the current one
+ GaussianDistribution result = *this;
+ result *=rhs;
+ return result;
+}
+
} //end of pbdlib namespace
+
diff --git a/src/quaternion.cpp b/src/quaternion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c15ab4ada799bb990d1f2c1f5df41b36af4a45e0
--- /dev/null
+++ b/src/quaternion.cpp
@@ -0,0 +1,588 @@
+/**
+ *
+Copyright (C) 2015, João Silvério
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ 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 Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "pbdlib/quaternion.h"
+
+namespace pbdlib
+{
+
+// -- Constructors --
+// Default constructor - initializes identity quaternion
+Quaternion::Quaternion(SortConvention sortInit)
+{
+ sort = sortInit;
+ if(sort==VEC_SCALAR)
+ {
+ coeffs.subvec(0,2) = zeros(3);
+ coeffs(3) = 1;
+ }
+ else
+ {
+ coeffs(0) = 1;
+ coeffs.subvec(1,3) = zeros(3);
+ }
+}
+
+// Initialize a quaternion from vec3 and scalar
+Quaternion::Quaternion(const vec3& vec, double scalar, SortConvention sortInit)
+{
+ sort = sortInit;
+ if(sort==VEC_SCALAR)
+ {
+ coeffs.subvec(0,2) = vec;
+ coeffs(3) = scalar;
+ }
+ else
+ {
+ coeffs(0) = scalar;
+ coeffs.subvec(1,3) = vec;
+ }
+}
+
+// Initialize a quaternion from a vec4
+Quaternion::Quaternion(const vec4& vec, SortConvention sortInit)
+{
+ sort = sortInit;
+ coeffs = vec;
+}
+
+// Initialize a quaternion from an array of double
+Quaternion::Quaternion(const double* array, SortConvention sortInit)
+{
+ sort = sortInit;
+ for (uint i = 0; i < 4; i++)
+ {
+ coeffs[i] = array[i];
+ }
+}
+
+// Initialize a quaternion from 4 scalars of type double
+Quaternion::Quaternion(double q1, double q2, double q3, double q4, SortConvention sortInit)
+{
+ sort = sortInit;
+ coeffs[0] = q1;
+ coeffs[1] = q2;
+ coeffs[2] = q3;
+ coeffs[3] = q4;
+}
+
+// Initialize a quaternion from an angle and an axis
+Quaternion::Quaternion(double angle, vec3 axis, SortConvention sortInit)
+{
+ sort = sortInit;
+
+ double scalar = cos(angle/2.0);
+ vec3 vector = axis*sin(angle/2.0);
+
+ if(sort==VEC_SCALAR)
+ {
+ coeffs.subvec(0,2) = vector;
+ coeffs(3) = scalar;
+ }
+ else
+ {
+ coeffs(0) = scalar;
+ coeffs.subvec(1,3) = vector;
+ }
+}
+
+// Initialize a quaternion from a vec3 whose direction is the rotation axis and norm is the angle of rotation
+// -> this is not working because apparently it conflicts with the one that uses vec4 as type of the first argument
+//Quaternion::Quaternion(const vec3& angleAxis, SortConvention sortInit)
+//{
+// sort = sortInit;
+//
+// double angle = arma::norm(angleAxis);
+// double scalar = cos(angle/2.0);
+// vec3 vector = arma::normalise(angleAxis)*sin(angle/2.0);
+//
+// if(sort==VEC_SCALAR)
+// {
+// coeffs.subvec(0,2) = vector;
+// coeffs(3) = scalar;
+// }
+// else
+// {
+// coeffs(0) = scalar;
+// coeffs.subvec(1,3) = vector;
+// }
+//}
+
+
+// -- Coefficients --
+// Return the quaternion elements as x,y,z,w
+// -> using .x(),.y(),.z(),.w() may read more neatly though (it also avoids lots of 'if' clauses down the road)
+double Quaternion::x() const
+{
+ if(sort==VEC_SCALAR) return coeffs[0];
+ else return coeffs[1];
+}
+double Quaternion::y() const
+{
+ if(sort==VEC_SCALAR) return coeffs[1];
+ else return coeffs[2];
+}
+double Quaternion::z() const
+{
+ if(sort==VEC_SCALAR) return coeffs[2];
+ else return coeffs[3];
+}
+double Quaternion::w() const
+{
+ if(sort==VEC_SCALAR) return coeffs[3];
+ else return coeffs[0];
+}
+
+// Return the vector part of a quaternion as a vec3 type
+vec3 Quaternion::vector() const
+{
+ vec3 vPart;
+ vPart[0] = x();
+ vPart[1] = y();
+ vPart[2] = z();
+ return vPart;
+}
+
+void Quaternion::setVector(const vec3& vec){
+ if(sort==VEC_SCALAR)
+ {
+ coeffs.subvec(0,2) = vec;
+ }
+ else
+ {
+ coeffs.subvec(1,3) = vec;
+ }
+}
+void Quaternion::setScalar(double scalar){
+ if(sort==VEC_SCALAR)
+ {
+ coeffs(3) = scalar;
+ }
+ else
+ {
+ coeffs(0) = scalar;
+ }
+}
+void Quaternion::setAllCoeffs(const vec4& vec){
+ coeffs = vec;
+}
+
+
+void Quaternion::changeSortConvention(SortConvention new_sort){
+
+ colvec4 tmpcoeffs;
+ tmpcoeffs = coeffs;
+ if (new_sort == SCALAR_VEC && sort == VEC_SCALAR){
+ coeffs(0) = tmpcoeffs(3);
+ coeffs.subvec(1,3) = tmpcoeffs.subvec(0,2);
+ sort = new_sort;
+ }else if(new_sort == VEC_SCALAR && sort == SCALAR_VEC){
+ coeffs.subvec(0,2) = tmpcoeffs.subvec(1,3);
+ coeffs(3) = tmpcoeffs(0);
+ sort = new_sort;
+ }else{
+ cout << "sort convention is already in requested order" << endl;
+ }
+}
+
+// -- Operations --
+// Computes quaternion product
+Quaternion Quaternion::product(const Quaternion& rhs) const
+{
+ if(sort!=rhs.sort)
+ {
+ cout << "WARNING: Product between two quaternions using different conventions on the order of their elements." <<endl;
+ }
+ return Quaternion(this->matrix()*rhs.coeffs,sort); // Note that the quaternion matrix was used here, since its easier to implement (also faster I think)
+}
+
+// Rotation of a 3D point using quaternion product
+vec3 Quaternion::operator*(const vec3& rhs) const
+{
+ Quaternion qOut = (*this)*Quaternion(0,rhs[0],rhs[1],rhs[2],this->getSort())*this->conjugate(); // rotation operation: q*Quaternion([0 px py pz])*q.inv
+ return qOut.vector();
+}
+
+// /**
+// * @brief Quaternion scalar product operator.
+// * @param s A scalar by which to multiply all components
+// * of this quaternion.
+// * @return The quaternion (*this) * s.
+// */
+// Quaternion operator*(double s) const {
+// }
+// return Quaternion(complex()*s, real()*s);
+//
+// /**
+// * @brief Produces the sum of this quaternion and rhs.
+// */
+// Quaternion operator+(const Quaternion& rhs) const {
+// return Quaternion(x()+rhs.x(), y()+rhs.y(), z()+rhs.z(), w()+rhs.w());
+// }
+//
+// /**
+// * @brief Produces the difference of this quaternion and rhs.
+// */
+// Quaternion operator-(const Quaternion& rhs) const {
+// return Quaternion(x()-rhs.x(), y()-rhs.y(), z()-rhs.z(), w()-rhs.w());
+// }
+
+// Quaternion scalar division operator.
+Quaternion Quaternion::operator/(double s) const
+{
+ if (s == 0) cout << "Dividing quaternion by 0." << std::endl;
+ return Quaternion(vector()/s, scalar()/s, sort);
+}
+
+
+// -- Quaternion matrices --
+
+// Returns a matrix representation of (*this) quaternion for implementing
+// the quaternion product through matrix-vector product, using the same order
+// of the quaternions:
+//
+// q_new = (*this) * q
+// <=>
+// q_new = this->matrix * q
+//
+mat44 Quaternion::matrix() const
+{
+ mat44 m;
+ if(sort==VEC_SCALAR)
+ {
+ // double m[16]= {
+ // w(),-z(), y(), x(),
+ // z(), w(),-x(), y(),
+ // -y(), x(), w(), z(),
+ // -x(),-y(),-z(), w()
+ // };
+ m << w() << -z() << y() << x() << endr
+ << z() << w() << -x() << y() << endr
+ << -y() << x() << w() << z() << endr
+ << -x() << -y() << -z() << w() << endr;
+ }
+ else
+ {
+ // double m[16]= {
+ // w(), x(), y(), z(),
+ // -x(), w(),-z(), y(),
+ // -y(), z(), w(),-x(),
+ // -z(),-y(), x(), w()
+ // };
+ m << w() << -x() << -y() << -z() << endr
+ << x() << w() << -z() << y() << endr
+ << y() << z() << w() << -x() << endr
+ << z() << -y() << x() << w() << endr;
+ }
+ return m;
+
+}
+
+// Returns a matrix representation of (*this) quaternion for implementing
+// the quaternion product through matrix-vector product, using the *reversed* order
+// of the quaternions:
+//
+// q_new = q * (*this)
+// <=>
+// q_new = this->matrix * q
+//
+mat44 Quaternion::matrixReverse() const
+{
+ mat44 m;
+ if(sort==VEC_SCALAR)
+ {
+ m << w() << z() << -y() << x() << endr
+ << -z() << w() << x() << y() << endr
+ << y() << -x() << w() << z() << endr
+ << -x() << -y() << -z() << w() << endr;
+ }
+ else
+ {
+ m << w() << -x() << -y() << -z() << endr
+ << x() << w() << z() << -y() << endr
+ << y() << -z() << w() << x() << endr
+ << z() << y() << -x() << w() << endr;
+ }
+ return m;
+}
+
+// -- SO(3) conversions --
+// inner product between two quaternions
+double inner(const Quaternion& q1, const Quaternion& q2)
+{
+ return dot(q1.getAllCoeffs(),q2.getAllCoeffs());
+};
+
+// Logarithm of a quaternion
+vec3 qLog(const Quaternion& q)
+{
+ return acos(q.scalar())*q.vector()/arma::norm(q.vector());
+};
+
+// Exponential of an angular displacement (= a quaternion) [maybe the name of the function may be improved?]
+Quaternion qExp(const vec3& aa, SortConvention sortType)
+{
+ return Quaternion(2*arma::norm(aa),arma::normalise(aa),sortType);
+};
+
+// Rotation between two quaternions as a 3D vector (angular displacement)
+vec3 angDiff(const Quaternion& qh, const Quaternion& q)
+{
+ // consider the closest to qh (q or -q)
+ if(inner(qh,q)<0)
+ return 2*qLog(qh*Quaternion(-q.getAllCoeffs(),q.getSort()).inverse());
+ else
+ return 2*qLog(qh*q.inverse());
+}
+
+// -- SLERP --
+Quaternion SLERP(const double& t, Quaternion qFrom, const Quaternion& qTo)
+{
+ if(inner(qTo,qFrom)<0)
+ qFrom = Quaternion(-qFrom.getAllCoeffs());
+
+ vec3 axis = t*qLog(qTo*qFrom.inverse());
+ return pbdlib::qExp(axis,qFrom.getSort())*qFrom; //add a reference to this expression?
+}
+
+Quaternion SLERP0(const double& t, Quaternion qFrom, const Quaternion& qTo)
+{
+ double cosTheta = inner(qTo,qFrom);
+ if (cosTheta < 0) {
+ qFrom = Quaternion(-qFrom.getAllCoeffs());
+ cosTheta = -cosTheta;
+ }
+ double theta = acos(cosTheta);
+
+ if (theta == 0.0){
+ return qFrom;
+ }else{
+ return Quaternion( ( sin((1-t)*theta)*qFrom.getAllCoeffs() +
+ sin(t*theta)*qTo.getAllCoeffs() ) / sin(theta), qFrom.getSort()
+ );
+ }
+}
+
+// /**
+// * @brief Returns a matrix representation of this
+// * quaternion for right multiplication.
+// *
+// * Specifically this is the matrix such that:
+// *
+// * q.vector().transpose() * this->matrix() = (q *
+// * (*this)).vector().transpose() for any quaternion q.
+// *
+// * Note that this is @e NOT the rotation matrix that may be
+// * represented by a unit quaternion.
+// */
+// TMatrix4 rightMatrix() const {
+// double m[16] = {
+// +w(), -z(), y(), -x(),
+// +z(), w(), -x(), -y(),
+// -y(), x(), w(), -z(),
+// +x(), y(), z(), w()
+// };
+// return TMatrix4(m);
+// }
+//
+// /**
+// * @brief Returns this quaternion as a 4-vector.
+// *
+// * This is simply the vector [x y z w]<sup>T</sup>
+// */
+// TVector4 vector() const { return TVector4(mData); }
+//
+
+//
+// /**
+// * @brief Computes the rotation matrix represented by a unit
+// * quaternion.
+// *
+// * @note This does not check that this quaternion is normalized.
+// * It formulaically returns the matrix, which will not be a
+// * rotation if the quaternion is non-unit.
+// */
+// TMatrix3 rotationMatrix() const {
+// double m[9] = {
+// 1-2*y()*y()-2*z()*z(), 2*x()*y() - 2*z()*w(), 2*x()*z() + 2*y()*w(),
+// 2*x()*y() + 2*z()*w(), 1-2*x()*x()-2*z()*z(), 2*y()*z() - 2*x()*w(),
+// 2*x()*z() - 2*y()*w(), 2*y()*z() + 2*x()*w(), 1-2*x()*x()-2*y()*y()
+// };
+// return TMatrix3(m);
+// }
+//
+// /**
+// * @brief Returns the scaled-axis representation of this
+// * quaternion rotation.
+// */
+// TVector3 scaledAxis(void) const {
+// double w[3];
+// HeliMath::scaled_axis_from_quaternion(w, mData);
+// return TVector3(w);
+// }
+//
+// /**
+// * @brief Sets quaternion to be same as rotation by scaled axis w.
+// */
+// void scaledAxis(const TVector3& w) {
+// double theta = w.norm();
+// if (theta > 0.0001) {
+// double s = sin(theta / 2.0);
+// TVector3 W(w / theta * s);
+// mData[0] = W[0];
+// mData[1] = W[1];
+// mData[2] = W[2];
+// mData[3] = cos(theta / 2.0);
+// } else {
+// mData[0]=mData[1]=mData[2]=0;
+// mData[3]=1.0;
+// }
+// }
+//
+// /**
+// * @brief Returns a vector rotated by this quaternion.
+// *
+// * Functionally equivalent to: (rotationMatrix() * v)
+// * or (q * Quaternion(0, v) * q.inverse()).
+// *
+// * @warning conjugate() is used instead of inverse() for better
+// * performance, when this quaternion must be normalized.
+// */
+// TVector3 rotatedVector(const TVector3& v) const {
+// return (((*this) * Quaternion(v, 0)) * conjugate()).complex();
+// }
+//
+//
+//
+// /**
+// * @brief Computes the quaternion that is equivalent to a given
+// * euler angle rotation.
+// * @param euler A 3-vector in order: roll-pitch-yaw.
+// */
+// void euler(const TVector3& euler) {
+// double c1 = cos(euler[2] * 0.5);
+// double c2 = cos(euler[1] * 0.5);
+// double c3 = cos(euler[0] * 0.5);
+// double s1 = sin(euler[2] * 0.5);
+// double s2 = sin(euler[1] * 0.5);
+// double s3 = sin(euler[0] * 0.5);
+//
+// mData[0] = c1*c2*s3 - s1*s2*c3;
+// mData[1] = c1*s2*c3 + s1*c2*s3;
+// mData[2] = s1*c2*c3 - c1*s2*s3;
+// mData[3] = c1*c2*c3 + s1*s2*s3;
+// }
+//
+// /** @brief Returns an equivalent euler angle representation of
+// * this quaternion.
+// * @return Euler angles in roll-pitch-yaw order.
+// */
+// TVector3 euler(void) const {
+// TVector3 euler;
+// const static double PI_OVER_2 = M_PI * 0.5;
+// const static double EPSILON = 1e-10;
+// double sqw, sqx, sqy, sqz;
+//
+// // quick conversion to Euler angles to give tilt to user
+// sqw = mData[3]*mData[3];
+// sqx = mData[0]*mData[0];
+// sqy = mData[1]*mData[1];
+// sqz = mData[2]*mData[2];
+//
+// euler[1] = asin(2.0 * (mData[3]*mData[1] - mData[0]*mData[2]));
+// if (PI_OVER_2 - fabs(euler[1]) > EPSILON) {
+// euler[2] = atan2(2.0 * (mData[0]*mData[1] + mData[3]*mData[2]),
+// sqx - sqy - sqz + sqw);
+// euler[0] = atan2(2.0 * (mData[3]*mData[0] + mData[1]*mData[2]),
+// sqw - sqx - sqy + sqz);
+// } else {
+// // compute heading from local 'down' vector
+// euler[2] = atan2(2*mData[1]*mData[2] - 2*mData[0]*mData[3],
+// 2*mData[0]*mData[2] + 2*mData[1]*mData[3]);
+// euler[0] = 0.0;
+//
+// // If facing down, reverse yaw
+// if (euler[1] < 0)
+// euler[2] = M_PI - euler[2];
+// }
+// return euler;
+// }
+//
+// /**
+// * @brief Computes a special representation that decouples the Z
+// * rotation.
+// *
+// * The decoupled representation is two rotations, Qxy and Qz,
+// * so that Q = Qxy * Qz.
+// */
+// void decoupleZ(Quaternion* Qxy, Quaternion* Qz) const {
+// TVector3 ztt(0,0,1);
+// TVector3 zbt = this->rotatedVector(ztt);
+// TVector3 axis_xy = ztt.cross(zbt);
+// double axis_norm = axis_xy.norm();
+//
+// double axis_theta = acos(HeliMath::saturate(zbt[2], -1,+1));
+// if (axis_norm > 0.00001) {
+// axis_xy = axis_xy * (axis_theta/axis_norm); // limit is *1
+// }
+//
+// Qxy->scaledAxis(axis_xy);
+// *Qz = (Qxy->conjugate() * (*this));
+// }
+//
+// /**
+// * @brief Returns the quaternion slerped between this and q1 by fraction 0 <= t <= 1.
+// */
+// Quaternion slerp(const Quaternion& q1, double t) {
+// return slerp(*this, q1, t);
+// }
+//
+// /// Returns quaternion that is slerped by fraction 't' between q0 and q1.
+// static Quaternion slerp(const Quaternion& q0, const Quaternion& q1, double t) {
+//
+// double omega = acos(HeliMath::saturate(q0.mData[0]*q1.mData[0] +
+// q0.mData[1]*q1.mData[1] +
+// q0.mData[2]*q1.mData[2] +
+// q0.mData[3]*q1.mData[3], -1,1));
+// if (fabs(omega) < 1e-10) {
+// omega = 1e-10;
+// }
+// double som = sin(omega);
+// double st0 = sin((1-t) * omega) / som;
+// double st1 = sin(t * omega) / som;
+//
+// return Quaternion(q0.mData[0]*st0 + q1.mData[0]*st1,
+// q0.mData[1]*st0 + q1.mData[1]*st1,
+// q0.mData[2]*st0 + q1.mData[2]*st1,
+// q0.mData[3]*st0 + q1.mData[3]*st1);
+// }
+//
+// /**
+// * @brief Returns pointer to the internal array.
+// *
+// * Array is in order x,y,z,w.
+// */
+// double* row(uint32_t i) { return mData + i; }
+// // Const version of the above.
+// const double* row(uint32_t i) const { return mData + i; }
+
+};
+
+
diff --git a/src/taskparameters.cpp b/src/taskparameters.cpp
index bf27345de96c3f9cb6678e5cf6f91a36fdf6b966..e2d73ba325b435c351ba7257d0b754a3a8093fa7 100644
--- a/src/taskparameters.cpp
+++ b/src/taskparameters.cpp
@@ -27,6 +27,12 @@ TaskParameters::TaskParameters(uint _nVARS, uint nTaskParameters)
{
nVARS = _nVARS;
MyTPs.resize(nTaskParameters);
+ // Initialize task parameters to zero:
+ for (uint i = 0;i<nTaskParameters;i++)
+ {
+ MyTPs[i].A = zeros(_nVARS,_nVARS);
+ MyTPs[i].b = zeros(_nVARS,1);
+ }
}
std::vector<TaskParameter>& TaskParameters::getTaskParameters()
@@ -58,14 +64,14 @@ void TaskParameters::loadFromFile(std::string path)
// Load Data
if( !rawTPs.load(path, raw_ascii) )
{
- throw std::invalid_argument( "\n [ERROR]::TaskParameters::readParamsFromTxtFile(std::string path) if( paramTmp.load(path, raw_ascii) ) ... else .");
+ throw std::invalid_argument( "\n [ERROR]::TaskParameters::loadFromFile(std::string path) if( !rawTPs.load(path, raw_ascii) ) ... else .");
return;
}
// Check dimensions
if (rawTPs.n_rows % (nVARS+1)!=0|| rawTPs.n_cols!=nVARS)
{
- throw std::invalid_argument("[Error]::TaskParameters::readParamsFromTxtFiel(std::string path) the size of the data in the file is not consistent with the specified parameter dimensions" );
+ throw std::invalid_argument("[Error]::TaskParameters::loadFromFile(std::string path) the size of the data in the file is not consistent with the specified parameter dimensions" );
}
@@ -113,7 +119,7 @@ void TaskParameters::saveToFile(std::string prefix, std::vector<std::string> TPn
// Save Task Parameter names:
std::ofstream fTPnames(TPnames_fname.c_str());
for(unsigned int i=0; i<TPnames.size(); i++)
- fTPnames<< TPnames[i] << endl;
+ fTPnames<< TPnames[i] << " ";
fTPnames.close();
diff --git a/src/tpdemonstration.cpp b/src/tpdemonstration.cpp
index 658d0295ff79d1efc8c7b63662e105a7fe0b24df..df7ccfeb013d836bb06bef52140f47a47811e96d 100644
--- a/src/tpdemonstration.cpp
+++ b/src/tpdemonstration.cpp
@@ -49,11 +49,6 @@ TPDemonstration::TPDemonstration(mat& _data, TaskParameters _TPs)
}
-
-
-
-
-
// ------------------ SET TASK PARAMETERS --------------------------------
void TPDemonstration::setDataPointTPs(std::vector<TaskParameters> _PointTPs)
{
@@ -222,7 +217,7 @@ void TPDemonstration::loadFromFiles(std::string data_path, std::string tp_path,
this->Data.loadFromFile(data_path, DataTrans);
// Set dimensions:
- PointTPs.resize(this->getNumPOINTS());
+ PointTPs.reserve(this->getNumPOINTS());
// Load Parameters:
loadTaskParametersFromFile(tp_path);
@@ -301,6 +296,12 @@ void TPDemonstration::loadTaskParametersFromFile(std::string tp_path)
// Create a vector of Taskparameters object
std::vector<TaskParameters> tmpVTPs;
+ cout << "loading " << endl;
+// cout << "TPraw" << endl << TPraw << endl;
+ cout << "nVars: " << nVARS << endl;
+ cout << "nTPs : " << nTPs << endl;
+ cout << "_nPoints: " << _nPoints << endl;
+
// Create a tmp TaskParameters Object
uint TPind;
TaskParameters tmpTPs(nVARS,nTPs);
@@ -317,20 +318,26 @@ void TPDemonstration::loadTaskParametersFromFile(std::string tp_path)
trans(TPraw.submat(TPind, p*nVARS,TPind,(p+1)*nVARS-1));
}
// Add to vector of TPs:
+// cout << "Push back ";
tmpVTPs.push_back(tmpTPs);
}
+ cout << "loading task parameters done" << endl;
// Set task parameters to object:
if(_nPoints==1)
{
// Set the same task parameters for each data point
+ cout << "Set Data Points1 " << endl;
+ cout << tmpVTPs.size() << endl;
setDataPointTPs(tmpVTPs[0]);
}
else
{
// Set unique task parameters for each data point:
+ cout << "Set Data Points2 " ;
setDataPointTPs(tmpVTPs);
}
+ cout << "Done " << endl;
}
@@ -393,17 +400,17 @@ void TPDemonstration::saveInFiles(std::string prefix,
this->Data.setVarNames(varnames);
this->Data.saveInFile(data_fname);
- // Save Task Parameter names:
+ // Save variable names:
std::ofstream fVarNames(varNames_fname.c_str());
for(unsigned int i=0; i<varnames.size(); i++)
- fVarNames<< varnames[i] << endl;
+ fVarNames<< varnames[i] << " ";
fVarNames.close();
// Put Task Parameters in Matrix:
if (uniqueTPs)
{
- TaskParameters tmpTPs;
+ TaskParameters tmpTPs(this->getNumVARS(),this->getNumTASKPARAMETERS());
TaskParameter tmpTP;
mat tmpMat1;
mat tmpMat2;
diff --git a/src/tpdpgmm.cpp b/src/tpdpgmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c63bdbb1e7d7a5512c9304c13ce0f805fb0388f6
--- /dev/null
+++ b/src/tpdpgmm.cpp
@@ -0,0 +1,734 @@
+//
+// Created by pignate on 22.10.15.
+//
+
+#include <stdio.h>
+#include "pbdlib/tpdpgmm.h"
+#include <pbdlib/gmm.h>
+#include "math.h"
+
+using namespace arma;
+using namespace std;
+
+namespace pbdlib {
+
+void TPDPGMM::addDatapoints(mat _dataPoint,vec _trParam, bool startFlag, bool endFlag, float _lambda,float _minSigma ) {
+ if (_lambda != 0.0)
+ lambda = _lambda;
+ if (_minSigma != 0.0)
+ minSigma = _minSigma;
+ //cout<<minSigma<<endl;
+ uint curState;
+ if (nbPoints == 0)
+ {
+ std::vector<GaussianDistribution> comps;
+ mat minSIGMA = eye(nVARS,nVARS)*minSigma;
+
+ GMM_Model tmpGMM(nSTATES,nVARS);
+ colvec dataFrame;
+
+ for(uint i=0;i<nTaskParameters;i++)
+ {
+ dataFrame = colvec(_dataPoint.col(i));
+ //cout<<dataFrame<<endl;
+ GaussianDistribution componentTmp(dataFrame,minSIGMA);
+ comps.push_back(componentTmp);
+
+ tmpGMM.setCOMPONENTS(comps);
+ GMMS.push_back(tmpGMM);
+
+ //cout<<GMMS[i].getMU(0)<<endl;
+ //cout<<nSTATES<<endl;
+
+ comps.clear();
+ }
+ PRIORS = 1;
+
+ framePRIORS = ones(1,nTaskParameters+1);
+ framePRIORS(0,nTaskParameters) = 1;
+ curState = 0;
+ }
+ else
+ {
+ colvec dataFrame;
+
+ mat lik(nTaskParameters+1,nSTATES+1);
+
+ mat SigmaTmp = eye(nVARS,nVARS)*lambda*lambda;
+ colvec MuTmp = zeros(nVARS);
+
+ GaussianDistribution cluster(MuTmp,SigmaTmp);
+
+ for(uint k=0; k<nSTATES;k++)
+ {
+ for(uint m =0;m<nTaskParameters;m++)
+ {
+ dataFrame = colvec(_dataPoint.col(m));
+ //likelihood
+ cluster.setMU(GMMS[m].getMU(k));
+ lik(m,k) = cluster.getPDFValue(dataFrame)(0);
+ }
+
+ cluster.setMU(prodGMM->getMU(k));
+// lik(nTaskParameters,k) = cluster.getPDFValue(_dataPoint.col(nTaskParameters))(0);
+ // disable assignement for gaussian product
+ lik(nTaskParameters,k) = 0.0;
+ }
+
+ double probLim = 1./pow(lambda,nVARS)/sqrt(pow(2*PI,nVARS))*P_1VAR;
+
+ for(uint m =0;m<nTaskParameters+1;m++)
+ {
+ lik(m,nSTATES) = probLim;
+ }
+ //cout << lik <<endl;
+ // store likelihood without the frame priors to update the frame prior ( avoid self-reinforcement )
+ mat likTmp = lik;
+
+ // multiplying prob to be in frame with prob to be in cluster -- comment this line to see the effect
+ lik.cols(0,nSTATES-1) %= framePRIORS.t();
+
+ //cout<<probFrame<<endl;
+ //cout<<framePRIORS<<endl;
+
+ uvec vecMaxIdx = find(lik == max(max(lik)),1);
+
+ //cout<<d<<endl;
+ uint maxIndex = vecMaxIdx(0)/(nTaskParameters+1);
+
+ umat frameAssign = (likTmp.col(maxIndex)>probLim);
+ //cout<< frameAssign<<endl;
+
+ uvec frameIndex = find(frameAssign);
+
+ curState = maxIndex;
+// cout<<framePRIORS.row(maxIndex)<<endl;
+
+ if(maxIndex==nSTATES)
+ {
+ rowvec priorsTmp = zeros(1,nSTATES+1);
+ priorsTmp.cols(0,nSTATES-1) = this->getPRIORS();
+ priorsTmp(nSTATES) = 1./nbPoints;
+ priorsTmp = priorsTmp/arma::norm(priorsTmp,1);
+ PRIORS = priorsTmp;
+
+ nSTATES++;
+ // new states : resize priors frame Priors
+ mat framePRIORSTmp = framePRIORS;
+ framePRIORS = ones(nSTATES,nTaskParameters+1);
+ framePRIORS.rows(0,nSTATES-2) = framePRIORSTmp;
+ framePRIORS(nSTATES-1,nTaskParameters) = 1.0;
+
+ for(uint k=0;k<nTaskParameters;k++)
+ {
+ mat minSIGMA = eye(nVARS, nVARS) * minSigma;
+ dataFrame = colvec(_dataPoint.col(k));
+ GaussianDistribution componentTmp(dataFrame,minSIGMA);
+
+ std::vector<GaussianDistribution> prevComps = GMMS[k].getCOMPONENTS();
+
+ prevComps.push_back(componentTmp);
+
+ GMMS[k] = GMM_Model(nSTATES,nVARS);
+ GMMS[k].setCOMPONENTS(prevComps);
+ //GMMS[k].setCOMPONENTS(prevComps);
+ }
+ }
+ else
+ {
+ double PriorsTmp = 1./nbPoints + PRIORS[maxIndex];
+
+ uvec maxIndexVec(1);
+ maxIndexVec(0) = maxIndex;
+
+ for(uint k=0; k<nTaskParameters;k++)
+ {
+ colvec P = colvec(_dataPoint.col(k));
+ colvec MuTmp = 1./PriorsTmp*(PRIORS[maxIndex]*GMMS[k].getMU(maxIndex)+P/nbPoints);
+ mat SigmaTmp = PRIORS[maxIndex]/PriorsTmp*(GMMS[k].getSIGMA(maxIndex)+
+ (GMMS[k].getMU(maxIndex)-MuTmp)*(GMMS[k].getMU(maxIndex)-MuTmp).t()) +
+ 1./(nbPoints*PriorsTmp)*(minSigma*eye(nVARS,nVARS)+(P-MuTmp)*(P-MuTmp).t());
+
+ GMMS[k].setMU(maxIndex,MuTmp);
+ GMMS[k].setSIGMA(maxIndex,SigmaTmp);
+ }
+
+ // the probabilty to be in both frames is added to the probabilty to be in one state and not the other
+ framePRIORS.row(maxIndex) =(PRIORS[maxIndex]/PriorsTmp) *framePRIORS.row(maxIndex) +
+ (1.0/((nbPoints)*PriorsTmp))* (frameAssign.t()%ones(1,nTaskParameters+1));
+
+ PRIORS[maxIndex] = PriorsTmp;
+ PRIORS = PRIORS/arma::norm(PRIORS,1);
+ }
+ }
+ timeStepCNT++;
+ if(prevState!=curState||endFlag){ // transition detected
+
+ tpTrans->addTransition(prevState,curState,_trParam,startFlag,timeStepCNT);
+ if(endFlag&&prevState!=curState)
+ {
+ tpTrans->addTransition(curState,curState,_trParam,startFlag,timeStepCNT);
+ }
+
+ tpTrans->updateTransitionMat(_trParam);
+ //cout<<timeStepCNT<<endl;
+ timeStepCNT =0;
+ }
+ //cout<<framePRIORS<<endl;
+ setAuxiliaryVarsProdGauss();
+ //getTransformedGMM()
+ prevState = curState;
+ nbPoints++;
+}
+
+void TPDPGMM::addDatapoints_d(mat _dataPoint,vec _trParam, bool startFlag, bool endFlag, float _lambda,
+ float _lambda2, float _lambda3,float _minSigma, int type ,bool sticky) {
+ if (_lambda != 0.0)
+ lambda = _lambda;
+ if (_lambda2 != -1.f)
+ lambda2 = _lambda2;
+ if (_lambda3 != -1.f)
+ lambda3 = _lambda3;
+ if (_minSigma != 0.0)
+ minSigma = _minSigma;
+
+ uint curState;
+ if (nbPoints == 0)
+ {
+ std::vector<GaussianDistribution> comps;
+ mat minSIGMA = eye(nVARS,nVARS)*minSigma;
+
+ GMM_Model tmpGMM(nSTATES,nVARS);
+ colvec dataFrame;
+
+ for(uint i=0;i<nTaskParameters;i++)
+ {
+ dataFrame = colvec(_dataPoint.col(i));
+ //cout<<dataFrame<<endl;
+ GaussianDistribution componentTmp(dataFrame,minSIGMA);
+ comps.push_back(componentTmp);
+
+ tmpGMM.setCOMPONENTS(comps);
+ GMMS.push_back(tmpGMM);
+
+ //cout<<GMMS[i].getMU(0)<<endl;
+ //cout<<nSTATES<<endl;
+
+ comps.clear();
+ }
+ PRIORS = 1;
+
+ framePRIORS = ones(1,nTaskParameters+1);
+ framePRIORS(0,nTaskParameters) = 1;
+ curState = 0;
+ }
+ else
+ {
+ colvec dataFrame;
+
+ mat lik(nTaskParameters+1,nSTATES+1);
+ mat dist(nTaskParameters+1,nSTATES+1);
+
+ mat SigmaTmp = eye(nVARS,nVARS)*lambda*lambda;
+ colvec MuTmp = zeros(nVARS);
+
+ GaussianDistribution cluster(MuTmp,SigmaTmp);
+
+ mat transTmp = tpTrans->getTRANSITION();
+ transTmp.elem( find_nonfinite(transTmp)).zeros();//change NaN to zero
+
+ // compute the probability to go out of the state given the HSMM duration
+// mat timeVal(1,1);
+// timeVal(0,0) = (double)timeStepCNT;
+// double transOutProb = tpTrans->getDurationCOMPONENTS(prevState).getCDFValue(timeVal)(0);
+
+ double transOutProb = erf(((double)timeStepCNT-tpTrans->getDurMU(prevState)(0))/tpTrans->getDurSIGMA(prevState)(0,0));
+ transOutProb = (1.0 + transOutProb)/2.0;
+ // cout << "Transition out prob :"<<transOutProb<<endl;
+
+ for(uint k=0; k<nSTATES;k++)
+ {
+ for(uint m =0;m<nTaskParameters;m++)
+ {
+ dataFrame = colvec(_dataPoint.col(m));
+ //likelihood
+// cluster.setMU(GMMS[m].getMU(k));
+// lik(m,k) = cluster.getPDFValue(dataFrame)(0);
+
+ if(k==prevState||startFlag)
+ {
+
+ if(sticky)
+ dist(m,k) = arma::norm(GMMS[m].getMU(k) - dataFrame,2)+transOutProb*lambda3;
+ else if(transTmp.n_rows>0&&sum(transTmp.row(prevState))==0.0f)
+ dist(m,k) = arma::norm(GMMS[m].getMU(k) - dataFrame,2)+lambda3;
+ else
+ dist(m,k) = arma::norm(GMMS[m].getMU(k) - dataFrame,2);
+// dist(m,k) = arma::norm(GMMS[m].getMU(k) - dataFrame,2);
+ }
+ else
+ { if(sticky)
+ {
+ dist(m,k) = arma::norm(GMMS[m].getMU(k) - dataFrame,2)+
+ lambda3*(1.0-transOutProb*transTmp(prevState,k));
+ }
+ else
+ {
+ dist(m,k) = arma::norm(GMMS[m].getMU(k) - dataFrame,2)+
+ lambda3*(1.0-transTmp(prevState,k));
+ }
+
+ }
+ }
+
+// cluster.setMU(prodGMM->getMU(k));
+// lik(nTaskParameters,k) = cluster.getPDFValue(_dataPoint.col(nTaskParameters))(0);
+ // disable assignement for gaussian product
+// lik(nTaskParameters,k) = 0.0;
+ dist(nTaskParameters,k) = 1000*lambda;
+ }
+
+// double probLim = 1./pow(lambda,nVARS)/sqrt(pow(2*PI,nVARS))*P_1VAR;
+
+ for(uint m =0;m<nTaskParameters+1;m++)
+ {
+// lik(m,nSTATES) = probLim;
+// if(sum(transTmp.row(prevState))==0)
+// dist(m,nSTATES) = lambda;
+// else
+ dist(m,nSTATES) = lambda+lambda3;
+ }
+ dist(nTaskParameters,nSTATES) = 1000*lambda;
+ // cout<<"Time in current state : "<< timeStepCNT << " / " << tpTrans->getDurMU(prevState)<<endl;
+
+ //cout << lik <<endl;
+ // store likelihood without the frame priors to update the frame prior ( avoid self-reinforcement )
+// mat likTmp = lik;
+// cout << transTmp<<endl;
+// cout << dist <<endl;
+// cout << framePRIORS.t()<<endl;
+
+ uint maxIndex;
+ if(type==0)
+ {
+ // multiplying prob to be in frame with prob to be in cluster -- comment this line to see the effect
+// lik.cols(0, nSTATES - 1) %= framePRIORS.t();
+ dist.cols(0, nSTATES - 1) += lambda2* (1. - framePRIORS.t());
+ //cout << dist << endl;
+ //cout<<probFrame<<endl;
+ //cout<<framePRIORS<<endl;
+
+
+ // uvec vecMaxIdx = find(lik == max(max(lik)),1);
+ uvec vecMaxIdx = find(dist == min(min(dist)), 1);
+
+ //cout<<d<<endl;
+ maxIndex = vecMaxIdx(0) / (nTaskParameters + 1);
+ }
+ else if(type==1)
+ {
+ mat sumDist = sum(dist,0);
+ cout <<sumDist<<endl;
+
+ uvec vecMaxIdx = find(sumDist == min(min(sumDist)), 1);
+ //cout<<d<<endl;
+ maxIndex = vecMaxIdx(0);
+ }
+ umat frameAssign = (dist.col(maxIndex)<lambda+lambda3);
+// umat frameAssign = (dist.col(maxIndex)<lambda);
+// cout<< frameAssign<<endl;
+
+ //uvec frameIndex = find(frameAssign);
+
+ curState = maxIndex;
+
+ if(maxIndex==nSTATES)
+ {
+ rowvec priorsTmp = zeros(1,nSTATES+1);
+ priorsTmp.cols(0,nSTATES-1) = this->getPRIORS();
+ priorsTmp(nSTATES) = 1./nbPoints;
+ priorsTmp = priorsTmp/arma::norm(priorsTmp,1);
+ PRIORS = priorsTmp;
+
+ nSTATES++;
+ // new states : resize priors frame Priors
+ mat framePRIORSTmp = framePRIORS;
+ framePRIORS = ones(nSTATES,nTaskParameters+1);
+ framePRIORS.rows(0,nSTATES-2) = framePRIORSTmp;
+ framePRIORS(nSTATES-1,nTaskParameters) = 1.0;
+
+ for(uint k=0;k<nTaskParameters;k++)
+ {
+ mat minSIGMA = eye(nVARS, nVARS) * minSigma;
+ dataFrame = colvec(_dataPoint.col(k));
+ GaussianDistribution componentTmp(dataFrame,minSIGMA);
+
+ std::vector<GaussianDistribution> prevComps = GMMS[k].getCOMPONENTS();
+
+ prevComps.push_back(componentTmp);
+
+ GMMS[k] = GMM_Model(nSTATES,nVARS);
+ GMMS[k].setCOMPONENTS(prevComps);
+ //GMMS[k].setCOMPONENTS(prevComps);
+ }
+ }
+ else
+ {
+ double PriorsTmp = 1./nbPoints + PRIORS[maxIndex];
+
+ uvec maxIndexVec(1);
+ maxIndexVec(0) = maxIndex;
+
+ for(uint k=0; k<nTaskParameters;k++)
+ {
+ colvec P = colvec(_dataPoint.col(k));
+ colvec MuTmp = 1./PriorsTmp*(PRIORS[maxIndex]*GMMS[k].getMU(maxIndex)+P/nbPoints);
+ mat SigmaTmp = PRIORS[maxIndex]/PriorsTmp*(GMMS[k].getSIGMA(maxIndex)+
+ (GMMS[k].getMU(maxIndex)-MuTmp)*(GMMS[k].getMU(maxIndex)-MuTmp).t()) +
+ 1./(nbPoints*PriorsTmp)*(minSigma*eye(nVARS,nVARS)+(P-MuTmp)*(P-MuTmp).t());
+
+ GMMS[k].setMU(maxIndex,MuTmp);
+ GMMS[k].setSIGMA(maxIndex,SigmaTmp);
+ }
+
+ // the probabilty to be in both frames is added to the probabilty to be in one state and not the other
+ framePRIORS.row(maxIndex) =(PRIORS[maxIndex]/PriorsTmp) *framePRIORS.row(maxIndex) +
+ (1.0/((nbPoints)*PriorsTmp))* (frameAssign.t()%ones(1,nTaskParameters+1));
+
+ PRIORS[maxIndex] = PriorsTmp;
+ PRIORS = PRIORS/arma::norm(PRIORS,1);
+ }
+ }
+ timeStepCNT++;
+ if(prevState!=curState||endFlag){ // transition detected
+
+ tpTrans->addTransition(prevState,curState,_trParam,startFlag,timeStepCNT);
+ if(endFlag&&prevState!=curState)
+ {
+ tpTrans->addTransition(curState,curState,_trParam,startFlag,timeStepCNT);
+ }
+
+ tpTrans->updateTransitionMat(_trParam);
+ //cout<<timeStepCNT<<endl;
+ timeStepCNT =0;
+ }
+ //cout<<framePRIORS<<endl;
+ setAuxiliaryVarsProdGauss();
+ //getTransformedGMM()
+ prevState = curState;
+ nbPoints++;
+}
+
+
+GMM_Model* TPDPGMM::getProdGMM()
+{
+ return prodGMM;
+}
+
+uint TPDPGMM::getNumPoints()
+{
+ return nbPoints;
+}
+mat TPDPGMM::getTransitionMatrix()
+{
+ return tpTrans->getTRANSITION();
+}
+void TPDPGMM::setTransitionMatrix(mat tm)
+{
+ tpTrans->setTRANSITION(tm);
+}
+
+bool TPDPGMM::reduceStates()
+{
+// cube div = zeros(nSTATES, nSTATES, nTaskParameters); // Kullback-Leibler divergence
+ cube like = zeros(nSTATES, nSTATES, nTaskParameters+1);
+ mat transProb = zeros(nSTATES,nSTATES); // probability to have the sames transitions
+
+ mat probFrame = framePRIORS;
+
+
+ for (uint k = 0; k < nSTATES; k++)
+ {
+ for (uint l = 0; l < k; l++)
+ {
+ transProb(k,l) = tpTrans->getTransSimilarity(k,l);
+
+ for (uint m = 0; m < nTaskParameters; m++)
+ {
+ //div(k,l,m)=GMMS[m].getCOMPONENTS(k).getKLdiv(GMMS[m].getCOMPONENTS(l));
+ /*
+ like(k, l, m) = transProb(k,l)*sqrt(probFrame(k, m) *probFrame(l, m))
+ * exp(-0.5 * arma::norm(GMMS[m].getMU(k) - GMMS[m].getMU(l)) / lambda);
+ */
+ like(k, l, m) = sqrt(probFrame(k, m) *probFrame(l, m))
+ * exp(-0.5 * arma::norm(GMMS[m].getMU(k) - GMMS[m].getMU(l)) / lambda);
+ /*
+ like(k, l, m) = sqrt(probFrame(k, m) *probFrame(l, m))
+ * exp(-0.5 * arma::norm(GMMS[m].getMU(k) - GMMS[m].getMU(l)) / lambda);
+ */
+ }
+ like(k, l, nTaskParameters) = sqrt(probFrame(k, nTaskParameters) * probFrame(l, nTaskParameters)) *
+ exp(-0.5 * arma::norm(prodGMM->getMU(k) - prodGMM->getMU(l)) / lambda);
+ // to avoid merging in absolute reference frame
+ like(k, l, nTaskParameters) = 0;
+ }
+
+ }
+// cout<<like<<endl;
+// cout<<transProb<<endl;
+
+ uword k,l,m;
+ double maxProb = like.max(k,l,m);
+
+ if (maxProb > P_1VAR)
+ {
+ cout << "States " <<k << " and " << l << " will be merged."<< endl;
+ mergeStates(k,l);
+ return true;
+ }
+ else
+ return false;
+}
+
+bool TPDPGMM::reduceStates_d()
+{
+// cube div = zeros(nSTATES, nSTATES, nTaskParameters); // Kullback-Leibler divergence
+ cube like = zeros(nSTATES, nSTATES, nTaskParameters+1);
+ cube dist = ones(nSTATES, nSTATES, nTaskParameters+1);
+ dist.fill(2*lambda);
+ mat transProb = zeros(nSTATES,nSTATES); // probability to have the sames transitions
+
+ mat probFrame = framePRIORS;
+
+
+ for (uint k = 0; k < nSTATES; k++)
+ {
+ for (uint l = 0; l < k; l++)
+ {
+
+ transProb(k,l) = tpTrans->getTransSimilarity(k,l);
+
+ for (uint m = 0; m < nTaskParameters; m++)
+ {
+
+ // like(k, l, m) = sqrt(probFrame(k, m) *probFrame(l, m))
+ // * exp(-0.5 * arma::norm(GMMS[m].getMU(k) - GMMS[m].getMU(l)) / lambda);
+ dist(k,l,m) = arma::norm(GMMS[m].getMU(k) - GMMS[m].getMU(l))
+ +(1.0-framePRIORS(k, m))*(1.0-framePRIORS(l, m))*lambda3;
+ }
+ // like(k, l, nTaskParameters) = sqrt(probFrame(k, nTaskParameters) * probFrame(l, nTaskParameters)) *
+ // exp(-0.5 * arma::norm(prodGMM->getMU(k) - prodGMM->getMU(l)) / lambda);
+ // // to avoid merging in absolute reference frame
+ // like(k, l, nTaskParameters) = 0;
+ dist(k,l,nTaskParameters) = 2*lambda;
+ }
+
+ }
+// cout<<like<<endl;
+// cout<<transProb<<endl;
+
+ uword k,l,m;
+// double maxProb = like.max(k,l,m);
+ float minDist = dist.min(k,l,m);
+ //cout<< dist<<endl;
+
+// if (maxProb > P_1VAR)
+ if (minDist < lambda)
+ {
+ cout << "States " <<k << " and " << l << " will be merged."<< endl;
+ mergeStates(k,l);
+ return true;
+ }
+ else
+ return false;
+}
+
+void TPDPGMM::mergeStates(uint k, uint l)
+{
+ double PriorsTmp = PRIORS(k) + PRIORS(l);
+ for (uint m = 0; m < nTaskParameters; m++)
+ {
+ colvec MuTmp = 1.0 / PriorsTmp * (PRIORS[k] * GMMS[m].getMU(k) + PRIORS[l] * GMMS[m].getMU(l));
+ mat SigmaTmp = 1.0 / PriorsTmp * (PRIORS[k] * (GMMS[m].getSIGMA(k) +
+ (GMMS[m].getMU(k) - MuTmp) * (GMMS[m].getMU(k) - MuTmp).t())
+ + PRIORS[l] * (GMMS[m].getSIGMA(l) +
+ (GMMS[m].getMU(l) - MuTmp) * (GMMS[m].getMU(l) - MuTmp).t()));
+ GMMS[m].setMU(l, MuTmp);
+ GMMS[m].setSIGMA(l, SigmaTmp);
+ GMMS[m].deleteCOMPONENT(k);
+ }
+ tpTrans->mergeTransition(k,l);
+ prodGMM->deleteCOMPONENT(k);
+ framePRIORS.shed_row(k);
+ PRIORS.shed_col(k);
+ if (prevState == k)
+ prevState = l;
+ else if(prevState>k)
+ prevState--;
+ nSTATES--;
+
+}
+void TPDPGMM::updateTransition(vec _trParam){
+
+ tpTrans->updateTransitionMat(_trParam);
+// cout<<tpTrans->getDurationCOMPONENTS(2).getMU()<<endl;
+// cout<<tpTrans->getTRANSITION()<<endl;
+}
+mat TPDPGMM::predictForwardVariable(uint _N, uint startState){
+
+ tpTrans->setPRIORS(prodGMM->getPRIORS());
+ tpTrans->initializeFwdCalculation();
+
+ mat pred;
+// pred.set_size(tpTrans->getNumSTATES(),_N);
+ pred.set_size(prodGMM->getNumSTATES(),_N);
+ tpTrans->predictForwardVariableDeterministic(pred, startState);
+
+ return pred;
+}
+GMM_Model* TPDPGMM::getTransformedGMM(TaskParameters _TPs, EMatrixType AType){
+
+ return getTransformedGMM(_TPs.getTaskParameters(),AType);
+}
+
+GMM_Model* TPDPGMM::getTransformedGMM(std::vector<TaskParameter> _TPs, EMatrixType AType)
+{
+ double diagRegularizationFactor = 1E-4; // regularization factor to invert A*Sigma*A' which is often close to singular
+
+ // Check size of Task taskparameters
+ if (_TPs.size()!=nTaskParameters)
+ throw std::invalid_argument("TPGMM::getTransformedProdGMM(std::vector<TaskParameter> _TPs, EMatrixType AType) Number of supplied task parameters is not consistent expected number of taskparameters");
+
+
+ //Projections in the different candidate taskParameters (see Eq. (3) Calinon et al, Humanoids'2012 paper)
+ if (AType==OTHER || nSTATES<2)
+ {
+ // Use implementation based on covariance matrices when type of matrix A is not specified or
+ // the number of states in the GMM is less than 2
+ for (uint i=0; i<nSTATES; i++){
+ accMu = zeros(nVARS, 1);
+ accLambda= zeros(nVARS, nVARS);
+ for (uint m = 0; m < nTaskParameters ; m++)
+ {
+ // Projecting Zmu of state "i" at taskParameter "m"
+ tmpMu = _TPs[m].A * GMMS[m].getMU(i) + _TPs[m].b;
+
+ // Projecting Zsigma of state "i" at taskParameter "m"
+ tmpLambda= inv(_TPs[m].A * GMMS[m].getSIGMA(i) * trans(_TPs[m].A) + eye(nVARS,nVARS)*diagRegularizationFactor);
+
+ // Accumulate:
+ accLambda+= tmpLambda;
+ accMu+= tmpLambda* tmpMu;
+ }
+ // Saving the Gaussian distribution for state "i"
+ prodGMM->setMU(i,accLambda.i()*accMu);
+ prodGMM->setLAMBDA(i,accLambda);
+ }
+ prodGMM->setPRIORS(this->PRIORS);
+ }
+ else
+ {
+ // Approach based on Precision matrices:
+ // Requires less inverses
+
+ // Pre-compute A^-1 and (A^T)^-1.
+ // First select method based on type of matrix A:
+ switch (AType)
+ {
+ case ORTHONORMAL:
+ for (uint m =0; m<nTaskParameters; m++)
+ {
+ invA[m] = _TPs[m].A.t();
+ invAT[m] = _TPs[m].A;
+ }
+ break;
+ case SYMMETRIC:
+ for (uint m =0; m<nTaskParameters; m++)
+ {
+ invA[m] = _TPs[m].A.i();
+ invAT[m] = invA[m];
+ }
+ break;
+ case INVERTIBLE:
+ for (uint m =0; m<nTaskParameters; m++)
+ {
+ invA[m] = _TPs[m].A.i();
+ invAT[m] = inv(_TPs[m].A.t());
+ }
+ break;
+ default:
+ break;
+ }
+
+ mat refPRIORS;
+
+ if(framePRIORSProd) {
+ refPRIORS = framePRIORS.cols(0, nTaskParameters - 1);
+
+ // the probabilty to be in both frames is added to the probabilty to be in one state and not the other
+ //refPRIORS = refPRIORS + repmat(framePRIORSbis.col(nTaskParameters), 1, nTaskParameters);
+
+ // to test but better without
+ //refPRIORS = refPRIORS / repmat(sum(refPRIORS,1),1,nTaskParameters);
+ //cout << refPRIORS << endl;
+ }
+
+ // Compute product of Gaussian
+ for (uint i=0; i<nSTATES; i++){
+ accMu = zeros(nVARS, 1);
+ accLambda= zeros(nVARS, nVARS);
+
+ for (uint m = 0; m < nTaskParameters ; m++)
+ {
+ // Projecting Zmu of state "i" at taskParameter "m"
+ tmpMu= _TPs[m].A * GMMS[m].getMU(i) + _TPs[m].b;
+
+ // Projecting Zsigma of state "i" at taskParameter "m"
+ tmpLambda= invAT[m] * GMMS[m].getLAMBDA(i) * invA[m];
+
+ // Accumulate
+ if(framePRIORSProd) {
+ accLambda += pow(refPRIORS(i, m),1) * tmpLambda;
+
+ accMu += pow(refPRIORS(i, m),1) * tmpLambda * tmpMu;
+ }
+ else{
+ accLambda += tmpLambda;
+
+ accMu += tmpLambda * tmpMu;
+ }
+ }
+
+ // Saving the Gaussian distribution for state "i"
+ prodGMM->setMU(i,accLambda.i()*accMu);
+ prodGMM->setLAMBDA(i,accLambda);
+ }
+ prodGMM->setPRIORS(this->PRIORS);
+ }
+
+ /*
+ for (uint i=0;i<nSTATES;i++)
+ {
+ cout << prodGMM->getMU(i) << endl;
+ cout << prodGMM->getSIGMA(i) << endl;
+ }
+ */
+
+ return prodGMM;
+}
+
+void TPDPGMM::saveInFiles(std::string path) {
+ TPGMM::saveInFiles(path);
+ tpTrans->saveInFiles(path);
+ framePRIORS.save(path + "TPGMM_frame_PRIORS.txt",raw_ascii);
+ colvec nbPts(1);
+ nbPts(0) = nbPoints;
+ nbPts.save(path + "TPGMM_nbPoints.txt",raw_ascii);
+}
+void TPDPGMM::loadFromFiles(std::string path)
+{
+ TPGMM::loadFromFiles(path);
+}
+}
+
+
diff --git a/src/tpgmm.cpp b/src/tpgmm.cpp
index d23121776cfe4528d07b2522ffacbf6711938c3b..3b31da6277011f46709c07242fa86f3e6351882c 100644
--- a/src/tpgmm.cpp
+++ b/src/tpgmm.cpp
@@ -109,15 +109,132 @@ void TPGMM::loadTPGMMfromMATLAB(std::string PriorsFileName, std::string VarsFile
this->GMMS.push_back(GMM_temp);
// Setting variables' name using the last temporary GMM (For avoiding the re-implementation o$
+
+
if(this->vars_names.size() == 0)
this->setVARSNames(GMM_temp.getVARSNames());
}
- cout << "PGMM loaded from text files!" << endl;
+ cout << "TP-GMM loaded from text files!" << endl;
// Prepare for performing Gaussian Product:
setAuxiliaryVarsProdGauss();
}
+void TPGMM::loadFromFiles(std::string path)
+{
+
+}
+
+TPGMM::TPGMM(std::vector<GMM_Model> gmms){
+
+// this->nVARS = GMMs[0]
+
+ assert(gmms.size() > 0);
+ this->GMMS = gmms;
+ this->nTaskParameters = this->GMMS.size();
+ this->nSTATES = this->GMMS[0].getNumSTATES();
+ this->nVARS = this->GMMS[0].getNumVARS();
+ this->PRIORS = this->GMMS[0].getPRIORS();
+
+ setAuxiliaryVarsProdGauss();
+}
+TPGMM::TPGMM(std::string path, uint _nTPs)
+{
+ this->nTaskParameters = _nTPs;
+ this->GMMS.reserve(this->nTaskParameters);
+
+ for(int l=0;l<_nTPs;l++)
+ {
+ mat priors, mu, sigma;
+ char filename[256];
+
+ sprintf(filename,"GMM_priors%i.txt",l);
+ priors.load(path + filename, raw_ascii);
+
+ sprintf(filename,"GMM_mu%i.txt",l);
+ mu.load(path + filename, raw_ascii);
+
+ sprintf(filename,"GMM_sigma%i.txt",l);
+ sigma.load(path + filename, raw_ascii);
+ cout << "Priors \n" << priors << endl;
+
+ nVARS = mu.n_rows;
+ nSTATES = priors.n_elem;
+
+ mat _SIGMA = zeros(nVARS, nVARS);
+ colvec _MU = zeros(nVARS,1);
+ rowvec _PRIORS(1, nSTATES);
+ std::vector<GaussianDistribution> components;
+
+ for(uint i=0; i<nSTATES; i++){
+ _PRIORS(0,i) = priors(0,i);
+ for(uint j=0; j<nVARS; j++){
+ _MU(j,0) = mu(j,i);
+ for(uint k=0; k<nVARS; k++)
+ _SIGMA(j,k) = sigma(j,k + i*(nVARS));
+ }
+ components.push_back(GaussianDistribution(_MU, _SIGMA));
+ }
+// GMMS[l] = GMM_Model(nSTATES,nVARS);
+// GMMS[l].setPRIORS(_PRIORS);
+// GMMS[l].setCOMPONENTS(components);
+//
+ GMM_Model GMM_temp(nSTATES,nVARS);
+ GMM_temp.setPRIORS(_PRIORS);
+ GMM_temp.setCOMPONENTS(components);
+ this->GMMS.push_back(GMM_temp);
+
+ }
+ // Assign object variables:
+
+ this->PRIORS.resize(nSTATES);
+
+
+ PRIORS.load(path+"TPGMM_priors.txt", raw_ascii );
+
+ // Prepare for calculating Gaussian Product:
+ setAuxiliaryVarsProdGauss();
+}
+void TPGMM::saveInFiles(std::string path)
+{
+ for(int l=0;l<nTaskParameters;l++)
+ {
+ mat priors(1, nSTATES);
+ mat mu(nVARS, nSTATES);
+ mat sigma(nVARS, nVARS*nSTATES);
+
+ for(uint i=0; i<nSTATES; i++){
+ priors(0,i) = GMMS[l].getPRIORS()(i);
+ for(uint j=0; j<nVARS; j++){
+ mu(j,i) = GMMS[l].getMU(i)(j);
+ for(uint k=0; k<nVARS; k++)
+ sigma(j,k + i*(nVARS)) = GMMS[l].getSIGMA(i)(j,k);
+ }
+ }
+// char filename[256];
+// sprintf(filename,"GMM_priors%i.txt",l);
+// priors.save(path + filename, raw_ascii);
+//
+// sprintf(filename,"GMM_mu%i.txt",l);
+// mu.save(path + filename, raw_ascii);
+//
+// sprintf(filename,"GMM_sigma%i.txt",l);
+// sigma.save(path + filename, raw_ascii);
+
+ char filename[256];
+ sprintf(filename,"GMM_priors_P%i.txt",l+1);
+ priors.save(path + filename, raw_ascii);
+
+ sprintf(filename,"GMM_mu_P%i.txt",l+1);
+ mu.save(path + filename, raw_ascii);
+
+ sprintf(filename,"GMM_sigma_P%i.txt",l+1);
+ sigma.save(path + filename, raw_ascii);
+
+ }
+
+ PRIORS.save(path + "TPGMM_priors.txt", raw_ascii);
+}
void TPGMM::setAuxiliaryVarsProdGauss()
{
@@ -145,6 +262,8 @@ GMM_Model* TPGMM::getTransformedGMM(TaskParameters _TPs, EMatrixType AType)
GMM_Model* TPGMM::getTransformedGMM(std::vector<TaskParameter> _TPs, EMatrixType AType)
{
+ double diagRegularizationFactor = 1E-4; // regularization factor to invert A*Sigma*A' which is often close to singular
+
// Check size of Task taskparameters
if (_TPs.size()!=nTaskParameters)
throw std::invalid_argument("TPGMM::getTransformedProdGMM(std::vector<TaskParameter> _TPs, EMatrixType AType) Number of supplied task parameters is not consistent expected number of taskparameters");
@@ -164,7 +283,7 @@ GMM_Model* TPGMM::getTransformedGMM(std::vector<TaskParameter> _TPs, EMatrixType
tmpMu = _TPs[m].A * GMMS[m].getMU(i) + _TPs[m].b;
// Projecting Zsigma of state "i" at taskParameter "m"
- tmpLambda= inv(_TPs[m].A * GMMS[m].getSIGMA(i) * trans(_TPs[m].A));
+ tmpLambda= inv(_TPs[m].A * GMMS[m].getSIGMA(i) * trans(_TPs[m].A) + eye(nVARS,nVARS)*diagRegularizationFactor);
// Accumulate:
accLambda+= tmpLambda;
@@ -206,6 +325,8 @@ GMM_Model* TPGMM::getTransformedGMM(std::vector<TaskParameter> _TPs, EMatrixType
invAT[m] = inv(_TPs[m].A.t());
}
break;
+ default:
+ break;
}
// Compute product of Gaussian
@@ -244,6 +365,44 @@ GMM_Model* TPGMM::getTransformedGMM(std::vector<TaskParameter> _TPs, EMatrixType
return prodGMM;
}
+void TPGMM::tpOnlineEMDP(TaskParameters _TPs, int N,colvec P,double lambda, double minSigma){
+ // Check size of Task taskparameters
+ if (_TPs.getTaskParameters().size()!=nTaskParameters)
+ throw std::invalid_argument("TPGMM::getTransformedProdGMM(std::vector<TaskParameter> _TPs, EMatrixType AType) Number of supplied task parameters is not consistent expected number of taskparameters");
+
+ for (uint m = 0; m < nTaskParameters ; m++) {
+ // Projecting Zmu of state "i" at taskParameter "m"
+ //solve(tmpParam.A,this->Data.getData(i)-tmpParam.b);
+ //colvec tmpP = _TPs.getTaskParameters()[m].A * P + _TPs.getTaskParameters()[m].b;
+ colvec tmpP = solve(_TPs.getTaskParameters()[m].A, P - _TPs.getTaskParameters()[m].b);
+ if (N == 1){
+ GMM_Model tmpGMM(nSTATES,nVARS);
+ GMMS.push_back(tmpGMM);
+ mat minSIGMA = eye(nVARS,nVARS) * minSigma;
+ // GaussianDistribution componentTmp(tmpP,minSIGMA);
+ // std::vector<GaussianDistribution> comps;
+ // comps.push_back(componentTmp);
+ // GMMS[m].setCOMPONENTS(comps);
+ GMMS[m].setMU(0,tmpP);
+ GMMS[m].setSIGMA(0,minSIGMA);
+ }
+// cout << "mu: " << GMMS[m].getMU(0);
+// cout << "s: " << GMMS[m].getSIGMA(0);
+ // Projecting Zsigma of state "i" at taskParameter "m"
+ GMMS[m].onlineEMDP(N,tmpP,lambda,minSigma);
+ }
+ //cout << "this->nSTATES = " << this->nSTATES;
+ //cout << "GMMS[0].getNumSTATES() = " << GMMS[0].getNumSTATES();
+ if (this->nSTATES!=GMMS[0].getNumSTATES()){
+ this->nSTATES=GMMS[0].getNumSTATES();
+ prodGMM= new GMM_Model(nSTATES, nVARS);
+ this->PRIORS = GMMS[0].getPRIORS();
+ }
+
+
+
+}
+
/*----------------------------------------------------------------------------
* EM-ESTIMATION FUNCTIONS
*--------------------------------------------------------------------------- */
@@ -323,13 +482,192 @@ void TPGMM::initTensorGMM_timeBased()
}
-
/*----------------------------------------------------------------------------*/
-void TPGMM::initTensorGMM_kmeans()
+void TPGMM::initTensorGMM_kbins(std::vector<TPDemonstration>& demos){
+ std::vector<mat> bins;
+ for (int j = 0; j < this->nSTATES; ++j) {
+ mat tmpMat;
+ bins.push_back(tmpMat);
+ }
+
+ // Split each demonstration in K equal bins
+ for (int i = 0; i < demos.size(); ++i) {
+ mat data = demos[i].getDataInTaskParameters();
+ vec binSep = round(linspace(0, demos[i].getNumPOINTS(), this->nSTATES+1));
+ for (int j = 0; j < this->nSTATES; ++j) {
+ bins[j] = join_horiz(bins[j], data.cols(binSep(j), binSep(j+1)-1));
+ }
+ }
+
+ // Auxiliary variables and initialization
+ mat _Mu = zeros(nVARS*nTaskParameters, nSTATES);
+ cube _Sigma = zeros(nVARS*nTaskParameters, nVARS*nTaskParameters, nSTATES);
+ rowvec _Priors = zeros(1, nSTATES);
+
+ // Calculate statistics on bin data
+ for (int i = 0; i < this->nSTATES; ++i) {
+ _Mu.col(i) = mean(bins[i], 1);
+ _Sigma.slice(i) = cov(bins[i].t());
+ _Priors(i) = bins[i].n_cols;
+ }
+ this->PRIORS=_Priors/sum(_Priors);
+
+ // Copy result into a TPGMM structure.
+ // Here we use a vector of GMM in which each GMM represents a representation in one Task Parameter
+ mat _StateSigma, _TPSigma;
+ colvec _TPMu;
+
+ GMM_Model tmpGMM(nSTATES, nVARS);
+ for (uint m=0; m<nTaskParameters; m++){
+ GMMS.push_back(tmpGMM);
+ for (uint i=0; i<nSTATES; i++){
+ // Separate the GMMs for each Task Parameter:
+ _TPMu = _Mu.submat(m*nVARS, i, (m+1)*nVARS-1, i);
+ _StateSigma = _Sigma.slice(i);
+ _TPSigma = _StateSigma.submat(m*nVARS, m*nVARS, (m+1)*nVARS-1, (m+1)*nVARS-1);
+
+ // Saving the Gaussian distribution for state "i"
+ GMMS[m].setSIGMA(i, _TPSigma);
+ GMMS[m].setMU(i, _TPMu);
+ }
+ GMMS[m].setPRIORS(this->PRIORS);
+ }
+}
+
+static uvec randperm( int n )
{
- //Joao: you can implement this based on the Matlab code
+ //return sort_index(randn<urowvec>(nbData));
+ return sort_index(randu(n)); // was randn, should work equally well?
}
+void TPGMM::initTensorGMM_kmeans(){
+
+ //Ajay Tanwani, 2016
+
+ double diagRegularizationFactor = 1E-4;
+ double cumdist_threshold = 1e-10;
+ uint maxIter = 100;
+
+ mat _Mu;
+ cube _Sigma;
+ rowvec _Priors;
+
+ // Check if Data is of correct size
+ if (Data.n_rows != nTaskParameters*nVARS){
+ throw std::invalid_argument("[ERROR]::TPGMM::initTensorGMM_kmeans() Data dimension is not consistent with specified number of Parameters and number of Variables.");
+ }
+
+ //Criterion to stop the EM iterative update
+ uint nbVar = Data.n_rows;
+ uint nbData = Data.n_cols;
+
+ // Initialize
+ _Mu = zeros(nVARS*nTaskParameters,nSTATES);
+ _Sigma = zeros(nVARS*nTaskParameters,nVARS*nTaskParameters,nSTATES);
+ _Priors = zeros(1,nSTATES);
+
+ double cumdist_old = -std::numeric_limits<double>::max();
+ uint nbStep = 0;
+
+// cout << "rand perm" << endl;
+// uvec idTmp = randperm(nbData);
+ uvec idTmp = linspace<uvec>(1, nbData, nbData);
+
+// cout << "linspace" << endl;
+ uvec allrows = linspace<uvec>(0, nbVar-1, nbVar);
+
+ // nSTATES means
+ _Mu = Data.submat(allrows, idTmp.subvec(0,nSTATES-1));
+
+ uvec idList = zeros<uvec>(nbData,1);
+
+ //k-means iterations
+ while(true){
+ mat distTmp = zeros(nbData,nSTATES);
+
+ //E-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ for (uint i=0; i<nSTATES; i++){
+ //Compute distances
+ distTmp.col(i) = trans( sum( pow(Data - repmat(_Mu.col(i), 1, nbData), 2.0) ) );
+ }
+
+ vec vTmp = zeros<vec>(nbData,1);
+ uvec idList = zeros<uvec>(nbData,1);
+
+ for( uint i = 0; i < nbData; i++ ){
+ // there is a chance that here two elements will be as close and will skip one?s
+ vTmp[i] = ((rowvec)distTmp.row(i)).min(idList[i]);
+ }
+
+ double cumdist = sum(vTmp);
+
+ //%%%%%%%% M-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ for (uint i=0; i<nSTATES; i++){
+ mat TmpData;
+ for (uint j=0; j < nbData; j++){
+ if (idList(j) == i){
+ TmpData = join_horiz(Data.col(j), TmpData);
+ }
+ }
+// uvec idTmp = find(idList == i);
+// _Mu.col(i) = mean(Data.submat(allrows, idTmp), 1);
+ _Mu.col(i) = mean(TmpData, 1);
+ }
+
+ //Stopping criterion %%%%%%%%%%%%%%%%%%%%
+ if (fabs(cumdist-cumdist_old) < cumdist_threshold){
+ cout << "%%%%%%%%%%% KMEANS %%%%%%%%%%%%%%%%%%" << endl;
+ cout << nbStep << "Steps" << endl;
+ cout << endl << "Mu_kmeans :" << endl << _Mu << endl;
+
+ for(int i=0; i<nSTATES; i++){
+ uvec idTmp = find(idList == i);
+
+ PRIORS(i) = idTmp.n_elem;
+
+ _Sigma.slice(i) = cov( trans(join_rows(Data.submat(allrows, idTmp),Data.submat(allrows, idTmp))) ,0 )
+ + eye(nbVar, nbVar)*diagRegularizationFactor;
+ }
+
+ PRIORS = PRIORS / nbData;
+ this->PRIORS = PRIORS;
+ cout << "PRIORS:" << endl << PRIORS << endl;
+
+ cout << "%%%%%%% KMEANS FINISHED %%%%%%%%%%%%%" << endl;
+ break;
+ }
+ cumdist_old = cumdist;
+ nbStep = nbStep + 1;
+
+ if (nbStep > maxIter){
+ cout << "Max Iterations Reached: KMeans finished before convergence " << endl;
+ break;
+ }
+ }
+
+// %Reshape GMM parameters into a tensor
+ mat _StateSigma, _TPSigma;
+ colvec _TPMu;
+
+ GMM_Model tmpGMM(nSTATES,nVARS);
+ for (uint m=0; m<nTaskParameters; m++){
+ GMMS.push_back(tmpGMM);
+ for (uint i=0; i<nSTATES; i++){
+ // Separate the GMMs for each Task Parameter:
+ _TPMu = _Mu.submat(m*nVARS,i,(m+1)*nVARS-1,i);
+ _StateSigma= _Sigma.slice(i);
+ _TPSigma= _StateSigma.submat(m*nVARS,m*nVARS, (m+1)*nVARS-1,(m+1)*nVARS-1);
+
+ // Saving the Gaussian distribution for state "i"
+ GMMS[m].setSIGMA(i,_TPSigma);
+ GMMS[m].setMU(i,_TPMu);
+ }
+ GMMS[m].setPRIORS(this->PRIORS);
+ }
+
+}
+
+
/*----------------------------------------------------------------------------*/
void TPGMM::EM_tensorGMM()
{
@@ -352,7 +690,8 @@ void TPGMM::EM_tensorGMM()
mat SigmaTmp;
std::vector<mat> GAMMA0;
mat GAMMA = zeros(nVARS, nDATA);
- mat GAMMA2 = zeros(nVARS, nDATA);
+// mat GAMMA2 = zeros(nVARS, nDATA);
+ GAMMA2 = zeros(nVARS, nDATA);
vec LL = zeros(nbMaxSteps);
GAMMA0.resize(nTaskParameters);
for (uint m=0;m<nTaskParameters; m++){
@@ -363,6 +702,7 @@ void TPGMM::EM_tensorGMM()
for (uint nbIter=0; nbIter<nbMaxSteps; nbIter++){
// Expectation-Step, see Eq. (6.1) in doc/TechnicalReport.pdf
mat L = ones(nSTATES,nDATA);
+
for (uint m=0; m<nTaskParameters; m++){
DataTmp = Data.rows(m*nVARS, (m+1)*nVARS-1);
for(uint i=0; i<nSTATES; i++){
@@ -437,6 +777,10 @@ std::vector<GMM_Model>& TPGMM::getGMMS(){
return this->GMMS;
}
+ void TPGMM::setGMMS(std::vector<GMM_Model>& vGMMS){
+ this->GMMS = vGMMS;
+}
+
GMM_Model& TPGMM::getGMMS(uint id)
{
// Check if GMM is available:
@@ -461,6 +805,10 @@ rowvec& TPGMM::getPRIORS()
return this->PRIORS;
}
+void TPGMM::setPRIORS(rowvec pr) {
+ this->PRIORS = pr;
+}
+
double TPGMM::getPRIORS(uint id)
{
if (nSTATES<=id)
@@ -475,11 +823,19 @@ std::vector<std::string>& TPGMM::getVARSNames()
void TPGMM::setVARSNames(const std::vector<std::string>& vars)
{
+
if(vars.size() == nVARS)
this->vars_names = vars;
else
- std::cout << "\n [ERROR]::TPGMM::setVARSNames if(vars.size() == nVARS) ... else .";
+ {
+
+ std::ostringstream msg;
+ msg << "\n [ERROR]::TPGMM::usetVARSNames if(vars.size() == nVARS) number of varnames (";
+ msg << vars.size() << ") names does not correspond with specified number of variables (" ;
+ msg << nVARS << ").";
+
+ throw std::invalid_argument(msg.str());
+ }
}
} //end of namespace pbdlib
-
diff --git a/src/tphsmm.cpp b/src/tphsmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3f1f89e714ede46cdbbe9b16ade058402a888f63
--- /dev/null
+++ b/src/tphsmm.cpp
@@ -0,0 +1,835 @@
+/*
+ * tphsmm.cpp
+ *
+ * Created on: Sep 15, 2015
+ * Author: Ajay Tanwani
+ */
+
+#include "pbdlib/tphsmm.h"
+namespace pbdlib
+{
+TPHSMM::TPHSMM(uint _nVARS, uint _nSTATES, uint _nTPs): HSMM(_nSTATES, _nVARS){
+ this->nbVARPOS = _nVARS;
+ this->nSTATES = _nSTATES;
+ tpgmm = new TPGMM(_nVARS, _nSTATES, _nTPs);
+ taskparam = new TaskParameters(_nVARS, _nTPs);
+ double dt = 0.1f;
+ double rf = 1E-6;
+ init_TPHSMM_LQR(dt, rf);
+}
+
+TPHSMM::TPHSMM(uint _nVARS, uint _nSTATES, uint _nTPs, uint _nbVARPOS, double dt, double rf): HSMM(_nSTATES, _nVARS){
+ this->nbVARPOS = _nbVARPOS;
+ this->nSTATES = _nSTATES;
+ tpgmm = new TPGMM(_nVARS, _nSTATES, _nTPs);
+ taskparam = new TaskParameters(_nVARS, _nTPs);
+ init_TPHSMM_LQR(dt, rf);
+}
+
+TPHSMM::TPHSMM(uint _nVARS,uint _nTPS, float _minSigma): HSMM(1,_nVARS){
+
+ this->nTPS = _nTPS;
+ this->minSigma = _minSigma;
+ TPTRANSITION transTmp(_nTPS);
+ std::vector<TPTRANSITION> outTransTmp;
+ outTransTmp.push_back(transTmp);
+
+ TPTransitionMatrix.push_back(outTransTmp);
+ hsmm_priors_ticks = zeros(1);
+ colvec MuDurTmp = zeros(1);
+ mat SigmaDurTmp = eye(1,1);
+ GaussianDistribution DurTmp(MuDurTmp,SigmaDurTmp);
+ DurationCOMPONENTS.push_back(DurTmp);
+ this->nSTATES = 1;
+}
+
+TPHSMM::TPHSMM(std::vector<GMM_Model> GMMS, mat transition, std::vector<GaussianDistribution> components, uint _nVARS, uint _nSTATES, uint _nTPs): HSMM(_nSTATES, _nVARS){
+
+// this->nTaskParameters = this->GMMS.size();
+ tpgmm = new TPGMM(GMMS);
+
+ this->nSTATES = GMMS[0].getNumSTATES();
+ this->nbVARPOS = GMMS[0].getNumVARS();
+
+ taskparam = new TaskParameters(_nVARS, _nTPs);
+
+ TransitionMatrix = transition;
+
+ // Set state duration components:
+ mat _SIGMA = zeros(1,1);
+ colvec _MU = zeros(1,1);
+// std::vector<GaussianDistribution> components;
+//
+// for(uint i=0; i<_nSTATES; i++){
+// _MU(0,0) = durMu(0,i);
+// _SIGMA(0,0) = durSigma(0,i);
+// // for debugging
+// components.push_back(GaussianDistribution(_MU, _SIGMA));
+// }
+ setDurationCOMPONENTS(components);
+
+ // Forward variable calculation initialization:
+ initializeFwdCalculation();
+
+ double dt = 0.1f;
+ double rf = 1E-6;
+ init_TPHSMM_LQR(dt, rf);
+}
+
+TPHSMM::TPHSMM(std::string path,uint _nSTATES,uint _nVARS,uint _nTPS, float _minSigma): HSMM(1,_nVARS){
+
+ this->nTPS = _nTPS;
+ this->minSigma = _minSigma;
+ this->nSTATES = _nSTATES;
+ mat mu,nTrans;
+ cube sigma;
+
+ nTrans.load(path + "nTrans_tp_transition.txt", raw_ascii);
+ mu.load(path + "mu_tp_transition.txt", raw_ascii);
+ sigma.load(path + "sigma_tp_transition.txt", arma_binary);
+
+ for(int i=0;i<nSTATES;i++){
+ std::vector<TPTRANSITION> outTransTmp;
+ for (int j = 0; j < nSTATES; j++){
+ TPTRANSITION transTmp(_nTPS);
+
+ colvec MuTmp(_nTPS);
+ mat SigmaTmp(_nTPS,_nTPS);
+
+ for(int i_T=0;i_T<nTPS;i_T++){
+ MuTmp(i_T)= mu(j + i * nSTATES, i_T);
+ for (int j_T = 0; j_T < nTPS; j_T++)
+ {
+ SigmaTmp(i_T,j_T) = sigma(j + i * nSTATES, i_T, j_T);
+ }
+ }
+ transTmp.transParam->setMU(MuTmp);
+ transTmp.transParam->setSIGMA(SigmaTmp);
+ transTmp.nTrans = nTrans(i,j);
+ outTransTmp.push_back(transTmp);
+ }
+ TPTransitionMatrix.push_back(outTransTmp);
+ }
+ hsmm_priors_ticks.load(path + "hsmm_priors_ticks.txt", raw_ascii);
+
+ mat durMu, durSigma;
+
+ // Load state duration components
+ durMu.load(path + "durMu.txt", raw_ascii);
+ durSigma.load(path + "durSigma.txt", raw_ascii);
+
+ // Set state duration components:
+ mat _SIGMA = zeros(1,1);
+ colvec _MU = zeros(1,1);
+ std::vector<GaussianDistribution> components;
+
+ for(uint i=0; i<_nSTATES; i++){
+ _MU(0,0) = durMu(0,i);
+ _SIGMA(0,0) = durSigma(0,i);
+ // for debugging
+ components.push_back(GaussianDistribution(_MU, _SIGMA));
+ }
+ setDurationCOMPONENTS(components);
+
+ // Forward variable calculation initialization:
+ initializeFwdCalculation();
+}
+
+void TPHSMM::init_TPHSMM_LQR(double dt, double rf){
+ InitialState.zeros(tpgmm->getNumVARS(),1);
+ CurrentState = InitialState;
+
+ mat A, B;
+
+ mat A1d; A1d << 0 << 1 << endr << 0 << 0 << endr;
+ mat B1d; B1d << 0 << endr << 1 << endr;
+
+ A = kron(A1d, eye(nbVARPOS,nbVARPOS));
+ B = kron(B1d, eye(nbVARPOS,nbVARPOS));
+
+ lqr = new LQR(A,B,dt);
+ pred_length = 200;
+ RegularizationConstLQR = 1E-3;
+ HorizonMethod = 0;
+ SamplingMethod = 0;
+ SimMethod = 0;
+
+ mat R = eye(nbVARPOS,nbVARPOS) * rf;
+ lqr->setR(R);
+}
+
+/*
+ * variable knobs to keep
+ * - diagRegularziation Mat
+ * - R
+ * - pred_length
+ * - infinite horizon vs finite horizon
+ * - stochastic sampling vs deterministic sampling
+ */
+
+
+//TPHSMM::TPHSMM(std::string PriorsFileName,
+// std::string VarsFileName,
+// std::string MuFilePrefix,
+// std::string SigmaFilePrefix,
+// const std::string &transition_path,
+// const std::string &durMu_path,
+// const std::string &durSigma_path){
+//
+// // load TPGMM
+// //loadTPHSMMfromFiles(PriorsFileName,VarsFileName,MuFilePrefix, SigmaFilePrefix);
+// loadTPGMMfromMATLAB(PriorsFileName,VarsFileName,MuFilePrefix, SigmaFilePrefix);
+//
+// // load transition matrix
+// mat transition;
+// transition.load(transition_path,raw_ascii);
+// TransitionMatrix = transition;
+//
+// // load duration probability distributions
+// mat durMu, durSigma;
+//
+// // Load state duration components
+// durMu.load(durMu_path, raw_ascii);
+// durSigma.load(durSigma_path, raw_ascii);
+//
+// // Set state duration components:
+// mat _SIGMA = zeros(1,1);
+// colvec _MU = zeros(1,1);
+// std::vector<GaussianDistribution> components;
+//
+// for(uint i=0; i<this->nSTATES; i++){
+// _MU(0,0) = durMu(0,i);
+// _SIGMA(0,0) = durSigma(0,i);
+// // for debugging
+// components.push_back(GaussianDistribution(_MU, _SIGMA));
+// }
+// setDurationCOMPONENTS(components);
+//
+// // Forward variable calculation initialization:
+// initializeFwdCalculation();
+//
+//}
+
+TPHSMM::~TPHSMM() {
+ // TODO Auto-generated destructor stub
+ delete lqr;
+ delete tpgmm;
+ delete taskparam;
+}
+
+
+void TPHSMM::setSamplingMethod(uint SamplingMethod){
+ //SamplingMethod = 0 implies deterministic sampling
+ //SamplingMethod = 1 implies stochastic sampling
+ assert(SamplingMethod == 0 || SamplingMethod == 1);
+ this->SamplingMethod = SamplingMethod;
+}
+
+void TPHSMM::setLQRHorizonMethod(uint HorizonMethod){
+ //HorizonMethod = 0 implies infinite Horizon
+ //HorizonMethod = 1 implies finite Horizon
+ assert(HorizonMethod == 0 || HorizonMethod == 1);
+ this->HorizonMethod = HorizonMethod;
+}
+
+void TPHSMM::setSimulationMethod(uint SimMethod){
+ assert(SimMethod == 0 || SimMethod == 1);
+ this->SimMethod = SimMethod;
+}
+
+void TPHSMM::setPredictionLength(uint PredictionLength){
+ this->pred_length = PredictionLength;
+}
+
+void TPHSMM::setScalingR(double rf){
+
+// mat R = eye(nbVARPOS,nbVARPOS) * rf;
+ mat R = eye(nbVARPOS,nbVARPOS) * pow(10.0f,rf);
+ lqr->setR(R);
+}
+
+void TPHSMM::setRegularizationConstLQR(double RC){
+// mat R = eye(nbVARPOS,nbVARPOS) * pow(10.0f,rf);
+ RegularizationConstLQR = pow(10.0f,RC);
+}
+
+void TPHSMM::loadTPHSMMfromFiles(std::string PriorsFileName,
+ std::string VarsFileName,
+ std::string MuFilePrefix,
+ std::string SigmaFilePrefix,
+ const std::string &transition_path,
+ const std::string &durMu_path,
+ const std::string &durSigma_path){
+
+ tpgmm->loadTPGMMfromMATLAB(PriorsFileName,VarsFileName,MuFilePrefix, SigmaFilePrefix);
+
+ // load transition matrix
+ mat transition;
+ transition.load(transition_path,raw_ascii);
+ TransitionMatrix = transition;
+
+ // load duration probability distributions
+ mat durMu, durSigma;
+
+ // Load state duration components
+ durMu.load(durMu_path, raw_ascii);
+ durSigma.load(durSigma_path, raw_ascii);
+
+ // Set state duration components:
+ mat _SIGMA = zeros(1,1);
+ colvec _MU = zeros(1,1);
+ std::vector<GaussianDistribution> components;
+
+ for(uint i=0; i<this->getNumSTATES(); i++){
+ _MU(0,0) = durMu(0,i);
+ _SIGMA(0,0) = durSigma(0,i);
+ // for debugging
+ components.push_back(GaussianDistribution(_MU, _SIGMA));
+ }
+ setDurationCOMPONENTS(components);
+
+ // Forward variable calculation initialization:
+ initializeFwdCalculation();
+
+}
+
+void TPHSMM::setCurrentTaskFrameFromFile(std::string FramePath){
+ taskparam->loadFromFile(FramePath);
+}
+
+void TPHSMM::setCurrentTaskFrame(std::vector<TaskParameter> TaskParameters){
+ taskparam->setTaskParameters(TaskParameters);
+}
+
+std::vector<TaskParameter>& TPHSMM::getTaskParameters(){
+ return taskparam->getTaskParameters();
+}
+
+mat TPHSMM::HSMMLQR(GMM_Model& GMM, colvec init_state, uint pred_length){
+
+ for (int i=0; i < tpgmm->getNumVARS(); i++){
+ cout << init_state[i] << endl;
+ }
+ mat h_i(this->getNumSTATES(), 1);
+
+ for (int i=0; i<this->getNumSTATES(); i++){
+ h_i.row(i) = trans(GMM.getCOMPONENTS(i).getPDFValue(init_state));
+ }
+ uword start_state;
+ h_i.max(start_state);
+
+ //Predict the forward variable
+ AlphaHSMM.zeros(this->getNumSTATES(), pred_length);
+
+ if (SamplingMethod == 0){
+ predictForwardVariableDeterministic(AlphaHSMM, start_state);
+ }else{
+ predictForwardVariableStochasticStart(AlphaHSMM, start_state);
+ }
+
+ uword StateSequence[pred_length];
+
+ get_state_seq(StateSequence, AlphaHSMM);
+
+ //Set Q and Target
+ std::vector<mat> Qt;
+ mat Q(nbVARPOS*2,nbVARPOS*2, fill::zeros);
+ mat S_T(nbVARPOS*2,nbVARPOS*2, fill::zeros);
+ colvec d_T(nbVARPOS*2, fill::zeros);
+ mat Target(nbVARPOS*2, pred_length, fill::zeros);
+ vec vTmp(nbVARPOS*2, fill::zeros);
+ mat RegularizationMat = eye(tpgmm->getNumVARS(),tpgmm->getNumVARS())*RegularizationConstLQR;
+
+ int counter = 0;
+ for (int i=0; i<pred_length-1; i++){
+ if (StateSequence[i] == StateSequence[i+1]){
+ counter = counter + 1;
+ }
+ else{
+ cout << "State " << i << ",\t" << StateSequence[i] << ",\t" << counter << endl;
+ counter = 0;
+ }
+ }
+ cout << "State " << pred_length-1 << ",\t" << StateSequence[pred_length-1] << ",\t" << counter << endl;
+
+
+ for (int i=0; i < pred_length; i++){
+// cout << i << ",\t" << StateSequence[i] << endl;
+ Q.submat(0,0,tpgmm->getNumVARS()-1,tpgmm->getNumVARS()-1) = inv(ProdGauss->getSIGMA(StateSequence[i]) + RegularizationMat);
+ Qt.push_back(Q);
+ vTmp.subvec(0,tpgmm->getNumVARS()-1) = ProdGauss->getMU(StateSequence[i]);
+ Target.col(i) = vTmp;
+ }
+
+ // set up the lqr problem
+ lqr->setProblem(lqr->getR(), Qt, Target);
+ if (HorizonMethod == 1){
+ lqr->evaluate_gains_finiteHorizon(S_T, d_T);
+ }else{
+ lqr->evaluate_gains_infiniteHorizon();
+ }
+
+
+// S_T = lqr->solveAlgebraicRiccati(lqr->getA(), lqr->getB(), (Qt[pred_length-1] + Qt[pred_length-1].t())/2, lqr->getR());
+
+ std::vector<mat> S = lqr->getS();
+ std::vector<mat> L = lqr->getGains();
+// for (int k = pred_length-1; k >= 0; k--){
+// cout << k << endl;
+// for (int i=0; i< nbVARPOS; i++){
+// for (int j=0; j < TPGMM::getNumVARS(); j++){
+// // cout << S_T.at(i,j) << ",\t";
+// // cout << Qt[pred_length - 1].at(i,j) << ",\t";
+// cout << L[k].at(i,j) << ",\t";
+// }
+// cout << endl;
+// }
+// cout << endl << endl;
+// }
+//
+// cout << "###############################################" << endl << endl;
+// for (int k = pred_length-1; k >= 0; k--){
+// cout << k << endl;
+// for (int i=0; i< S_T.n_rows; i++){
+// for (int j=0; j < S_T.n_cols; j++){
+// // cout << S_T.at(i,j) << ",\t";
+// // cout << Qt[pred_length - 1].at(i,j) << ",\t";
+// cout << S[k].at(i,j) << ",\t";
+// }
+// cout << endl;
+// }
+// cout << endl << endl;
+// }
+// cout << "dt: " << lqr->getdt() << endl;
+ //Retrieve data
+ mat rD(nbVARPOS, pred_length, fill::zeros);
+
+ if (SimMethod == 0){
+
+ vec x = init_state.subvec(0,nbVARPOS-1);
+ vec dx(nbVARPOS, fill::zeros); // [x dx] = join_cols(init_state, zeros<vec>(nbVARPOS));
+ vec u(nbVARPOS);
+
+ for (int t=0; t<pred_length; t++){
+ u = - lqr->getGains().at(t)*(join_cols(x, dx) - Target.col(t)) + lqr->getFF().at(t);
+ // u = - lqr->getGains().at(t) * (x - Target.col(t));
+ dx += u*lqr->getdt();
+ x += dx*lqr->getdt();
+ setCurrentState(x);
+ rD.col(t) = x;
+ }
+ }else{
+ vec u(nbVARPOS);
+ vec X(nbVARPOS*2,fill::zeros);
+ X.subvec(0,nbVARPOS-1) = init_state.subvec(0,nbVARPOS-1); //startPoint.rows(0, tpgmm.getNumVARS()-1 );
+
+ for (int t=0; t < pred_length; t++){
+ rD.col(t) = X.rows(0,nbVARPOS-1);
+ u = lqr->getGains().at(t) * (Target.col(t)-X);
+ X += (lqr->getA()*X+lqr->getB()*u) * lqr->getdt();
+ }
+ }
+ return rD;
+}
+
+mat TPHSMM::TPHSMMController(std::vector<TaskParameter> cTaskParam, colvec init_state, uint pred_length){
+
+ //Take the product of Gaussians
+ ProdGauss = tpgmm->getTransformedGMM(cTaskParam, OTHER);
+ //printGMM(*ProdGauss);
+ setInitialState(init_state);
+ rData = HSMMLQR(*ProdGauss, getInitialState(), pred_length);
+ return rData;
+
+}
+
+mat TPHSMM::TPHSMMController(std::vector<TaskParameter> cTaskParam, uint pred_length){
+ //Take the product of Gaussians
+ ProdGauss = tpgmm->getTransformedGMM(cTaskParam, OTHER);
+ setInitialState(*ProdGauss);
+
+ rData = HSMMLQR(*ProdGauss, getInitialState(), pred_length);
+ return rData;
+}
+
+mat TPHSMM::TPHSMMController(){
+ cTaskParam = taskparam->getTaskParameters();
+
+ //Take the product of Gaussians
+ ProdGauss = tpgmm->getTransformedGMM(cTaskParam, OTHER);
+ setInitialState(*ProdGauss);
+
+ rData = HSMMLQR(*ProdGauss, getInitialState(), pred_length);
+ return rData;
+}
+
+void TPHSMM::setCurrentState(colvec state){
+ CurrentState = state;
+}
+
+void TPHSMM::setInitialState(colvec init_state){
+ InitialState = init_state;
+}
+
+void TPHSMM::setInitialState(GMM_Model& ProdGauss){
+ InitialState.zeros(tpgmm->getNumVARS(),1);
+ InitialState.subvec(0, tpgmm->getNumVARS() - 1) = ProdGauss.getMU(0);
+}
+
+colvec& TPHSMM::getCurrentState(){
+ return CurrentState;
+}
+
+colvec& TPHSMM::getInitialState(){
+ return InitialState;
+}
+
+
+TPGMM* TPHSMM::get_TPGMM(){
+ return this->tpgmm;
+}
+
+GMM_Model* TPHSMM::get_ProdGMM(std::vector<TaskParameter> cTaskParam){
+ ProdGauss = tpgmm->getTransformedGMM(cTaskParam, OTHER);
+ return ProdGauss;
+}
+
+LQR* TPHSMM::get_LQR(){
+ return this->lqr;
+}
+
+//uint TPHSMM::getNumSTATES(){
+// return nSTATES;
+//}
+
+void TPHSMM::printGMM(GMM_Model& GMM){
+
+ for (int i=0; i<tpgmm->getNumSTATES(); i++){
+ std::cout << "Gaussian Component id: " << i << std::endl;
+ std::cout << "Prior: " << GMM.getPRIORS(i) << std::endl;
+ printGaussian(GMM.getCOMPONENTS(i));
+ }
+}
+
+void TPHSMM::printGaussian(GaussianDistribution& GaussPr){
+
+ colvec MuTmp = GaussPr.getMU();
+ mat SigmaTmp = GaussPr.getSIGMA();
+
+ std::cout << "Mean: " << std::endl;
+ for (int i=0; i < tpgmm->getNumVARS(); i++){
+ std::cout << MuTmp[i] << ",\t";
+ }
+ std::cout << std::endl;
+
+ std::cout << "Covariance Matrix: " << std::endl;
+ for (int i=0; i < tpgmm->getNumVARS(); i++){
+ for (int j=0; j < tpgmm->getNumVARS(); j++){
+ std::cout << SigmaTmp.at(i,j) << ",\t";
+ }
+ std::cout << std::endl;
+ }
+}
+
+void TPHSMM::updateTransitionMat(vec _trParam)
+{
+ TransitionMatrix = zeros(nSTATES,nSTATES);
+ TransCount = zeros(nSTATES,nSTATES);
+
+ for(int i =0;i<nSTATES;i++) {
+ for(int j=0;j<nSTATES;j++){
+ mat param = ones(nTPS,1);
+ // evaluate transition probability using current parameters
+ TransCount(i,j) = TPTransitionMatrix[i][j].nTrans;
+ TransitionMatrix(i,j) = TPTransitionMatrix[i][j].nTrans *
+ (TPTransitionMatrix[i][j].transParam->getPDFValue(_trParam))(0);
+ }
+ double sumTmp = sum(TransitionMatrix.row(i));
+ //if (sumTmp>std::numeric_limits<double>::epsilon())
+ TransitionMatrix.row(i) = TransitionMatrix.row(i)/sumTmp;
+ }
+
+}
+
+void TPHSMM::addTransition(uint _prevSTATE, uint _newSTATE, vec _trParam,bool startFlag, uint _timeStepCNT)
+{
+ // ****** new state and new transition
+ if(_newSTATE>=nSTATES) {
+ std::vector<TPTRANSITION> outTransTmp;
+ for (int i = 0; i < nSTATES; i++)// for each existing states create a transition to the new
+ {
+ if (i == _prevSTATE&&!startFlag) { // create an existing transition with this state
+ TPTRANSITION transTmp(nTPS, _trParam, minSigma);
+ TPTransitionMatrix[i].push_back(transTmp);
+
+ }
+ else { // create an empty transition for the other
+ TPTRANSITION transTmp(nTPS);
+ TPTransitionMatrix[i].push_back(transTmp);
+ }
+ // create a transition from new state to all the other
+ TPTRANSITION transTmp(nTPS);
+ outTransTmp.push_back(transTmp);
+ }
+ // create empty self-transition for new state
+ TPTRANSITION transTmp(nTPS);
+ outTransTmp.push_back(transTmp);
+
+ // add transition going out of new state to transition matrix
+ TPTransitionMatrix.push_back(outTransTmp);
+
+ colvec MuDurTmp = zeros(1);
+ mat SigmaDurTmp = eye(1,1);
+ GaussianDistribution DurTmp(MuDurTmp,SigmaDurTmp);
+ DurationCOMPONENTS.push_back(DurTmp);
+ //nbStatesVisit.push_back(0);
+ rowvec hsmm_priors_ticksTmp = zeros(1,nSTATES+1);
+ if (nSTATES>1)
+ hsmm_priors_ticksTmp.cols(0,nSTATES-1) = hsmm_priors_ticks;
+
+ hsmm_priors_ticks = hsmm_priors_ticksTmp;
+
+ nSTATES++;
+
+
+ }
+ else if(!startFlag) {// update transition between existing states
+ TPTransitionMatrix[_prevSTATE][_newSTATE].addTransition(_trParam,minSigma);
+ }
+ if(!startFlag)
+ updateDuration(_prevSTATE,_timeStepCNT);
+
+ //for(int i =0;i<nSTATES;i++)
+ // cout<<nbStatesVisit[i];
+ //cout<<endl;
+
+}
+void TPHSMM::updateDuration(uint _idxSTATE, uint _timeStepCNT) {
+// nbStatesVisit[_idxSTATE]++;
+ hsmm_priors_ticks(_idxSTATE) = hsmm_priors_ticks(_idxSTATE) +1;
+// for(int i=0;i<nSTATES;i++)
+// cout<<nbStatesVisit[i];
+// cout<<endl;
+// cout<<hsmm_priors_ticks<<endl;
+ uint N = hsmm_priors_ticks(_idxSTATE);
+
+ colvec MuTmp = 1.0/N * ((N-1.0)*DurationCOMPONENTS[_idxSTATE].getMU() + _timeStepCNT);
+ mat SigmaTmp;
+ if(N>1){
+ SigmaTmp = 1.0/N * ((N-1.0)*DurationCOMPONENTS[_idxSTATE].getSIGMA() +
+ (double)(N/(N-1.0))*((_timeStepCNT-MuTmp)*(_timeStepCNT-MuTmp).t()));
+ }
+ else SigmaTmp = MuTmp(0)*eye(1,1);
+ DurationCOMPONENTS[_idxSTATE].setMU(MuTmp);
+ DurationCOMPONENTS[_idxSTATE].setSIGMA(SigmaTmp);
+}
+
+double TPHSMM::getTransSimilarity(uint _k, uint _l)
+{
+// uint nSTATES = TPGMM::getNumSTATES();
+ for (uint i=0;i<nSTATES;i++)
+ {
+ if(TPTransitionMatrix[_k][i].nTrans*TPTransitionMatrix[_l][i].nTrans!=0)
+ return 1.0;
+ }
+ return 0.0;
+}
+void TPHSMM::mergeTransition(uint _k, uint _l)
+{
+// uint nSTATES = TPGMM::getNumSTATES();
+ for (int i = 0; i < nSTATES; i++)
+ {
+ if(i==_k||i==_l)
+ continue;
+ // for each state merge TO deleted state
+ uint nTransTmp = TPTransitionMatrix[i][_l].nTrans + TPTransitionMatrix[i][_k].nTrans;
+ if(nTransTmp>0)
+ {
+ colvec MuTmp = 1.0/nTransTmp *
+ (TPTransitionMatrix[i][_l].nTrans *TPTransitionMatrix[i][_l].transParam->getMU() +
+ TPTransitionMatrix[i][_k].nTrans *TPTransitionMatrix[i][_k].transParam->getMU());
+
+ mat SigmaTmp = 1.0/nTransTmp *
+ (TPTransitionMatrix[i][_l].nTrans * (TPTransitionMatrix[i][_l].transParam->getSIGMA() +
+ (TPTransitionMatrix[i][_l].transParam->getMU()-MuTmp) *
+ (TPTransitionMatrix[i][_l].transParam->getMU()).t())+
+ TPTransitionMatrix[i][_k].nTrans * (TPTransitionMatrix[i][_k].transParam->getSIGMA() +
+ (TPTransitionMatrix[i][_k].transParam->getMU()-MuTmp) *
+ (TPTransitionMatrix[i][_k].transParam->getMU()).t()));
+
+ TPTransitionMatrix[i][_l].transParam->setMU(MuTmp);
+ TPTransitionMatrix[i][_l].transParam->setSIGMA(SigmaTmp);
+ TPTransitionMatrix[i][_l].nTrans = nTransTmp;
+ }
+ // for each state merge from deleted state
+ nTransTmp = TPTransitionMatrix[_l][i].nTrans + TPTransitionMatrix[_k][i].nTrans;
+
+ if(nTransTmp>0)
+ {
+ colvec MuTmp = 1.0 / nTransTmp *
+ (TPTransitionMatrix[_l][i].nTrans * TPTransitionMatrix[_l][i].transParam->getMU() +
+ TPTransitionMatrix[_k][i].nTrans * TPTransitionMatrix[_k][i].transParam->getMU());
+
+ mat SigmaTmp = 1.0 / nTransTmp *
+ (TPTransitionMatrix[_l][i].nTrans * (TPTransitionMatrix[_l][i].transParam->getSIGMA() +
+ (TPTransitionMatrix[_l][i].transParam->getMU() - MuTmp) *
+ (TPTransitionMatrix[_l][i].transParam->getMU()).t()) +
+ TPTransitionMatrix[_k][i].nTrans * (TPTransitionMatrix[_k][i].transParam->getSIGMA() +
+ (TPTransitionMatrix[_k][i].transParam->getMU() - MuTmp) *
+ (TPTransitionMatrix[_k][i].transParam->getMU()).t()));
+
+ TPTransitionMatrix[_l][i].transParam->setMU(MuTmp);
+ TPTransitionMatrix[_l][i].transParam->setSIGMA(SigmaTmp);
+ TPTransitionMatrix[_l][i].nTrans = nTransTmp;
+ }
+ }
+ for (int i = 0; i < nSTATES; i++)// for each state merge and remove transition TO deleted state
+ {
+ TPTransitionMatrix[i].erase(TPTransitionMatrix[i].begin()+_k);
+ }
+ //remove all transition FROM deleted state
+ //nbStatesVisit.erase(nbStatesVisit.begin()+_k);
+ hsmm_priors_ticks.shed_col(_k);
+ TPTransitionMatrix.erase(TPTransitionMatrix.begin()+_k);
+ nSTATES--;
+
+}
+
+void TPHSMM::saveInFiles(std::string path) {
+// uint nSTATES = TPGMM::getNumSTATES();
+
+ // save TPGMM
+ tpgmm->saveInFiles(path);
+
+ // save duration components
+ mat durMu(1, nSTATES);
+ mat durSigma(1, nSTATES);
+
+ for(uint i=0; i<nSTATES; i++){
+// priors(0,i) = getPRIORS()(i);
+ durMu(0,i) = getDurMU(i)(0);
+ durSigma(0,i) = getDurSIGMA(i)(0);
+// for(uint j=0; j<nVARS; j++){
+// mu(j,i) = getMU(i)(j);
+// for(uint k=0; k<nVARS; k++)
+// sigma(j,k + i*(nVARS)) = getSIGMA(i)(j,k);
+// }
+ }
+
+// priors.save(path + "priors.txt", raw_ascii);
+// mu.save(path + "mu.txt", raw_ascii);
+// sigma.save(path + "sigma.txt", raw_ascii);
+// transition.save(path + "transition.txt", raw_ascii);
+ durMu.save(path + "durMu.txt", raw_ascii);
+ durSigma.save(path + "durSigma.txt", raw_ascii);
+
+ //save transition matrix
+ TransitionMatrix.save(path + "transitionMatrix.txt", raw_ascii);
+
+ std::ofstream Varfile(path + "varnames.txt");
+ for (int i=0; i < tpgmm->getNumVARS(); i++){
+ Varfile << "x" << i << " ";
+ }
+ Varfile.close();
+ cout << "Saving in path: "<< path << endl;
+
+//
+// uint nVARS = tpgmm->getNumVARS();
+// mat priors(1, nSTATES);
+// mat mu(nVARS, nSTATES);
+// mat sigma(nVARS, nVARS*nSTATES);
+// mat transition(nSTATES, nSTATES);
+// mat durMu(1, nSTATES);
+// mat durSigma(1, nSTATES);
+//
+// for(uint i=0; i<nSTATES; i++){
+//// priors(0,i) = getPRIORS()(i);
+// durMu(0,i) = getDurMU(i)(0);
+// durSigma(0,i) = getDurSIGMA(i)(0);
+//// for(uint j=0; j<nVARS; j++){
+//// mu(j,i) = getMU(i)(j);
+//// for(uint k=0; k<nVARS; k++)
+//// sigma(j,k + i*(nVARS)) = getSIGMA(i)(j,k);
+//// }
+// }
+// transition = this->getTRANSITION();
+//
+// cout << "Saving in path: "<< path << endl;
+//// priors.save(path + "priors.txt", raw_ascii);
+//// mu.save(path + "mu.txt", raw_ascii);
+//// sigma.save(path + "sigma.txt", raw_ascii);
+//// transition.save(path + "transition.txt", raw_ascii);
+// durMu.save(path + "durMu.txt", raw_ascii);
+// durSigma.save(path + "durSigma.txt", raw_ascii);
+//
+//// hsmm_transition.save(path + "hsmm_transition.txt", raw_ascii);
+//// hsmm_transition_ticks.save(path + "hsmm_transition_ticks.txt", raw_ascii);
+//// hsmm_priors.save(path + "hsmm_priors.txt", raw_ascii);
+// hsmm_priors_ticks.save(path + "hsmm_priors_ticks.txt", raw_ascii);
+// saveTPTtransitionMatrix(path);
+}
+
+
+
+
+//void TPHSMM::saveInFiles(std::string path) {
+//// uint nSTATES = TPGMM::getNumSTATES();
+// uint nVARS = tpgmm->getNumVARS();
+// mat priors(1, nSTATES);
+// mat mu(nVARS, nSTATES);
+// mat sigma(nVARS, nVARS*nSTATES);
+// mat transition(nSTATES, nSTATES);
+// mat durMu(1, nSTATES);
+// mat durSigma(1, nSTATES);
+//
+// for(uint i=0; i<nSTATES; i++){
+//// priors(0,i) = getPRIORS()(i);
+// durMu(0,i) = getDurMU(i)(0);
+// durSigma(0,i) = getDurSIGMA(i)(0);
+//// for(uint j=0; j<nVARS; j++){
+//// mu(j,i) = getMU(i)(j);
+//// for(uint k=0; k<nVARS; k++)
+//// sigma(j,k + i*(nVARS)) = getSIGMA(i)(j,k);
+//// }
+// }
+// transition = this->getTRANSITION();
+//
+// cout << "Saving in path: "<< path << endl;
+//// priors.save(path + "priors.txt", raw_ascii);
+//// mu.save(path + "mu.txt", raw_ascii);
+//// sigma.save(path + "sigma.txt", raw_ascii);
+//// transition.save(path + "transition.txt", raw_ascii);
+// durMu.save(path + "durMu.txt", raw_ascii);
+// durSigma.save(path + "durSigma.txt", raw_ascii);
+//
+//// hsmm_transition.save(path + "hsmm_transition.txt", raw_ascii);
+//// hsmm_transition_ticks.save(path + "hsmm_transition_ticks.txt", raw_ascii);
+//// hsmm_priors.save(path + "hsmm_priors.txt", raw_ascii);
+// hsmm_priors_ticks.save(path + "hsmm_priors_ticks.txt", raw_ascii);
+// saveTPTtransitionMatrix(path);
+//}
+void TPHSMM::saveTPTtransitionMatrix(std::string path)
+{
+// uint nSTATES = TPGMM::getNumSTATES();
+ mat nTrans(nSTATES,nSTATES);
+ mat mu(nSTATES*nSTATES,nTPS);
+ cube sigma(nSTATES*nSTATES,nTPS,nTPS);
+
+ for(int i=0;i<nSTATES;i++) {
+ for(int j=0;j<nSTATES;j++) {
+ nTrans(i,j)= TPTransitionMatrix[i][j].nTrans;
+ for(int i_T=0;i_T<nTPS;i_T++) {
+ mu(j+i*nSTATES,i_T) = TPTransitionMatrix[i][j].transParam->getMU()(i_T);
+ for(int j_T=0;j_T<nTPS;j_T++) {
+ sigma(j+i*nSTATES,i_T,j_T) = TPTransitionMatrix[i][j].
+ transParam->getSIGMA()(i_T,j_T);
+ }
+ }
+ }
+ }
+ nTrans.save(path + "nTrans_tp_transition.txt", raw_ascii);
+ mu.save(path + "mu_tp_transition.txt", raw_ascii);
+ sigma.save(path + "sigma_tp_transition.txt", arma_binary);
+}
+
+}
diff --git a/src/tptrajMPC.cpp b/src/tptrajMPC.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3e1a43956b4c26da88db7d470fe452eb2692c249
--- /dev/null
+++ b/src/tptrajMPC.cpp
@@ -0,0 +1,126 @@
+/*
+ * tptrajMPC.cpp
+ *
+ * Created on: 7 Jan 2016
+ * Author: ihavoutis
+ */
+
+#include "pbdlib/tptrajMPC.h"
+
+namespace pbdlib {
+
+TpTrajMPC::~TpTrajMPC() {
+ // TODO Auto-generated destructor stub
+}
+
+colvec& TpTrajMPC::computeControlCommand(colvec& X) //X is cur pos
+{
+ // Construct the Su and Sx matrices
+ uint nbData = Np;
+ uint nbVar = getNumVARS();
+ uint nbVarPos = getNumVARSPos();
+
+ mat Su = zeros( nbVar*nbData, nbVarPos*(nbData-1) );
+ mat I(nbVar,nbVar,fill::eye);
+ mat Sx = kron(ones(nbData,1), I);
+
+ mat M = getInputDynamics();
+
+ for (uint n=1; n==nbData; n++) {
+// %Build Sx matrix
+ uvec id1 = linspace<uvec>(n*nbVar+1, nbData*nbVar);
+
+ Sx.rows(id1) = Sx.rows(id1) * getSystemDynamics();
+// Sx(id1,:) = Sx(id1,:) * A;
+// %Build Su matrix
+ id1 = linspace<uvec>(n*nbVar+1, (n+1)*nbVar);
+ uvec id2 = linspace<uvec>(1, n*nbVarPos);
+
+ Su.elem(id1,id2) = M;
+// Su(id1,id2) = M;
+// M = [A*M(:,0:model.nbVarPos), M];
+ M = join_horiz( getSystemDynamics() * M.cols(0,nbVarPos-1), M);
+}
+
+ uint nbFrames = tp->getNumTASKPARAMETERS();
+ //GMM projection, see Eq. (5)
+ vector<GMM_Model> gmm_in_frames;
+ for (uint m = 0; m < nbFrames; ++m) {
+ gmm_in_frames.push_back(tpdpgmm->getGMMS(m));
+ }
+
+ // Update hidden markov model
+ hsmm->stepForwardVariable(X, in);
+
+ // Create State Sequence:
+ // Alpha Predictions
+ hsmm->predictForwardVariable(AlphaPred);
+
+ // Calculate the largest alpha values
+ maxAlpha = max(AlphaPred,0);
+
+ // Find corresponding corresponding indices (i.e. argmax())
+ for (uint i = 0;i<myMPC->getNp();i++)
+ {
+ tmpAlphaCol = AlphaPred.col(i);
+ tmpMax = ones(hsmm->getNumSTATES(),1)*maxAlpha(i);
+ q(i)= accu(find(tmpAlphaCol==tmpMax, 1,"first"));
+ }
+
+//cout << q.t() << endl;
+
+ vector<mat> MuQ, SigmaQ;
+ //Build a reference trajectory for each frame, see Eq. (27)
+ mat invSigmaQ = zeros(nbVar*nbData, nbVar*nbData);
+ for (uint m=0; m<nbFrames; m++){
+ mat mtemp;
+ for (int j = 0; j < myMPC->getNp(); ++j) {
+ mtemp = join_vert(mtemp, gmm_in_frames[m].getMU(q(j)).col(0));
+ }
+ MuQ.push_back(mtemp);
+// MuQ.push_back( reshape(gmm_in_frames[m].getMU(0).cols(q),
+// nbVar*nbData, 1 ) );
+ mat ctemp;
+ for (int j = 0; j < myMPC->getNp(); ++j) {
+ ctemp = join_horiz(ctemp, gmm_in_frames[m].getSIGMA(q(j)));
+ }
+ mat tA,tB,tC;
+ tA = kron(ones(nbData,1),I.eye(nbVar,nbVar));
+ tB = ctemp;
+ tC = kron(I.eye(nbData,nbData), ones(nbVar,nbVar));
+ SigmaQ.push_back( (tA * tB) % tC );
+// SigmaQ.push_back( (kron(ones(nbData,1),I.eye(nbVar,nbVar)) * ctemp) %
+// kron(I.eye(nbData,nbData), ones(nbVar,nbVar)) );
+ invSigmaQ = invSigmaQ + inv(SigmaQ.back());
+ }
+
+// R = eye(nbVarPos*Np,nbVarPos*Np)*(alpha*alpha);
+
+// %Recompute R
+ R = I.eye(nbVarPos,nbVarPos) * alpha;
+ R = kron(eye(nbData-1,nbData-1),R);
+
+ //%Batch LQR (unconstrained linear MPC), see Eq. (37)
+ mat SuInvSigmaQ = Su.t() * invSigmaQ;
+ mat Rq = SuInvSigmaQ * Su + R;
+ //X = [s(1).Data0(:,1) + randn(model.nbVarPos,1)*0E0; zeros(model.nbVarPos,1)];
+ mat rq = zeros(nbVar*nbData,1);
+ for (uint m = 0; m<nbFrames; m++) {
+ rq = rq + solve(SigmaQ[m], MuQ[m] - Sx*X);
+ }
+ rq = Su.t() * rq;
+ U = solve(Rq, rq); //%can also be computed with u = lscov(Rq, rq);
+ U = U.rows(0,3);
+ return U;
+ //r(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData);
+
+ //
+// // Update update Reference Q and Muq:
+// updateReference(q);
+//
+// // Compute control command:
+// return myMPC->computeControlCommand(X,this->Muq,this->Q,this->R);
+// return TrajMPC::computeControlCommand(X);
+}
+
+}
diff --git a/src/trajMPC.cpp b/src/trajMPC.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a3cf561bba3b7b0a7f2f12a89591cd5d45e9f857
--- /dev/null
+++ b/src/trajMPC.cpp
@@ -0,0 +1,270 @@
+// Martijn Zeestraten, June 2015
+//
+#include "pbdlib/trajMPC.h"
+
+namespace pbdlib
+{
+
+TrajMPC::TrajMPC(HSMM* _hsmm, double _dt, uint _nbVarPos, uint _nbDeriv, uint _Np, uint _Nc, double _alpha)
+{
+
+ nbVarPos = _nbVarPos; // Assume fully actuated system
+ nbDeriv = _nbDeriv;
+ nbVar = _nbVarPos*_nbDeriv; // Total number of variables
+
+ // Check HSMM size and copy pointer:
+ assignHSMM(_hsmm,_nbVarPos,_nbDeriv);
+
+ // Invert all covariance matrices:
+ createPrecisionMatrices( _hsmm);
+
+ // Create system matrices:
+ createSystemMatrices(_dt, _nbVarPos, _nbDeriv);
+
+ // Create MPC object
+ myMPC = new MPC(Ad,Bd,C,_Np,_Nc);
+
+ // Initialize R and Q matrices for appropriate size
+ allocateReference(nbVar, nbVarPos, _Np, _Nc);
+
+ // Update Cost Matrix
+ updateCostMatrix(_alpha);
+
+ // Create vector that selects the variables used for observation probability.
+ // We assume that we only want to condition on the position information
+ in.set_size(nbVarPos);
+ for (uint i=0;i<nbVarPos;i++)
+ in(i) = i;
+
+ // Allocate memory for state sequence generation
+ maxAlpha.set_size(1,_Np);
+ tmpMax.set_size(_hsmm->getNumVARS(),1);
+ tmpAlphaCol.set_size(_hsmm->getNumVARS(),1);
+
+ AlphaPred.set_size(_hsmm->getNumSTATES(),_Np);
+
+
+}
+
+TrajMPC::TrajMPC(HSMM* _hsmm,mat& _Ad, mat& _Bd, mat& _C, uint _Np, uint _Nc, double _alpha)
+{
+ // Get variable sizes:
+ nbVarPos = _Bd.n_cols; // Assume fully actuated system
+ nbVar = _Ad.n_rows; // Total number of variables
+ nbDeriv = nbVar/nbVarPos; //
+
+ // Check hsmm vars
+ assignHSMM(_hsmm,nbVarPos, nbDeriv);
+
+ // Invert all covariance matrices:
+ createPrecisionMatrices( _hsmm);
+
+ // Create MPC object
+ myMPC = new MPC(_Ad,_Bd,_C,_Np,_Nc);
+
+ // Initialize R and Q matrices for appropriate size
+ allocateReference(nbVar, nbVarPos, _Np, _Nc);
+
+ // Update Cost Matrix
+ updateCostMatrix(_alpha);
+
+ // Create vector that selects the variables used for observation probability.
+ // We assume that we only want to condition on the position information
+ in.set_size(nbVarPos);
+ for (uint i=0;i<nbVarPos;i++)
+ in(i) = i;
+
+ // Allocate memory for state sequence generation
+ maxAlpha.set_size(1,_Np);
+ tmpMax.set_size(_hsmm->getNumVARS(),1);
+ tmpAlphaCol.set_size(_hsmm->getNumVARS(),1);
+
+ AlphaPred.set_size(_hsmm->getNumSTATES(),_Np);
+}
+
+void TrajMPC::setMu(int i, const colvec& m){
+ hsmm->setMU(i,m);
+}
+void TrajMPC::setSigma(int i, const mat& sigma){
+ hsmm->setSIGMA(i,sigma);
+}
+
+colvec TrajMPC::getCurrentAttractor()
+{
+
+ // Make prediction of first most likeli state:
+ mat AlphaPred = hsmm->predictForwardVariable(1);
+
+ // Find corresponding corresponding indices (i.e. argmax())
+ maxAlpha = max(AlphaPred,0);
+ tmpAlphaCol = AlphaPred;
+ tmpMax = ones(hsmm->getNumSTATES(),1)*maxAlpha;
+ uint qAt = accu(find(tmpAlphaCol==tmpMax, 1,"first"));
+
+ // Return attractor of most likeli state:
+ return hsmm->getMU(qAt);
+
+}
+
+void TrajMPC::createPrecisionMatrices(HSMM* _hsmm)
+{
+ // Get Relevant variables:
+ uint _nbStates = _hsmm->getNumSTATES();
+ uint _nbVar = _hsmm->getNumVARS();
+
+ // Resize Lambda
+ Lambda.set_size(_nbVar,_nbStates*_nbVar);
+
+ // Copy Data
+ for (uint i = 0;i<_nbStates;i++)
+ Lambda.cols(i*_nbVar,_nbVar*(i+1)-1) = _hsmm->getSIGMA(i).i();
+
+}
+
+void TrajMPC::assignHSMM(HSMM* _hsmm, uint _nbVarPos, uint _nbDeriv)
+{
+ if (_hsmm->getNumVARS() != _nbVarPos*_nbDeriv)
+ {
+ cout << "The number of varialbles in the HSMM ("<< _hsmm->getNumVARS()
+ << ") does not correspond with the specified number of position variables and derivatives ("
+ << _nbVarPos << "*" << _nbDeriv << " = " << _nbVarPos*_nbDeriv<<".";
+ throw std::invalid_argument("Invalid Parameters");
+ }
+ else
+ {
+ hsmm = _hsmm;
+ }
+
+}
+
+void TrajMPC::allocateReference(uint _nbVar, uint _nbVarPos, uint _Np, uint _Nc)
+{
+ R = zeros(_nbVarPos*_Nc,_nbVarPos*_Nc);
+ Q = zeros(_nbVar*_Np, _nbVar*_Np);
+ Muq = zeros(_nbVar*_Np);
+ // Allocate space for q
+ q = uvec(_Np);
+
+ // allocate space for Mean and Covariance matrices
+ U = zeros(_nbVarPos*_Nc,1);
+ SigmaU = zeros(_nbVarPos*_Nc, _nbVarPos*_Nc);
+ muX = zeros(_nbVar*_Np,1);
+ SigmaX = zeros(_nbVar*_Np, _nbVarPos*_Np);
+
+
+}
+
+void TrajMPC::createSystemMatrices(double _dt, uint _nbVarPos, uint _nbDeriv)
+{
+ // Create System Matrices
+ // Assistive Matrices for System construction:
+ uint _nbVar = _nbVarPos*_nbDeriv;
+ mat IvarPos, Idisc;
+ IvarPos = eye(_nbVarPos,_nbVarPos);
+ Idisc = eye(_nbVar,_nbVar);
+
+ // A matrix:
+ A = zeros(_nbDeriv,_nbDeriv);
+ A.submat(0,1,_nbDeriv-2,_nbDeriv-1) = eye(_nbDeriv-1,_nbDeriv-1);
+ A = kron(A,IvarPos);
+
+ // B Matrix:
+ B = zeros(_nbDeriv,1);
+ B(_nbDeriv-1,0)=1;
+ B = kron(B,IvarPos);
+
+ // C Matrix:
+ C = zeros(1,_nbDeriv);
+ C(0,0) = 1;
+ C = kron(C,IvarPos);
+
+ // Discretize system matrices (Euler discretization):
+ Ad = A*_dt+Idisc;
+ Bd = B*_dt;
+
+
+// cout <<"Ad: "<< endl << Ad << endl;
+// cout <<"Bd: "<< endl << Bd << endl;
+
+}
+
+colvec& TrajMPC::computeControlCommand(colvec& X)
+{
+ // Update hidden markov model
+ hsmm->stepForwardVariable(X, in);
+
+ // Create State Sequence:
+ // Alpha Predictions
+ hsmm->predictForwardVariable(AlphaPred);
+
+ // Calculate the largest alpha values
+ maxAlpha = max(AlphaPred,0);
+
+ // Find corresponding corresponding indices (i.e. argmax())
+ for (uint i = 0;i<myMPC->getNp();i++)
+ {
+ tmpAlphaCol = AlphaPred.col(i);
+ tmpMax = ones(hsmm->getNumSTATES(),1)*maxAlpha(i);
+ q(i)= accu(find(tmpAlphaCol==tmpMax, 1,"first"));
+ }
+
+ // Update update Reference Q and Muq:
+ updateReference(q);
+
+ // Compute control command:
+ return myMPC->computeControlCommand(X,this->Muq,this->Q,this->R);
+
+}
+
+TrajDist& TrajMPC::computeTrajDist(colvec& X,bool stepHSMM=true)
+{
+ // Update hidden markov model
+ if (stepHSMM==true)
+ hsmm->stepForwardVariable(X, in);
+
+ // Create State Sequence:
+ // Alpha Predictions
+ hsmm->predictForwardVariable(AlphaPred);
+
+ // Calculate the largest alpha values
+ maxAlpha = max(AlphaPred,0);
+
+ // Find corresponding corresponding indices (i.e. argmax())
+ for (uint i = 0;i<myMPC->getNp();i++)
+ {
+ tmpAlphaCol = AlphaPred.col(i);
+ tmpMax = ones(hsmm->getNumSTATES(),1)*maxAlpha(i);
+ q(i)= accu(find(tmpAlphaCol==tmpMax, 1,"first"));
+ }
+
+ // Update update Reference Q and Muq:
+ updateReference(q);
+
+ // Compute control command distribution
+ return myMPC->computeTrajDist(X,this->Muq,this->Q,this->R);
+}
+
+void TrajMPC::updateReference(uvec& _q)
+{
+ for (uint i = 0; i<_q.n_rows;i++)
+ {
+ // Construct Precision matrix based on state sequence
+ Q.submat(i*nbVar,
+ i*nbVar,
+ (i+1)*nbVar-1,
+ (i+1)*nbVar-1) =
+ Lambda.cols(_q(i)*nbVar,(_q(i)+1)*nbVar-1);
+
+ // Construct Mean vector
+ Muq.rows(i*nbVar,(i+1)*nbVar-1) =hsmm->getMU(_q(i));
+ }
+
+}
+
+void TrajMPC::updateCostMatrix(double _alpha)
+{
+ // Construct Cost Matrix
+ R = eye(nbVarPos*myMPC->getNc(),nbVarPos*myMPC->getNc())*(_alpha*_alpha);
+}
+
+} // end namespace
diff --git a/src/trajdist.cpp b/src/trajdist.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7659f559f85b8b487bcc3da595ad53c7a22d51b7
--- /dev/null
+++ b/src/trajdist.cpp
@@ -0,0 +1,77 @@
+#include "pbdlib/trajdist.h"
+
+
+namespace pbdlib
+{
+
+TrajDist::TrajDist(const colvec& _Mu, const mat& _Sigma, const uint& _nbVars)
+{
+ Mu = _Mu;
+ Sigma = _Sigma;
+ nbVars = _nbVars;
+}
+
+TrajDist TrajDist::condition(vector<ConditionPoint> points)
+{
+ // Todo Implement
+ cout << "Function not yet Implemented" << endl;
+
+ return *this;
+}
+
+TrajDist TrajDist::lintrans(const mat& A, const colvec& b)
+{
+ // Perform linear transformation on trajectory distribution
+ this->nbData = this->Mu.n_elem/this->nbVars;
+
+ // Repeat A matrix and b vector overr all data points of distribution
+ mat Ahat = kron(eye(this->nbData,this->nbData),A);
+ colvec bhat = kron(ones(this->nbData,1),b);
+
+ // Perform linear transformation
+ mat Sigma_ = Ahat*this->Sigma*Ahat.t();
+ colvec Mu_ = Ahat*this->Mu + bhat;
+
+ // Return new distribution
+ return TrajDist(Mu_, Sigma_, this->nbVars);
+}
+
+TrajDist& TrajDist::operator=(const TrajDist& rhs)
+{
+ // Check for self assignment:
+ if (this != &rhs)
+ {
+ // Copy conents:
+ this->Mu = rhs.Mu;
+ this->Sigma = rhs.Sigma;
+ this->nbVars = rhs.nbVars;
+ }
+
+ return *this;
+}
+
+TrajDist& TrajDist::operator*=(const TrajDist& rhs)
+{
+ // Compute precision matrices:
+ mat reg = eye(rhs.Sigma.n_rows,rhs.Sigma.n_cols)*1e-10;
+ mat iSrhs =inv(rhs.Sigma+reg);
+ mat iSlhs =inv(this->Sigma+reg);
+
+ // Compute new Covariance:
+ this->Sigma = inv(iSlhs + iSlhs);
+
+ // Compute new mean
+ this->Mu = this->Sigma*(iSlhs*this->Mu + iSrhs*rhs.Mu);
+
+ return *this;
+}
+
+TrajDist TrajDist::operator*(const TrajDist& rhs)
+{
+ // Create new object based on the current one
+ TrajDist result = *this;
+ result *=rhs;
+ return result;
+}
+
+} // Namespace
diff --git a/src/trajgmm.cpp b/src/trajgmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b505db779c1d95ccd59c0dee99a431d521c3e986
--- /dev/null
+++ b/src/trajgmm.cpp
@@ -0,0 +1,303 @@
+/**
+Copyright (C) 2015, Leonel Rozo, Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
+
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+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 Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "pbdlib/trajgmm.h"
+
+namespace pbdlib
+{
+TrajGMM::TrajGMM(std::vector<Demonstration> &demos, uint _nSTATES,
+ uint _nVARPOS, uint _nDERIV, double _dt):GMM_Model(demos, _nSTATES)
+{
+ // Initializing Trajectory GMM variables
+ this->nVARPOS = _nVARPOS;
+ this->nDERIV = _nDERIV;
+ this->dt = _dt;
+}
+
+TrajGMM::TrajGMM(uint _nSTATES, uint _nVARPOS, uint _nDERIV, double _dt)
+ :GMM_Model(_nSTATES, _nVARPOS*_nDERIV)
+{
+ // Initializing Trajectory GMM variables
+ this->nVARPOS = _nVARPOS;
+ this->nDERIV = _nDERIV;
+ this->dt = _dt;
+}
+
+TrajGMM::TrajGMM(const std::string &priors_path,
+ const std::string &mu_path, const std::string &sigma_path,
+ const std::string &vars_path, const std::string &modelInfo_path)
+ :GMM_Model(priors_path, mu_path, sigma_path, vars_path)
+{
+ // Setting Trajectory GMM variables
+ mat modelInfo;
+ modelInfo.load(modelInfo_path);
+ this->nVARPOS = modelInfo(0);
+ this->nDERIV = modelInfo(1);
+ this->dt = modelInfo(2);
+}
+
+TrajGMM::TrajGMM(const std::string &priors_path,
+ const std::string &mu_path, const std::string &sigma_path, const std::string &modelInfo_path)
+ :GMM_Model(priors_path, mu_path, sigma_path)
+{
+ // Setting Trajectory GMM variables
+ mat modelInfo;
+ modelInfo.load(modelInfo_path);
+ this->nVARPOS = modelInfo(0);
+ this->nDERIV = modelInfo(1);
+ this->dt = modelInfo(2);
+}
+
+void TrajGMM::init_TrajGMM_timeBased(double _regTerm)
+{
+ // Auxiliar variables
+ mat _tmpData;
+ rowvec _tmpPriors;
+ uvec _idTmp;
+
+ // Storing 1st demonstration in matrix format
+ mat _Data = DEMONSTRATIONS[0].getDatapoints().getData();
+ cout << "Size(data) = " << _Data.n_rows << "," << _Data.n_cols << endl;
+ // Time dimension
+ rowvec _timeDim = linspace<rowvec>(1, _Data.n_cols, _Data.n_cols);
+ cout << "Size(_timeDim) = " << _timeDim.n_rows << "," << _timeDim.n_cols << endl;
+ // Adding time dimension to first demo
+ _Data.insert_rows(0,_timeDim);
+ cout << "Size(data) = " << _Data.n_rows << "," << _Data.n_cols << endl;
+
+ // Storing remaining demos
+ for(int i=1; i<DEMONSTRATIONS.size(); i++)
+ {
+ _tmpData = DEMONSTRATIONS[i].getDatapoints().getData(); // Temporary storing
+ _tmpData.insert_rows(0,_timeDim); // Adding time dimension
+ _Data = join_rows(_Data,_tmpData); // Adding demo
+ cout << "Size(data) = " << _Data.n_rows << "," << _Data.n_cols << endl;
+ }
+
+ // Linear Space Gaussian in time:
+ mat _TimingSep = linspace(min(_Data.row(0)), max(_Data.row(0)),this->nSTATES+1).t();
+
+ // Setting Mu, Sigma and Priors
+ _tmpPriors = zeros(1,nSTATES);
+ for (uint i=0; i<nSTATES; i++)
+ {
+ // Find All indices of data that
+ _idTmp = find(_Data.row(0) >= _TimingSep(i));
+ _tmpData = _Data.cols(_idTmp);
+ _idTmp = find(_tmpData.row(0) <= _TimingSep(i+1));
+
+ // Calculate Properties of GMM;
+ this->setMU(i,mean(_tmpData.cols(_idTmp),1));
+ this->setSIGMA(i, cov(_tmpData.cols(_idTmp).t()) + eye(this->nVARS, this->nVARS)*_regTerm);
+ _tmpPriors(i) = _idTmp.n_elem;
+ }
+ this->setPRIORS(_tmpPriors/sum(_tmpPriors));
+}
+
+uint TrajGMM::EM_learn(double _regTerm)
+{
+ // Setting new (temporary) values for nVARS
+ this->nVARS += 1;
+ init_TrajGMM_timeBased(_regTerm);
+
+ // Setting back nVARS value
+ this->nVARS -= 1;
+
+ // Removing time dimension from model parameters
+ for(uint i = 0 ; i < this->nSTATES ; i++)
+ {
+ this->setMU(i, this->getMU(i).rows(1, this->nVARS));
+ this->setSIGMA(i, this->getSIGMA(i).submat(1,1,this->nVARS, this->nVARS));
+ }
+
+ // EM learning
+ return EM(std::numeric_limits<double>::min(),_regTerm);
+}
+
+void TrajGMM::constructPHI(uint _nData, uint _nSamples)
+{
+ // Initializing and setting a one dimensional operator for one datapoint
+ mat oneDimOperator = zeros<mat>(this->nDERIV,this->nDERIV);
+ oneDimOperator(0, oneDimOperator.n_cols-1) = 1;
+
+ for(uint i = 1 ; i < this->nDERIV ; i++)
+ oneDimOperator.row(i) = (oneDimOperator.row(i-1) - circshift(arma::conv_to<mat>::from(oneDimOperator.row(i-1)), 0, -1)) / this->dt;
+
+ // Initializing and setting a one dimensional operator for all the datapoints of a dataset (e.g., demonstrations)
+ sp_mat bigOperator = zeros<sp_mat>(_nData*this->nDERIV, _nData);
+ sp_mat PHI0 = zeros<sp_mat>(_nData*this->nDERIV, _nData);
+ bigOperator.submat((this->nDERIV-1)*this->nDERIV, 0, this->nDERIV*this->nDERIV-1, this->nDERIV-1) = oneDimOperator;
+
+ // Setting matrix PHI for one dataset
+ for(int t = 0 ; t <= _nData-this->nDERIV ; t++)
+ PHI0 = PHI0 + circshift(bigOperator, this->nDERIV*t, t);
+
+ // Handling of borders
+ for(int i = 1 ; i < this->nDERIV ; i++)
+ {
+ bigOperator.row(this->nDERIV*this->nDERIV-i).zeros();
+ bigOperator.col(i-1).zeros();
+ PHI0 = PHI0 + circshift(bigOperator, -i*this->nDERIV, -i);
+ }
+
+ // Application to multiple dimensions and multiple datasets (e.g., demonstrations)
+ this->PHI1 = conv_to<sp_mat>::from( kron(conv_to<mat>::from(PHI0), arma::eye(this->nVARPOS, this->nVARPOS)) );
+ this->PHI = conv_to<sp_mat>::from( kron(arma::eye(_nSamples, _nSamples), conv_to<mat>::from(this->PHI1)) );
+}
+
+template<class armaType>
+armaType TrajGMM::circshift(const armaType& _matrix, int _shX, int _shY)
+{
+ int iOutputInd, iInputInd, ii, jj;
+ int ydim = _matrix.n_cols;
+ int xdim = _matrix.n_rows;
+ armaType outMat = _matrix;
+
+ for (int j = 0 ; j < ydim ; j++)
+ {
+ // Setting index for columns shift (vertical shifting in the input matrix)
+ jj = (j + _shY) % ydim; // Here operator % is computing the modulo
+
+ if (jj <0)
+ jj = jj + ydim;
+
+ for(int i = 0; i < xdim ; i++)
+ {
+ // Setting index for rows shift (horizontal shifting in the input matrix)
+ ii = (i + _shX) % xdim; // Here operator % is computing the modulo
+
+ if (ii <0)
+ ii = ii + xdim;
+
+ // Shifting
+ outMat[(jj * xdim) + ii] = _matrix[(j * xdim) + i];
+ }
+ }
+
+ return outMat;
+}
+
+colvec TrajGMM::getMuQ(vec _q)
+{
+ colvec muQ = zeros<colvec>(this->nVARS*_q.n_elem);
+
+ for(uint i = 0 ; i < _q.n_elem ; i++)
+ muQ.rows(i*this->nVARS, (i+1)*this->nVARS-1) = this->getMU(_q(i));
+
+ return muQ;
+}
+
+mat TrajGMM::getSigmaQ(vec _q)
+{
+ mat sigmaQ = zeros<mat>(this->nVARS*_q.n_elem, this->nVARS*_q.n_elem);
+ uint ind1, ind2;
+
+ for(uint i = 0 ; i < _q.n_elem ; i++)
+ {
+ ind1 = i*this->nVARS;
+ ind2 = (i+1)*this->nVARS-1;
+ sigmaQ.submat(ind1, ind1, ind2, ind2) = this->getSIGMA(_q(i));
+ }
+ return sigmaQ;
+}
+
+mat TrajGMM::leastSquaresOptimalData(colvec _q)
+{
+ mat optimalData;
+
+ // Get big vector Mu and big matrix Sigma
+ colvec muQ = getMuQ(_q);
+ mat sigmaQ = getSigmaQ(_q);
+
+ // Least squares computation using a readable code (optimized using sp_mat type and spsolve() function for solving the WLS problem)
+ if(this->PHI1.is_empty())
+ this->constructPHI(_q.n_elem,1);
+
+ sp_mat PHI1invSigmaQ = this->PHI1.t() * conv_to<sp_mat>::from(sigmaQ.i()); // PHI1^\trsp * Sigma_Q
+ sp_mat Rq = PHI1invSigmaQ * this->PHI1; // (PHI1^\trsp * Sigma_Q)^-1 * PHI1
+ mat rq = PHI1invSigmaQ * muQ; // (PHI1^\trsp * Sigma_Q)^-1 * Mu_Q
+ mat zeta = spsolve(Rq,rq,"lapack"); // Solving least squares problem
+ optimalData = reshape(zeta, this->nVARPOS,_q.n_elem);
+
+ // Least squares computation using Cholesky and QR decompositions
+ // -> THE CODE BELOW WORKS BUT IT IS NOT OPTIMIZED FOR HANDLING SPARSE MATRICES AND THEREFORE IT IS MUCH SLOWER THAN THE PIECE
+ // OF CODE ABOVE.
+ /*
+ mat T = chol(sigmaQ).t();
+ mat Tphi = T.i() * this->PHI1;
+ mat Tmu = T.i() * muQ;
+ mat Q, R;
+ qr_econ(Q, R, Tphi);
+ mat z = Q.t() * Tmu;
+ mat zeta1 = R.i() * z;
+ optimalData = reshape(zeta1, this->nVARPOS, _q.n_elem);
+ */
+
+ return optimalData;
+}
+
+std::vector<GaussianDistribution> TrajGMM::leastSquaresOptimalProb(colvec _q)
+{
+ std::vector<GaussianDistribution> outputProb;
+
+ // Computing optimal path
+ mat optimalData;
+
+ // Get big vector Mu and big matrix Sigma
+ colvec muQ = getMuQ(_q);
+ mat sigmaQ = getSigmaQ(_q);
+
+ // Least squares computation using a readable code (optimized using sp_mat type and spsolve() function for solving the WLS problem)
+ if(this->PHI1.is_empty())
+ this->constructPHI(_q.n_elem,1);
+
+ sp_mat PHI1invSigmaQ = this->PHI1.t() * conv_to<sp_mat>::from(sigmaQ.i());
+ sp_mat Rq = PHI1invSigmaQ * this->PHI1;
+ mat rq = PHI1invSigmaQ * muQ;
+ mat zeta = spsolve(Rq,rq,"lapack");
+ optimalData = reshape(zeta, this->nVARPOS,_q.n_elem);
+
+ // Covariance Matrix computation of ordinary least squares estimate
+ mat mse = (muQ.t()*sigmaQ.i()*muQ - rq.t()*inv(conv_to<mat>::from(Rq))*rq) / ((this->nVARS-this->nVARPOS)*_q.n_elem);
+ mat S = inv(conv_to<mat>::from(Rq)) * conv_to<double>::from(mse);
+
+ // Saving optimal data and corresponding covariance matrix in a vector of Gaussian distributions
+ uint id1, id2;
+ colvec _tmpMu;
+ mat _tmpSigma;
+ for(uint t = 0 ; t < _q.n_elem ; t++)
+ {
+ _tmpMu = optimalData.col(t);
+ id1 = t*this->nVARPOS;
+ id2 = (t+1)*this->nVARPOS-1;
+ _tmpSigma = S.submat(id1,id1,id2,id2) * _q.n_elem;
+
+ // Saving current Gaussian distribution
+ outputProb.push_back(GaussianDistribution(_tmpMu, _tmpSigma));
+ }
+
+ return outputProb;
+}
+} //end of pbdlib namespace
+
+
+
+