diff --git a/clik/.cache/clangd/index/clik.cpp.F1A65FD4C293F4CF.idx b/clik/.cache/clangd/index/clik.cpp.F1A65FD4C293F4CF.idx
index 73d3d3fa03db6f7f065ccacb9963572a00102dcb..2da35dc33eead9685bd9d905e909500ce8e5bf4f 100644
Binary files a/clik/.cache/clangd/index/clik.cpp.F1A65FD4C293F4CF.idx and b/clik/.cache/clangd/index/clik.cpp.F1A65FD4C293F4CF.idx differ
diff --git a/clik/build/CMakeCache.txt b/clik/build/CMakeCache.txt
index 5883d0d0dfbd9fee87273d9846fd65890eb90ad5..bc5843c7d0e0770471a11cf0ddc45c73b91ed7b8 100644
--- a/clik/build/CMakeCache.txt
+++ b/clik/build/CMakeCache.txt
@@ -1,5 +1,5 @@
 # This is the CMakeCache file.
-# For build in directory: /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build
+# For build in directory: /home/gospodar/lund/praxis/ur_simple_control/clik/build
 # It was generated by CMake: /home/gospodar/.local/lib/python3.10/site-packages/cmake/data/bin/cmake
 # You can edit this file to change values found and used by cmake.
 # If you do not want to change any of the values, simply exit the editor.
@@ -138,7 +138,7 @@ CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO:STRING=
 CMAKE_EXPORT_COMPILE_COMMANDS:BOOL=
 
 //Value Computed by CMake.
-CMAKE_FIND_PACKAGE_REDIRECTS_DIR:STATIC=/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/pkgRedirects
+CMAKE_FIND_PACKAGE_REDIRECTS_DIR:STATIC=/home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/pkgRedirects
 
 //Install path prefix, prepended onto install directories.
 CMAKE_INSTALL_PREFIX:PATH=/usr/local
@@ -274,13 +274,13 @@ boost_system_DIR:PATH=/usr/lib/x86_64-linux-gnu/cmake/boost_system-1.74.0
 boost_thread_DIR:PATH=/usr/lib/x86_64-linux-gnu/cmake/boost_thread-1.74.0
 
 //Value Computed by CMake
-clik_BINARY_DIR:STATIC=/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build
+clik_BINARY_DIR:STATIC=/home/gospodar/lund/praxis/ur_simple_control/clik/build
 
 //Value Computed by CMake
 clik_IS_TOP_LEVEL:STATIC=ON
 
 //Value Computed by CMake
-clik_SOURCE_DIR:STATIC=/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik
+clik_SOURCE_DIR:STATIC=/home/gospodar/lund/praxis/ur_simple_control/clik
 
 //The directory containing a CMake configuration file for console_bridge.
 console_bridge_DIR:PATH=/usr/lib/x86_64-linux-gnu/console_bridge/cmake
@@ -343,7 +343,7 @@ CMAKE_ADDR2LINE-ADVANCED:INTERNAL=1
 //ADVANCED property for variable: CMAKE_AR
 CMAKE_AR-ADVANCED:INTERNAL=1
 //This is the directory where this CMakeCache.txt was created
-CMAKE_CACHEFILE_DIR:INTERNAL=/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build
+CMAKE_CACHEFILE_DIR:INTERNAL=/home/gospodar/lund/praxis/ur_simple_control/clik/build
 //Major version of cmake used to create the current loaded cache
 CMAKE_CACHE_MAJOR_VERSION:INTERNAL=3
 //Minor version of cmake used to create the current loaded cache
diff --git a/clik/build/CMakeFiles/CMakeConfigureLog.yaml b/clik/build/CMakeFiles/CMakeConfigureLog.yaml
index a67f15f79fbb3fe557ac0791050699c165cf9bf4..9985f1dd8286bc9f9a7173f69a034d3bd6d66e81 100644
--- a/clik/build/CMakeFiles/CMakeConfigureLog.yaml
+++ b/clik/build/CMakeFiles/CMakeConfigureLog.yaml
@@ -1757,3 +1757,45 @@ events:
         201402
       exitCode: 0
 ...
