diff --git a/c++/build/CMakeFiles/3.16.3/CompilerIdCXX/CMakeCXXCompilerId.cpp b/c++/build/CMakeFiles/3.16.3/CompilerIdCXX/CMakeCXXCompilerId.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..69cfdba6bc7bccb09bf234388908de035caa0969
--- /dev/null
+++ b/c++/build/CMakeFiles/3.16.3/CompilerIdCXX/CMakeCXXCompilerId.cpp
@@ -0,0 +1,660 @@
+/* This source file must have a .cpp extension so that all C++ compilers
+ recognize the extension without flags. Borland does not know .cxx for
+ example. */
+#ifndef __cplusplus
+# error "A C compiler has been selected for C++."
+#endif
+
+
+/* Version number components: V=Version, R=Revision, P=Patch
+ Version date components: YYYY=Year, MM=Month, DD=Day */
+
+#if defined(__COMO__)
+# define COMPILER_ID "Comeau"
+ /* __COMO_VERSION__ = VRR */
+# define COMPILER_VERSION_MAJOR DEC(__COMO_VERSION__ / 100)
+# define COMPILER_VERSION_MINOR DEC(__COMO_VERSION__ % 100)
+
+#elif defined(__INTEL_COMPILER) || defined(__ICC)
+# define COMPILER_ID "Intel"
+# if defined(_MSC_VER)
+# define SIMULATE_ID "MSVC"
+# endif
+# if defined(__GNUC__)
+# define SIMULATE_ID "GNU"
+# endif
+ /* __INTEL_COMPILER = VRP */
+# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100)
+# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10)
+# if defined(__INTEL_COMPILER_UPDATE)
+# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER_UPDATE)
+# else
+# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10)
+# endif
+# if defined(__INTEL_COMPILER_BUILD_DATE)
+ /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */
+# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE)
+# endif
+# if defined(_MSC_VER)
+ /* _MSC_VER = VVRR */
+# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100)
+# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100)
+# endif
+# if defined(__GNUC__)
+# define SIMULATE_VERSION_MAJOR DEC(__GNUC__)
+# elif defined(__GNUG__)
+# define SIMULATE_VERSION_MAJOR DEC(__GNUG__)
+# endif
+# if defined(__GNUC_MINOR__)
+# define SIMULATE_VERSION_MINOR DEC(__GNUC_MINOR__)
+# endif
+# if defined(__GNUC_PATCHLEVEL__)
+# define SIMULATE_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__)
+# endif
+
+#elif defined(__PATHCC__)
+# define COMPILER_ID "PathScale"
+# define COMPILER_VERSION_MAJOR DEC(__PATHCC__)
+# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__)
+# if defined(__PATHCC_PATCHLEVEL__)
+# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__)
+# endif
+
+#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__)
+# define COMPILER_ID "Embarcadero"
+# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF)
+# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF)
+# define COMPILER_VERSION_PATCH DEC(__CODEGEARC_VERSION__ & 0xFFFF)
+
+#elif defined(__BORLANDC__)
+# define COMPILER_ID "Borland"
+ /* __BORLANDC__ = 0xVRR */
+# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8)
+# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF)
+
+#elif defined(__WATCOMC__) && __WATCOMC__ < 1200
+# define COMPILER_ID "Watcom"
+ /* __WATCOMC__ = VVRR */
+# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100)
+# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10)
+# if (__WATCOMC__ % 10) > 0
+# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10)
+# endif
+
+#elif defined(__WATCOMC__)
+# define COMPILER_ID "OpenWatcom"
+ /* __WATCOMC__ = VVRP + 1100 */
+# define COMPILER_VERSION_MAJOR DEC((__WATCOMC__ - 1100) / 100)
+# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10)
+# if (__WATCOMC__ % 10) > 0
+# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10)
+# endif
+
+#elif defined(__SUNPRO_CC)
+# define COMPILER_ID "SunPro"
+# if __SUNPRO_CC >= 0x5100
+ /* __SUNPRO_CC = 0xVRRP */
+# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>12)
+# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xFF)
+# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF)
+# else
+ /* __SUNPRO_CC = 0xVRP */
+# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>8)
+# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xF)
+# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF)
+# endif
+
+#elif defined(__HP_aCC)
+# define COMPILER_ID "HP"
+ /* __HP_aCC = VVRRPP */
+# define COMPILER_VERSION_MAJOR DEC(__HP_aCC/10000)
+# define COMPILER_VERSION_MINOR DEC(__HP_aCC/100 % 100)
+# define COMPILER_VERSION_PATCH DEC(__HP_aCC % 100)
+
+#elif defined(__DECCXX)
+# define COMPILER_ID "Compaq"
+ /* __DECCXX_VER = VVRRTPPPP */
+# define COMPILER_VERSION_MAJOR DEC(__DECCXX_VER/10000000)
+# define COMPILER_VERSION_MINOR DEC(__DECCXX_VER/100000 % 100)
+# define COMPILER_VERSION_PATCH DEC(__DECCXX_VER % 10000)
+
+#elif defined(__IBMCPP__) && defined(__COMPILER_VER__)
+# define COMPILER_ID "zOS"
+ /* __IBMCPP__ = VRP */
+# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100)
+# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10)
+# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10)
+
+#elif defined(__ibmxl__) && defined(__clang__)
+# define COMPILER_ID "XLClang"
+# define COMPILER_VERSION_MAJOR DEC(__ibmxl_version__)
+# define COMPILER_VERSION_MINOR DEC(__ibmxl_release__)
+# define COMPILER_VERSION_PATCH DEC(__ibmxl_modification__)
+# define COMPILER_VERSION_TWEAK DEC(__ibmxl_ptf_fix_level__)
+
+
+#elif defined(__IBMCPP__) && !defined(__COMPILER_VER__) && __IBMCPP__ >= 800
+# define COMPILER_ID "XL"
+ /* __IBMCPP__ = VRP */
+# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100)
+# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10)
+# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10)
+
+#elif defined(__IBMCPP__) && !defined(__COMPILER_VER__) && __IBMCPP__ < 800
+# define COMPILER_ID "VisualAge"
+ /* __IBMCPP__ = VRP */
+# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100)
+# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10)
+# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10)
+
+#elif defined(__PGI)
+# define COMPILER_ID "PGI"
+# define COMPILER_VERSION_MAJOR DEC(__PGIC__)
+# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__)
+# if defined(__PGIC_PATCHLEVEL__)
+# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__)
+# endif
+
+#elif defined(_CRAYC)
+# define COMPILER_ID "Cray"
+# define COMPILER_VERSION_MAJOR DEC(_RELEASE_MAJOR)
+# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR)
+
+#elif defined(__TI_COMPILER_VERSION__)
+# define COMPILER_ID "TI"
+ /* __TI_COMPILER_VERSION__ = VVVRRRPPP */
+# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000)
+# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000)
+# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000)
+
+#elif defined(__FUJITSU) || defined(__FCC_VERSION) || defined(__fcc_version)
+# define COMPILER_ID "Fujitsu"
+
+#elif defined(__ghs__)
+# define COMPILER_ID "GHS"
+/* __GHS_VERSION_NUMBER = VVVVRP */
+# ifdef __GHS_VERSION_NUMBER
+# define COMPILER_VERSION_MAJOR DEC(__GHS_VERSION_NUMBER / 100)
+# define COMPILER_VERSION_MINOR DEC(__GHS_VERSION_NUMBER / 10 % 10)
+# define COMPILER_VERSION_PATCH DEC(__GHS_VERSION_NUMBER % 10)
+# endif
+
+#elif defined(__SCO_VERSION__)
+# define COMPILER_ID "SCO"
+
+#elif defined(__ARMCC_VERSION) && !defined(__clang__)
+# define COMPILER_ID "ARMCC"
+#if __ARMCC_VERSION >= 1000000
+ /* __ARMCC_VERSION = VRRPPPP */
+ # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/1000000)
+ # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 100)
+ # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000)
+#else
+ /* __ARMCC_VERSION = VRPPPP */
+ # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/100000)
+ # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 10)
+ # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000)
+#endif
+
+
+#elif defined(__clang__) && defined(__apple_build_version__)
+# define COMPILER_ID "AppleClang"
+# if defined(_MSC_VER)
+# define SIMULATE_ID "MSVC"
+# endif
+# define COMPILER_VERSION_MAJOR DEC(__clang_major__)
+# define COMPILER_VERSION_MINOR DEC(__clang_minor__)
+# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__)
+# if defined(_MSC_VER)
+ /* _MSC_VER = VVRR */
+# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100)
+# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100)
+# endif
+# define COMPILER_VERSION_TWEAK DEC(__apple_build_version__)
+
+#elif defined(__clang__) && defined(__ARMCOMPILER_VERSION)
+# define COMPILER_ID "ARMClang"
+ # define COMPILER_VERSION_MAJOR DEC(__ARMCOMPILER_VERSION/1000000)
+ # define COMPILER_VERSION_MINOR DEC(__ARMCOMPILER_VERSION/10000 % 100)
+ # define COMPILER_VERSION_PATCH DEC(__ARMCOMPILER_VERSION % 10000)
+# define COMPILER_VERSION_INTERNAL DEC(__ARMCOMPILER_VERSION)
+
+#elif defined(__clang__)
+# define COMPILER_ID "Clang"
+# if defined(_MSC_VER)
+# define SIMULATE_ID "MSVC"
+# endif
+# define COMPILER_VERSION_MAJOR DEC(__clang_major__)
+# define COMPILER_VERSION_MINOR DEC(__clang_minor__)
+# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__)
+# if defined(_MSC_VER)
+ /* _MSC_VER = VVRR */
+# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100)
+# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100)
+# endif
+
+#elif defined(__GNUC__) || defined(__GNUG__)
+# define COMPILER_ID "GNU"
+# if defined(__GNUC__)
+# define COMPILER_VERSION_MAJOR DEC(__GNUC__)
+# else
+# define COMPILER_VERSION_MAJOR DEC(__GNUG__)
+# endif
+# if defined(__GNUC_MINOR__)
+# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__)
+# endif
+# if defined(__GNUC_PATCHLEVEL__)
+# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__)
+# endif
+
+#elif defined(_MSC_VER)
+# define COMPILER_ID "MSVC"
+ /* _MSC_VER = VVRR */
+# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100)
+# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100)
+# if defined(_MSC_FULL_VER)
+# if _MSC_VER >= 1400
+ /* _MSC_FULL_VER = VVRRPPPPP */
+# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000)
+# else
+ /* _MSC_FULL_VER = VVRRPPPP */
+# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000)
+# endif
+# endif
+# if defined(_MSC_BUILD)
+# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD)
+# endif
+
+#elif defined(__VISUALDSPVERSION__) || defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__)
+# define COMPILER_ID "ADSP"
+#if defined(__VISUALDSPVERSION__)
+ /* __VISUALDSPVERSION__ = 0xVVRRPP00 */
+# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24)
+# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF)
+# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF)
+#endif
+
+#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC)
+# define COMPILER_ID "IAR"
+# if defined(__VER__) && defined(__ICCARM__)
+# define COMPILER_VERSION_MAJOR DEC((__VER__) / 1000000)
+# define COMPILER_VERSION_MINOR DEC(((__VER__) / 1000) % 1000)
+# define COMPILER_VERSION_PATCH DEC((__VER__) % 1000)
+# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__)
+# elif defined(__VER__) && (defined(__ICCAVR__) || defined(__ICCRX__) || defined(__ICCRH850__) || defined(__ICCRL78__) || defined(__ICC430__) || defined(__ICCRISCV__) || defined(__ICCV850__) || defined(__ICC8051__))
+# define COMPILER_VERSION_MAJOR DEC((__VER__) / 100)
+# define COMPILER_VERSION_MINOR DEC((__VER__) - (((__VER__) / 100)*100))
+# define COMPILER_VERSION_PATCH DEC(__SUBVERSION__)
+# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__)
+# endif
+
+
+/* These compilers are either not known or too old to define an
+ identification macro. Try to identify the platform and guess that
+ it is the native compiler. */
+#elif defined(__hpux) || defined(__hpua)
+# define COMPILER_ID "HP"
+
+#else /* unknown compiler */
+# define COMPILER_ID ""
+#endif
+
+/* Construct the string literal in pieces to prevent the source from
+ getting matched. Store it in a pointer rather than an array
+ because some compilers will just produce instructions to fill the
+ array rather than assigning a pointer to a static array. */
+char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]";
+#ifdef SIMULATE_ID
+char const* info_simulate = "INFO" ":" "simulate[" SIMULATE_ID "]";
+#endif
+
+#ifdef __QNXNTO__
+char const* qnxnto = "INFO" ":" "qnxnto[]";
+#endif
+
+#if defined(__CRAYXE) || defined(__CRAYXC)
+char const *info_cray = "INFO" ":" "compiler_wrapper[CrayPrgEnv]";
+#endif
+
+#define STRINGIFY_HELPER(X) #X
+#define STRINGIFY(X) STRINGIFY_HELPER(X)
+
+/* Identify known platforms by name. */
+#if defined(__linux) || defined(__linux__) || defined(linux)
+# define PLATFORM_ID "Linux"
+
+#elif defined(__CYGWIN__)
+# define PLATFORM_ID "Cygwin"
+
+#elif defined(__MINGW32__)
+# define PLATFORM_ID "MinGW"
+
+#elif defined(__APPLE__)
+# define PLATFORM_ID "Darwin"
+
+#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32)
+# define PLATFORM_ID "Windows"
+
+#elif defined(__FreeBSD__) || defined(__FreeBSD)
+# define PLATFORM_ID "FreeBSD"
+
+#elif defined(__NetBSD__) || defined(__NetBSD)
+# define PLATFORM_ID "NetBSD"
+
+#elif defined(__OpenBSD__) || defined(__OPENBSD)
+# define PLATFORM_ID "OpenBSD"
+
+#elif defined(__sun) || defined(sun)
+# define PLATFORM_ID "SunOS"
+
+#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__)
+# define PLATFORM_ID "AIX"
+
+#elif defined(__hpux) || defined(__hpux__)
+# define PLATFORM_ID "HP-UX"
+
+#elif defined(__HAIKU__)
+# define PLATFORM_ID "Haiku"
+
+#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS)
+# define PLATFORM_ID "BeOS"
+
+#elif defined(__QNX__) || defined(__QNXNTO__)
+# define PLATFORM_ID "QNX"
+
+#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__)
+# define PLATFORM_ID "Tru64"
+
+#elif defined(__riscos) || defined(__riscos__)
+# define PLATFORM_ID "RISCos"
+
+#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__)
+# define PLATFORM_ID "SINIX"
+
+#elif defined(__UNIX_SV__)
+# define PLATFORM_ID "UNIX_SV"
+
+#elif defined(__bsdos__)
+# define PLATFORM_ID "BSDOS"
+
+#elif defined(_MPRAS) || defined(MPRAS)
+# define PLATFORM_ID "MP-RAS"
+
+#elif defined(__osf) || defined(__osf__)
+# define PLATFORM_ID "OSF1"
+
+#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv)
+# define PLATFORM_ID "SCO_SV"
+
+#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX)
+# define PLATFORM_ID "ULTRIX"
+
+#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX)
+# define PLATFORM_ID "Xenix"
+
+#elif defined(__WATCOMC__)
+# if defined(__LINUX__)
+# define PLATFORM_ID "Linux"
+
+# elif defined(__DOS__)
+# define PLATFORM_ID "DOS"
+
+# elif defined(__OS2__)
+# define PLATFORM_ID "OS2"
+
+# elif defined(__WINDOWS__)
+# define PLATFORM_ID "Windows3x"
+
+# else /* unknown platform */
+# define PLATFORM_ID
+# endif
+
+#elif defined(__INTEGRITY)
+# if defined(INT_178B)
+# define PLATFORM_ID "Integrity178"
+
+# else /* regular Integrity */
+# define PLATFORM_ID "Integrity"
+# endif
+
+#else /* unknown platform */
+# define PLATFORM_ID
+
+#endif
+
+/* For windows compilers MSVC and Intel we can determine
+ the architecture of the compiler being used. This is because
+ the compilers do not have flags that can change the architecture,
+ but rather depend on which compiler is being used
+*/
+#if defined(_WIN32) && defined(_MSC_VER)
+# if defined(_M_IA64)
+# define ARCHITECTURE_ID "IA64"
+
+# elif defined(_M_X64) || defined(_M_AMD64)
+# define ARCHITECTURE_ID "x64"
+
+# elif defined(_M_IX86)
+# define ARCHITECTURE_ID "X86"
+
+# elif defined(_M_ARM64)
+# define ARCHITECTURE_ID "ARM64"
+
+# elif defined(_M_ARM)
+# if _M_ARM == 4
+# define ARCHITECTURE_ID "ARMV4I"
+# elif _M_ARM == 5
+# define ARCHITECTURE_ID "ARMV5I"
+# else
+# define ARCHITECTURE_ID "ARMV" STRINGIFY(_M_ARM)
+# endif
+
+# elif defined(_M_MIPS)
+# define ARCHITECTURE_ID "MIPS"
+
+# elif defined(_M_SH)
+# define ARCHITECTURE_ID "SHx"
+
+# else /* unknown architecture */
+# define ARCHITECTURE_ID ""
+# endif
+
+#elif defined(__WATCOMC__)
+# if defined(_M_I86)
+# define ARCHITECTURE_ID "I86"
+
+# elif defined(_M_IX86)
+# define ARCHITECTURE_ID "X86"
+
+# else /* unknown architecture */
+# define ARCHITECTURE_ID ""
+# endif
+
+#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC)
+# if defined(__ICCARM__)
+# define ARCHITECTURE_ID "ARM"
+
+# elif defined(__ICCRX__)
+# define ARCHITECTURE_ID "RX"
+
+# elif defined(__ICCRH850__)
+# define ARCHITECTURE_ID "RH850"
+
+# elif defined(__ICCRL78__)
+# define ARCHITECTURE_ID "RL78"
+
+# elif defined(__ICCRISCV__)
+# define ARCHITECTURE_ID "RISCV"
+
+# elif defined(__ICCAVR__)
+# define ARCHITECTURE_ID "AVR"
+
+# elif defined(__ICC430__)
+# define ARCHITECTURE_ID "MSP430"
+
+# elif defined(__ICCV850__)
+# define ARCHITECTURE_ID "V850"
+
+# elif defined(__ICC8051__)
+# define ARCHITECTURE_ID "8051"
+
+# else /* unknown architecture */
+# define ARCHITECTURE_ID ""
+# endif
+
+#elif defined(__ghs__)
+# if defined(__PPC64__)
+# define ARCHITECTURE_ID "PPC64"
+
+# elif defined(__ppc__)
+# define ARCHITECTURE_ID "PPC"
+
+# elif defined(__ARM__)
+# define ARCHITECTURE_ID "ARM"
+
+# elif defined(__x86_64__)
+# define ARCHITECTURE_ID "x64"
+
+# elif defined(__i386__)
+# define ARCHITECTURE_ID "X86"
+
+# else /* unknown architecture */
+# define ARCHITECTURE_ID ""
+# endif
+#else
+# define ARCHITECTURE_ID
+#endif
+
+/* Convert integer to decimal digit literals. */
+#define DEC(n) \
+ ('0' + (((n) / 10000000)%10)), \
+ ('0' + (((n) / 1000000)%10)), \
+ ('0' + (((n) / 100000)%10)), \
+ ('0' + (((n) / 10000)%10)), \
+ ('0' + (((n) / 1000)%10)), \
+ ('0' + (((n) / 100)%10)), \
+ ('0' + (((n) / 10)%10)), \
+ ('0' + ((n) % 10))
+
+/* Convert integer to hex digit literals. */
+#define HEX(n) \
+ ('0' + ((n)>>28 & 0xF)), \
+ ('0' + ((n)>>24 & 0xF)), \
+ ('0' + ((n)>>20 & 0xF)), \
+ ('0' + ((n)>>16 & 0xF)), \
+ ('0' + ((n)>>12 & 0xF)), \
+ ('0' + ((n)>>8 & 0xF)), \
+ ('0' + ((n)>>4 & 0xF)), \
+ ('0' + ((n) & 0xF))
+
+/* Construct a string literal encoding the version number components. */
+#ifdef COMPILER_VERSION_MAJOR
+char const info_version[] = {
+ 'I', 'N', 'F', 'O', ':',
+ 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[',
+ COMPILER_VERSION_MAJOR,
+# ifdef COMPILER_VERSION_MINOR
+ '.', COMPILER_VERSION_MINOR,
+# ifdef COMPILER_VERSION_PATCH
+ '.', COMPILER_VERSION_PATCH,
+# ifdef COMPILER_VERSION_TWEAK
+ '.', COMPILER_VERSION_TWEAK,
+# endif
+# endif
+# endif
+ ']','\0'};
+#endif
+
+/* Construct a string literal encoding the internal version number. */
+#ifdef COMPILER_VERSION_INTERNAL
+char const info_version_internal[] = {
+ 'I', 'N', 'F', 'O', ':',
+ 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','_',
+ 'i','n','t','e','r','n','a','l','[',
+ COMPILER_VERSION_INTERNAL,']','\0'};
+#endif
+
+/* Construct a string literal encoding the version number components. */
+#ifdef SIMULATE_VERSION_MAJOR
+char const info_simulate_version[] = {
+ 'I', 'N', 'F', 'O', ':',
+ 's','i','m','u','l','a','t','e','_','v','e','r','s','i','o','n','[',
+ SIMULATE_VERSION_MAJOR,
+# ifdef SIMULATE_VERSION_MINOR
+ '.', SIMULATE_VERSION_MINOR,
+# ifdef SIMULATE_VERSION_PATCH
+ '.', SIMULATE_VERSION_PATCH,
+# ifdef SIMULATE_VERSION_TWEAK
+ '.', SIMULATE_VERSION_TWEAK,
+# endif
+# endif
+# endif
+ ']','\0'};
+#endif
+
+/* Construct the string literal in pieces to prevent the source from
+ getting matched. Store it in a pointer rather than an array
+ because some compilers will just produce instructions to fill the
+ array rather than assigning a pointer to a static array. */
+char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]";
+char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]";
+
+
+
+
+#if defined(__INTEL_COMPILER) && defined(_MSVC_LANG) && _MSVC_LANG < 201403L
+# if defined(__INTEL_CXX11_MODE__)
+# if defined(__cpp_aggregate_nsdmi)
+# define CXX_STD 201402L
+# else
+# define CXX_STD 201103L
+# endif
+# else
+# define CXX_STD 199711L
+# endif
+#elif defined(_MSC_VER) && defined(_MSVC_LANG)
+# define CXX_STD _MSVC_LANG
+#else
+# define CXX_STD __cplusplus
+#endif
+
+const char* info_language_dialect_default = "INFO" ":" "dialect_default["
+#if CXX_STD > 201703L
+ "20"
+#elif CXX_STD >= 201703L
+ "17"
+#elif CXX_STD >= 201402L
+ "14"
+#elif CXX_STD >= 201103L
+ "11"
+#else
+ "98"
+#endif
+"]";
+
+/*--------------------------------------------------------------------------*/
+
+int main(int argc, char* argv[])
+{
+ int require = 0;
+ require += info_compiler[argc];
+ require += info_platform[argc];
+#ifdef COMPILER_VERSION_MAJOR
+ require += info_version[argc];
+#endif
+#ifdef COMPILER_VERSION_INTERNAL
+ require += info_version_internal[argc];
+#endif
+#ifdef SIMULATE_ID
+ require += info_simulate[argc];
+#endif
+#ifdef SIMULATE_VERSION_MAJOR
+ require += info_simulate_version[argc];
+#endif
+#if defined(__CRAYXE) || defined(__CRAYXC)
+ require += info_cray[argc];
+#endif
+ require += info_language_dialect_default[argc];
+ (void)argv;
+ return require;
+}
diff --git a/c++/src/py2gpmbind.cpp b/c++/src/py2gpmbind.cpp
index 46484c6f3c0e7a88995a25934a4fd2d4126b851e..c7c8715e07b1f033071162f88ec7619a34f45193 100644
--- a/c++/src/py2gpmbind.cpp
+++ b/c++/src/py2gpmbind.cpp
@@ -26,5 +26,6 @@ PYBIND11_MODULE(invKin, m) {
.def("set_additionalManipConstraint", &Yumi::set_additionalManipConstraint)
.def("set_nullspaceWeight", &Yumi::set_nullspaceWeight)
.def("set_driftCompGain", &Yumi::set_driftCompGain)
+ .def("set_sampleTime", &Yumi::set_sampleTime)
.def("set_force", &Yumi::set_force);
}
diff --git a/c++/src/yumi.cpp b/c++/src/yumi.cpp
index 235b4b559dbaffc3c51aa8a38dda1d76c3acb5db..798e611730230f9b6795e6c540e2a0b9690789a3 100644
--- a/c++/src/yumi.cpp
+++ b/c++/src/yumi.cpp
@@ -30,7 +30,7 @@ void Yumi::doForwardKinematics(){
rl::math::Transform t = kinematic->getOperationalPosition(0);
m_position = t.translation();
m_orientation = t.rotation().eulerAngles(2, 1, 0).reverse();
- m_rotationMatrix = t.rotation(); // rotation from world frame to ee frame
+ m_rotationMatrix = t.rotation(); // rotation from task frame to ee frame
m_jacobian = kinematic->getJacobian();
m_manipulabilty = kinematic->calculateManipulabilityMeasure();
}
@@ -76,11 +76,12 @@ void Yumi::compTaskSpaceInput(){
}
// calculate delta between quaternions
Eigen::Quaterniond errorQuaternion = currentOrientation.inverse() * desiredOrientation;
- Eigen::Vector3d errorRotationInWorldFrame = currentOrientation * errorQuaternion.vec();
+ Eigen::Vector3d errorRotationInTaskFrame = currentOrientation * errorQuaternion.vec();
- m_effectiveTaskSpaceInput.head(3) = m_modSelectVelMatrix * (m_driftCompGain/m_sampleTime * (m_desPosition - m_position) + m_desPositionDot)
+ // rotate the profile into ee frame, then select the correct directions for position controll and then transform back to world frame
+ m_effectiveTaskSpaceInput.head(3) = m_rotationMatrix.transpose() * m_modSelectVelMatrix * m_rotationMatrix*(m_driftCompGain/m_sampleTime * (m_desPosition - m_position) + m_desPositionDot)
+ m_forceTaskSpaceInput;
- m_effectiveTaskSpaceInput.tail(3) = m_driftCompGain/m_sampleTime * errorRotationInWorldFrame + m_desOrientationDot;
+ m_effectiveTaskSpaceInput.tail(3) = m_driftCompGain/m_sampleTime * errorRotationInTaskFrame + m_desOrientationDot;
}
@@ -120,10 +121,7 @@ void Yumi::compForce2VelocityController(){
velocityEE << 0, m_force-m_forceOP, 0;
velocityEE *= m_kp;
velocityEE = (Eigen::Matrix3d::Identity() - m_modSelectVelMatrix) * velocityEE; // perform blending - transition from position control to force control
- //std::cout << "selectionmatrix force: " << (Eigen::Matrix3d::Identity() - m_modSelectVelMatrix) << std::endl;
- //std::cout << "selectionmatrix velocity: " << m_modSelectVelMatrix << std::endl;
- //std::cout << "velocityEE force: " << velocityEE << std::endl;
- // transform the velocities computed in the ee frame to the world frame
+ // transform the velocities computed in the ee frame to the task space
m_forceTaskSpaceInput = m_rotationMatrix.transpose() * velocityEE;
}
diff --git a/python/abb_egm_pyclient/README.md b/python/abb_egm_pyclient/README.md
index e48bab27094e32b3cc9034d1716735d0d753c27d..4608eb97e23bc9c062281d23aeb13b1881775c87 100644
--- a/python/abb_egm_pyclient/README.md
+++ b/python/abb_egm_pyclient/README.md
@@ -1,38 +1,94 @@
-# abb_egm_pyclient
+# Installation instructions
+## C++
+The project needs a few dependencies to be installed. These are broccoli, the robotics library as well as pybind11 in order to create bindings between Python and C++ code. To prevent any troubles while installing them in the next step make sure you have the following packages installed. Check or otherwise install them running `sudo apt install cmake python3-dev python3-distutils` in the command line.
-Tested on RobotWare version:
+Download eigen and put them into your `/opt/` folder.
+### pybind11
+There are several ways how to use pybind11. Number 1 was tested succesfully and is recommanded
-- 6.08.01.
+1. Inlcude pybind11 as submodule
+Within the `c++` folder run
+```
+git submodule add -b stable https://github.com/pybind/pybind11 deps/pybind11
+git submodule update --init
+```
-## Setup
+2. Run the following commands to install pybind11 globally to your system
+```
+# Classic CMake
+cd pybind11
+mkdir build
+cd build
+cmake ..
+make install
-1. Clone this repo.
-2. Install `protoc` from protobuf. There are [prebuilt binaries available from
- GitHub](https://github.com/protocolbuffers/protobuf/releases/latest) if `protoc` is not available in your package manager.
- 3.Find `egm.proto` in either
- `%LOCALAPPDATA%\ABB\RobotWare\RobotWare_6.XXXXX\utility\Template\EGM\egm.proto`
- or on the robot.
+# CMake 3.15+
+cd pybind11
+cmake -S . -B build
+cmake --build build -j 2 # Build on 2 cores
+cmake --install build
+```
+Installation instruction for pybind11 can be also found [here](https://pybind11.readthedocs.io/en/stable/compiling.html#building-with-cmake)
-3. Run protoc to generate protobuf classes for python. Substitute `$SRC_DIR` for
- the location of `egm.proto`
+### broccoli
+Broccoli can be directly installed to the system (tested and works) or included as an external project. To install broccoli directly to the system clone the respective repo using git. Then cd in cloned brocolli repo and run the following commands to install brocolli to your system
+```
+mkdir build
+cd build
+cmake ..
+make install
+```
+Broccoli and installation instruction can be found [here](https://gitlab.control.lth.se/gosda/broccoli-library).
-```bash
-protoc --python_out=abb_egm_pyclient $SRC_DIR/egm.proto
+### Robotics Library
+To install the whole robotics library to your system run:
```
+sudo apt-add-repository ppa:roblib/ppa
+sudo apt-get update
+sudo apt-get install librl librl-demos librl-examples
+sudo apt-get install librl-dev
+sudo apt-get install librl-extras librl-doc librl-dbg
-4. Install this package in your environment of choice.
+```
+Official installation instruction can be found [here](https://www.roboticslibrary.org/tutorials/install-ubuntu/).
-```bash
-cd abb_egm_pyclient
-pip install -e .
+### Build the shared library and put it into correct directory to import it in Python
+```
+cd c++/build
+cmake ..
+make
+cp *.so ../../python/libs/
```
-## Usage
-Run the EGM on your robot controller. [Here are some instructions if needed](https://gitlab.control.lth.se/tetov/abb_egm_instructions).
+## Python
+Follow the steps to create a virtual environment and install the neccessary dependencies:
+
+1. Create a directory for your virtual environment within the python folder
+2. Create a virtual environment running `python3 -m venv ./myvenv`
+3. Activate the virtual environment running `source myvenv/bin/activate`
+4. Install the dependencies for the abb-egm-client:
+
+- Install protoc from protobuf. There are prebuilt binaries available from
+GitHub if protoc is not available in your package manager.
+- Find egm.proto in either %LOCALAPPDATA%\ABB\RobotWare\RobotWare_6.XXXXX\utility\Template\EGM\egm.proto or on the robot.
-And on your computer (and your python environment):
-```bash
-python -m abb_egm_pyclient.run --help
+Run protoc to generate protobuf classes for python. Substitute `$SRC_DIR` for the location of `egm.proto`
+
+`protoc --python_out=abb_egm_pyclient $SRC_DIR/egm.proto`
+
+Install this package in your environment of choice.
+
+```
+cd python/abb_egm_pyclient
+pip install -e .
+```
+
+5. Install all other dependencies with `pip install -r requirements.txt`
+
+6. Install the `python/data` folder as a module
+```
+cd python
+pip install -e .
```
diff --git a/python/abb_egm_pyclient/abb_egm_pyclient/feedback/control_final.py b/python/abb_egm_pyclient/abb_egm_pyclient/feedback/control_final.py
index 195c1f5a087b792f5a13745378cd3d5999c4e5e0..85afe401a40f47b7b8d6d27043c462d5d27bca97 100644
--- a/python/abb_egm_pyclient/abb_egm_pyclient/feedback/control_final.py
+++ b/python/abb_egm_pyclient/abb_egm_pyclient/feedback/control_final.py
@@ -41,10 +41,11 @@ p1, p2 = place_trajectory(p1_start_des, p1, p2)
# setup for UDP connection
UDP_PORT_LEFT = 6510
UDP_PORT_RIGHT = 6511
-rate = 80
+
# take the first desired taskSpaceInput for the right arm and simulate force sensor data
#translation_R_start = copy.copy(p2[0, :])
+#syncTranslation_R = np.array([0.3, -0.18, 0.2]) # starting pose synched with robot
syncTranslation_R = np.array([0.3, -0.2, 0.2]) # starting pose synched with robot
#rotation_R_start = copy.copy(phi_delta[0, :])
syncRotation_R = np.array([0, 0, 0])
@@ -60,15 +61,18 @@ desPose_L_start = np.concatenate((p1[0, :], phi_delta[0, :]), axis=0)
egm_client_R = EGMClient(port=UDP_PORT_RIGHT)
egm_client_L = EGMClient(port=UDP_PORT_LEFT)
+rate = 80.0
yumi_right = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
yumi_right.set_operationPoint(4.0)
yumi_right.set_kp(0.05)
yumi_right.set_hybridControl(False)
yumi_right.set_transitionTime(3.0)
yumi_right.set_additionalManipConstraint(True)
+yumi_right.set_sampleTime(1.0/rate)
yumi_left = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf")
yumi_left.set_additionalManipConstraint(True)
+yumi_left.set_sampleTime(1.0/rate)
desVelocities_R_start = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
jointVelocities_R = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
@@ -137,6 +141,8 @@ log_compJoints_L = np.zeros((traj_samples, 7))
log_force = np.zeros((traj_samples, 1))
+log_compTime = np.zeros((traj_samples, 1))
+
print("\n Force control only to tension the wire...")
foamCutting.start_syncronizing()
@@ -144,10 +150,10 @@ force = 0
i = 0
while True and arduino.isOpen():
# check for new force data
- if arduino.in_waiting: # get the number of bytes in the input buffer
+ """ if arduino.in_waiting: # get the number of bytes in the input buffer
packet = arduino.readline() # type: bytes
str_receive = packet.decode('utf-8').rstrip('\n')
- force = float(str_receive)/1000.0 # mN to Newton
+ force = float(str_receive)/1000.0 # mN to Newton """
if foamCutting.is_moveToStartPose:
if (time.time() - timestamp) >= (1.0/rate):
@@ -196,7 +202,7 @@ while True and arduino.isOpen():
elif (foamCutting.is_mountWire):
if keyboard.is_pressed('enter'):
- yumi_right.set_hybridControl(True)
+ #yumi_right.set_hybridControl(True)
foamCutting.start_tensioning()
@@ -240,8 +246,8 @@ while True and arduino.isOpen():
jointAngles_L_old = copy.copy(jointAngles_L)
i = i+1
- if (force > 3) and keyboard.is_pressed('enter'):
- #if keyboard.is_pressed('enter'):
+ #if (force > 3.6) and keyboard.is_pressed('enter'):
+ if keyboard.is_pressed('enter'):
i = 0 # reset counter
print("Changing to hybrid control now...")
foamCutting.start_cutting()
@@ -315,6 +321,10 @@ while True and arduino.isOpen():
jointAngles_R_old = copy.copy(jointAngles_R)
i = i + 1
pbar.update(1)
+ log_compTime[i, :] = time.time() - timestamp
+
+ if keyboard.is_pressed('s'):
+ break
if (i >= (traj_samples-1)):
pbar.close()
@@ -323,7 +333,7 @@ while True and arduino.isOpen():
# check if end pose for right arm has been reached
-n = 0
+""" n = 0
while n < 10:
robot_msg_R = egm_client_R.receive_msg()
if robot_msg_R.mciConvergenceMet:
@@ -348,11 +358,11 @@ while n < 10:
n += 1
time.sleep(0.1)
else:
- raise TimeoutError(f"Joint positions for the left arm did not converge.")
+ raise TimeoutError(f"Joint positions for the left arm did not converge.") """
-experimentLogs = np.hstack((p2, phi_delta, log_compPose_R, log_realPose_R, log_compJoints_R, log_realJoints_R, log_force, p1, log_compJoints_R, log_realPose_L, log_compJoints_L, log_realJoints_L, log_abbPos_R, log_abbPos_L))
+experimentLogs = np.hstack((p2, phi_delta, log_compPose_R, log_realPose_R, log_compJoints_R, log_realJoints_R, log_force, p1, log_compJoints_R, log_realPose_L, log_compJoints_L, log_realJoints_L, log_abbPos_R, log_abbPos_L, log_compTime))
#experimentLogs = np.hstack((p2, phi_delta, log_realPose_R, p1, log_realPose_L))
-#np.save('./plots/force_control/refKp0.05-Op4_v3', experimentLogs)
\ No newline at end of file
+#np.save('./plots/control_positionv2/v2-300-351015_S900', experimentLogs)
\ No newline at end of file
diff --git a/python/abb_egm_pyclient/abb_egm_pyclient/feedback/stateMachine.py b/python/abb_egm_pyclient/abb_egm_pyclient/feedback/stateMachine.py
index 4bd8b825f594b4aac519cebfae9b6183fcf9de06..44740e99efb37f0740e1407f2b5857c4838a12f8 100644
--- a/python/abb_egm_pyclient/abb_egm_pyclient/feedback/stateMachine.py
+++ b/python/abb_egm_pyclient/abb_egm_pyclient/feedback/stateMachine.py
@@ -1,9 +1,6 @@
from statemachine import StateMachine, State
class FoamCuttingMachine(StateMachine):
- # green = State('Green', initial=True)
- # yellow = State('Yellow')
- # red = State('Red')
# define states
standby = State('Standby', initial=True)
@@ -33,14 +30,3 @@ class FoamCuttingMachine(StateMachine):
def on_enter_standby(self):
print('State changed to standby')
-
-
- # slowdown = green.to(yellow)
- # stop = yellow.to(red)
- # go = red.to(green)
-
- # states
- # standby
- # moveToStartPose
- # tensionWire
- # cutting
\ No newline at end of file
diff --git a/python/experiment/plots.py b/python/experiment/plots.py
index d40fc05639b10927a3201bfd3715b21b5d2f4807..549ce464231a3180042a1b8d05c70b1802ec10e0 100644
--- a/python/experiment/plots.py
+++ b/python/experiment/plots.py
@@ -34,7 +34,27 @@ ax1.set_xlabel('samples')
plt.show()
+y = data2[2500:]
+n = len(y) # length of the signal
+k = np.arange(n)
+T = n/80
+frq = k/T # two sides frequency range
+frq = frq[:len(frq)//2] # one side frequency range
+
+Y = np.fft.fft(y)/n # dft and normalization
+Y = Y[:n//2]
+
+### new
+fig = plt.figure()
+
+ax3 = fig.add_subplot(111)
+ax3.plot(frq, abs(Y), label='noise with const weight')
+#ax3.plot(frq015, abs(Y015), label='$K_p=0.15$ ')
+ax3.set_xlabel('freq in Hz')
+ax3.set_ylabel('magnitude in dB')
+plt.xscale('log')
+ax3.legend()
diff --git a/python/optimizeTaskSpacePlacement/checkManipulabiltyMeasure.py b/python/optimizeTaskSpacePlacement/checkManipulabiltyMeasure.py
index 98709b9c532b201410941b309b5d4801bd81c281..c9d63911b3b6abacc61831ab24eac9d4e81c1bf8 100644
--- a/python/optimizeTaskSpacePlacement/checkManipulabiltyMeasure.py
+++ b/python/optimizeTaskSpacePlacement/checkManipulabiltyMeasure.py
@@ -137,29 +137,9 @@ for x in range(-20, 21, 5):
else:
smallestMan.append(np.min(manipulability_L))
-#print("manipulability measure min right: ", manipulabilityMin_R)
-#print("manipulability measure min left: ", manipulabilityMin_L)
-
plot_path = '/home/joschua/Coding/forceControl/master-project/python/plots/taskSpacePlacement/'
np.save(plot_path +'myX_L_nullgradient', myX_L)
np.save(plot_path +'myY_L_nullgradient', myY_L)
np.save(plot_path +'myZ_L_nullgradient', myZ_L)
np.save(plot_path +'smallestMan_nullgradient', smallestMan)
-
-""" def scatter3d(x,y,z, cs, colorsMap='jet'):
- cm = plt.get_cmap(colorsMap)
- cNorm = matplotlib.colors.Normalize(vmin=min(cs), vmax=max(cs))
- scalarMap = cmx.ScalarMappable(norm=cNorm, cmap=cm)
- fig = plt.figure()
- ax = Axes3D(fig)
- ax.scatter(x, y, z, c=scalarMap.to_rgba(cs))
- scalarMap.set_array(cs)
- fig.colorbar(scalarMap)
- plt.show() """
-
-
-#scatter3d(np.array(myX_R), np.array(myY_R), np.array(myZ_R), np.array(myMan_R))
-#scatter3d(np.array(myX_L), np.array(myY_L), np.array(myZ_L), np.array(myMan_L))
-#scatter3d(np.array(myX_L), np.array(myY_L), np.array(myZ_L), np.array(myMan_L)+np.array(myMan_R))
-# do not add, but search iterate through and choose index where both array entries are higher
diff --git a/python/plots/control_position/plotting.py b/python/plots/control_position/plotting.py
index 0240844a762979583a9908caf235105db49f483c..823f880dd61ad307b44ee10be0354956f3bde1a5 100644
--- a/python/plots/control_position/plotting.py
+++ b/python/plots/control_position/plotting.py
@@ -1,10 +1,12 @@
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.gridspec import GridSpec
+from matplotlib.patches import Rectangle
R_01_80 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01.npy')
R_01_250 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01_250.npy')
+R_01_600 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01_600.npy')
''' Only 300-351015 and 300-351015-rawc20_v2 were saved in this format'''
#experimentLogs = np.hstack((p2, phi_delta, log_compPose_R, log_realPose_R, log_compJoints_R, log_realJoints_R, log_force, p1, log_compJoints_R, log_realPose_L, log_compJoints_L, log_realJoints_L))
@@ -32,15 +34,35 @@ p1_isc20 = datac20[:, 15:18]* 1000
ec20 = (p1_desc20 - p1_isc20)
datatc250 = np.load('./plots/control_position/300-351015-rawtc250.npy')
-p1_destc250 = datatc250[:, 12:15]* 1000
+p1_destc250 = datatc250[:, 12:15]* 1000
p1_istc250 = datatc250[:, 15:18]* 1000
# error in workspace
etc250 = (p1_destc250 - p1_istc250)
+datatc250_noM = np.load('./plots/control_position/300-351015-rawtc250_noM.npy')
+p1_destc250_noM = datatc250_noM[:, 33:36]* 1000
+p1_istc250no_M = datatc250_noM[:, 43:46]* 1000
+comptimeno_M = datatc250_noM[:, 69:70]
+# error in workspace
+etc250no_M = (p1_destc250_noM - p1_istc250no_M)
+
+data600 = np.load('./plots/control_position/600-351015-raw.npy')
+p1_des600 = data600[:, 33:36]* 1000
+p1_is600 = data600[:, 43:46]* 1000
+comptime = data600[:, 69:70]
+# error in workspace
+e600 = (p1_des600 - p1_is600)
+
+
+
#noSamples = len(p1_desf80)
noSamples80 = len(ef80)
noSamples250 = len(etc250)
+noSamples600 = len(e600)
+
+for i in range(noSamples600):
+ e600[i, :] = np.transpose(R_01_600[i] @ e600[i, :].T)
# transform error into eef80frame
for i in range(noSamples80):
@@ -50,9 +72,11 @@ for i in range(noSamples80):
for i in range(noSamples250):
etc250[i, :] = np.transpose(R_01_250[i] @ etc250[i, :].T)
+ etc250no_M[i, :] = np.transpose(R_01_250[i] @ etc250no_M[i, :].T)
time80 = np.linspace(0, round(1.0/80.0 * noSamples80), num=noSamples80)
time250 = np.linspace(0, round(1.0/250.0 * noSamples250), num=noSamples250)
+time600 = np.linspace(0, round(1.0/80.0 * noSamples600), num=noSamples600)
# use self-defined tum-cycler for TUM-blue colors
plt.style.use('mylatex')
@@ -66,48 +90,68 @@ plt.rcParams.update({
"savefig.directory": '/home/joschua/Documents/Studium/TUM/Thesis/documentation/thesis/myWorkFiles/AMStudentThesis/figures/plots/'
})
+fig = plt.figure(figsize=(6.25, 2))
+ax1 = fig.add_subplot(111)
+
+ax1.plot(time250 [30:-10], comptimeno_M [30:-10, 0])
+ax1.plot(time600 [30:-10], comptime [30:-10, 0])
+ax1.set_ylabel('comp. time in ms')
+ax1.set_xlabel('time in s')
+plt.show()
+######################300
# error plot
-fig = plt.figure()
+fig = plt.figure(figsize=(6.25, 6))
+
+ax0 = fig.add_subplot(411)
+ax1 = fig.add_subplot(412)
+ax2 = fig.add_subplot(413)
+ax3 = fig.add_subplot(414)
+
-ax1 = fig.add_subplot(311)
-ax2 = fig.add_subplot(312)
-ax3 = fig.add_subplot(313)
ax1.plot(time80 [30:-10], e [30:-10, 0])
-ax1.plot(time80 [30:-10], ec20 [30:-10, 0], linestyle='dashed')
-ax1.plot(time80 [30:-10], ef80 [30:-10, 0], linestyle='dotted')
+ax1.plot(time80 [30:-10], ec20 [30:-10, 0])
+ax1.plot(time80 [30:-10], ef80 [30:-10, 0])
ax1.plot(time250[30:-10], etc250[30:-10, 0])
+ax1.plot(time250[30:-10], etc250no_M[30:-10, 0])
+ax1.set_ylabel('$x_{EE,R}$ in mm')
+ax1.add_patch(Rectangle((20, 0.78), 2.5, 0.34,
+ edgecolor = 'black',
+ fill=False,
+ zorder=2,
+ lw=1))
+
+ax2.plot(time80 [30:-10], e [30:-10, 0])
+ax2.plot(time80 [30:-10], ec20 [30:-10, 0])
+ax2.plot(time80 [30:-10], ef80 [30:-10, 0])
+ax2.plot(time250[30:-10], etc250[30:-10, 0])
+ax2.plot(time250[30:-10], etc250no_M[30:-10, 0])
+ax2.set_ylabel('$x_{EE,R}$ in mm')
+ax2.set_xlim([20, 22.5])
+ax2.set_ylim([0.78, 1.12])
-ax2.plot(time80 [30:-10], e [30:-10, 1])
-ax2.plot(time80 [30:-10], ec20 [30:-10, 1], linestyle='dashed')
-ax2.plot(time80 [30:-10], ef80 [30:-10, 1], linestyle='dotted')
-ax2.plot(time250[30:-10], etc250[30:-10, 1])
ax3.plot(time80 [30:-10], e [30:-10, 2])
-ax3.plot(time80 [30:-10], ec20 [30:-10, 2], linestyle='dashed')
-ax3.plot(time80 [30:-10], ef80 [30:-10, 2], linestyle='dotted')
+ax3.plot(time80 [30:-10], ec20 [30:-10, 2])
+ax3.plot(time80 [30:-10], ef80 [30:-10, 2])
ax3.plot(time250[30:-10], etc250[30:-10, 2])
+ax3.plot(time250[30:-10], etc250no_M[30:-10, 2])
+ax0.plot(time600[30:-10], e600[30:-10, 0],color=next(ax3._get_lines.prop_cycler)['color'])
+ax0.set_ylabel('$x_{EE,R}$ in mm')
-#ax4 = ax2.twinx()
-#ax4.plot(time[0:-10], force[0:-10], label='force', color='r')
-#ax4.set_ylim(-1, 1)
-#ax4.set_ylabel('N')
-
-#ax1.set_title('position error in EE frame')
-ax1.set_ylabel('$x_{EE,R}$ in mm')
-ax2.set_ylabel('$y_{EE,R}$ in mm')
ax3.set_ylabel('$z_{EE,R}$ in mm')
ax3.set_xlabel('time in s')
-labels = ["case 1", "case 2", "case 3", "case 4"]
+labels = ["Case 6", "Case 1", "Case 2", "Case 3", "Case 4", "Case 5"]
fig.legend( labels=labels,
- loc="upper center", ncol=4)
+ loc="upper center", ncol=6)
#fig.subplots_adjust(top=0.9, hspace=0.3)
fig.tight_layout()
fig.subplots_adjust(top=0.9)
plt.show()
+
# profile plot
""" fig = plt.figure() """
""" """
@@ -127,23 +171,39 @@ ax1.plot(p1_is [30:-10, 0], p1_is [30:-10, 2])
ax1.plot(p1_isc20 [30:-10, 0], p1_isc20 [30:-10, 2])
ax1.plot(p1_isf80 [30:-10, 0], p1_isf80 [30:-10, 2])
ax1.plot(p1_istc250 [30:-10, 0], p1_istc250 [30:-10, 2])
-ax1.plot(p1_des [30:-10, 0], p1_des [30:-10, 2], linestyle='dotted')
+ax1.plot(p1_istc250no_M [30:-10, 0], p1_istc250no_M [30:-10, 2])
+ax1.plot(p1_is600 [30:-10, 0], p1_is600 [30:-10, 2])
+ax1.plot(p1_des [30:-10, 0], p1_des [30:-10, 2], linestyle='dotted', color='#808080')
+ax1.add_patch(Rectangle((357.5, 146.5), 6.5, 6.5,
+ edgecolor = 'black',
+ #facecolor = 'blue',
+ fill=False,
+ lw=1))
+ax1.add_patch(Rectangle((514.5, 150.5), 6.5, 6.5,
+ edgecolor = 'black',
+ #facecolor = 'blue',
+ fill=False,
+ lw=1))
ax2.plot(p1_is [30:-10, 0], p1_is [30:-10, 2])
ax2.plot(p1_isc20 [30:-10, 0], p1_isc20 [30:-10, 2])
ax2.plot(p1_isf80 [30:-10, 0], p1_isf80 [30:-10, 2])
ax2.plot(p1_istc250 [30:-10, 0], p1_istc250 [30:-10, 2])
-ax2.plot(p1_des [30:-10, 0], p1_des [30:-10, 2], linestyle='dotted')
-ax2.set_ylim([147.5, 152])
-ax2.set_xlim([359, 361])
+ax2.plot(p1_istc250no_M [30:-10, 0], p1_istc250no_M [30:-10, 2])
+ax2.plot(p1_is600 [30:-10, 0], p1_is600 [30:-10, 2])
+ax2.plot(p1_des [30:-10, 0], p1_des [30:-10, 2], linestyle='dotted', color='#808080')
+ax2.set_ylim([146.5, 153])
+ax2.set_xlim([357.5, 364])
ax3.plot(p1_is [30:-10, 0], p1_is [30:-10, 2])
ax3.plot(p1_isc20 [30:-10, 0], p1_isc20 [30:-10, 2])
ax3.plot(p1_isf80 [30:-10, 0], p1_isf80 [30:-10, 2])
ax3.plot(p1_istc250 [30:-10, 0], p1_istc250 [30:-10, 2])
-ax3.plot(p1_des [30:-10, 0], p1_des [30:-10, 2], linestyle='dotted')
-ax3.set_xlim([519, 520.4])
-ax3.set_ylim([153, 153.6])
+ax3.plot(p1_istc250no_M [30:-10, 0], p1_istc250no_M [30:-10, 2])
+ax3.plot(p1_is600 [30:-10, 0], p1_is600 [30:-10, 2])
+ax3.plot(p1_des [30:-10, 0], p1_des [30:-10, 2], linestyle='dotted', color='#808080')
+ax3.set_xlim([514.5, 521])
+ax3.set_ylim([150.5, 157])
ax1.set_ylabel('$z$ in mm')
ax2.set_ylabel('$z$ in mm')
@@ -151,7 +211,7 @@ ax1.set_xlabel('$x$ in mm')
ax2.set_xlabel('$x$ in mm')
ax3.set_xlabel('$x$ in mm')
-labels = ["case 1", "case 2", "case 3", "case 4", "reference"]
+labels = ["Case 1", "Case 2", "Case 3", "Case 4", "Case 5", "Case 6", "reference"]
fig.legend( labels=labels,
loc="upper center", ncol=5)
#fig.subplots_adjust(top=0.9, hspace=0.3)
diff --git a/python/plots/control_positionv2/plotting.py b/python/plots/control_positionv2/plotting.py
new file mode 100644
index 0000000000000000000000000000000000000000..70f1198e1d73a8954d3fd82ec5d450924ca2310f
--- /dev/null
+++ b/python/plots/control_positionv2/plotting.py
@@ -0,0 +1,246 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.gridspec import GridSpec
+from matplotlib.patches import Rectangle
+import pandas as pd
+
+
+R_01_80 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01.npy')
+R_01_250 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01_250.npy')
+R_01_600 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01_600.npy')
+R_01_900 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01_900.npy')
+
+''' Only 300-351015 and 300-351015-rawc20_v2 were saved in this format'''
+#experimentLogs = np.hstack((p2, phi_delta, log_compPose_R, log_realPose_R, log_compJoints_R, log_realJoints_R, log_force, p1, log_compJoints_R, log_realPose_L, log_compJoints_L, log_realJoints_L))
+datao = np.load('./plots/control_positionv2/v2-300-351015_default.npy') # o for original
+p1_des = datao[:, 33:36]* 1000
+p1_is = datao[:, 43:46]* 1000
+#p2_des = datao[:, 0:3]* 1000
+#p2_is = datao[:, 6:9]* 1000
+e = (p1_des - p1_is)
+
+''' Measurements were saved in this format '''
+#p2, phi_delta, log_realPose_R, p1, log_realPose_L))
+dataf80 = np.load('./plots/control_positionv2/v2-300-351015_L100.npy')
+p1_desf80 = dataf80[:, 33:36]* 1000
+p1_isf80 = dataf80[:, 43:46]* 1000
+#p1_desf80 = dataf80[:, 0:3]* 1000
+#p1_isf80 = dataf80[:, 6:9]* 1000
+# error in workspace
+ef80 = (p1_desf80 - p1_isf80)
+
+datac20 = np.load('./plots/control_positionv2/v2-300-351015_P20.npy')
+p1_desc20 = datac20[:, 33:36]* 1000
+p1_isc20 = datac20[:, 43:46]* 1000
+# error in workspace
+ec20 = (p1_desc20 - p1_isc20)
+
+datatc250 = np.load('./plots/control_positionv2/v2-300-351015_C250.npy')
+p1_destc250 = datatc250[:, 33:36]* 1000
+p1_istc250 = datatc250[:, 43:46]* 1000
+comptime = np.squeeze(datatc250[:, 69:70].reshape(1, len(datatc250)))*1000
+comptime_smooth= pd.Series(comptime).rolling(window=250).mean()
+# error in workspace
+etc250 = (p1_destc250 - p1_istc250)
+
+datatc250_noM = np.load('./plots/control_positionv2/v2-300-351015_C250_noM.npy')
+p1_destc250_noM = datatc250_noM[:, 33:36]* 1000
+p1_istc250no_M = datatc250_noM[:, 43:46]* 1000
+comptimeno_M = np.squeeze(datatc250_noM[:, 69:70].reshape(1, len(datatc250_noM)))
+comptimeno_M_smooth= pd.Series(comptimeno_M).rolling(window=250).mean()
+# error in workspace
+etc250no_M = (p1_destc250_noM - p1_istc250no_M)
+
+data600 = np.load('./plots/control_positionv2/v2-300-351015_S600.npy')
+p1_des600 = data600[:, 33:36]* 1000
+p1_is600 = data600[:, 43:46]* 1000
+
+# error in workspace
+e600 = (p1_des600 - p1_is600)
+
+data900 = np.load('./plots/control_positionv2/v2-300-351015_S900.npy')
+p1_des900 = data900[:, 33:36]* 1000
+p1_is900 = data900[:, 43:46]* 1000
+
+# error in workspace
+e900 = (p1_des900 - p1_is900)
+
+
+
+
+#noSamples = len(p1_desf80)
+noSamples80 = len(ef80)
+noSamples250 = len(etc250)
+noSamples600 = len(e600)
+noSamples900 = len(e900)
+
+
+
+for i in range(noSamples900):
+ e900[i, :] = np.transpose(R_01_900[i] @ e900[i, :].T)
+
+for i in range(noSamples600):
+ e600[i, :] = np.transpose(R_01_600[i] @ e600[i, :].T)
+
+# transform error into eef80frame
+for i in range(noSamples80):
+ e[i, :] = np.transpose(R_01_80[i] @ e[i, :].T)
+ ef80[i, :] = np.transpose(R_01_80[i] @ ef80[i, :].T)
+ ec20[i, :] = np.transpose(R_01_80[i] @ ec20[i, :].T)
+
+for i in range(noSamples250):
+ etc250[i, :] = np.transpose(R_01_250[i] @ etc250[i, :].T)
+ etc250no_M[i, :] = np.transpose(R_01_250[i] @ etc250no_M[i, :].T)
+
+time80 = np.linspace(0, round(1.0/80.0 * noSamples80), num=noSamples80)
+time250 = np.linspace(0, round(1.0/250.0 * noSamples250), num=noSamples250)
+time600 = np.linspace(0, round(1.0/80.0 * noSamples600), num=noSamples600)
+time900 = np.linspace(0, round(1.0/80.0 * noSamples900), num=noSamples900)
+
+# use self-defined tum-cycler for TUM-blue colors
+plt.style.use('mylatex')
+
+# plotting for thesis
+plt.rcParams.update({
+ "font.family": "serif", # use serif/main font for text elements
+ "text.usetex": True, # use inline math for ticks
+ "pgf.rcfonts": False, # don't setup fonts from rc parameters
+ #"legend.loc": 'upper right',
+ "savefig.directory": '/home/joschua/Documents/Studium/TUM/Thesis/documentation/thesis/myWorkFiles/AMStudentThesis/figures/plots/'
+ })
+
+fig = plt.figure(figsize=(6, 2))
+ax1 = fig.add_subplot(111)
+
+#ax1.plot(time250 [30:-10], comptimeno_M [30:-10], label='off')
+ax1.plot(time250 [30:-10], comptime [30:-10], label='on')
+ax1.set_ylabel('comp. time in ms')
+ax1.set_xlabel('time in s')
+fig.tight_layout()
+#ax1.legend()
+plt.show()
+######################300
+# error plot
+fig = plt.figure(figsize=(6, 6.5))
+
+
+ax1 = fig.add_subplot(411)
+ax0 = fig.add_subplot(412)
+ax2 = fig.add_subplot(413)
+ax3 = fig.add_subplot(414)
+
+
+
+ax1.plot(time80 [30:-10], e [30:-10, 0])
+#ax1.plot(time80 [30:-10], ec20 [30:-10, 0])
+#ax1.plot(time80 [30:-10], ef80 [30:-10, 0])
+ax1.plot(time250[30:-10], etc250[30:-10, 0])
+#ax1.plot(time250[30:-10], etc250no_M[30:-10, 0])
+ax1.set_ylabel('$x_{EE,L}$ in mm')
+#ax1.add_patch(Rectangle((20, 0.78), 2.5, 0.34,
+# edgecolor = 'black',
+# fill=False,
+# zorder=2,
+# lw=1))
+
+ax2.plot(time80 [30:-10], e [30:-10, 1])
+#ax2.plot(time80 [30:-10], ec20 [30:-10, 0])
+#ax2.plot(time80 [30:-10], ef80 [30:-10, 0])
+ax2.plot(time250[30:-10], etc250[30:-10, 1])
+#ax2.plot(time250[30:-10], etc250no_M[30:-10, 0])
+ax2.set_ylabel('$y_{EE,L}$ in mm')
+#x2.set_xlim([20, 22.5])
+ax2.set_ylim([-0.2, 0.2])
+
+
+ax3.plot(time80 [30:-10], e [30:-10, 2])
+#ax3.plot(time80 [30:-10], ec20 [30:-10, 2])
+#ax3.plot(time80 [30:-10], ef80 [30:-10, 2])
+ax3.plot(time250[30:-10], etc250[30:-10, 2])
+#ax3.plot(time250[30:-10], etc250no_M[30:-10, 2])
+
+ax0.plot(time900[30:-10], e900[30:-10, 0],color=next(ax3._get_lines.prop_cycler)['color'])
+ax0.set_ylabel('$x_{EE,L}$ in mm')
+
+ax3.set_ylabel('$z_{EE,L}$ in mm')
+ax3.set_xlabel('time in s')
+#ax3.set_ylim([-1, 1])
+
+#labels = ["Case 6", "Case 1, 2, 3", "Case 4", "Case 5"]
+labels = [ "Case 1, 2, 3", "Case 4", "Case 5"]
+fig.legend( labels=labels,
+ loc="upper center", ncol=6)
+#fig.subplots_adjust(top=0.9, hspace=0.3)
+fig.tight_layout()
+fig.subplots_adjust(top=0.945)
+plt.show()
+
+
+# profile plot
+""" fig = plt.figure() """
+""" """
+""" ax1 = fig.add_subplot(311) """
+""" ax2 = fig.add_subplot(312) """
+""" ax3 = fig.add_subplot(323) """
+
+fig = plt.figure(constrained_layout=True, figsize=(6, 4))
+gs = GridSpec(2, 2, figure=fig)
+
+ax1 = fig.add_subplot(gs[0, :])
+ax2 = fig.add_subplot(gs[1, 0:1])
+ax3 = fig.add_subplot(gs[1, 1:2])
+
+
+ax1.plot(p1_is [30:-10, 0], p1_is [30:-10, 2], lw=4)
+#ax1.plot(p1_isc20 [30:-10, 0], p1_isc20 [30:-10, 2])
+#ax1.plot(p1_isf80 [30:-10, 0], p1_isf80 [30:-10, 2])
+ax1.plot(p1_istc250 [30:-10, 0], p1_istc250 [30:-10, 2])
+#ax1.plot(p1_istc250no_M [30:-10, 0], p1_istc250no_M [30:-10, 2])
+ax1.plot(p1_is900 [30:-10, 0], p1_is900 [30:-10, 2])
+ax1.plot(p1_des [30:-10, 0], p1_des [30:-10, 2], linestyle='dotted', color='#808080')
+ax1.add_patch(Rectangle((357.5, 146.5), 6.5, 6.5,
+ edgecolor = 'black',
+ #facecolor = 'blue',
+ fill=False,
+ lw=1))
+ax1.add_patch(Rectangle((514.5, 150.5), 6.5, 6.5,
+ edgecolor = 'black',
+ #facecolor = 'blue',
+ fill=False,
+ lw=1))
+ax1.set_xticks([360, 380, 420, 440, 460, 480, 500, 520])
+
+ax2.plot(p1_is [30:-10, 0], p1_is [30:-10, 2], lw=4)
+#ax2.plot(p1_isc20 [30:-10, 0], p1_isc20 [30:-10, 2])
+#ax2.plot(p1_isf80 [30:-10, 0], p1_isf80 [30:-10, 2])
+ax2.plot(p1_istc250 [30:-10, 0], p1_istc250 [30:-10, 2])
+#ax2.plot(p1_istc250no_M [30:-10, 0], p1_istc250no_M [30:-10, 2])
+ax2.plot(p1_is900 [30:-10, 0], p1_is900 [30:-10, 2])
+ax2.plot(p1_des [30:-10, 0], p1_des [30:-10, 2], linestyle='dotted', color='#808080')
+ax2.set_ylim([146.5, 153])
+ax2.set_xlim([357.5, 364])
+
+ax3.plot(p1_is [30:-10, 0], p1_is [30:-10, 2], lw=4)
+#ax3.plot(p1_isc20 [30:-10, 0], p1_isc20 [30:-10, 2])
+#ax3.plot(p1_isf80 [30:-10, 0], p1_isf80 [30:-10, 2])
+ax3.plot(p1_istc250 [30:-10, 0], p1_istc250 [30:-10, 2])
+#ax3.plot(p1_istc250no_M [30:-10, 0], p1_istc250no_M [30:-10, 2])
+ax3.plot(p1_is900 [30:-10, 0], p1_is900 [30:-10, 2])
+ax3.plot(p1_des [30:-10, 0], p1_des [30:-10, 2], linestyle='dotted', color='#808080')
+ax3.set_xlim([514.5, 521])
+ax3.set_ylim([150.5, 157])
+
+ax1.set_ylabel('$z$ in mm')
+ax2.set_ylabel('$z$ in mm')
+ax1.set_xlabel('$x$ in mm')
+ax2.set_xlabel('$x$ in mm')
+ax3.set_xlabel('$x$ in mm')
+
+labels = ["Case 1, 2, 3", "Case 4", "Case 5", "reference"]
+fig.legend( labels=labels,
+ loc="upper center", ncol=5)
+#fig.subplots_adjust(top=0.9, hspace=0.3)
+fig.tight_layout()
+fig.subplots_adjust(top=0.9)
+plt.show()
+
diff --git a/python/plots/cutting/plotting.py b/python/plots/cutting/plotting.py
new file mode 100644
index 0000000000000000000000000000000000000000..e5e0b0edd77e8993b0b25ae9b683117ea8165a96
--- /dev/null
+++ b/python/plots/cutting/plotting.py
@@ -0,0 +1,202 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.gridspec import GridSpec
+from numpy.linalg import norm
+import tikzplotlib
+
+
+
+R_01_80 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01.npy')
+R_01_250 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01_250.npy')
+
+data25 = np.load('./plots/cutting/300-351015-2.5A_0.05_v2_real.npy') # o for original
+#force005 = data25[:, 32:33]-4
+force = data25[:, 32:33]
+p1_des = data25[:, 33:36]* 1000
+p2_des = data25[:, 0:3]* 1000 + np.array([0, -20, 0])
+p1_is = data25[:, 43:46]* 1000
+p2_is = data25[:, 12:15]* 1000
+p2_meas = data25[:, 63:66] + np.array([0, -3.07, 2.678])
+p1_meas = data25[:, 66:69]
+e_f = force - 4.0
+e_p = p2_des - p2_is
+
+data225 = np.load('./plots/cutting/300-351015-2.25A_0.05_v2.npy') # o for original
+force_2 = data225[:, 32:33]
+p1_des_2 = data225[:, 33:36]* 1000
+p2_des_2 = data225[:, 0:3]* 1000 + np.array([0, -20, 0])
+p1_is_2 = data225[:, 43:46]* 1000
+p2_is_2 = data225[:, 12:15]* 1000
+p2_meas_2 = data225[:, 63:66] + np.array([0, -3.07, 2.678])
+p1_meas_2 = data225[:, 66:69]
+e_f_2 = force_2 - 4.0
+e_p_2 = p2_des_2 - p1_is_2
+
+noSamples80 = len(e_p)
+
+
+# transform error into eef80frame
+for i in range(noSamples80):
+ e_p[i, :] = np.transpose(R_01_80[i] @ e_p[i, :].T)
+
+
+time80 = np.linspace(0, round(1.0/80.0 * noSamples80), num=noSamples80)
+
+distance = norm(p1_is - p2_is, axis=1)
+distance_meas = norm(p1_meas - p2_meas, axis=1)
+distance_meas2 = norm(p1_meas_2 - p2_meas_2, axis=1)
+
+
+y = e_f[10:1000]
+
+n = len(y) # length of the signal
+k = np.arange(n)
+T = n/80
+frq = k/T # two sides frequency range
+frq = frq[:len(frq)//2] # one side frequency range
+
+Y = np.fft.fft(y)/n # dft and normalization
+Y = Y[:n//2]
+
+# use self-defined tum-cycler for TUM-blue colors
+plt.style.use('mylatex')
+
+# plotting for thesis
+plt.rcParams.update({
+ "font.family": "serif", # use serif/main font for text elements
+ "text.usetex": True, # use inline math for ticks
+ "pgf.rcfonts": False, # don't setup fonts from rc parameters
+ #"legend.loc": 'upper right',
+ "savefig.directory": '/home/joschua/Documents/Studium/TUM/Thesis/documentation/thesis/myWorkFiles/AMStudentThesis/figures/plots/'
+ })
+
+""" # error plot
+fig = plt.figure(figsize=(6.25, 2))
+
+ax1 = fig.add_subplot(111)
+ax1.plot(time80[0:-10] , e_p [0:-10, 1], label='position error')
+ax1.set_ylabel('$y_{EE, R}$ in mm')
+ax1.set_xlabel('time in s')
+
+ax4 = ax1.twinx()
+ax4.plot(time80[0:-10] , e_f[0:-10] , color=next(ax1._get_lines.prop_cycler)['color'], label='force')
+ax4.set_ylim(-1, 1)
+ax4.set_ylabel('force in N')
+labels = ["position error", "force error"]
+fig.legend( labels=labels,
+ loc='upper right', bbox_to_anchor=(0.6, 0.92),
+ ncol=1)
+
+fig.tight_layout()
+#fig.subplots_adjust(top=0.9)
+plt.show() """
+
+
+
+""" fig = plt.figure(figsize=(6.25, 2))
+ax2 = fig.add_subplot(111)
+#ax1.set_title('position error in EE frame')
+ax2.plot(time80[0:-10] , distance [0:-10], label='comp. pose')
+ax2.plot(time80[0:-10] , distance_abb [0:-10], label='meas. pose')
+ax2.set_ylabel('$l^2$-norm in mm')
+#plt.yticks([402, 402.5, 403, 403.5, 404])
+ax2.set_xlabel('time in s')
+ax2.legend()
+fig.tight_layout()
+#plt.title('hybrid control at Kp=0.05 and OP=4N')
+#fig.subplots_adjust(top=0.9)
+plt.show()
+
+
+
+fig = plt.figure()
+
+ax3 = fig.add_subplot(111)
+ax3.plot(frq, abs(Y), color=next(ax2._get_lines.prop_cycler)['color'], label='force')
+ax3.set_xlabel('freq (Hz)')
+ax3.set_ylabel('|Y(freq)|')
+ax3.legend()
+
+
+
+#labels = ["case 1", "case 2", "case 3", "case 4"]
+#fig.legend( labels=labels,
+# loc="upper center", ncol=4)
+#fig.subplots_adjust(top=0.9, hspace=0.3)
+fig.tight_layout()
+#fig.subplots_adjust(top=0.9)
+plt.show()
+"""
+
+# make a 3D visualization
+# right
+fig = plt.figure()
+#fig = plt.figure(figsize=(3, 3))
+ax = fig.add_subplot(111, projection='3d')
+ax.plot(p2_des[0:-10,0], p2_des[0:-10,1], p2_des[0:-10,2], label='des. pose')
+#ax.plot(p2_is[0:-10,0], p2_is[0:-10,1], p2_is[0:-10,2], label='real_pathR')
+#ax.plot(p2_is[0:-10,0], p2_is[0:-10,1], p2_is[0:-10,2], label='comp. pose')p2_meas_2
+ax.plot(p2_meas[0:-10,0], p2_meas[0:-10,1], p2_meas[0:-10,2], label='meas. pose, 2.5 A')
+ax.plot(p2_meas_2[0:-10,0], p2_meas_2[0:-10,1], p2_meas_2[0:-10,2], label='meas. pose, 2.25 A')
+plt.xticks([350, 400, 450, 500])
+
+#ax.set_xlim(54, 66)
+#ax.set_ylim(99, 101)
+ax.set_xlabel('$x$ in mm')
+ax.set_ylabel('$y$ in mm')
+ax.set_zlabel('$z$ in mm')
+ax.view_init(elev=19., azim=-21.)
+ax.legend(loc='upper right')
+#fig.tight_layout()
+plt.show()
+
+#left
+""" fig = plt.figure(figsize=(3, 3))
+ax2 = fig.add_subplot(111, projection='3d')
+ax2.plot(p1_des[0:-10,0], p1_des[0:-10,1], p1_des[0:-10,2], label='des. pose')
+#ax2.plot(p1_is[0:-10,0], p1_is[0:-10,1], p1_is[0:-10,2], label='real_pathR')
+ax2.plot(p1_is[0:-10,0], p1_is[0:-10,1], p1_is[0:-10,2], label='comp. pose')
+ax2.plot(p1_meas[0:-10,0], p1_meas[0:-10,1], p1_meas[0:-10,2], label='meas. pose')
+ax2.set_xlabel('$x$ in mm')
+ax2.set_ylabel('$y$ in mm')
+#ax2.set_zlabel('$z$ in mm')
+ax2.set_ylim(98, 101)
+ax2.view_init(elev=33., azim=0.)
+ax2.legend(loc='upper right')
+plt.yticks([98, 99, 100, 101])
+#fig.tight_layout()
+plt.show() """
+
+
+fig = plt.figure(figsize=(6, 4))
+
+ax0 = fig.add_subplot(211)
+ax1 = fig.add_subplot(212)
+
+
+ax0.plot(time80[0:-10] , distance_meas [0:-10], color='#005293')
+ax0.plot(time80[0:-10] , distance_meas2 [0:-10], color='#A2AD00')
+ax0.set_yticks([395, 396, 397, 398])
+ax0.set_ylabel('$l^2$-norm in mm')
+
+
+ax1.plot(time80[0:-10], force[0:-10], label='2.5 A', color='#005293')
+ax1.plot(time80[0:-10], force_2[0:-10], label='2.25 A', color='#A2AD00')
+#ax1.plot(time80[0:-10], forcenof[0:-10], label='pure position')
+
+ax1.set_ylabel('force in N')
+ax1.set_xlabel('time in s')
+
+labels = [ "2.5 A", "2.25 A"]
+fig.legend( labels=labels,
+ loc="upper center", ncol=2)
+#fig.subplots_adjust(top=0.9, hspace=0.3)
+fig.tight_layout()
+fig.subplots_adjust(top=0.9)
+#tikzplotlib.clean_figure()
+#tikzplotlib.save("bowingforce.tex")
+
+plt.show()
+
+
+
diff --git a/python/plots/force_control/plotting.py b/python/plots/force_control/plotting.py
index 99892da5a28d6aa2648d37070a027aff4755ca21..96e6f876e89cf42ac77836f9cf7611b9d18663bc 100644
--- a/python/plots/force_control/plotting.py
+++ b/python/plots/force_control/plotting.py
@@ -2,6 +2,9 @@ import numpy as np
import matplotlib.pyplot as plt
from matplotlib.gridspec import GridSpec
from numpy.linalg import norm
+from matplotlib.patches import Rectangle
+
+
R_01_80 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01.npy')
@@ -46,12 +49,17 @@ for i in range(noSamples80):
time80 = np.linspace(0, round(1.0/80.0 * noSamples80), num=noSamples80)
+time_reshape = np.reshape(time80, (len(time80), 1))
+
+export_data = np.concatenate((e_f, force01, time_reshape), axis=1)
+
+#np.savetxt("forceErrorKp0.05-forceErrorKp0.1-timeStamp.csv", export_data, delimiter=",")
distance = norm(p1_is_new - p2_is_new, axis=1)
distance_abb = norm(p1_abb - p2_abb, axis=1)
-y = e_f[10:1000]
+y = e_f[10:-1]
n = len(y) # length of the signal
k = np.arange(n)
@@ -62,6 +70,30 @@ frq = frq[:len(frq)//2] # one side frequency range
Y = np.fft.fft(y)/n # dft and normalization
Y = Y[:n//2]
+#######
+y015 = force015[round(0.4*len(force015)):round(0.8*len(force015))]
+
+n015 = len(y015) # length of the signal
+k015 = np.arange(n015)
+T015 = n015/80
+frq015 = k015/T015 # two sides frequency range
+frq015 = frq015[:len(frq015)//2] # one side frequency range
+
+Y015 = np.fft.fft(y015)/n015 # dft and normalization
+Y015 = Y015[:n015//2]
+
+######
+y01 = force01[10:-1]
+
+n01 = len(y01) # length of the signal
+k01 = np.arange(n01)
+T01 = n01/80
+frq01 = k01/T01 # two sides frequency range
+frq01 = frq01[:len(frq01)//2] # one side frequency range
+
+Y01 = np.fft.fft(y01)/n01 # dft and normalization
+Y01 = Y01[:n01//2]
+
# use self-defined tum-cycler for TUM-blue colors
plt.style.use('mylatex')
@@ -75,7 +107,7 @@ plt.rcParams.update({
})
# error plot
-fig = plt.figure(figsize=(6.25, 2))
+fig = plt.figure(figsize=(6.5, 2))
ax1 = fig.add_subplot(111)
ax1.plot(time80[0:-10] , e_p [0:-10, 1], label='position error')
@@ -92,12 +124,13 @@ fig.legend( labels=labels,
ncol=1)
fig.tight_layout()
+
#fig.subplots_adjust(top=0.9)
-plt.show()
+#plt.show()
-fig = plt.figure(figsize=(6.25, 2))
+fig = plt.figure(figsize=(6, 2))
ax2 = fig.add_subplot(111)
#ax1.set_title('position error in EE frame')
ax2.plot(time80[0:-10] , distance [0:-10], label='comp. pose')
@@ -116,25 +149,23 @@ plt.show()
fig = plt.figure()
ax3 = fig.add_subplot(111)
-ax3.plot(frq, abs(Y), color=next(ax2._get_lines.prop_cycler)['color'], label='force')
-ax3.set_xlabel('freq (Hz)')
-ax3.set_ylabel('|Y(freq)|')
+ax3.plot(frq, abs(Y), label='$K_p=0.05$')
+ax3.plot(frq01, abs(Y01), label='$K_p=0.1$ ')
+#ax3.plot(frq015, abs(Y015), label='$K_p=0.15$ ')
+ax3.set_xlabel('freq in Hz')
+ax3.set_ylabel('magnitude')
+plt.xscale('log')
ax3.legend()
-
-
-#labels = ["case 1", "case 2", "case 3", "case 4"]
-#fig.legend( labels=labels,
-# loc="upper center", ncol=4)
-#fig.subplots_adjust(top=0.9, hspace=0.3)
-fig.tight_layout()
-#fig.subplots_adjust(top=0.9)
-plt.show()
+import tikzplotlib
+tikzplotlib.save("test.tex")
+#plt.show()
# make a 3D visualization
# right
-fig = plt.figure(figsize=(3, 3))
+fig = plt.figure()
+#fig = plt.figure(figsize=(3, 3))
ax = fig.add_subplot(111, projection='3d')
ax.plot(p2_des[0:-10,0], p2_des[0:-10,1], p2_des[0:-10,2], label='des. pose')
#ax.plot(p2_is[0:-10,0], p2_is[0:-10,1], p2_is[0:-10,2], label='real_pathR')
@@ -148,6 +179,7 @@ ax.set_ylabel('$y$ in mm')
ax.set_zlabel('$z$ in mm')
ax.view_init(elev=33., azim=0.)
ax.legend(loc='upper right')
+tikzplotlib.save("testplot.tex")
#fig.tight_layout()
plt.show()
@@ -169,19 +201,44 @@ plt.yticks([98, 99, 100, 101])
plt.show()
-fig = plt.figure()
+fig = plt.figure(figsize=(6, 4))
-ax1 = fig.add_subplot(111)
-ax1.plot(time80[0:-10], force015[0:-10], label='0.15')
-ax1.plot(time80[0:-10], force01[0:-10], label='0.1')
-ax1.plot(time80[0:-10], e_f[0:-10], label='0.05')
-ax1.plot(time80[0:-10], forcenof[0:-10], label='pure position')
-
-ax1.set_ylabel('N')
-ax1.set_xlabel('s')
-ax1.legend()
-plt.title('force errors in comparision')
+ax1 = fig.add_subplot(211)
+ax2 = fig.add_subplot(212)
+ax1.plot(time80[30:-10], force015[30:-10], label='$K_p=0.15$', linewidth=0.5)
+ax1.plot(time80[30:-10], force01 [30:-10], label='$K_p=0.1$')
+ax1.plot(time80[30:-10], e_f [30:-10], label='$K_p=0.05$')
+ax1.plot(time80[30:-10], forcenof[30:-10], label='Position Control')
+ax1.add_patch(Rectangle((31, -0.2), 2, 0.4,
+ edgecolor = 'black',
+ #facecolor = 'blue',
+ fill=False,
+ zorder=2,
+ lw=1))
+
+
+ax2.plot(time80[30:-10], force015[30:-10], label='$K_p=0.15$')
+ax2.plot(time80[30:-10], force01 [30:-10], label='$K_p=0.1$')
+ax2.plot(time80[30:-10], e_f [30:-10], label='$K_p=0.05$')
+
+ax1.set_ylabel('force in N')
+
+ax2.set_ylabel('force in N')
+ax2.set_xlabel('time in s')
+ax2.set_ylim(-0.3, 0.3)
+ax2.set_xlim(31, 33)
+
+labels = [ "$K_p = 0.15$", "$K_p = 0.1$", "$K_p=0.05$", "Position Control"]
+fig.legend( labels=labels,
+ loc="upper center", ncol=4)
+#fig.subplots_adjust(top=0.9, hspace=0.3)
fig.tight_layout()
+fig.subplots_adjust(top=0.9)
+
+#ax1.legend()
+#ax2.legend()
+#plt.title('force errors in comparision')
+#fig.tight_layout()
plt.show()
diff --git a/python/plots/force_controlv2/plottingv2.py b/python/plots/force_controlv2/plottingv2.py
new file mode 100644
index 0000000000000000000000000000000000000000..62dffbcae0b5ac74c577a132bf5e68d5a0fb6ad2
--- /dev/null
+++ b/python/plots/force_controlv2/plottingv2.py
@@ -0,0 +1,184 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.gridspec import GridSpec
+from numpy.linalg import norm
+
+
+R_01_80 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01.npy')
+R_01_250 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01_250.npy')
+
+data01 = np.load('./plots/force_controlv2/Kp0.1-Op2.npy') # o for original
+force01 = data01[:, 32:33]-2
+
+data015 = np.load('./plots/force_controlv2/Kp0.15-Op2.npy') # o for original
+force015 = data015[:, 32:33]-2
+
+datan005 = np.load('./plots/force_controlv2/Kp0.05-Op2.npy') # o for original
+force005 = datan005[:, 32:33]-2
+
+p1_is_new = datan005[:, 43:46]* 1000
+p2_is_new = datan005[:, 12:15]* 1000
+p2_abb = datan005[:, 63:66]
+p1_abb = datan005[:, 66:69]
+
+
+#experimentLogs = np.hstack((p2, phi_delta, log_compPose_R, log_realPose_R, log_compJoints_R, log_realJoints_R, log_force, p1, log_compJoints_R, log_realPose_L, log_compJoints_L, log_realJoints_L))
+force = datan005[:, 32:33]
+p1_des = datan005[:, 33:36]* 1000
+p2_des = datan005[:, 0:3]* 1000
+p1_is = datan005[:, 43:46]* 1000
+p2_is = datan005[:, 12:15]* 1000
+e_f = force - 2.0
+e_p = p2_des - p2_is
+
+noSamples80 = len(e_p)
+
+
+# transform error into eef80frame
+for i in range(noSamples80):
+ e_p[i, :] = np.transpose(R_01_80[i] @ e_p[i, :].T)
+
+
+time80 = np.linspace(0, round(1.0/80.0 * noSamples80), num=noSamples80)
+
+distance = norm(p1_is_new - p2_is_new, axis=1)
+distance_abb = norm(p1_abb - p2_abb, axis=1)
+
+
+y = e_f[10:1000]
+
+n = len(y) # length of the signal
+k = np.arange(n)
+T = n/80
+frq = k/T # two sides frequency range
+frq = frq[:len(frq)//2] # one side frequency range
+
+Y = np.fft.fft(y)/n # dft and normalization
+Y = Y[:n//2]
+
+# use self-defined tum-cycler for TUM-blue colors
+plt.style.use('mylatex')
+
+# plotting for thesis
+plt.rcParams.update({
+ "font.family": "serif", # use serif/main font for text elements
+ "text.usetex": True, # use inline math for ticks
+ "pgf.rcfonts": False, # don't setup fonts from rc parameters
+ #"legend.loc": 'upper right',
+ "savefig.directory": '/home/joschua/Documents/Studium/TUM/Thesis/documentation/thesis/myWorkFiles/AMStudentThesis/figures/plots/'
+ })
+
+# error plot
+fig = plt.figure(figsize=(6.25, 2))
+
+ax1 = fig.add_subplot(111)
+ax1.plot(time80[0:-10] , e_p [0:-10, 1], label='position error')
+ax1.set_ylabel('$y_{EE, R}$ in mm')
+ax1.set_xlabel('time in s')
+
+ax4 = ax1.twinx()
+ax4.plot(time80[0:-10] , e_f[0:-10] , color=next(ax1._get_lines.prop_cycler)['color'], label='force')
+ax4.set_ylim(-1, 1)
+ax4.set_ylabel('force in N')
+labels = ["position error", "force error"]
+fig.legend( labels=labels,
+ loc='upper right', bbox_to_anchor=(0.6, 0.92),
+ ncol=1)
+
+fig.tight_layout()
+#fig.subplots_adjust(top=0.9)
+plt.show()
+
+
+
+fig = plt.figure(figsize=(6.25, 2))
+ax2 = fig.add_subplot(111)
+#ax1.set_title('position error in EE frame')
+ax2.plot(time80[0:-10] , distance [0:-10], label='comp. pose')
+ax2.plot(time80[0:-10] , distance_abb [0:-10], label='meas. pose')
+ax2.set_ylabel('$l^2$-norm in mm')
+plt.yticks([402, 402.5, 403, 403.5, 404])
+ax2.set_xlabel('time in s')
+ax2.legend()
+fig.tight_layout()
+#plt.title('hybrid control at Kp=0.05 and OP=4N')
+#fig.subplots_adjust(top=0.9)
+plt.show()
+
+
+
+fig = plt.figure()
+
+ax3 = fig.add_subplot(111)
+ax3.plot(frq, abs(Y), color=next(ax2._get_lines.prop_cycler)['color'], label='force')
+ax3.set_xlabel('freq (Hz)')
+ax3.set_ylabel('|Y(freq)|')
+plt.xscale('log')
+ax3.legend()
+
+
+
+#labels = ["case 1", "case 2", "case 3", "case 4"]
+#fig.legend( labels=labels,
+# loc="upper center", ncol=4)
+#fig.subplots_adjust(top=0.9, hspace=0.3)
+fig.tight_layout()
+#fig.subplots_adjust(top=0.9)
+plt.show()
+
+
+# make a 3D visualization
+# right
+fig = plt.figure()
+#fig = plt.figure(figsize=(3, 3))
+ax = fig.add_subplot(111, projection='3d')
+ax.plot(p2_des[0:-10,0], p2_des[0:-10,1], p2_des[0:-10,2], label='des. pose')
+#ax.plot(p2_is[0:-10,0], p2_is[0:-10,1], p2_is[0:-10,2], label='real_pathR')
+ax.plot(p2_is_new[0:-10,0], p2_is_new[0:-10,1], p2_is_new[0:-10,2], label='comp. pose')
+ax.plot(p2_abb[0:-10,0], p2_abb[0:-10,1], p2_abb[0:-10,2], label='meas. pose')
+
+#ax.set_xlim(54, 66)
+#ax.set_ylim(99, 101)
+#ax.set_xlabel('$x$ in mm')
+ax.set_ylabel('$y$ in mm')
+ax.set_zlabel('$z$ in mm')
+ax.view_init(elev=33., azim=0.)
+ax.legend(loc='upper right')
+#fig.tight_layout()
+plt.show()
+
+#left
+fig = plt.figure(figsize=(3, 3))
+ax2 = fig.add_subplot(111, projection='3d')
+ax2.plot(p1_des[0:-10,0], p1_des[0:-10,1], p1_des[0:-10,2], label='des. pose')
+#ax2.plot(p1_is[0:-10,0], p1_is[0:-10,1], p1_is[0:-10,2], label='real_pathR')
+ax2.plot(p1_is_new[0:-10,0], p1_is_new[0:-10,1], p1_is_new[0:-10,2], label='comp. pose')
+ax2.plot(p1_abb[0:-10,0], p1_abb[0:-10,1], p1_abb[0:-10,2], label='meas. pose')
+ax2.set_xlabel('$x$ in mm')
+ax2.set_ylabel('$y$ in mm')
+#ax2.set_zlabel('$z$ in mm')
+ax2.set_ylim(98, 101)
+ax2.view_init(elev=33., azim=0.)
+ax2.legend(loc='upper right')
+plt.yticks([98, 99, 100, 101])
+#fig.tight_layout()
+plt.show()
+
+
+fig = plt.figure()
+
+ax1 = fig.add_subplot(111)
+ax1.plot(time80[0:-10], force015[0:-10], label='0.15')
+ax1.plot(time80[0:-10], force01[0:-10], label='0.1')
+ax1.plot(time80[0:-10], e_f[0:-10], label='0.05')
+ax1.plot(time80[0:-10], forcenof[0:-10], label='pure position')
+
+ax1.set_ylabel('N')
+ax1.set_xlabel('s')
+ax1.legend()
+plt.title('force errors in comparision')
+fig.tight_layout()
+plt.show()
+
+
+
diff --git a/python/plots/preprocessing/plotting.py b/python/plots/preprocessing/plotting.py
index a803585275f4a44c4ad57775a84fbd13a5dd75dc..a0d7f3be3be364c849c7bc2a8d0b5d3bda01f04e 100644
--- a/python/plots/preprocessing/plotting.py
+++ b/python/plots/preprocessing/plotting.py
@@ -28,42 +28,51 @@ plt.rcParams.update({
})
# do plots for tcp1
-""" #fig = plt.figure()
-fig = plt.figure(figsize=(6.25, 2))
-ax = fig.add_subplot(111)
+ #fig = plt.figure()
+fig = plt.figure(figsize=(6, 4))
+ax = fig.add_subplot(211)
+ax2 = fig.add_subplot(212)
ax.scatter(p1[:,0]*1000, p1[:,1]*1000-53.7, label='$p_{1*}$', color='#005293')
ax.scatter(pos1[:,0]*1000, pos1[:,1]*1000-53.7, marker="x", label='$p_1$', color='#80a9c9')
# part view
-ax.set_xlim(10.8, 14.8)
-ax.set_ylim(-4.4, -1.95)
+ax2.scatter(p1[:,0]*1000, p1[:,1]*1000-53.7, label='$p_{1*}$', color='#005293')
+ax2.scatter(pos1[:,0]*1000, pos1[:,1]*1000-53.7, marker="x", label='$p_1$', color='#80a9c9')
+ax2.set_xlim(10.8, 14.8)
+ax2.set_ylim(-4.4, -1.95)
+ax2.set_ylabel('$y_1$')
# overview
-#ax.set_xlim(-10, 175)
-#ax.set_ylim(-10, 25)
-ax.set_xlabel('$x_1$')
+ax.set_xlim(-10, 175)
+ax.set_ylim(-10, 25)
+ax2.set_xlabel('$x_1$')
ax.set_ylabel('$y_1$')
-ax.legend()
-#ax.add_patch(Rectangle((10.8, -4.4), 4, 2.45,
-# edgecolor = 'black',
-# #facecolor = 'blue',
-# fill=False,
-# lw=1))
+labels = [ '$p_{1*}$', '$p_1$']
+fig.legend( labels=labels,
+ loc="upper center", ncol=2)
+ax.add_patch(Rectangle((10.8, -4.4), 4, 2.45,
+ edgecolor = 'black',
+ #facecolor = 'blue',
+ fill=False,
+ lw=1))
+ax.set_xticks([0, 50, 100, 150])
+fig.tight_layout()
+fig.subplots_adjust(top=0.9)
plt.show()
-fig.tight_layout()
+
#fig.savefig('/home/joschua/Documents/Studium/TUM/Thesis/documentation/thesis/myWorkFiles/AMStudentThesis/figures/plots/lefttraj_part.pgf', format='pgf')
- """
-fig = plt.figure(figsize=(6.25, 2))
+
+fig = plt.figure(figsize=(6, 2))
ax = fig.add_subplot(111)
v1_abs = np.array(list(map(lambda x: norm(x, 2), v1))).reshape(len(v1[:,0]),1)
v2_abs = np.array(list(map(lambda x: norm(x, 2), v2))).reshape(len(v2[:,0]),1)
timeline = np.linspace(0, (len(v1[:,0])-1)*1.0/80.0, len(v1[:,0])).reshape(len(v1[:,0]),1)
-v_des = np.ones((len(timeline), 1))* 1000
-ax.plot( timeline, v1_abs*1000*60, label='$v_1$')
-ax.plot( timeline, v2_abs*1000*60, label='$v_2$')
-ax.plot( timeline, v_des, linestyle='dashed', label='$v_f$')
-ax.set_xlabel('s')
-ax.set_ylabel('mm/min')
+v_des = np.ones((len(timeline), 1))* 300
+ax.plot( timeline, v1_abs*1000*60, label='$v_L$')
+ax.plot( timeline, v2_abs*1000*60, label='$v_R$')
+ax.plot( timeline, v_des, linestyle='dashed', label='$v_{f,d}$', color='#808080')
+ax.set_xlabel('time in s')
+ax.set_ylabel('$v_f$ in mm/min')
ax.legend()
#plt.ylim(bottom=0)
fig.tight_layout()
@@ -99,7 +108,7 @@ plt.show()
# plot the data for visualisation
-fig = plt.figure(figsize=(3, 3))
+fig = plt.figure(figsize=(2.7, 2.7))
ax = fig.add_subplot(111, projection='3d')
#ax.plot(p1m[:,0]*1000, p1m[:,2]*1000, p1m[:,1]*1000-53.7, color='#005293', label='$p_{1*}$')
ax.plot(p2m_ref[:,0]*1000,p2m_ref[:,2]*1000, p2m_ref[:,1]*1000-53.7, label='$p_2$')
@@ -110,10 +119,11 @@ ax.set_ylim(405, 395)#axes were switched to better rotate plot is z
ax.set_zlim(-50,15)#axes were switched to better rotate plot is y
#ax.set_zlim(-15,25)
ax.view_init(elev=13., azim=36.)
-ax.set_xlabel('$x_2$')
-ax.set_ylabel('$z_2$')
-ax.set_zlabel('$y_2$')
+ax.set_xlabel('$x_1$')
+ax.set_ylabel('$z_1$')
+ax.set_zlabel('$y_1$')
ax.legend()
+ax.set_yticks([402, 400, 398 , 396])
+fig.tight_layout()
plt.show()
-fig.tight_layout()
\ No newline at end of file
diff --git a/python/plots/step_response/plotting.py b/python/plots/step_response/plotting.py
index 13e41aacd7429af475155c1898afa7868533bbf1..2f7767a6c070ff7e7a1eb55adfe4a82049aaccac 100644
--- a/python/plots/step_response/plotting.py
+++ b/python/plots/step_response/plotting.py
@@ -14,7 +14,7 @@ e = (p1_des - p1_is)
#noSamples = len(p1_desf80)
noSamples80 = len(p1_des)
-time80 = np.linspace(0, round(1.0/80.0 * noSamples80), num=noSamples80)
+time80 = np.linspace(0, round(1.0/80.0 * noSamples80), num=noSamples80) - 0.6135
# use self-defined tum-cycler for TUM-blue colors
plt.style.use('mylatex')
@@ -29,21 +29,22 @@ plt.rcParams.update({
})
# error plot
-fig = plt.figure()
+fig = plt.figure(figsize=(6, 2))
-ax1 = fig.add_subplot(311)
-ax2 = fig.add_subplot(312)
+
+#ax1 = fig.add_subplot(311)
+ax2 = fig.add_subplot(111)
#ax3 = fig.add_subplot(313)
-ax1.plot(time80 [30:-10], e [30:-10, 0])
-ax2.plot(time80 [30:-10], p1_des [30:-10, 0])
-ax2.plot(time80 [30:-10], p1_is [30:-10, 0])
+#ax1.plot(time80 [30:-10], e [30:len(e)/5, 0])
+ax2.plot(time80 [30:round(len(e)/5)], p1_des [30:round(len(e)/5), 0])
+ax2.plot(time80 [30:round(len(e)/5)], p1_is [30:round(len(e)/5), 0])
#ax1.set_title('position error in EE frame')
-ax1.set_ylabel('$x$ in mm')
+#ax1.set_ylabel('$x$ in mm')
ax2.set_ylabel('$x$ in mm')
-ax2.set_xlabel('s')
+ax2.set_xlabel('time in s')
#labels = ["case 1", "case 2", "case 3", "case 4"]
#fig.legend( labels=labels,
diff --git a/python/preprocessing/genPaths.py b/python/preprocessing/genPaths.py
index 189912e2ce0346d3078f0ccca9c43f77d461b59e..44706fa38201e853a26e2a42ad30449937c75dbb 100644
--- a/python/preprocessing/genPaths.py
+++ b/python/preprocessing/genPaths.py
@@ -21,24 +21,14 @@ with open('preprocessing/gcode.txt') as f:
if row > 0:
pos[col, row-1] = np.asarray(str[1:len(str)]) # cut the first element of string which is not needed
-# goal must be to have both tcp positions in the world frame, due to different approach with fixed wire length
-# second tcp must be corrected. Can be calculated from the first one based on orientation and length of wire.
-# 1) extract wires orientation from 4 axis approach - two axis with x-y coordinates and fixed distance (in z-direction)
-# 2) use the orienation angles to do a rotation transformation and walk fixed distance in wires axis
-
# extract original x & y of both tcp's
pos_split = np.split(pos, 2, axis=1)
pos1, pos2 = pos_split[0]*0.001, pos_split[1]*0.001 # transform to SI-units - [m]
-# TODO: compute trajectory based on path of g-code
# set a cutting speed, sample rate and compute necessary
-# position at every time step
c_speed = 300 * 0.001 * 1/60 # define cutting speed as 300 mm/min [m/s]
st = 1.0/80.0 # highest possible sample time - 250 Hz
-# modificatin to delete in order to check angles in RS
-# pos2 = pos2 + np.array([0, 0.1]) # get 10 cm higher
-
# initialize list with first element
p1x = []
p1y = []
@@ -58,7 +48,6 @@ for i in range(1, len(pos1[:,0])):
dp2_len = norm(dp2, 2)
# number of section in between original points
nr_sect = round(dp1_len/(c_speed*st))
- #print(len(p1x), "+ ", nr_sect)
# append everthing to lists
for j in range(nr_sect):
@@ -70,7 +59,6 @@ for i in range(1, len(pos1[:,0])):
p2y.append(pos2[i-1,1] + j* dp2[0,1] * 1/nr_sect)
-# TODO: plot and test for different cutting speeds and check functionality
p1 = np.hstack((np.array(p1x).reshape(len(p1x),1), np.array(p1y).reshape(len(p1y),1)))
p2 = np.hstack((np.array(p2x).reshape(len(p2x),1), np.array(p2y).reshape(len(p2y),1)))
@@ -86,7 +74,7 @@ v1 = np.hstack((v1, np.zeros((len(v1[:,0]), 1))))
# number of points in paths
pNum = len(p1[:,0])
# length of wire, defined in 4-axis setup
-wLen = 0.4
+wLen = 0.40
z1 = np.zeros((pNum,1))
z2 = np.ones((pNum,1)) * wLen
# append z axis to position vectors
@@ -127,7 +115,6 @@ for i in range(pNum):
# rotation matrix
# self defined convention: rotate around y axis, then around x axis the get from initial frame to wire frame
# rotation from initial to first frame (z-axis aligned with wire)
- #R01 = Ry @ Rx
R01 = Rx @ Ry
# rotation from first frame to initial frame
R10 = np.transpose(R01)
@@ -185,7 +172,6 @@ traj_data = np.hstack((p1m, v1, p2m, v2, ang, odot))
# save data for application
np.save('./data/traj_data', traj_data)
-
# save data to decouple preprocessing and plotting
# p1, pos1, p2, pos2, p2m_ref, p1m , p2m, v1
plot_path = '/home/joschua/Coding/forceControl/master-project/python/plots/preprocessing/'
@@ -199,4 +185,4 @@ np.save(plot_path+'p2m_ref', p2m_ref)
np.save(plot_path+'v1', v1)
np.save(plot_path+'v2', v2)
-#np.save('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01_250', R_01)
\ No newline at end of file
+#np.save('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01_900', R_01)
\ No newline at end of file
diff --git a/python/requirements.txt b/python/requirements.txt
index 95732bdd33bab66676b308005d0d461f4215fb93..842683e979679c16beacd3a3f396eb4ab69cc976 100644
--- a/python/requirements.txt
+++ b/python/requirements.txt
@@ -1,13 +1,40 @@
+-e git+git@gitlab.control.lth.se:gosda/master-project.git@8ece398c666598c99ead77d9afa4eadc78d6a7ce#egg=abb_egm_pyclient&subdirectory=python/abb_egm_pyclient
cycler==0.11.0
+data==0.1.0
+-e git+git@gitlab.control.lth.se:gosda/master-project.git@8ece398c666598c99ead77d9afa4eadc78d6a7ce#egg=data_libs&subdirectory=python
+decorator==5.1.1
+evdev==1.5.0
fonttools==4.32.0
+freetype-py==2.3.0
+imageio==2.19.2
+keyboard==0.13.5
kiwisolver==1.4.2
+lxml==4.8.0
matplotlib==3.5.1
+networkx==2.2
numpy==1.22.3
packaging==21.3
+pandas==1.4.3
Pillow==9.1.0
protobuf==3.20.0
pybind11==2.9.1
+pycollada==0.6
+pyglet==1.5.24
+PyOpenGL==3.1.0
pyparsing==3.0.8
+pyrender==0.1.45
pyserial==3.5
python-dateutil==2.8.2
+python-statemachine==0.8.0
+python-xlib==0.31
+pytz==2022.1
+PyYAML==6.0
+scipy==1.8.0
six==1.16.0
+tikzplotlib==0.10.1
+tqdm==4.64.0
+transforms3d==0.3.1
+trimesh==3.12.0
+urdf-parser-py==0.0.4
+urdfpy==0.0.22
+webcolors==1.12
diff --git a/python/taskspace_placement/computeJoints.py b/python/taskspace_placement/computeJoints.py
index a808a44172701dc9ded7e07af5c7e1658ccb8a51..3a165597149ab230f4ba38db5cd25b776f35ce5f 100644
--- a/python/taskspace_placement/computeJoints.py
+++ b/python/taskspace_placement/computeJoints.py
@@ -71,7 +71,7 @@ error_right = np.zeros((len(p1[:,0]),6))
jointVelocities_right = np.zeros((len(p1[:,0]),7))
jointAngles = np.array([-110.0, 29.85, 35.92, 49.91, 117.0, 123.0, -117.0]) * np.pi/180.0
-jointVelocities = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+jointVelocities_ = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
desPose_old = np.concatenate((p2[0,: ], phi_delta[0, :]), axis=0)
@@ -79,7 +79,7 @@ desPose_old = np.concatenate((p2[0,: ], phi_delta[0, :]), axis=0)
for index, (pos, vel, phi, phi_dot) in enumerate(zip(p2, v2, phi_delta, dphi)): # loop through all the desired position of left arm
desPose = np.concatenate((pos, phi), axis=0)
desVelocities = np.concatenate((vel, phi_dot), axis=0)
- yumi_right.set_jointValues(jointAngles, jointVelocities)
+ yumi_right.set_jointValues(jointAngles, jointVelocities_)
yumi_right.set_desPoseVel(desPose_old, desVelocities)
yumi_right.process()
@@ -87,9 +87,9 @@ for index, (pos, vel, phi, phi_dot) in enumerate(zip(p2, v2, phi_delta, dphi)):
computedPose_right[index, :] = yumi_right.get_pose() # resulting pose with joint values from IK
if index > 0:
- jointVelocities = (compJointAngles_left[index, :] - compJointAngles_left[index-1, :])/dt
+ jointVelocities_ = (compJointAngles_right[index, :] - compJointAngles_right[index-1, :])/dt
error_right[index, :] = desPose - computedPose_right[index, :]
- jointVelocities_right[index, :] = jointVelocities
+ jointVelocities_right[index, :] = jointVelocities_
jointAngles = compJointAngles_right[index, :]
# save desired pose to give it to ik in the next iteration since drift compensation must consider if current task space pose is the old desired task space pose