From d140500b226340a4e6ffead389f62df91d35f107 Mon Sep 17 00:00:00 2001
From: Joschua Gosda <joschua.gosda@control.lth.se>
Date: Tue, 16 Aug 2022 10:27:58 +0200
Subject: [PATCH] final
---
.../CompilerIdCXX/CMakeCXXCompilerId.cpp | 660 ++++++++++++++++++
c++/src/py2gpmbind.cpp | 1 +
c++/src/yumi.cpp | 14 +-
python/abb_egm_pyclient/README.md | 102 ++-
.../feedback/control_final.py | 30 +-
.../abb_egm_pyclient/feedback/stateMachine.py | 14 -
python/experiment/plots.py | 20 +
.../checkManipulabiltyMeasure.py | 20 -
python/plots/control_position/plotting.py | 122 +++-
python/plots/control_positionv2/plotting.py | 246 +++++++
python/plots/cutting/plotting.py | 202 ++++++
python/plots/force_control/plotting.py | 113 ++-
python/plots/force_controlv2/plottingv2.py | 184 +++++
python/plots/preprocessing/plotting.py | 66 +-
python/plots/step_response/plotting.py | 19 +-
python/preprocessing/genPaths.py | 18 +-
python/requirements.txt | 27 +
python/taskspace_placement/computeJoints.py | 8 +-
18 files changed, 1675 insertions(+), 191 deletions(-)
create mode 100644 c++/build/CMakeFiles/3.16.3/CompilerIdCXX/CMakeCXXCompilerId.cpp
create mode 100644 python/plots/control_positionv2/plotting.py
create mode 100644 python/plots/cutting/plotting.py
create mode 100644 python/plots/force_controlv2/plottingv2.py
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 0000000..69cfdba
--- /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 46484c6..c7c8715 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 235b4b5..798e611 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 e48bab2..4608eb9 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 195c1f5..85afe40 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 4bd8b82..44740e9 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 d40fc05..549ce46 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 98709b9..c9d6391 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 0240844..823f880 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 0000000..70f1198
--- /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 0000000..e5e0b0e
--- /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 99892da..96e6f87 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 0000000..62dffbc
--- /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 a803585..a0d7f3b 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 13e41aa..2f7767a 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 189912e..44706fa 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 95732bd..842683e 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 a808a44..3a16559 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
--
GitLab