+
+---
+events:
+  -
+    kind: "try_run-v1"
+    backtrace:
+      - "/opt/openrobots/lib/cmake/hpp-fcl/cxx-standard.cmake:49 (try_run)"
+      - "/opt/openrobots/lib/cmake/hpp-fcl/hpp-fclConfig.cmake:186 (CHECK_MINIMAL_CXX_STANDARD)"
+      - "/home/gospodar/.local/lib/python3.10/site-packages/cmake/data/share/cmake-3.27/Modules/CMakeFindDependencyMacro.cmake:76 (find_package)"
+      - "/opt/openrobots/lib/cmake/pinocchio/pinocchioConfig.cmake:159 (find_dependency)"
+      - "CMakeLists.txt:47 (find_package)"
+    directories:
+      source: "/home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/CMakeTmp"
+      binary: "/home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/CMakeTmp"
+    cmakeVariables:
+      CMAKE_CXX_FLAGS: "     -D_XOPEN_SOURCE=700     -D_FORTIFY_SOURCE=2     -O2     -fstack-protector     -fomit-frame-pointer     -pipe     -pedantic -pedantic-errors     -Werror     -Weffc++     -Wall -Wextra -Wshadow -Wdeprecated     -Wdiv-by-zero -Wfloat-equal -Wfloat-conversion -Wsign-compare -Wpointer-arith     -Wuninitialized -Wunreachable-code     -Wunused -Wunused-function -Wunused-label -Wunused-parameter -Wunused-but-set-parameter -Wunused-but-set-variable     -Wunused-value -Wunused-variable -Wunused-result     -Wmissing-field-initializers -Wmissing-format-attribute -Wmissing-include-dirs -Wmissing-noreturn"
+      CMAKE_CXX_FLAGS_DEBUG: "-g"
+      CMAKE_EXE_LINKER_FLAGS: ""
+    buildResult:
+      variable: "_cxx_standard_build_status"
+      cached: true
+      stdout: |
+        Change Dir: '/home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/CMakeTmp'
+        
+        Run Build Command(s): /home/gospodar/.local/lib/python3.10/site-packages/cmake/data/bin/cmake -E env VERBOSE=1 /usr/bin/gmake -f Makefile cmTC_95b27/fast
+        /usr/bin/gmake  -f CMakeFiles/cmTC_95b27.dir/build.make CMakeFiles/cmTC_95b27.dir/build
+        gmake[1]: Entering directory '/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/CMakeTmp'
+        Building CXX object CMakeFiles/cmTC_95b27.dir/tmp-cxx-standard.cpp.o
+        /usr/bin/c++   -D_XOPEN_SOURCE=700     -D_FORTIFY_SOURCE=2     -O2     -fstack-protector     -fomit-frame-pointer     -pipe     -pedantic -pedantic-errors     -Werror     -Weffc++     -Wall -Wextra -Wshadow -Wdeprecated     -Wdiv-by-zero -Wfloat-equal -Wfloat-conversion -Wsign-compare -Wpointer-arith     -Wuninitialized -Wunreachable-code     -Wunused -Wunused-function -Wunused-label -Wunused-parameter -Wunused-but-set-parameter -Wunused-but-set-variable     -Wunused-value -Wunused-variable -Wunused-result     -Wmissing-field-initializers -Wmissing-format-attribute -Wmissing-include-dirs -Wmissing-noreturn  -std=gnu++14 -o CMakeFiles/cmTC_95b27.dir/tmp-cxx-standard.cpp.o -c /home/gospodar/lund/praxis/ur_simple_control/clik/build/cmake/tmp-cxx-standard.cpp
+        Linking CXX executable cmTC_95b27
+        /home/gospodar/.local/lib/python3.10/site-packages/cmake/data/bin/cmake -E cmake_link_script CMakeFiles/cmTC_95b27.dir/link.txt --verbose=1
+        /usr/bin/c++      -D_XOPEN_SOURCE=700     -D_FORTIFY_SOURCE=2     -O2     -fstack-protector     -fomit-frame-pointer     -pipe     -pedantic -pedantic-errors     -Werror     -Weffc++     -Wall -Wextra -Wshadow -Wdeprecated     -Wdiv-by-zero -Wfloat-equal -Wfloat-conversion -Wsign-compare -Wpointer-arith     -Wuninitialized -Wunreachable-code     -Wunused -Wunused-function -Wunused-label -Wunused-parameter -Wunused-but-set-parameter -Wunused-but-set-variable     -Wunused-value -Wunused-variable -Wunused-result     -Wmissing-field-initializers -Wmissing-format-attribute -Wmissing-include-dirs -Wmissing-noreturn  "CMakeFiles/cmTC_95b27.dir/tmp-cxx-standard.cpp.o" -o cmTC_95b27 
+        gmake[1]: Leaving directory '/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/CMakeTmp'
+        
+      exitCode: 0
+    runResult:
+      variable: "_cxx_standard_run_status"
+      cached: true
+      stdout: |
+        201402
+      exitCode: 0
+...
diff --git a/clik/build/CMakeFiles/CMakeDirectoryInformation.cmake b/clik/build/CMakeFiles/CMakeDirectoryInformation.cmake
index c39b3d50a264d67de9124caad8ee0ba8a4c96f09..51fea85c288b0c54521356ac87b541c623e5b5cc 100644
--- a/clik/build/CMakeFiles/CMakeDirectoryInformation.cmake
+++ b/clik/build/CMakeFiles/CMakeDirectoryInformation.cmake
@@ -2,8 +2,8 @@
 # Generated by "Unix Makefiles" Generator, CMake Version 3.27
 
 # Relative path conversion top directories.
-set(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik")
-set(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build")
+set(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/gospodar/lund/praxis/ur_simple_control/clik")
+set(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/gospodar/lund/praxis/ur_simple_control/clik/build")
 
 # Force unix paths in dependencies.
 set(CMAKE_FORCE_UNIX_PATHS 1)
diff --git a/clik/build/CMakeFiles/Makefile.cmake b/clik/build/CMakeFiles/Makefile.cmake
index 1518c77ce68a1ab2e7bbda1eb724a2f1e6690b64..428afd4f15517ad177edb3d7a6f8b484cd8d0fed 100644
--- a/clik/build/CMakeFiles/Makefile.cmake
+++ b/clik/build/CMakeFiles/Makefile.cmake
@@ -34,7 +34,7 @@ set(CMAKE_MAKEFILE_DEPENDS
   "/home/gospodar/.local/lib/python3.10/site-packages/cmake/data/share/cmake-3.27/Modules/Platform/Linux-Initialize.cmake"
   "/home/gospodar/.local/lib/python3.10/site-packages/cmake/data/share/cmake-3.27/Modules/Platform/Linux.cmake"
   "/home/gospodar/.local/lib/python3.10/site-packages/cmake/data/share/cmake-3.27/Modules/Platform/UnixPaths.cmake"
-  "/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/CMakeLists.txt"
+  "/home/gospodar/lund/praxis/ur_simple_control/clik/CMakeLists.txt"
   "CMakeFiles/3.27.4/CMakeCCompiler.cmake"
   "CMakeFiles/3.27.4/CMakeCXXCompiler.cmake"
   "CMakeFiles/3.27.4/CMakeSystem.cmake"
diff --git a/clik/build/CMakeFiles/Makefile2 b/clik/build/CMakeFiles/Makefile2
index 6535a8533cc1f8bf56ccbd398d4a2985a99bcf72..75a8c7e5c71d21dc414c7c95048dbdef85d30bd3 100644
--- a/clik/build/CMakeFiles/Makefile2
+++ b/clik/build/CMakeFiles/Makefile2
@@ -54,10 +54,10 @@ RM = /home/gospodar/.local/lib/python3.10/site-packages/cmake/data/bin/cmake -E
 EQUALS = =
 
 # The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik
+CMAKE_SOURCE_DIR = /home/gospodar/lund/praxis/ur_simple_control/clik
 
 # The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build
+CMAKE_BINARY_DIR = /home/gospodar/lund/praxis/ur_simple_control/clik/build
 
 #=============================================================================
 # Directory level rules for the build root directory
@@ -81,14 +81,14 @@ clean: CMakeFiles/clik.dir/clean
 CMakeFiles/clik.dir/all:
 	$(MAKE) $(MAKESILENT) -f CMakeFiles/clik.dir/build.make CMakeFiles/clik.dir/depend
 	$(MAKE) $(MAKESILENT) -f CMakeFiles/clik.dir/build.make CMakeFiles/clik.dir/build
-	@$(CMAKE_COMMAND) -E cmake_echo_color "--switch=$(COLOR)" --progress-dir=/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles --progress-num=1,2 "Built target clik"
+	@$(CMAKE_COMMAND) -E cmake_echo_color "--switch=$(COLOR)" --progress-dir=/home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles --progress-num=1,2 "Built target clik"
 .PHONY : CMakeFiles/clik.dir/all
 
 # Build rule for subdir invocation for target.
 CMakeFiles/clik.dir/rule: cmake_check_build_system
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles 2
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles 2
 	$(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 CMakeFiles/clik.dir/all
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles 0
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles 0
 .PHONY : CMakeFiles/clik.dir/rule
 
 # Convenience name for target.
diff --git a/clik/build/CMakeFiles/TargetDirectories.txt b/clik/build/CMakeFiles/TargetDirectories.txt
index 877642d089a69c6ce56fac7bb38d7cae627d3630..3acf413fa66f73a0a9cc9c507733aa6df31ee264 100644
--- a/clik/build/CMakeFiles/TargetDirectories.txt
+++ b/clik/build/CMakeFiles/TargetDirectories.txt
@@ -1,3 +1,3 @@
-/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/clik.dir
-/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/edit_cache.dir
-/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/rebuild_cache.dir
+/home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/clik.dir
+/home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/edit_cache.dir
+/home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/rebuild_cache.dir
diff --git a/clik/build/CMakeFiles/clik.dir/DependInfo.cmake b/clik/build/CMakeFiles/clik.dir/DependInfo.cmake
index f10f937c19366dd00bdde41b580ce88ac9cc42cc..9704757bbd2d3d93205acb544fe6059f1ac2444d 100644
--- a/clik/build/CMakeFiles/clik.dir/DependInfo.cmake
+++ b/clik/build/CMakeFiles/clik.dir/DependInfo.cmake
@@ -8,7 +8,7 @@ set(CMAKE_DEPENDS_LANGUAGES
 
 # The set of dependency files which are needed:
 set(CMAKE_DEPENDS_DEPENDENCY_FILES
-  "/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp" "CMakeFiles/clik.dir/src/clik.cpp.o" "gcc" "CMakeFiles/clik.dir/src/clik.cpp.o.d"
+  "/home/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp" "CMakeFiles/clik.dir/src/clik.cpp.o" "gcc" "CMakeFiles/clik.dir/src/clik.cpp.o.d"
   )
 
 # Targets to which this target links which contain Fortran sources.
diff --git a/clik/build/CMakeFiles/clik.dir/build.make b/clik/build/CMakeFiles/clik.dir/build.make
index c27b32ad576ae94baf25040190a1e0ece4fb3141..e47acdb7da8d640e4495c10887c8dee7b316e5df 100644
--- a/clik/build/CMakeFiles/clik.dir/build.make
+++ b/clik/build/CMakeFiles/clik.dir/build.make
@@ -53,10 +53,10 @@ RM = /home/gospodar/.local/lib/python3.10/site-packages/cmake/data/bin/cmake -E
 EQUALS = =
 
 # The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik
+CMAKE_SOURCE_DIR = /home/gospodar/lund/praxis/ur_simple_control/clik
 
 # The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build
+CMAKE_BINARY_DIR = /home/gospodar/lund/praxis/ur_simple_control/clik/build
 
 # Include any dependencies generated for this target.
 include CMakeFiles/clik.dir/depend.make
@@ -70,18 +70,18 @@ include CMakeFiles/clik.dir/progress.make
 include CMakeFiles/clik.dir/flags.make
 
 CMakeFiles/clik.dir/src/clik.cpp.o: CMakeFiles/clik.dir/flags.make
-CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp
+CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp
 CMakeFiles/clik.dir/src/clik.cpp.o: CMakeFiles/clik.dir/compiler_depend.ts
-	@$(CMAKE_COMMAND) -E cmake_echo_color "--switch=$(COLOR)" --green --progress-dir=/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_1) "Building CXX object CMakeFiles/clik.dir/src/clik.cpp.o"
-	/usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -MD -MT CMakeFiles/clik.dir/src/clik.cpp.o -MF CMakeFiles/clik.dir/src/clik.cpp.o.d -o CMakeFiles/clik.dir/src/clik.cpp.o -c /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp
+	@$(CMAKE_COMMAND) -E cmake_echo_color "--switch=$(COLOR)" --green --progress-dir=/home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_1) "Building CXX object CMakeFiles/clik.dir/src/clik.cpp.o"
+	/usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -MD -MT CMakeFiles/clik.dir/src/clik.cpp.o -MF CMakeFiles/clik.dir/src/clik.cpp.o.d -o CMakeFiles/clik.dir/src/clik.cpp.o -c /home/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp
 
 CMakeFiles/clik.dir/src/clik.cpp.i: cmake_force
 	@$(CMAKE_COMMAND) -E cmake_echo_color "--switch=$(COLOR)" --green "Preprocessing CXX source to CMakeFiles/clik.dir/src/clik.cpp.i"
-	/usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp > CMakeFiles/clik.dir/src/clik.cpp.i
+	/usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp > CMakeFiles/clik.dir/src/clik.cpp.i
 
 CMakeFiles/clik.dir/src/clik.cpp.s: cmake_force
 	@$(CMAKE_COMMAND) -E cmake_echo_color "--switch=$(COLOR)" --green "Compiling CXX source to assembly CMakeFiles/clik.dir/src/clik.cpp.s"
-	/usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp -o CMakeFiles/clik.dir/src/clik.cpp.s
+	/usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp -o CMakeFiles/clik.dir/src/clik.cpp.s
 
 # Object files for target clik
 clik_OBJECTS = \
@@ -109,7 +109,7 @@ clik: /usr/lib/x86_64-linux-gnu/libboost_chrono.so
 clik: /usr/lib/x86_64-linux-gnu/liboctomap.so.1.9.7
 clik: /usr/lib/x86_64-linux-gnu/liboctomath.so.1.9.7
 clik: CMakeFiles/clik.dir/link.txt
-	@$(CMAKE_COMMAND) -E cmake_echo_color "--switch=$(COLOR)" --green --bold --progress-dir=/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_2) "Linking CXX executable clik"
+	@$(CMAKE_COMMAND) -E cmake_echo_color "--switch=$(COLOR)" --green --bold --progress-dir=/home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_2) "Linking CXX executable clik"
 	$(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/clik.dir/link.txt --verbose=$(VERBOSE)
 
 # Rule to build all files generated by this target.
@@ -121,6 +121,6 @@ CMakeFiles/clik.dir/clean:
 .PHONY : CMakeFiles/clik.dir/clean
 
 CMakeFiles/clik.dir/depend:
-	cd /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/clik.dir/DependInfo.cmake "--color=$(COLOR)"
+	cd /home/gospodar/lund/praxis/ur_simple_control/clik/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/gospodar/lund/praxis/ur_simple_control/clik /home/gospodar/lund/praxis/ur_simple_control/clik /home/gospodar/lund/praxis/ur_simple_control/clik/build /home/gospodar/lund/praxis/ur_simple_control/clik/build /home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles/clik.dir/DependInfo.cmake "--color=$(COLOR)"
 .PHONY : CMakeFiles/clik.dir/depend
 
diff --git a/clik/build/CMakeFiles/clik.dir/compiler_depend.internal b/clik/build/CMakeFiles/clik.dir/compiler_depend.internal
index 4a438f882fe2c507b7747fe2c0f519b5a20c3912..4a6ad7838898fe8afb07428cd6dc7c11b62e0afc 100644
--- a/clik/build/CMakeFiles/clik.dir/compiler_depend.internal
+++ b/clik/build/CMakeFiles/clik.dir/compiler_depend.internal
@@ -2,7 +2,7 @@
 # Generated by "Unix Makefiles" Generator, CMake Version 3.27
 
 CMakeFiles/clik.dir/src/clik.cpp.o
- /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp
+ /home/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp
  /opt/openrobots/include/hpp/fcl/BV/AABB.h
  /opt/openrobots/include/hpp/fcl/BV/BV.h
  /opt/openrobots/include/hpp/fcl/BV/BV_node.h
diff --git a/clik/build/CMakeFiles/clik.dir/compiler_depend.make b/clik/build/CMakeFiles/clik.dir/compiler_depend.make
index 1c973afbdef359a3b8f4d568a7f23d2eaf4d54b8..cdee0109aef05beef8532b568e1a1eba46cf3241 100644
--- a/clik/build/CMakeFiles/clik.dir/compiler_depend.make
+++ b/clik/build/CMakeFiles/clik.dir/compiler_depend.make
@@ -1,7 +1,7 @@
 # CMAKE generated file: DO NOT EDIT!
 # Generated by "Unix Makefiles" Generator, CMake Version 3.27
 
-CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp \
+CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp \
   /opt/openrobots/include/hpp/fcl/BV/AABB.h \
   /opt/openrobots/include/hpp/fcl/BV/BV.h \
   /opt/openrobots/include/hpp/fcl/BV/BV_node.h \
@@ -3422,12 +3422,6 @@ CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur
 
 /usr/include/boost/date_time/time_system_counted.hpp:
 
-/usr/include/boost/detail/reference_content.hpp:
-
-/usr/include/boost/mpl/aux_/pop_front_impl.hpp:
-
-/usr/include/boost/fusion/iterator/equal_to.hpp:
-
 /usr/include/boost/fusion/algorithm/transformation/remove.hpp:
 
 /usr/include/boost/mpl/has_xxx.hpp:
@@ -3578,6 +3572,10 @@ CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur
 
 /usr/include/boost/asio/detail/scheduler_thread_info.hpp:
 
+/usr/include/boost/asio/associated_allocator.hpp:
+
+/opt/openrobots/include/pinocchio/math/taylor-expansion.hpp:
+
 /opt/openrobots/include/pinocchio/multibody/geometry.hpp:
 
 /usr/include/boost/range/iterator_range_core.hpp:
@@ -3738,6 +3736,12 @@ CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur
 
 /usr/include/boost/archive/basic_archive.hpp:
 
+/usr/include/boost/asio/execution/set_value.hpp:
+
+/usr/include/boost/asio/ip/detail/impl/endpoint.ipp:
+
+/opt/openrobots/include/pinocchio/multibody/joint/joint-basic-visitors.hpp:
+
 /usr/include/boost/mpl/always.hpp:
 
 /usr/include/boost/assert/source_location.hpp:
@@ -3832,8 +3836,6 @@ CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur
 
 /usr/include/boost/archive/detail/interface_oarchive.hpp:
 
-/usr/include/boost/asio/associated_allocator.hpp:
-
 /usr/include/asm-generic/types.h:
 
 /usr/include/boost/asio/traits/equality_comparable.hpp:
@@ -3866,14 +3868,6 @@ CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur
 
 /usr/include/boost/asio/execution/prefer_only.hpp:
 
-/opt/openrobots/include/pinocchio/math/taylor-expansion.hpp:
-
-/opt/openrobots/include/pinocchio/multibody/joint/joint-basic-visitors.hpp:
-
-/usr/include/boost/asio/execution/set_value.hpp:
-
-/usr/include/boost/asio/ip/detail/impl/endpoint.ipp:
-
 /opt/openrobots/include/pinocchio/multibody/visitor/joint-unary-visitor.hpp:
 
 /opt/openrobots/include/hpp/fcl/collision_func_matrix.h:
@@ -4154,23 +4148,31 @@ CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur
 
 /usr/include/boost/current_function.hpp:
 
-/opt/openrobots/include/hpp/fcl/BV/kIOS.h:
+/usr/include/boost/fusion/view/reverse_view/detail/value_of_data_impl.hpp:
 
-/opt/openrobots/include/hpp/fcl/mesh_loader/assimp.h:
+/usr/include/boost/fusion/algorithm/transformation/detail/preprocessed/zip10.hpp:
 
-/usr/include/c++/11/ext/concurrence.h:
+/usr/include/boost/lexical_cast/detail/converter_lexical_streams.hpp:
 
-/usr/include/boost/fusion/algorithm/iteration/fold.hpp:
+/usr/include/boost/archive/text_oarchive.hpp:
 
-/opt/openrobots/include/pinocchio/algorithm/joint-configuration.hxx:
+/opt/openrobots/include/hpp/fcl/BV/BV.h:
 
-/usr/include/c++/11/cstring:
+/usr/include/boost/mpl/aux_/config/typeof.hpp:
 
-/opt/openrobots/include/pinocchio/deprecation.hpp:
+/usr/include/boost/type_traits/disjunction.hpp:
 
-/opt/openrobots/include/hpp/fcl/shape/geometric_shapes.h:
+/usr/include/boost/asio/detail/impl/timer_queue_set.ipp:
 
-/usr/include/boost/math/special_functions/nonfinite_num_facets.hpp:
+/opt/openrobots/include/pinocchio/deprecated-macros.hpp:
+
+/usr/include/c++/11/bits/algorithmfwd.h:
+
+/opt/openrobots/include/hpp/fcl/BV/kIOS.h:
+
+/opt/openrobots/include/hpp/fcl/mesh_loader/assimp.h:
+
+/usr/include/c++/11/ext/concurrence.h:
 
 /usr/include/boost/move/detail/workaround.hpp:
 
@@ -4190,6 +4192,18 @@ CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur
 
 /usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorMacros.h:
 
+/usr/include/boost/fusion/algorithm/iteration/fold.hpp:
+
+/opt/openrobots/include/pinocchio/algorithm/joint-configuration.hxx:
+
+/usr/include/c++/11/cstring:
+
+/opt/openrobots/include/pinocchio/deprecation.hpp:
+
+/opt/openrobots/include/hpp/fcl/shape/geometric_shapes.h:
+
+/usr/include/boost/math/special_functions/nonfinite_num_facets.hpp:
+
 /usr/include/boost/asio/detail/assert.hpp:
 
 /usr/include/asm-generic/int-ll64.h:
@@ -4262,25 +4276,13 @@ CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur
 
 /usr/include/boost/fusion/view/single_view/detail/size_impl.hpp:
 
-/usr/include/boost/lexical_cast/detail/converter_lexical_streams.hpp:
-
-/usr/include/boost/archive/text_oarchive.hpp:
-
-/opt/openrobots/include/hpp/fcl/BV/BV.h:
-
-/usr/include/boost/mpl/aux_/config/typeof.hpp:
-
-/usr/include/boost/type_traits/disjunction.hpp:
-
-/usr/include/boost/asio/detail/impl/timer_queue_set.ipp:
-
-/opt/openrobots/include/pinocchio/deprecated-macros.hpp:
+/usr/include/boost/detail/reference_content.hpp:
 
-/usr/include/c++/11/bits/algorithmfwd.h:
+/usr/include/boost/mpl/aux_/pop_front_impl.hpp:
 
-/usr/include/boost/fusion/view/reverse_view/detail/value_of_data_impl.hpp:
+/usr/include/boost/fusion/iterator/equal_to.hpp:
 
-/usr/include/boost/fusion/algorithm/transformation/detail/preprocessed/zip10.hpp:
+/home/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp:
 
 /opt/openrobots/include/pinocchio/math/tensor.hpp:
 
@@ -4308,20 +4310,6 @@ CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur
 
 /opt/openrobots/include/pinocchio/multibody/constraint-base.hpp:
 
-/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp:
-
-/usr/include/boost/asio/detail/impl/timer_queue_ptime.ipp:
-
-/usr/include/boost/asio/detail/thread_info_base.hpp:
-
-/usr/include/boost/mpl/aux_/front_impl.hpp:
-
-/usr/include/boost/type_traits/promote.hpp:
-
-/usr/include/boost/bind/bind_mf_cc.hpp:
-
-/usr/include/eigen3/Eigen/src/Core/DenseStorage.h:
-
 /usr/include/boost/fusion/container/vector/detail/prior_impl.hpp:
 
 /usr/include/boost/mpl/list/aux_/size.hpp:
@@ -4594,6 +4582,18 @@ CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur
 
 /opt/openrobots/include/pinocchio/multibody/liegroup/liegroup-base.hxx:
 
+/usr/include/boost/function_types/detail/encoding/aliases_undef.hpp:
+
+/usr/include/x86_64-linux-gnu/bits/types/error_t.h:
+
+/usr/include/c++/11/string:
+
+/usr/include/eigen3/Eigen/src/Core/Swap.h:
+
+/usr/include/eigen3/Eigen/src/Core/SolverBase.h:
+
+/opt/openrobots/include/pinocchio/parsers/urdf.hpp:
+
 /usr/include/x86_64-linux-gnu/c++/11/bits/c++locale.h:
 
 /opt/openrobots/include/pinocchio/spatial/fcl-pinocchio-conversions.hpp:
@@ -4968,6 +4968,18 @@ CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur
 
 /usr/include/boost/asio/detail/string_view.hpp:
 
+/usr/include/boost/asio/detail/impl/timer_queue_ptime.ipp:
+
+/usr/include/boost/asio/detail/thread_info_base.hpp:
+
+/usr/include/boost/mpl/aux_/front_impl.hpp:
+
+/usr/include/boost/type_traits/promote.hpp:
+
+/usr/include/boost/bind/bind_mf_cc.hpp:
+
+/usr/include/eigen3/Eigen/src/Core/DenseStorage.h:
+
 /usr/include/boost/asio/detail/timer_queue.hpp:
 
 /usr/include/boost/fusion/algorithm/transformation/replace.hpp:
@@ -5336,18 +5348,6 @@ CMakeFiles/clik.dir/src/clik.cpp.o: /home/gospodar/home2/gospodar/lund/praxis/ur
 
 /opt/openrobots/include/pinocchio/multibody/joint/fwd.hpp:
 
-/usr/include/boost/function_types/detail/encoding/aliases_undef.hpp:
-
-/usr/include/x86_64-linux-gnu/bits/types/error_t.h:
-
-/usr/include/c++/11/string:
-
-/usr/include/eigen3/Eigen/src/Core/Swap.h:
-
-/usr/include/eigen3/Eigen/src/Core/SolverBase.h:
-
-/opt/openrobots/include/pinocchio/parsers/urdf.hpp:
-
 /usr/include/boost/asio/execution.hpp:
 
 /usr/include/boost/asio/detail/is_buffer_sequence.hpp:
diff --git a/clik/build/CMakeFiles/clik.dir/flags.make b/clik/build/CMakeFiles/clik.dir/flags.make
index 7a08f4031f751db56b31258b7ec5ab1068509c20..b4ffbcd84ef99e8b8b7d509c85d95c05b32f0e14 100644
--- a/clik/build/CMakeFiles/clik.dir/flags.make
+++ b/clik/build/CMakeFiles/clik.dir/flags.make
@@ -4,7 +4,7 @@
 # compile CXX with /usr/bin/c++
 CXX_DEFINES = -DBOOST_ALL_NO_LIB -DBOOST_ATOMIC_DYN_LINK -DBOOST_MPL_LIMIT_LIST_SIZE=30 -DBOOST_MPL_LIMIT_VECTOR_SIZE=30 -DBOOST_SYSTEM_DYN_LINK -DBOOST_THREAD_DYN_LINK -DHPP_FCL_HAS_OCTOMAP -DHPP_FCL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=1 -DOCTOMAP_MINOR_VERSION=9 -DOCTOMAP_PATCH_VERSION=7 -DPINOCCHIO_WITH_HPP_FCL -DPINOCCHIO_WITH_URDFDOM
 
-CXX_INCLUDES = -I/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/src -isystem /opt/openrobots/include -isystem /usr/include/eigen3
+CXX_INCLUDES = -I/home/gospodar/lund/praxis/ur_simple_control/clik/src -isystem /opt/openrobots/include -isystem /usr/include/eigen3
 
 CXX_FLAGS =      -D_XOPEN_SOURCE=700     -D_FORTIFY_SOURCE=2     -O2     -fstack-protector     -fomit-frame-pointer     -pipe     -pedantic -pedantic-errors     -Werror     -Weffc++     -Wall -Wextra -Wshadow -Wdeprecated     -Wdiv-by-zero -Wfloat-equal -Wfloat-conversion -Wsign-compare -Wpointer-arith     -Wuninitialized -Wunreachable-code     -Wunused -Wunused-function -Wunused-label -Wunused-parameter -Wunused-but-set-parameter -Wunused-but-set-variable     -Wunused-value -Wunused-variable -Wunused-result     -Wmissing-field-initializers -Wmissing-format-attribute -Wmissing-include-dirs -Wmissing-noreturn -std=gnu++14
 
diff --git a/clik/build/CMakeFiles/clik.dir/src/clik.cpp.o b/clik/build/CMakeFiles/clik.dir/src/clik.cpp.o
index ff1bc201759f3016a423eed5a18a500979213503..902c729b97cf91710a04711536197bf96d275ca3 100644
Binary files a/clik/build/CMakeFiles/clik.dir/src/clik.cpp.o and b/clik/build/CMakeFiles/clik.dir/src/clik.cpp.o differ
diff --git a/clik/build/CMakeFiles/clik.dir/src/clik.cpp.o.d b/clik/build/CMakeFiles/clik.dir/src/clik.cpp.o.d
index e151c401012cc4d8058606321b02446d57535108..fc0f474d5048bebc07fcedfb09d752c96695bc8b 100644
--- a/clik/build/CMakeFiles/clik.dir/src/clik.cpp.o.d
+++ b/clik/build/CMakeFiles/clik.dir/src/clik.cpp.o.d
@@ -1,5 +1,5 @@
 CMakeFiles/clik.dir/src/clik.cpp.o: \
- /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp \
+ /home/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp \
  /usr/include/stdc-predef.h \
  /opt/openrobots/include/pinocchio/algorithm/jacobian.hpp \
  /opt/openrobots/include/pinocchio/multibody/fwd.hpp \
diff --git a/clik/build/Makefile b/clik/build/Makefile
index cf5555fe70f2bcefe388699cfbaeec572f15590d..7354d162dc28c63c0f04d3a76a6bbf151611e8ba 100644
--- a/clik/build/Makefile
+++ b/clik/build/Makefile
@@ -57,10 +57,10 @@ RM = /home/gospodar/.local/lib/python3.10/site-packages/cmake/data/bin/cmake -E
 EQUALS = =
 
 # The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik
+CMAKE_SOURCE_DIR = /home/gospodar/lund/praxis/ur_simple_control/clik
 
 # The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build
+CMAKE_BINARY_DIR = /home/gospodar/lund/praxis/ur_simple_control/clik/build
 
 #=============================================================================
 # Targets provided globally by CMake.
@@ -87,9 +87,9 @@ rebuild_cache/fast: rebuild_cache
 
 # The main all target
 all: cmake_check_build_system
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build//CMakeFiles/progress.marks
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles /home/gospodar/lund/praxis/ur_simple_control/clik/build//CMakeFiles/progress.marks
 	$(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 all
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles 0
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/gospodar/lund/praxis/ur_simple_control/clik/build/CMakeFiles 0
 .PHONY : all
 
 # The main clean target
diff --git a/clik/build/clik b/clik/build/clik
index 1d053f340f657eb2f5ade293abebcb098ff4898b..9fcfb2b7ae5ee9745e04adafce099603c556af05 100755
Binary files a/clik/build/clik and b/clik/build/clik differ
diff --git a/clik/build/cmake_install.cmake b/clik/build/cmake_install.cmake
index 732a1ff045a7c180c2cf3a27c09ed0880e013fdc..bd0229d9c0fc8ca470359875a44926c01b4b17a0 100644
--- a/clik/build/cmake_install.cmake
+++ b/clik/build/cmake_install.cmake
@@ -1,4 +1,4 @@
-# Install script for directory: /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik
+# Install script for directory: /home/gospodar/lund/praxis/ur_simple_control/clik
 
 # Set the install prefix
 if(NOT DEFINED CMAKE_INSTALL_PREFIX)
@@ -50,5 +50,5 @@ endif()
 
 string(REPLACE ";" "\n" CMAKE_INSTALL_MANIFEST_CONTENT
        "${CMAKE_INSTALL_MANIFEST_FILES}")
-file(WRITE "/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build/${CMAKE_INSTALL_MANIFEST}"
+file(WRITE "/home/gospodar/lund/praxis/ur_simple_control/clik/build/${CMAKE_INSTALL_MANIFEST}"
      "${CMAKE_INSTALL_MANIFEST_CONTENT}")
diff --git a/clik/build/compile_commands.json b/clik/build/compile_commands.json
index 4fab5e1fd73d558aed3cb795fa8486dd164cd21c..44da3f8aaa38f0947e993c44bd932d567494240c 100644
--- a/clik/build/compile_commands.json
+++ b/clik/build/compile_commands.json
@@ -1,8 +1,8 @@
 [
 {
-  "directory": "/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/build",
-  "command": "/usr/bin/c++ -DBOOST_ALL_NO_LIB -DBOOST_ATOMIC_DYN_LINK -DBOOST_MPL_LIMIT_LIST_SIZE=30 -DBOOST_MPL_LIMIT_VECTOR_SIZE=30 -DBOOST_SYSTEM_DYN_LINK -DBOOST_THREAD_DYN_LINK -DHPP_FCL_HAS_OCTOMAP -DHPP_FCL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=1 -DOCTOMAP_MINOR_VERSION=9 -DOCTOMAP_PATCH_VERSION=7 -DPINOCCHIO_WITH_HPP_FCL -DPINOCCHIO_WITH_URDFDOM -I/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/src -isystem /opt/openrobots/include -isystem /usr/include/eigen3      -D_XOPEN_SOURCE=700     -D_FORTIFY_SOURCE=2     -O2     -fstack-protector     -fomit-frame-pointer     -pipe     -pedantic -pedantic-errors     -Werror     -Weffc++     -Wall -Wextra -Wshadow -Wdeprecated     -Wdiv-by-zero -Wfloat-equal -Wfloat-conversion -Wsign-compare -Wpointer-arith     -Wuninitialized -Wunreachable-code     -Wunused -Wunused-function -Wunused-label -Wunused-parameter -Wunused-but-set-parameter -Wunused-but-set-variable     -Wunused-value -Wunused-variable -Wunused-result     -Wmissing-field-initializers -Wmissing-format-attribute -Wmissing-include-dirs -Wmissing-noreturn -std=gnu++14 -o CMakeFiles/clik.dir/src/clik.cpp.o -c /home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp",
-  "file": "/home/gospodar/home2/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp",
+  "directory": "/home/gospodar/lund/praxis/ur_simple_control/clik/build",
+  "command": "/usr/bin/c++ -DBOOST_ALL_NO_LIB -DBOOST_ATOMIC_DYN_LINK -DBOOST_MPL_LIMIT_LIST_SIZE=30 -DBOOST_MPL_LIMIT_VECTOR_SIZE=30 -DBOOST_SYSTEM_DYN_LINK -DBOOST_THREAD_DYN_LINK -DHPP_FCL_HAS_OCTOMAP -DHPP_FCL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=1 -DOCTOMAP_MINOR_VERSION=9 -DOCTOMAP_PATCH_VERSION=7 -DPINOCCHIO_WITH_HPP_FCL -DPINOCCHIO_WITH_URDFDOM -I/home/gospodar/lund/praxis/ur_simple_control/clik/src -isystem /opt/openrobots/include -isystem /usr/include/eigen3      -D_XOPEN_SOURCE=700     -D_FORTIFY_SOURCE=2     -O2     -fstack-protector     -fomit-frame-pointer     -pipe     -pedantic -pedantic-errors     -Werror     -Weffc++     -Wall -Wextra -Wshadow -Wdeprecated     -Wdiv-by-zero -Wfloat-equal -Wfloat-conversion -Wsign-compare -Wpointer-arith     -Wuninitialized -Wunreachable-code     -Wunused -Wunused-function -Wunused-label -Wunused-parameter -Wunused-but-set-parameter -Wunused-but-set-variable     -Wunused-value -Wunused-variable -Wunused-result     -Wmissing-field-initializers -Wmissing-format-attribute -Wmissing-include-dirs -Wmissing-noreturn -std=gnu++14 -o CMakeFiles/clik.dir/src/clik.cpp.o -c /home/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp",
+  "file": "/home/gospodar/lund/praxis/ur_simple_control/clik/src/clik.cpp",
   "output": "CMakeFiles/clik.dir/src/clik.cpp.o"
 }
 ]
\ No newline at end of file
diff --git a/clik/src/clik.cpp b/clik/src/clik.cpp
index 0d668ad536eca3c3a0d6b6c05e65395ce91a4724..8cc3dd92933e8ff48ae78013426f2d15ee03a361 100644
--- a/clik/src/clik.cpp
+++ b/clik/src/clik.cpp
@@ -11,76 +11,84 @@
 #include <ur_rtde/rtde_receive_interface.h>
 
 #include <iostream>
+// so that i can get my unique pointer
+#include <memory> 
 
-int main(void) {
-  //  std::cout << "help" << std::endl;
-
-  // TODO put in some ifs to run it in sim/something if you're not connected
-  // to the robot
-  ur_rtde::RTDEControlInterface rtde_control("192.168.1.102");
-  ur_rtde::RTDEReceiveInterface rtde_receive("192.168.1.102");
-  std::vector<double> init_q = rtde_receive.getActualQ();
+#define SIMULATION true
 
-//  const std::string urdf_filename = std::string(
-//      "/home/gospodar/lund/praxis/pinocchio/models/example-robot-data/robots/"
-//      "ur_description/urdf/ur5_robot.urdf");
-  const std::string urdf_filename = std::string(
-      "/home/gospodar/lund/praxis/ur_resources/ur5e_with_robotiq_hande/urdf/ur5e_with_robotiq_hande.urdf");
+int main(void) {
+  // load robot model from urdf (most common type of robot description file)
+  const std::string urdf_filename =
+      std::string("/home/gospodar/lund/praxis/ur_resources/"
+                  "ur5e_with_robotiq_hande/urdf/ur5e_with_robotiq_hande.urdf");
   pinocchio::Model model;
   pinocchio::urdf::buildModel(urdf_filename, model);
-  // pinocchio::buildModels::manipulator(model);
-
   pinocchio::Data data(model);
 
-  const int JOINT_ID = 6;
-  // oMdes is the desired pose
-
-  //const pinocchio::SE3 oMdes(Eigen::Matrix3d(1.0, 0.0, 0.0,
-  //  										 0.0, 1.0, 0.0,
-  //  										 0.0, 0.0, 1.0),
-  const pinocchio::SE3 oMdes(Eigen::Matrix3d::Zero(),
-                             Eigen::Vector3d(0.5, 0.3,  0.5));
+  // need to make these pointers so that i can have the object
+  // without calling the constructor
+  // unique pointer as i would otherwise use these variables directly
+  std::unique_ptr<ur_rtde::RTDEControlInterface> rtde_control;
+  std::unique_ptr<ur_rtde::RTDEReceiveInterface> rtde_receive;
+  if ( !SIMULATION) {
+    rtde_control = std::make_unique<ur_rtde::RTDEControlInterface>("192.168.1.102");
+    rtde_receive = std::make_unique<ur_rtde::RTDEReceiveInterface>("192.168.1.102");
+  }
 
-  //Eigen::VectorXd q = pinocchio::neutral(model);
+  std::vector<double> init_q;
   Eigen::VectorXd q(model.nq);
-  q = Eigen::Map<Eigen::VectorXd>(&rtde_receive.getActualQ()[0], model.nq);
-  // random print to make sure it works
-  std::cout << q[2] << std::endl;
-//  Eigen::VectorXd q;
+  if (!SIMULATION) {
+    q = Eigen::Map<Eigen::VectorXd>(&rtde_receive->getActualQ()[0], model.nq);
+  } else {
+    q = pinocchio::neutral(model);
+  }
+
+  // oMdes is the desired pose
+  // const pinocchio::SE3 oMdes(Eigen::Matrix3d(1.0, 0.0, 0.0,
+  //  										 0.0, 1.0,
+  //  0.0, 										 0.0,
+  //  0.0, 1.0),
+  const pinocchio::SE3 oMdes(Eigen::Matrix3d::Zero(),
+                             Eigen::Vector3d(0.5, 0.3, 0.5));
   const double eps = 1e-4;
   const int IT_MAX = 10000000;
-//  const double DT = 1e-1;
+  const double DT = 1e-1;
   const double damp = 1e-6;
 
+  // define manipulator jacobian
+  // 6 is the velocity dimension: 3 linear, 3 angular directions
   pinocchio::Data::Matrix6x J(6, model.nv);
   J.setZero();
 
   bool success = false;
-  typedef Eigen::Matrix<double, 6, 1> Vector6d;
-  Vector6d err;
+  Eigen::Matrix<double, 6, 1> err;
   Eigen::VectorXd v(model.nv);
+  // TODO make casting cleaner somehow
   std::vector<double> v_as_std_vec;
+  const int JOINT_ID = 6;
   for (int i = 0;; i++) {
-    q = Eigen::Map<Eigen::VectorXd>(&rtde_receive.getActualQ()[0], model.nq);
+    if (!SIMULATION) {
+      q = Eigen::Map<Eigen::VectorXd>(&rtde_receive->getActualQ()[0], model.nq);
+    }
     pinocchio::forwardKinematics(model, data, q);
-// data.omi[joint_id] is the placement of joint_id
-	// se3.act is the group action,
-	// se3.actinv is the inverse group action
-	// you need to check the actual math to see what and why is this here
+    // data.omi[joint_id] is the placement of joint_id
+    // se3.act is the group action,
+    // se3.actinv is the inverse group action
+    // you need to check the actual math to see what and why is this here
     const pinocchio::SE3 iMd = data.oMi[JOINT_ID].actInv(oMdes);
     err = pinocchio::log6(iMd).toVector(); // in joint frame
     if (err.norm() < eps) {
       success = true;
-	  std::cout << "reached destionation" << std::endl;
+      std::cout << "reached destionation" << std::endl;
       break;
     }
     if (i >= IT_MAX) {
       success = false;
-	  std::cout << "FAIL: did not succed in IT_MAX iterations" << std::endl;
+      std::cout << "FAIL: did not succed in IT_MAX iterations" << std::endl;
       break;
     }
-    pinocchio::computeJointJacobian(model, data, q, JOINT_ID,
-                                    J); // J in joint frame
+// J in joint frame
+    pinocchio::computeJointJacobian(model, data, q, JOINT_ID, J); 
     pinocchio::Data::Matrix6 Jlog;
     pinocchio::Jlog6(iMd.inverse(), Jlog);
     J = -Jlog * J;
@@ -88,20 +96,24 @@ int main(void) {
     JJt.noalias() = J * J.transpose();
     JJt.diagonal().array() += damp;
     v.noalias() = -J.transpose() * JJt.ldlt().solve(err);
-	//    we're running it on the real system
-//    q = pinocchio::integrate(model, q, v * DT);
-//    make a cleaner cast, but also who cares
-	v_as_std_vec.push_back(v[0]);
-	v_as_std_vec.push_back(v[1]);
-	v_as_std_vec.push_back(v[2]);
-	v_as_std_vec.push_back(v[3]);
-	v_as_std_vec.push_back(v[4]);
-	v_as_std_vec.push_back(v[5]);
-    rtde_control.speedJ(v_as_std_vec, 0.1, 1.0/500);
-	v_as_std_vec.clear();
-    if (!(i % 1000))
+    //    we're running it on the real system
+	if ( !SIMULATION) {
+    // TODO check that this works, otherwise fall back to uglier assignment
+      v_as_std_vec.assign(v.data(), v.data() + v.size());
+//      v_as_std_vec.push_back(v[0]);
+//      v_as_std_vec.push_back(v[1]);
+//      v_as_std_vec.push_back(v[2]);
+//      v_as_std_vec.push_back(v[3]);
+//      v_as_std_vec.push_back(v[4]);
+//      v_as_std_vec.push_back(v[5]);
+      rtde_control->speedJ(v_as_std_vec, 0.1, 1.0 / 500);
+//      v_as_std_vec.clear();
+	} else {
+      q = pinocchio::integrate(model, q, v * DT);
+	}
+    if (!(i % 10))
       std::cout << i << ": error = " << err.transpose() << std::endl;
-	  std::cout << "pos" << data.oMi[JOINT_ID] << std::endl;
+    std::cout << "pos" << data.oMi[JOINT_ID] << std::endl;
   }
 
   if (success) {