├── debian ├── compat ├── source │ └── format ├── gbp.conf ├── control ├── copyright └── rules ├── rm501.png ├── .github ├── FUNDING.yml └── workflows │ ├── windows.yml │ └── ubuntu.yml ├── icons ├── rm501.png └── Makefile ├── doc └── DejaVuSansMono.ttf ├── rm501.desktop ├── .gitignore ├── rm501.1 ├── mqtt.h ├── cmake └── mingw-w64-x86_64.cmake ├── png.h ├── CMakeLists.txt ├── README.md ├── GNUmakefile ├── png.c ├── mqtt.c ├── LICENSE ├── trajgen.h └── rm501.c /debian/compat: -------------------------------------------------------------------------------- 1 | 12 2 | -------------------------------------------------------------------------------- /debian/source/format: -------------------------------------------------------------------------------- 1 | 3.0 (native) 2 | -------------------------------------------------------------------------------- /rm501.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koppi/rm501/HEAD/rm501.png -------------------------------------------------------------------------------- /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | github: koppi 2 | custom: 'https://koppi.github.io' 3 | -------------------------------------------------------------------------------- /icons/rm501.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koppi/rm501/HEAD/icons/rm501.png -------------------------------------------------------------------------------- /icons/Makefile: -------------------------------------------------------------------------------- 1 | all: rm501.cdata 2 | 3 | rm501.cdata: rm501.png 4 | xxd -i $< > $@ 5 | -------------------------------------------------------------------------------- /doc/DejaVuSansMono.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koppi/rm501/HEAD/doc/DejaVuSansMono.ttf -------------------------------------------------------------------------------- /debian/gbp.conf: -------------------------------------------------------------------------------- 1 | [DEFAULT] 2 | # the default branch for upstream sources: 3 | upstream-branch = master 4 | # the default branch for the debian patch: 5 | debian-branch = master 6 | # the default tag formats used: 7 | upstream-tag = %(version)s 8 | debian-tag = debian/%(version)s 9 | # use pristine-tar: 10 | #pristine-tar = True 11 | -------------------------------------------------------------------------------- /rm501.desktop: -------------------------------------------------------------------------------- 1 | [Desktop Entry] 2 | Encoding=UTF-8 3 | Version=1.0 4 | Name=RM-501 Robot Simulator 5 | GenericName=RM-501 Robot Simulator 6 | Comment=Mitsubishi RM-501 Movemaster II Robot Simulator 7 | Icon=/usr/share/icons/hicolor/512x512/apps/rm501.png 8 | 9 | Type=Application 10 | Categories=Engineering;Development 11 | 12 | Exec=rm501 %f 13 | Terminal=false 14 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | rm501 2 | *.o 3 | *.d 4 | build-stamp 5 | debian/changelog 6 | debian/rm501.substvars 7 | debian/files 8 | token 9 | Makefile 10 | 11 | build.ninja 12 | build 13 | 14 | CMakeCache.txt 15 | cmake_install.cmake 16 | CMakeFiles 17 | 18 | .vscode 19 | *.exe 20 | .vs/CMake Overview 21 | .vs/ProjectSettings.json 22 | .vs/slnx.sqlite 23 | .vs/VSWorkspaceState.json 24 | -------------------------------------------------------------------------------- /rm501.1: -------------------------------------------------------------------------------- 1 | .\" DO NOT MODIFY THIS FILE! It was generated by help2man 1.47.6. 2 | .TH RM501 "1" "June 2019" "rm501 0.0.1" "User Commands" 3 | .SH NAME 4 | rm501 \- manual page for rm501 0.0.1 5 | .SH SYNOPSIS 6 | .B rm501 7 | [\fI\,OPTIONS\/\fR] 8 | .SH DESCRIPTION 9 | .IP 10 | Where [OPTIONS] are zero or more of the following: 11 | .TP 12 | [\-s|\-\-sdl] 13 | SDL window mode 14 | .TP 15 | [\-f|\-\-fullscreen] 16 | Fullscreen mode 17 | .TP 18 | [\-x|\-\-verbose] 19 | Show verbose information 20 | .TP 21 | [\-h|\-\-help] 22 | Show help information 23 | .TP 24 | [\-v|\-\-version] 25 | Show version number 26 | -------------------------------------------------------------------------------- /mqtt.h: -------------------------------------------------------------------------------- 1 | #ifndef _MQTT_HANDLER_H_ 2 | #define _MQTT_HANDLER_H_ 3 | 4 | #define ADDRESS "tcp://localhost:1883" 5 | #define TOPIC "Robot/Coordinates" 6 | #define QOS 1 7 | #define TIMEOUT 10000L 8 | 9 | #define MIN_DELTA_T 0.005 // seconds 10 | #define EPSILON 1 // min error 11 | 12 | typedef struct { 13 | float x, y, z, pitch, roll; 14 | bool grip; 15 | } coord_t; 16 | 17 | void mqtt_handler_init(); 18 | int mqtt_handler_close(); 19 | int mqtt_periodic_callback(coord_t*, bool); 20 | bool coord_equal(coord_t, coord_t, float); 21 | 22 | void print_error(coord_t, coord_t); 23 | 24 | #endif //_MQTT_HANDLER_H_ 25 | 26 | -------------------------------------------------------------------------------- /debian/control: -------------------------------------------------------------------------------- 1 | Source: rm501 2 | Section: electronics 3 | Priority: optional 4 | Maintainer: Jakob Flierl 5 | Uploaders: Jakob Flierl 6 | Build-Depends: debhelper (>= 9), pkg-config, automake, libtool, lsb-release, help2man, libsdl2-dev, libsdl2-image-dev, libsdl2-ttf-dev, libglu1-mesa-dev, libpng-dev, libncursesw5-dev, libpaho-mqtt-dev 7 | Standards-Version: 3.9.7 8 | Homepage: https://github.com/koppi/rm501 9 | Vcs-Git: git://github.com/koppi/rm501.git 10 | Vcs-Browser: https://github.com/koppi/rm501 11 | 12 | Package: rm501 13 | Architecture: any 14 | Depends: ${shlibs:Depends}, ${misc:Depends} 15 | Description: Mitsubishi RM-501 Movemaster II robot simulator 16 | An application that simulates the Mitsubishi RM-501 Movemaster II robot. 17 | * SDL2 OpenGL GUI, ncurses TUI, 18 | * Control from computer keyboard, Gamepad or 3D mouse, 19 | * Forward and inverse kinematics (double precision math), 20 | * Implemented in one C file for simplicity and portability. 21 | -------------------------------------------------------------------------------- /debian/copyright: -------------------------------------------------------------------------------- 1 | rm501 is copyright 2013-2025 Jakob Flierl 2 | 3 | License: GPLv2+ 4 | 5 | This program is free software; you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation; either version 2 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA. 18 | 19 | See /usr/share/common-licenses/GPL-2, or 20 | for the terms of the latest version 21 | of the GNU General Public License. 22 | -------------------------------------------------------------------------------- /cmake/mingw-w64-x86_64.cmake: -------------------------------------------------------------------------------- 1 | # Sample toolchain file for building for Windows from an Ubuntu Linux system. 2 | # 3 | # Typical usage: 4 | # *) install cross compiler: `sudo apt-get install mingw-w64` 5 | # *) cd build 6 | # *) cmake -DCMAKE_TOOLCHAIN_FILE=~/mingw-w64-x86_64.cmake .. 7 | # This is free and unencumbered software released into the public domain. 8 | 9 | set(CMAKE_SYSTEM_NAME Windows) 10 | set(TOOLCHAIN_PREFIX x86_64-w64-mingw32) 11 | set(CMAKE_SYSTEM_PROCESSOR x86_64) 12 | 13 | # cross compilers to use for C, C++ and Fortran 14 | set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}-gcc) 15 | set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}-g++) 16 | set(CMAKE_Fortran_COMPILER ${TOOLCHAIN_PREFIX}-gfortran) 17 | set(CMAKE_RC_COMPILER ${TOOLCHAIN_PREFIX}-windres) 18 | 19 | # target environment on the build host system 20 | set(CMAKE_FIND_ROOT_PATH /usr/${TOOLCHAIN_PREFIX}) 21 | 22 | # modify default behavior of FIND_XXX() commands 23 | set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 24 | set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 25 | set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) -------------------------------------------------------------------------------- /png.h: -------------------------------------------------------------------------------- 1 | #ifndef _SDL_SAVEPNG 2 | #define _SDL_SAVEPNG 3 | /* 4 | * SDL_SavePNG -- libpng-based SDL_Surface writer. 5 | * 6 | * This code is free software, available under zlib/libpng license. 7 | * http://www.libpng.org/pub/png/src/libpng-LICENSE.txt 8 | */ 9 | #include 10 | /* 11 | * Save an SDL_Surface as a PNG file. 12 | * 13 | * Returns 0 success or -1 on failure, the error message is then retrievable 14 | * via SDL_GetError(). 15 | */ 16 | #define SDL_SavePNG(surface, file) \ 17 | SDL_SavePNG_RW(surface, SDL_RWFromFile(file, "wb"), 1) 18 | 19 | /* 20 | * Save an SDL_Surface as a PNG file, using writable RWops. 21 | * 22 | * surface - the SDL_Surface structure containing the image to be saved 23 | * dst - a data stream to save to 24 | * freedst - non-zero to close the stream after being written 25 | * 26 | * Returns 0 success or -1 on failure, the error message is then retrievable 27 | * via SDL_GetError(). 28 | */ 29 | extern int SDL_SavePNG_RW(SDL_Surface *surface, SDL_RWops *rw, int freedst); 30 | 31 | /* 32 | * Return new SDL_Surface with a format suitable for PNG output. 33 | */ 34 | extern SDL_Surface *SDL_PNGFormatAlpha(SDL_Surface *src); 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /.github/workflows/windows.yml: -------------------------------------------------------------------------------- 1 | name: windows 2 | 3 | env: 4 | CMAKE_BUILD_TYPE: Release 5 | CMAKE_GENERATOR: Ninja 6 | 7 | on: 8 | push: 9 | branches: [ main ] 10 | pull_request: 11 | branches: [ main ] 12 | 13 | jobs: 14 | 15 | windows: 16 | runs-on: windows-latest 17 | name: CMake build on Windows 18 | timeout-minutes: 15 19 | 20 | steps: 21 | - uses: msys2/setup-msys2@v2 22 | with: 23 | install: >- 24 | git 25 | base-devel 26 | cmake 27 | ninja 28 | gcc 29 | make 30 | mingw-w64-x86_64-cmake 31 | mingw-w64-x86_64-ninja 32 | mingw-w64-x86_64-cc 33 | mingw-w64-x86_64-SDL2 34 | mingw-w64-x86_64-SDL2_image 35 | mingw-w64-x86_64-SDL2_ttf 36 | mingw-w64-x86_64-freeglut 37 | 38 | - uses: actions/checkout@v2 39 | name: Checkout source code 40 | 41 | - name: Put MSYS2_MinGW64 on PATH 42 | run: echo "${{ runner.temp }}/msys64/mingw64/bin" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append 43 | 44 | - name: CMake configure 45 | run: cmake -DCMAKE_POLICY_VERSION_MINIMUM="3.5" -B build 46 | 47 | - name: CMake build 48 | run: cmake --build build 49 | -------------------------------------------------------------------------------- /debian/rules: -------------------------------------------------------------------------------- 1 | #!/usr/bin/make -f 2 | 3 | # Uncomment this to turn on verbose mode. 4 | export DH_VERBOSE=1 5 | 6 | CFLAGS = -Wall 7 | 8 | build: build-stamp 9 | build-stamp: $(QUILT_STAMPFN) 10 | dh_testdir 11 | 12 | $(MAKE) -j$(nproc) 13 | 14 | touch $@ 15 | 16 | clean: 17 | dh_testdir 18 | dh_testroot 19 | rm -f build-stamp 20 | [ ! -f Makefile ] || $(MAKE) clean 21 | dh_clean 22 | 23 | install: build 24 | dh_testdir 25 | dh_testroot 26 | dh_clean 27 | dh_installdirs 28 | 29 | $(MAKE) INSTALL_PREFIX=$(CURDIR)/debian/rm501 install 30 | 31 | # Build architecture-independent files here. 32 | binary-indep: build install 33 | dh_testdir 34 | dh_testroot 35 | # dh_installchangelogs ChangeLog 36 | dh_installdocs -i 37 | dh_install -i 38 | dh_installman -i 39 | dh_compress -i 40 | dh_fixperms 41 | dh_installdeb 42 | dh_gencontrol 43 | dh_md5sums 44 | dh_builddeb 45 | 46 | # Build architecture-dependent files here. 47 | binary-arch: build install 48 | dh_testdir 49 | dh_testroot 50 | # dh_installchangelogs ChangeLog 51 | dh_installdocs -a README.md 52 | dh_install -a --sourcedir=$(CURDIR)/debian/rm501 53 | dh_installman rm501.1 54 | dh_installmime 55 | dh_installmenu 56 | dh_link 57 | dh_strip 58 | dh_compress -a 59 | dh_fixperms 60 | dh_makeshlibs 61 | dh_installdeb 62 | dh_shlibdeps 63 | dh_gencontrol 64 | dh_md5sums 65 | dh_builddeb 66 | 67 | binary: binary-indep binary-arch 68 | .PHONY: build clean binary-indep binary-arch binary install configure 69 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.24) 2 | project(rm501 LANGUAGES C) 3 | 4 | include(FetchContent) 5 | Set(FETCHCONTENT_QUIET FALSE) 6 | 7 | FetchContent_Declare( 8 | SDL2 9 | GIT_REPOSITORY https://github.com/libsdl-org/SDL.git 10 | GIT_TAG release-2.32.8 11 | GIT_SHALLOW TRUE 12 | GIT_PROGRESS TRUE 13 | ) 14 | FetchContent_MakeAvailable(SDL2) 15 | 16 | FetchContent_Declare( 17 | SDL2_image 18 | GIT_REPOSITORY https://github.com/libsdl-org/SDL_image.git 19 | GIT_TAG release-2.8.8 20 | GIT_SHALLOW TRUE 21 | GIT_PROGRESS TRUE 22 | ) 23 | 24 | # set(SDL2IMAGE_INSTALL OFF) 25 | # set(BUILD_SHARED_LIBS FALSE) 26 | 27 | FetchContent_MakeAvailable(SDL2_image) 28 | 29 | set(SDL2TTF_VENDORED ON) 30 | FetchContent_Declare( 31 | SDL2_ttf 32 | GIT_REPOSITORY https://github.com/libsdl-org/SDL_ttf.git 33 | GIT_TAG release-2.24.0 34 | GIT_SHALLOW TRUE 35 | GIT_PROGRESS TRUE 36 | ) 37 | FetchContent_MakeAvailable(SDL2_ttf) 38 | set(SDL2TTF_FREETYPE OFF) 39 | 40 | add_definitions(-DHAVE_SDL -DHAVE_TRAJGEN) 41 | set(SOURCES rm501.c trajgen.c) 42 | add_executable(rm501 ${SOURCES}) 43 | 44 | find_package(OpenGL REQUIRED COMPONENTS OpenGL) 45 | find_package(GLUT REQUIRED) 46 | 47 | if (NOT ${OPENGL_FOUND}) 48 | message(FATAL_ERROR "OpenGL not found") 49 | endif() 50 | 51 | if (NOT TARGET OpenGL::GLU) 52 | message(FATAL_ERROR "GLU not found") 53 | endif(NOT TARGET OpenGL::GLU) 54 | 55 | target_link_libraries(rm501 56 | PRIVATE 57 | SDL2::SDL2main 58 | SDL2 59 | SDL2_image 60 | SDL2_ttf 61 | OpenGL::GL 62 | OpenGL::GLU 63 | GLUT::GLUT 64 | m 65 | ) 66 | -------------------------------------------------------------------------------- /.github/workflows/ubuntu.yml: -------------------------------------------------------------------------------- 1 | name: ubuntu 2 | 3 | on: 4 | push: 5 | branches: [ main ] 6 | pull_request: 7 | branches: [ main ] 8 | 9 | jobs: 10 | build: 11 | strategy: 12 | fail-fast: false 13 | matrix: 14 | target: 15 | - ubuntu-22.04 16 | - ubuntu-24.04 17 | runs-on: ${{ matrix.target }} 18 | 19 | steps: 20 | - uses: actions/checkout@v3 21 | 22 | - name: update build 23 | run: | 24 | sudo apt -qqq -y update 25 | sudo apt -y dist-upgrade 26 | 27 | - name: install devscripts 28 | run: | 29 | sudo DEBIAN_FRONTEND=noninteractive apt -qq -y install devscripts equivs lintian 30 | mk-build-deps -i -s sudo -t "apt --yes --no-install-recommends" 31 | 32 | - name: build source package 33 | env: 34 | DEBFULLNAME: "Jakob Flierl" 35 | DEBEMAIL: "jakob.flierl@gmail.com" 36 | run: | 37 | export TARGET=$(. /etc/lsb-release && echo $DISTRIB_CODENAME) 38 | git fetch --unshallow 39 | git fetch --tags 40 | VERSION="1-$(git describe --always --tags | sed -e "s/^v//" -e "s/-/+git/")" 41 | dch --create \ 42 | --distribution ${TARGET} \ 43 | --package rm501 \ 44 | --newversion ${VERSION}~${TARGET}1 \ 45 | "Automatic build from Github" 46 | debuild -S -sa -us -uc -d 47 | 48 | - name: build binary package 49 | run: dpkg-buildpackage -b -rfakeroot -us -uc 50 | 51 | - name: install binary package 52 | run: sudo dpkg -i ../rm501*deb || true 53 | 54 | - name: install binary package dependencies 55 | run: sudo apt -f install 56 | 57 | - name: run lintian 58 | run: lintian ../rm501*deb | lintian-info 59 | 60 | - name: upload artifacts 61 | uses: actions/upload-artifact@v4 62 | with: 63 | name: ${{ matrix.target }} 64 | if-no-files-found: error 65 | path: | 66 | *.buildinfo 67 | *.changes 68 | *.dsc 69 | *.tar.* 70 | *.deb 71 | ~/**/*/*.buildinfo 72 | ~/**/*/*.changes 73 | ~/**/*/*.dsc 74 | ~/**/*/*.tar.* 75 | ~/**/*/*.deb 76 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Mitsubishi RM-501 Movemaster II Robot Simulator 2 | 3 | A cross-platform simulator for the iconic Mitsubishi RM-501 Movemaster II robot arm, popular in the 1980s for industrial assembly, handling, and inspection tasks. 4 | 5 | ## Overview 6 | 7 | - **Five-axis robot arm simulation** 8 | - Interactive OpenGL GUI 9 | - Real-time keyboard, gamepad, and 3D mouse control 10 | - Forward and inverse kinematics 11 | - Simple and portable C codebase 12 | 13 | [![YouTube Demo](https://markdown-videos-api.jorgenkh.no/url?url=https%3A%2F%2Fwww.youtube.com%2Fwatch%3Fv%3DdLeDPIRKhOw)](https://www.youtube.com/watch?v=dLeDPIRKhOw) 14 | [![Build: Windows](../../actions/workflows/windows.yml/badge.svg)](../../actions/workflows/windows.yml) 15 | [![Build: Ubuntu](../../actions/workflows/ubuntu.yml/badge.svg)](../../actions/workflows/ubuntu.yml) 16 | 17 | --- 18 | 19 | ## Features 20 | 21 | **Working:** 22 | - OpenGL GUI 23 | - Control via keyboard, gamepad, or 3D mouse 24 | - Double-precision forward/inverse kinematics 25 | - Minimal C source files for easy portability 26 | 27 | **Experimental/Unfinished:** 28 | - Trajectory planner (by Stefan Wilhelm) 29 | - Command-line ncurses TUI 30 | - LinuxCNC HAL integration 31 | - Messaging: ØMQ, Eclipse MQTT, and others 32 | 33 | --- 34 | 35 | ## Getting Started 36 | 37 | ### Prerequisites 38 | 39 | - **Required:** OpenGL, SDL2, SDL2_ttf 40 | - **Optional:** ncurses, libpng, ZeroMQ, Eclipse Mosquitto, LinuxCNC HAL 41 | 42 | ### Installation (Debian/Ubuntu): 43 | 44 | ```bash 45 | sudo apt -y install git mesa-common-dev libsdl2-dev libsdl2-ttf-dev libsdl2-image-dev freeglut3-dev 46 | sudo apt -y install fonts-dejavu-core 47 | sudo apt -y install libncursesw5-dev libpng-dev libzmq5 libzmq3-dev libpaho-mqtt-dev mosquitto linuxcnc-uspace-dev 48 | sudo usermod -aG input $USER # joystick access 49 | git clone https://github.com/koppi/rm501.git 50 | ``` 51 | 52 | ### Compile & Run 53 | 54 | ```bash 55 | cd rm501 56 | make 57 | ./rm501 58 | ``` 59 | 60 | --- 61 | 62 | ## Controls 63 | 64 | | Key(s) | Action | 65 | |-------------------------------------- |---------------------------------------| 66 | | Esc / Ctrl-C | Quit | 67 | | Tab | Toggle 3D view | 68 | | Q/W/E/R/T, A/S/D/F/G | +/- joint values (waist, shoulder, etc.) | 69 | | I/K, arrow keys | Move end-effector | 70 | | C/V | Circle motion / Reset circle | 71 | | N/H | Move to nest/home positions | 72 | | O/L | Open/close tool | 73 | | Shift | Move all joints in parallel | 74 | 75 | - **SpaceNavigator:** Enable `HAVE_SPACENAV` 76 | - **Gamepad:** Enable `HAVE_JOYSTICK` 77 | - **Messaging (MQTT/ØMQ):** Enable `HAVE_MQTT` / `HAVE_ZMQ` 78 | - **LinuxCNC:** Enable `HAVE_HAL` 79 | 80 | --- 81 | 82 | ## Contributors 83 | 84 | - [Jakob Flierl (koppi)](https://github.com/koppi) 85 | - [Stefan Wilhelm (trajectory planner)](https://atwillys.de/content/cc/trajectory-generator-in-c) 86 | 87 | --- 88 | 89 | ## License 90 | 91 | LGPL-2.1-or-later. See [LICENSE](LICENSE). 92 | -------------------------------------------------------------------------------- /GNUmakefile: -------------------------------------------------------------------------------- 1 | CC=$(CROSS)gcc 2 | LD=$(CROSS)ld 3 | AR=$(CROSS)ar 4 | PKG_CONFIG=$(CROSS)pkg-config 5 | SDL_CONFIG=$(CROSS)sdl2-config 6 | 7 | BIN := rm501 8 | 9 | # install prefix either /usr or /usr/local on most unix systems 10 | PREFIX ?= /usr 11 | 12 | ifeq ($(OS),Windows_NT) 13 | OPENGL_LIBS:=-lopengl32 -lglu32 14 | else 15 | OPENGL_LIBS:=-lGL -lGLU 16 | endif 17 | 18 | REQPKG=sdl2 19 | REQPKG:=$(shell $(PKG_CONFIG) --exists $(REQPKG) && echo '$(REQPKG)') 20 | ifneq ($(REQPKG),) 21 | SDL_CFLAGS := -DHAVE_SDL $(shell $(SDL_CONFIG) --cflags) 22 | SDL_LDLIBS := $(shell $(SDL_CONFIG) --libs) $(OPENGL_LIBS) 23 | endif 24 | 25 | REQPKG=SDL2_ttf 26 | REQPKG:=$(shell $(PKG_CONFIG) --exists $(REQPKG) && echo '$(REQPKG)') 27 | ifneq ($(REQPKG),) 28 | SDL_CFLAGS += $(shell $(PKG_CONFIG) --cflags $(REQPKG)) 29 | SDL_LDLIBS += $(shell $(PKG_CONFIG) --libs $(REQPKG)) 30 | endif 31 | 32 | REQPKG=SDL2_image 33 | REQPKG:=$(shell $(PKG_CONFIG) --exists $(REQPKG) && echo '$(REQPKG)') 34 | ifneq ($(REQPKG),) 35 | SDL_CFLAGS += $(shell $(PKG_CONFIG) --cflags $(REQPKG)) 36 | SDL_LDLIBS += $(shell $(PKG_CONFIG) --libs $(REQPKG)) # -lpng 37 | endif 38 | 39 | REQPKG=libpng 40 | REQPKG:=$(shell $(PKG_CONFIG) --exists $(REQPKG) && echo '$(REQPKG)') 41 | ifneq ($(REQPKG),) 42 | SDL_CFLAGS += -DHAVE_PNG $(shell $(PKG_CONFIG) --cflags $(REQPKG)) 43 | SDL_LDLIBS += $(shell $(PKG_CONFIG) --libs $(REQPKG)) 44 | endif 45 | 46 | REQPKG=ncursesw 47 | REQPKG:=$(shell $(PKG_CONFIG) --exists $(REQPKG) && echo '$(REQPKG)') 48 | ifneq ($(REQPKG),) 49 | CURSES_CFLAGS := -DHAVE_NCURSES $(shell $(PKG_CONFIG) --cflags $(REQPKG)) 50 | CURSES_LDLIBS := $(shell $(PKG_CONFIG) --libs $(REQPKG)) 51 | endif 52 | 53 | # comment out to disable LinuxCNC / Machinekit HAL component functionality 54 | #HAL_CFLAGS := -DHAVE_HAL -I/usr/include/linuxcnc -DRTAPI -DTHREAD_FLAVOR_ID=RTAPI_POSIX_ID 55 | #HAL_LDLIBS := -llinuxcnchal -lprotobuf 56 | 57 | # comment out to disable ZeroMQ functionality 58 | #ZMQ_CFLAGS := -DHAVE_ZMQ 59 | #ZMQ_LDLIBS := -l:libzmq.so.5 60 | 61 | # comment out to disable Eclipse MQTT functionality 62 | REQPKG=libmosquitto 63 | REQPKG:=$(shell $(PKG_CONFIG) --exists $(REQPKG) && echo '$(REQPKG)') 64 | ifneq ($(REQPKG),) 65 | MQTT_CFLAGS := -DHAVE_MQTT 66 | MQTT_LDLIBS := -lpaho-mqtt3c 67 | MQTT_OBJS := mqtt_handler.o 68 | endif 69 | 70 | # comment out to disable trajectory calculation functionality 71 | TRAJGEN_CFLAGS := -DHAVE_TRAJGEN 72 | TRAJGEN_LDLIBS := 73 | TRAJGEN_OBJS := trajgen.o 74 | 75 | CFLAGS += $(SDL_CFLAGS) $(CURSES_CFLAGS) $(HAL_CFLAGS) $(ZMQ_CFLAGS) $(MQTT_CFLAGS) $(TRAJGEN_CFLAGS) -O2 -g -Wall -std=gnu11 76 | CFLAGS += -Wno-unused-variable 77 | LDLIBS += $(SDL_LDLIBS) $(CURSES_LDLIBS) $(HAL_LDLIBS) $(ZMQ_LDLIBS) $(MQTT_LDLIBS) $(TRAJGEN_LDLIBS) -lm 78 | LDFLAGS += -Wl,--as-needed 79 | 80 | OBJS = $(BIN).o $(MQTT_OBJS) $(TRAJGEN_OBJS) 81 | 82 | all: $(BIN) 83 | 84 | DEPS := $(OBJS:.o=.d) 85 | -include $(DEPS) 86 | 87 | %.o: %.c 88 | $(CC) $(CFLAGS) -MD -c -o $@ $< 89 | 90 | $(BIN): $(OBJS) 91 | $(CC) -o $@ $+ $(LDFLAGS) $(LDLIBS) 92 | 93 | $(BIN).1: $(BIN) 94 | help2man -N ./$+ > $@ 95 | 96 | install: $(BIN) 97 | strip $(BIN) 98 | mkdir -p $(INSTALL_PREFIX)$(PREFIX)/bin 99 | install -m 0755 $(BIN) $(INSTALL_PREFIX)$(PREFIX)/bin 100 | mkdir -p $(INSTALL_PREFIX)$(PREFIX)/share/icons/hicolor/512x512/apps 101 | install -m 0664 $(BIN).png $(INSTALL_PREFIX)$(PREFIX)/share/icons/hicolor/512x512/apps 102 | mkdir -p $(INSTALL_PREFIX)$(PREFIX)/share/applications 103 | install -m 0664 $(BIN).desktop $(INSTALL_PREFIX)$(PREFIX)/share/applications 104 | 105 | uninstall: 106 | rm $(INSTALL_PREFIX)$(PREFIX)/bin/$(BIN) 107 | rm $(INSTALL_PREFIX)$(PREFIX)/share/icons/hicolor/512x512/apps/$(BIN).png 108 | rm $(INSTALL_PREFIX)$(PREFIX)/share/applications/$(BIN).desktop 109 | 110 | clean: 111 | rm -f *~ $(OBJS) $(DEPS) $(BIN) 112 | 113 | pull: 114 | git pull origin main --rebase 115 | 116 | push: 117 | git push origin HEAD:main 118 | 119 | -------------------------------------------------------------------------------- /png.c: -------------------------------------------------------------------------------- 1 | /* 2 | * SDL_SavePNG -- libpng-based SDL_Surface writer. 3 | * 4 | * This code is free software, available under zlib/libpng license. 5 | * http://www.libpng.org/pub/png/src/libpng-LICENSE.txt 6 | */ 7 | #include 8 | #include 9 | 10 | #define SUCCESS 0 11 | #define ERROR -1 12 | 13 | #define USE_ROW_POINTERS 14 | 15 | #if SDL_BYTEORDER == SDL_BIG_ENDIAN 16 | #define rmask 0xFF000000 17 | #define gmask 0x00FF0000 18 | #define bmask 0x0000FF00 19 | #define amask 0x000000FF 20 | #else 21 | #define rmask 0x000000FF 22 | #define gmask 0x0000FF00 23 | #define bmask 0x00FF0000 24 | #define amask 0xFF000000 25 | #endif 26 | 27 | /* libpng callbacks */ 28 | static void png_error_SDL(png_structp ctx, png_const_charp str) 29 | { 30 | SDL_SetError("libpng: %s\n", str); 31 | } 32 | static void png_write_SDL(png_structp png_ptr, png_bytep data, png_size_t length) 33 | { 34 | SDL_RWops *rw = (SDL_RWops*)png_get_io_ptr(png_ptr); 35 | SDL_RWwrite(rw, data, sizeof(png_byte), length); 36 | } 37 | 38 | SDL_Surface *SDL_PNGFormatAlpha(SDL_Surface *src) 39 | { 40 | SDL_Surface *surf; 41 | SDL_Rect rect = { 0 }; 42 | 43 | /* NO-OP for images < 32bpp and 32bpp images that already have Alpha channel */ 44 | if (src->format->BitsPerPixel <= 24 || src->format->Amask) { 45 | src->refcount++; 46 | return src; 47 | } 48 | 49 | /* Convert 32bpp alpha-less image to 24bpp alpha-less image */ 50 | rect.w = src->w; 51 | rect.h = src->h; 52 | surf = SDL_CreateRGBSurface(src->flags, src->w, src->h, 24, 53 | src->format->Rmask, src->format->Gmask, src->format->Bmask, 0); 54 | SDL_LowerBlit(src, &rect, surf, &rect); 55 | 56 | return surf; 57 | } 58 | 59 | int SDL_SavePNG_RW(SDL_Surface *surface, SDL_RWops *dst, int freedst) 60 | { 61 | png_structp png_ptr; 62 | png_infop info_ptr; 63 | png_colorp pal_ptr; 64 | SDL_Palette *pal; 65 | int i, colortype; 66 | #ifdef USE_ROW_POINTERS 67 | png_bytep *row_pointers; 68 | #endif 69 | /* Initialize and do basic error checking */ 70 | if (!dst) 71 | { 72 | SDL_SetError("Argument 2 to SDL_SavePNG_RW can't be NULL, expecting SDL_RWops*\n"); 73 | return (ERROR); 74 | } 75 | if (!surface) 76 | { 77 | SDL_SetError("Argument 1 to SDL_SavePNG_RW can't be NULL, expecting SDL_Surface*\n"); 78 | if (freedst) SDL_RWclose(dst); 79 | return (ERROR); 80 | } 81 | png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, png_error_SDL, NULL); /* err_ptr, err_fn, warn_fn */ 82 | if (!png_ptr) 83 | { 84 | SDL_SetError("Unable to png_create_write_struct on %s\n", PNG_LIBPNG_VER_STRING); 85 | if (freedst) SDL_RWclose(dst); 86 | return (ERROR); 87 | } 88 | info_ptr = png_create_info_struct(png_ptr); 89 | if (!info_ptr) 90 | { 91 | SDL_SetError("Unable to png_create_info_struct\n"); 92 | png_destroy_write_struct(&png_ptr, NULL); 93 | if (freedst) SDL_RWclose(dst); 94 | return (ERROR); 95 | } 96 | if (setjmp(png_jmpbuf(png_ptr))) /* All other errors, see also "png_error_SDL" */ 97 | { 98 | png_destroy_write_struct(&png_ptr, &info_ptr); 99 | if (freedst) SDL_RWclose(dst); 100 | return (ERROR); 101 | } 102 | 103 | /* Setup our RWops writer */ 104 | png_set_write_fn(png_ptr, dst, png_write_SDL, NULL); /* w_ptr, write_fn, flush_fn */ 105 | 106 | /* Prepare chunks */ 107 | colortype = PNG_COLOR_MASK_COLOR; 108 | if (surface->format->BytesPerPixel > 0 109 | && surface->format->BytesPerPixel <= 8 110 | && (pal = surface->format->palette)) 111 | { 112 | colortype |= PNG_COLOR_MASK_PALETTE; 113 | pal_ptr = (png_colorp)malloc(pal->ncolors * sizeof(png_color)); 114 | for (i = 0; i < pal->ncolors; i++) { 115 | pal_ptr[i].red = pal->colors[i].r; 116 | pal_ptr[i].green = pal->colors[i].g; 117 | pal_ptr[i].blue = pal->colors[i].b; 118 | } 119 | png_set_PLTE(png_ptr, info_ptr, pal_ptr, pal->ncolors); 120 | free(pal_ptr); 121 | } 122 | else if (surface->format->BytesPerPixel > 3 || surface->format->Amask) 123 | colortype |= PNG_COLOR_MASK_ALPHA; 124 | 125 | png_set_IHDR(png_ptr, info_ptr, surface->w, surface->h, 8, colortype, 126 | PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT, PNG_FILTER_TYPE_DEFAULT); 127 | 128 | // png_set_packing(png_ptr); 129 | 130 | /* Allow BGR surfaces */ 131 | if (surface->format->Rmask == bmask 132 | && surface->format->Gmask == gmask 133 | && surface->format->Bmask == rmask) 134 | png_set_bgr(png_ptr); 135 | 136 | /* Write everything */ 137 | png_write_info(png_ptr, info_ptr); 138 | #ifdef USE_ROW_POINTERS 139 | row_pointers = (png_bytep*) malloc(sizeof(png_bytep)*surface->h); 140 | for (i = 0; i < surface->h; i++) 141 | row_pointers[i] = (png_bytep)(Uint8*)surface->pixels + i * surface->pitch; 142 | png_write_image(png_ptr, row_pointers); 143 | free(row_pointers); 144 | #else 145 | for (i = 0; i < surface->h; i++) 146 | png_write_row(png_ptr, (png_bytep)(Uint8*)surface->pixels + i * surface->pitch); 147 | #endif 148 | png_write_end(png_ptr, info_ptr); 149 | 150 | /* Done */ 151 | png_destroy_write_struct(&png_ptr, &info_ptr); 152 | if (freedst) SDL_RWclose(dst); 153 | return (SUCCESS); 154 | } 155 | -------------------------------------------------------------------------------- /mqtt.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include "mqtt_handler.h" 15 | 16 | MQTTClient client; 17 | MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer; 18 | MQTTClient_message pubmsg = MQTTClient_message_initializer; 19 | MQTTClient_deliveryToken token; 20 | int rc; 21 | 22 | char incoming_message[64] = ""; 23 | char incoming_flag = 0; 24 | 25 | pthread_mutex_t incoming_mutex; 26 | 27 | // forward declaration 28 | void publish_cordinate(coord_t); 29 | int subscribe_callback(void* , char*, int, MQTTClient_message*); 30 | 31 | void print_error(coord_t c1, coord_t c2){ 32 | fprintf(stderr, "\nerror: %.1f, %.1f, %.1f, %.1f, %.1f", \ 33 | c1.x-c2.x, c1.y-c2.y, c1.z-c2.z,\ 34 | c1.pitch-c2.pitch, c1.roll-c2.roll); 35 | } 36 | 37 | /** 38 | * @brief verifies if two coordinates are equal 39 | * @param c1 first coordinate 40 | * @param c2 second coordinate 41 | * @param epsilon allowable error 42 | * @retval result (true or false) 43 | */ 44 | bool coord_equal(coord_t c1, coord_t c2, float epsilon) { 45 | if ( fabs( c1.x-c2.x ) < epsilon && 46 | fabs( c1.y-c2.y ) < epsilon && 47 | fabs( c1.z-c2.z ) < epsilon && 48 | fabs( c1.pitch-c2.pitch ) < epsilon && 49 | fabs( c1.roll-c2.roll ) < epsilon && 50 | c1.grip==c2.grip 51 | ) { 52 | return true; 53 | } else { 54 | return false; 55 | } 56 | } 57 | 58 | /** 59 | * @brief Handle lost connection 60 | */ 61 | void connection_lost_callback() { 62 | fprintf(stderr, "\nConnectin lost\n"); 63 | while(1) { 64 | sleep(5); 65 | fprintf(stderr, "\nTrying to reconnect\n"); 66 | if (MQTTClient_connect(client, &conn_opts) == MQTTCLIENT_SUCCESS) { 67 | fprintf(stderr, "\nConnection re-established\n"); 68 | MQTTClient_subscribe(client, TOPIC, QOS); 69 | break; 70 | } 71 | } 72 | } 73 | 74 | /** 75 | * @brief Initialize the handler 76 | */ 77 | void mqtt_handler_init() { 78 | 79 | pthread_mutex_init(&incoming_mutex, NULL); 80 | 81 | char clientID[10]; 82 | //generate unique ID 83 | snprintf(clientID, sizeof(clientID), "sim%ld", time(0)%100000); 84 | 85 | MQTTClient_create(&client, ADDRESS, clientID, 86 | MQTTCLIENT_PERSISTENCE_NONE, NULL); 87 | 88 | // MQTT Connection parameters 89 | conn_opts.keepAliveInterval = 20; 90 | conn_opts.cleansession = 1; 91 | 92 | rc = MQTTClient_setCallbacks(client, NULL, connection_lost_callback, subscribe_callback, NULL); 93 | 94 | if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS) { 95 | fprintf(stderr, "Error: failed to connect to '%s', return code %d\n", ADDRESS, rc); 96 | exit(-1); 97 | } 98 | 99 | if (rc != MQTTCLIENT_SUCCESS) { 100 | MQTTClient_destroy(&client); 101 | return; 102 | } 103 | 104 | rc = MQTTClient_subscribe(client, TOPIC, QOS); 105 | } 106 | 107 | /** 108 | * @brief Close the handler 109 | */ 110 | int mqtt_handler_close() { 111 | 112 | // Disconnect 113 | rc = MQTTClient_unsubscribe(client, TOPIC); 114 | 115 | MQTTClient_disconnect(client, 10000); 116 | MQTTClient_destroy(&client); 117 | 118 | return rc; 119 | } 120 | 121 | /** 122 | * @brief this function must be called on the infinite loop 123 | * @param coord pointer to actual coordintes 124 | * @param allowPub flag to allow publishing 125 | * @retval flag if an update was made 126 | */ 127 | int mqtt_periodic_callback(coord_t* coord, bool allowPub) { 128 | static int run_flag = 0; 129 | static coord_t last_coord; 130 | static time_t last_time = 0; 131 | time_t now = clock(); 132 | int ret_flag = 0; 133 | 134 | //check if min delta t has passed 135 | if ( ((double)(now - last_time) / (double)CLOCKS_PER_SEC) >= (double)MIN_DELTA_T \ 136 | && allowPub) { 137 | 138 | last_time = now; 139 | 140 | //if this is the first time 141 | if (run_flag == 0) { 142 | run_flag = 1; 143 | last_coord = *coord; //this will prevent publish 144 | } 145 | 146 | //check if simulator has moved 147 | if ( !coord_equal(last_coord,*coord, EPSILON) ) { 148 | last_coord = *coord; //publish movement 149 | publish_cordinate(last_coord); 150 | } 151 | } 152 | 153 | //check subscription 154 | //take mutex 155 | pthread_mutex_lock(&incoming_mutex); 156 | 157 | if (incoming_flag) { //if a message arrived 158 | incoming_flag = 0; 159 | 160 | coord_t aux; 161 | char grip = 0; 162 | double time; 163 | 164 | //decode the message 165 | sscanf(incoming_message, "%lf %f %f %f %f %f %c", 166 | &time, &aux.x, &aux.y, &aux.z, &aux.pitch, &aux.roll, &grip); 167 | aux.grip = (grip=='C') ? 0 : 1; 168 | 169 | //only apply if movement is significant 170 | if (!coord_equal(last_coord, aux, EPSILON) ){ 171 | printf("\nApply : '%s'\n", incoming_message); 172 | *coord = aux; //apply coordinates 173 | last_coord = aux; //remember coordinates 174 | ret_flag = 1; //notify update 175 | 176 | } 177 | } 178 | 179 | //release mutex 180 | pthread_mutex_unlock(&incoming_mutex); 181 | 182 | return ret_flag; 183 | } 184 | 185 | void publish_cordinate(coord_t c) { 186 | char message[64]; 187 | struct timeval tv; 188 | gettimeofday(&tv, NULL); 189 | 190 | snprintf(message, sizeof(message), 191 | "%lf %+.1f %+.1f %+.1f %+.1f %.1f %c", 192 | tv.tv_sec + tv.tv_usec * 0.000001, 193 | c.x, c.y, c.z, c.pitch, c.roll, c.grip?'O':'C'); 194 | 195 | fprintf(stderr, "\npublish: '%s'\n", message); 196 | 197 | // Publish message 198 | pubmsg.payload = message; 199 | pubmsg.payloadlen = strlen(message); 200 | pubmsg.qos = QOS; 201 | pubmsg.retained = 1; 202 | MQTTClient_publishMessage(client, TOPIC, &pubmsg, &token); 203 | } 204 | 205 | int subscribe_callback(void* context, char* topicName, int topicLen, MQTTClient_message* m) { 206 | if (m->payloadlen >= sizeof(incoming_message)) 207 | return 1; // reject if message is too big 208 | 209 | // take mutex 210 | pthread_mutex_lock(&incoming_mutex); 211 | 212 | strncpy(incoming_message, m->payload, m->payloadlen); 213 | incoming_message[m->payloadlen] = '\0'; // terminate string 214 | incoming_flag = 1; 215 | 216 | fprintf(stderr, "\nsubscribe: '%s'\n", incoming_message); 217 | 218 | // release mutex 219 | pthread_mutex_unlock(&incoming_mutex); 220 | 221 | MQTTClient_free(topicName); 222 | MQTTClient_freeMessage(&m); 223 | return 1; 224 | } 225 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /trajgen.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * @file trajgen.h 3 | * 4 | * Allows to move the machine tool tip to specified positions, velocities and 5 | * accelerations by modifying a virtual machine position every generator cycle. 6 | * The output is always an updated position for every joint. The closed loop 7 | * controllers of the joints follow this position or, if the cannot, raise a 8 | * trailing error fault. 9 | * 10 | * @author: Stefan Wilhelm (c erberos@atwillys.de) 11 | * @license: BSD (see below) 12 | * @version: 1.0b 13 | * @standard C99 14 | * 15 | * ----------------------------------------------------------------------------- 16 | * 17 | * Coordinated motion (coord planer, manual) are related the planer pose (tool 18 | * tip position and direction), t.m. a new pose is calculated first, then the 19 | * joint position are determined using inverse kinematics. The joint planers 20 | * update the scalar position of every joint individually, the planer pose is 21 | * updated (for status or equivalence) using forward kinematics. After the new 22 | * joint positions are computed, they are used to feed the interpolators. The 23 | * output of the interpolatoes is the final output of the trajgen and can be 24 | * used as command position for the closed loop controllers. 25 | * 26 | * As wrapper for the different trajectory planers, the main trajgen organises 27 | * the initialisation, reset and synchronisation of the submodules. It is based 28 | * on a finite state machine. This means the generator can be only in one defined 29 | * state, and switching between states causes leaving the current state (wait 30 | * for the current motion to be finished and cleanup after the sub modules), and 31 | * entering a new state (initialise and switch to module run state). Hence, 32 | * switching takes at least one tick (cycle period). 33 | * 34 | * Usage: 35 | * 36 | * Variables: 37 | * 38 | * All data of the trajgen are stored in one single data structure of the 39 | * type trajgen_t. This structure contains the configuration, the current 40 | * state, the (requested) state to switch to, the last error occurred, 41 | * current pose, kinematic data, coordinated planer data, manual op data, 42 | * as well as data for individual joints. Latter are joint data structures 43 | * containing data of the interpolators, joint planers, and the 44 | * joint command position outputs. 45 | * 46 | * You can decide how and where this structure is located, either as part 47 | * of a bigger data structure, global, local, stack, heap, etc. Only few 48 | * aspects are important: 49 | * 50 | * (1) The location must not change, as the pointer is saved in a 51 | * static variable. 52 | * 53 | * (2) There can be only one main trajgen (but multiple sub-planers 54 | * e.g. for simulation purposes). 55 | * 56 | * Synchronisation and threading 57 | * 58 | * Thread safety is not implemented due to platform dependencies. You must 59 | * take care yourself that no other thread or shared memory write process 60 | * interferes with any function of the trajgen. It absolutely assumes that 61 | * the data are not changed during a function call. 62 | * 63 | * Configuration and initialisation: 64 | * 65 | * The trajgen is configured once in the startup phase of the machine. 66 | * Create a variable of type trajgen_config_t and pass it together with 67 | * the pointer to the main trajgen structure to trajgen_initialize(); 68 | * The config will not be modified by any internal function. 69 | * 70 | * Reset: 71 | * 72 | * Calling trajgen_reset() will reset the internal state and internal 73 | * variables of the trajgen and all submodules to a "clean" disabled state. 74 | * The config will not be changed. The state will be 0 (TRAJ_STATE_DISABLED) 75 | * and is_done will be 1 (true). 76 | * 77 | * Cyclic function: 78 | * 79 | * Call trajgen_tick() with the sample rate that the servo position 80 | * need to be updated. The configuration variable "interpolation_rate" 81 | * decides how frequently the trajectory needs to be recalculated. For a 82 | * closed loop sample rate of 2KHz and an interpolation rate of 4, the 83 | * trajectory will be recalculated with 500Hz, the missing cycles will be 84 | * filled by the cubic interpolator of each joint. This means also that 85 | * synchronous outputs and condition handling will be done with 500Hz. 86 | * 87 | * State switching: 88 | * 89 | * Use trajgen_switch_state() to switch between the submodules. After calling 90 | * the function, is_done will be 0 and remain zero until the current planer 91 | * stopped moving, is cleaned up and the new state is in running state. 92 | * However, the state might remain zero if the newly selected generator 93 | * started moving already. 94 | * 95 | * States: 96 | * 97 | * TRAJ_STATE_DISABLED: 98 | * 99 | * The trajgen is idle and does not perform any calculations or 100 | * position updates. 101 | * 102 | * TRAJ_STATE_COORDINATED: 103 | * 104 | * The coordinated motion planer is running and processes its 105 | * queue. 106 | * 107 | * TRAJ_STATE_MAN: 108 | * 109 | * The (coordinated) teleoperation planer is running and processes 110 | * according its given input velocities. 111 | * 112 | * TRAJ_STATE_JOINT: 113 | * 114 | * The joint individual planers are active. 115 | * 116 | * Submodule access: 117 | * 118 | * You have full access to the individual planners and get in touch with 119 | * each you want to use if you define the export-enable macros described 120 | * below. 121 | * 122 | * Errors: 123 | * 124 | * Each of the trajgen and submodule functions that can cause errors return 125 | * an error code. The convention is: code 0 (zero) means OK, nonzero means 126 | * error. You should always treat trajgen errors that are called cyclic in 127 | * trajgen_coord_tick(), as well as initialisation errors, as fatal and cause 128 | * an e-stop by opening the machine's intermediate circuit immediately. 129 | * 130 | * Every generator has a variable last_error, where you can query the last 131 | * error occurred. This values are only valid if a function returned nonzero 132 | * before (the variable is not explicitly reset to 0 if a function returns 133 | * OK). The main trajgen's last_error is a structure containing one single 134 | * encoded 16bit error value (type trajgen_error_details_t). This error 135 | * code contains the 136 | * 137 | * (1) the code 138 | * (2) the joint, which is only valid if an interpolator or joint planer 139 | * failed. 140 | * 141 | * This error code allows to identify quite accurately what went wrong using 142 | * a single number that can be saved or passed to the user space easily. 143 | * 144 | * ----------------------------------------------------------------------------- 145 | * License header: Copyright (c) 2008-2014, Stefan Wilhelm. All rights reserved. 146 | * Redistribution and use in source and binary forms, with or without modification, 147 | * are permitted provided that the following conditions are met: (1) Redistributions 148 | * of source code must retain the above copyright notice, this list of conditions 149 | * and the following disclaimer. (2) Redistributions in binary form must reproduce 150 | * the above copyright notice, this list of conditions and the following disclaimer 151 | * in the documentation and/or other materials provided with the distribution. 152 | * (3) Neither the name of atwillys.de nor the names of its contributors may be 153 | * used to endorse or promote products derived from this software without specific 154 | * prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 155 | * AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 156 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 157 | * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 158 | * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 159 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 160 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 161 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 162 | * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 163 | * WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 164 | * DAMAGE. 165 | * 166 | ******************************************************************************/ 167 | 168 | #ifndef SW_TRAJGEN_H 169 | #define SW_TRAJGEN_H 170 | #ifdef __cplusplus 171 | extern "C" { 172 | #endif 173 | 174 | /* */ 175 | 176 | #if defined __linux__ 177 | #include 178 | #include 179 | #elif defined __MACH__ 180 | #include 181 | #include 182 | #elif defined __WIN32__ 183 | #include 184 | #else 185 | #error "Can you please define the rttypes for this platform and send a patch? :)" 186 | /* 187 | #define int8_t char 188 | #define uint8_t unsigned char 189 | #define int16_t short 190 | #define uint16_t unsigned short 191 | #define int32_t int 192 | #define uint32_t unsigned int 193 | #define int64_t long 194 | #define uint64_t unsigned long 195 | #define float32_t float 196 | #define float64_t double 197 | #define size_t unsigned int 198 | */ 199 | #endif 200 | 201 | /** 202 | * Allows to specify 32 bit or 64 bot float, unfortunately "float_t" is already 203 | * reserved on many frames, so "real_t" is used instead. Can be also redefined 204 | * as typedef. The macro is used to be able to switch it easily using the pre 205 | * compiler. 206 | */ 207 | #ifndef real_t 208 | #define real_t double 209 | #endif 210 | 211 | #ifndef float32_t 212 | #define float32_t float 213 | #endif 214 | 215 | /** 216 | * Explicit declaration of a boolean type. Can be switched for memory optimizsation 217 | * (e.g. char) or best data type for the processor (e.g. int) 218 | */ 219 | #ifndef bool_t 220 | #define bool_t unsigned 221 | #endif 222 | 223 | /** 224 | * Explicit declaration of a byte, no matter if signed or unsigned. Normally 225 | * used for bit operations or 8bit port i/o. 226 | */ 227 | #ifndef byte_t 228 | #define byte_t uint8_t 229 | #endif 230 | 231 | /** 232 | * "Auto include" float.h 233 | * Note: This allows you to include float.h or a RT optimised one yourself. 234 | */ 235 | #ifndef FLT_MAX 236 | #include 237 | #endif 238 | 239 | /** 240 | * "Auto include" math.h, characteristic macro: M_PI 241 | * Note: This allows you to include math.h or a RT optimised one yourself. 242 | */ 243 | #ifndef M_PI 244 | #include 245 | #endif 246 | 247 | 248 | /* 249 | * Defines the maximum number of joints the program can handle. 250 | */ 251 | #ifndef JOINT_MAX_JOINTS 252 | #define JOINT_MAX_JOINTS 9 253 | #endif 254 | 255 | /* 256 | * The maximum number of joints that the trajectory generator supports 257 | */ 258 | #ifndef TG_MAX_JOINTS 259 | #define TG_MAX_JOINTS JOINT_MAX_JOINTS 260 | #endif 261 | 262 | /* 263 | * The maximum size if the coordinated motion generator queue (fixed allocated) 264 | * Note: This is an intermediate fifo with big data structures and should be small. 265 | * You are supposed to dispatch and push new values from a realtime fifo whenever 266 | * possible. 267 | */ 268 | #ifndef TG_QUEUE_SIZE 269 | #define TG_QUEUE_SIZE 32 270 | #endif 271 | 272 | /* 273 | * Defines then threshold when the coordinated motion planer sees numbers as 274 | * zero or not. 275 | */ 276 | #ifndef TG_RESOLUTION 277 | #define TG_RESOLUTION 1e-6 278 | #endif 279 | 280 | /* 281 | * Smallest floating point number that is not zero. (Epsilon) 282 | */ 283 | #ifndef FLOAT_EPS 284 | #define FLOAT_EPS (sizeof(real_t) == sizeof(double) ? FLT_EPSILON : DBL_EPSILON) 285 | #endif 286 | 287 | /* 288 | * Spline/bezier iterations to find the length 289 | */ 290 | #ifndef TG_CURVE_ACC 291 | #define TG_CURVE_ACC (16) 292 | #endif 293 | 294 | /* 295 | * Sin/cos, can be replaced platform dependent with the corresponding builtin. 296 | * #define SINCOS(x, sx, cx) do { *(sx)=sin((x)); *(cx)=cos((x)); } while(0); 297 | */ 298 | 299 | /* 300 | * You can set your own memory clear (set to 0) function. By default a static 301 | * inline function is used that is independent of any external library. 302 | * #define ZERO_MEMORY(ptr, size) 303 | */ 304 | 305 | /* 306 | * Define this to force the trajgen to check argument pointers 307 | * #define ARG_POINTER_CHECKS 308 | */ 309 | 310 | /* 311 | * Normally you like to define your own joint type and config for the machine, 312 | * but if you to it has to be compliant with the joint_t here. 313 | * #define JOINT_DECL_NONE 314 | */ 315 | 316 | /* 317 | * You can switch off the interpolator feature here. Any settings related to 318 | * it will be ignored. The interpolation rate must be set to 1. 319 | * #define NO_INTERPOLATION 320 | */ 321 | 322 | /* 323 | * You can switch off the trajectory position/velocity synchronisation using: 324 | * #define NO_TRAJECTORY_SYNC 325 | */ 326 | 327 | /* 328 | * You can optionally export the interpolators or leave them 329 | * inline static in the implementation file to improve code optimisation. 330 | * Interpolator types and error definitions are not affected by this setting, 331 | * they are always exported. 332 | * #define EXPORT_INTERPOLATORS 333 | */ 334 | 335 | /* 336 | * You can optionally not export the joint planers or leave them 337 | * inline static in the implementation file to improve code optimisation. 338 | * Types and error definitions are not affected by this setting, only functions. 339 | * #define EXPORT_JOINT_TG 340 | */ 341 | 342 | /* 343 | * You can optionally not export the manual/joystick generator or leave it 344 | * inline static in the implementation file to improve code optimisation. 345 | * Types and error definitions are not affected by this setting, only functions. 346 | * #define EXPORT_MANUAL_TG 347 | */ 348 | 349 | /* 350 | * You can optionally not export the coordinated motion generator or leave it 351 | * inline static in the implementation file to improve code optimisation. 352 | * Types and error definitions are not affected by this setting, only functions. 353 | * #define EXPORT_COORD_TG 354 | */ 355 | 356 | /* 357 | * You can switch off the interpolator feature here. Any settings related to 358 | * it will be ignored. The interpolation rate must be set to 1. 359 | * #define NO_MOTION_BLENDING 360 | */ 361 | 362 | /* 363 | * Defining this will enable stderr debug messages. Note: this can be quite 364 | * detailed. (command line option -DTG_DEBUG_LEVEL=<1,2,3>) 365 | * #define TG_DEBUG_LEVEL <0,1,2...> (default undefined) 366 | */ 367 | 368 | /* 369 | * Math "resolution", values define when a dimension or angle is treated 370 | * as zero. 371 | * #define TG_D_RES (default TG_RESOLUTION) 372 | * #define TG_A_RES (default TG_RESOLUTION) 373 | */ 374 | 375 | /* */ 376 | 377 | /* */ 378 | 379 | /******************************************************************** 380 | * 381 | * Data structure defining a position of logical axes: 382 | * 383 | * Translation : x,z,y 384 | * Rotation : a,b,c 385 | * Auxiliary translation: u,v,w 386 | * 387 | ********************************************************************/ 388 | 389 | /* Axis enumerators for double vector access */ 390 | typedef enum 391 | { 392 | POSE_X_AXIS = 0, POSE_Y_AXIS, POSE_Z_AXIS, POSE_A_AXIS, POSE_B_AXIS, 393 | POSE_C_AXIS, POSE_U_AXIS, POSE_V_AXIS, POSE_W_AXIS 394 | } pose_axis_t; 395 | 396 | /* Pose by axes */ 397 | typedef struct { real_t x, y, z, a, b, c, u, v, w; } pose_t; 398 | typedef struct { real_t x, y, z; } pose_vector_t; 399 | #define pose_position_t pose_vector_t 400 | 401 | /* Line definition */ 402 | typedef struct 403 | { 404 | pose_position_t start, end; /* Start/end point */ 405 | pose_vector_t u; /* Unit vector of translation */ 406 | real_t tm; /* Magnitude /length */ 407 | } pose_line_t; 408 | 409 | /* Circle definition */ 410 | typedef struct 411 | { 412 | pose_position_t cp; /* Circle centre point */ 413 | pose_vector_t nv; /* Circle normal vector */ 414 | pose_vector_t rt,rp,rh; /* radius vectors: tangent, perpendicular, helix */ 415 | real_t l, r, a, s; /* Length, start radius, angle, parallel component coeff */ 416 | } pose_circle_t; 417 | 418 | /* Curve (bezier/spline) definition */ 419 | typedef struct 420 | { 421 | #ifdef WITH_CURVES 422 | pose_position_t start,end; /* End point */ 423 | pose_vector_t suv, euv; /* Start/end unit vectors */ 424 | real_t len; /* Length of the segment */ 425 | real_t c[3][4]; /* Path component coefficients */ 426 | real_t d[TG_CURVE_ACC]; /* Correction */ 427 | #endif 428 | } pose_curve_t; 429 | 430 | typedef real_t pose_array_t[9]; 431 | 432 | /** 433 | * Inliners (These macros are not upper case) 434 | * Using byval for sources and by pointer for dst, so if somthing is switched 435 | * the compiler will raise an error. 436 | */ 437 | /* Assignment p=0 */ 438 | #define pose_set_zero(p) { \ 439 | (p)->x=(p)->y=(p)->z=(p)->a=(p)->b=(p)->c=(p)->u=(p)->v=(p)->w= 0.0; \ 440 | } 441 | 442 | #define pose_set_all(p, val) { \ 443 | (p)->x=(p)->y=(p)->z=(p)->a=(p)->b=(p)->c=(p)->u=(p)->v=(p)->w=val; \ 444 | } 445 | 446 | /** 447 | * Checks if all elements are valid numbers (not nan) and finite. 448 | */ 449 | #define pose_isfinite(p) ( \ 450 | isfinite((p).x) && isfinite((p).y) && isfinite((p).z) && \ 451 | isfinite((p).a) && isfinite((p).b) && isfinite((p).c) && \ 452 | isfinite((p).u) && isfinite((p).v) && isfinite((p).w) \ 453 | ) 454 | 455 | /* Assignment dst = src */ 456 | /* stfwi: no memcpy, let the compiler handle optimisation */ 457 | #define pose_set(dst, src) { \ 458 | (dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z; \ 459 | (dst)->a = (src).a; (dst)->b = (src).b; (dst)->c = (src).c; \ 460 | (dst)->u = (src).u; (dst)->v = (src).v; (dst)->w = (src).w; \ 461 | } 462 | 463 | /* Difference po = p1-p2 */ 464 | #define pose_diff(p1, p2, po) { \ 465 | (po)->x = (p1).x-(p2).x; (po)->y = (p1).y-(p2).y; (po)->z=(p1).z - (p2).z; \ 466 | (po)->a = (p1).a-(p2).a; (po)->b = (p1).b-(p2).b; (po)->c=(p1).c - (p2).c; \ 467 | (po)->u = (p1).u-(p2).u; (po)->v = (p1).v-(p2).v; (po)->w=(p1).w - (p2).w; \ 468 | } 469 | 470 | /* Sum po = p1+p2 */ 471 | #define pose_sum(p1, p2, po){ \ 472 | (po)->x = (p1).x+(p2).x; (po)->y = (p1).y+(p2).y; (po)->z = (p1).z+(p2).z; \ 473 | (po)->a = (p1).a+(p2).a; (po)->b = (p1).b+(p2).b; (po)->c = (p1).c+(p2).c; \ 474 | (po)->u = (p1).u+(p2).u; (po)->v = (p1).v+(p2).v; (po)->w = (p1).w+(p2).w; \ 475 | } 476 | 477 | /* Accumulate po += p2 */ 478 | #define pose_acc(po, p2){ \ 479 | (po)->x += (p2).x; (po)->y += (p2).y; (po)->z += (p2).z; \ 480 | (po)->a += (p2).a; (po)->b += (p2).b; (po)->c += (p2).c; \ 481 | (po)->u += (p2).u; (po)->v += (p2).v; (po)->w += (p2).w; \ 482 | } 483 | 484 | /* Substract po -= p2 */ 485 | #define pose_sub(po, p2){ \ 486 | (po)->x -= (p2).x; (po)->y -= (p2).y; (po)->z -= (p2).z; \ 487 | (po)->a -= (p2).a; (po)->b -= (p2).b; (po)->c -= (p2).c; \ 488 | (po)->u -= (p2).u; (po)->v -= (p2).v; (po)->w -= (p2).w; \ 489 | } 490 | 491 | /* Negation po = -po */ 492 | #define pose_neg(po){ \ 493 | (po)->x = -(po)->x; (po)->y = -(po)->y; (po)->z = -(po)->z; \ 494 | (po)->a = -(po)->a; (po)->b = -(po)->b; (po)->c = -(po)->c; \ 495 | (po)->u = -(po)->u; (po)->v = -(po)->v; (po)->w = -(po)->w; \ 496 | } 497 | 498 | /* Scalar multiply po *= (double) d */ 499 | #define pose_scale(po, d){ \ 500 | (po)->x *= (d); (po)->y *= (d); (po)->z *= (d); \ 501 | (po)->a *= (d); (po)->b *= (d); (po)->c *= (d); \ 502 | (po)->u *= (d); (po)->v *= (d); (po)->w *= (d); \ 503 | } 504 | 505 | /* Trim every axis to a maximum value (defined for every axis) */ 506 | #define pose_trim_all_upper(po, max) { \ 507 | if ((po)->x > (max).x) (po)->x = (max).x; \ 508 | if ((po)->y > (max).y) (po)->y = (max).y; \ 509 | if ((po)->z > (max).z) (po)->z = (max).z; \ 510 | if ((po)->a > (max).a) (po)->a = (max).a; \ 511 | if ((po)->b > (max).b) (po)->b = (max).b; \ 512 | if ((po)->c > (max).c) (po)->c = (max).c; \ 513 | if ((po)->u > (max).u) (po)->u = (max).u; \ 514 | if ((po)->v > (max).v) (po)->v = (max).v; \ 515 | if ((po)->w > (max).w) (po)->w = (max).w; \ 516 | } 517 | 518 | /* Trim every axis to a minimum value (defined for every axis) */ 519 | #define pose_trim_all_lower(po, min) { \ 520 | if ((po)->x < (min).x) (po)->x = (min).x; \ 521 | if ((po)->y < (min).y) (po)->y = (min).y; \ 522 | if ((po)->z < (min).z) (po)->z = (min).z; \ 523 | if ((po)->a < (min).a) (po)->a = (min).a; \ 524 | if ((po)->b < (min).b) (po)->b = (min).b; \ 525 | if ((po)->c < (min).c) (po)->c = (min).c; \ 526 | if ((po)->u < (min).u) (po)->u = (min).u; \ 527 | if ((po)->v < (min).v) (po)->v = (min).v; \ 528 | if ((po)->w < (min).w) (po)->w = (min).w; \ 529 | } 530 | 531 | /* Scalar multiply po *= (double) d */ 532 | /* StfWi: memcmp() with reference 0[9]? */ 533 | #define pose_is_zero(p) ( \ 534 | ((p).x == 0.0 && (p).y == 0.0 && (p).z == 0.0 && \ 535 | (p).a == 0.0 && (p).b == 0.0 && (p).c == 0.0 && (p).u == 0.0 && \ 536 | (p).v == 0.0 && (p).w == 0.0) \ 537 | ) 538 | 539 | /* Scalar multiply po *= (double) d */ 540 | #define pose_is_all_greater_equal_zero(p) ( \ 541 | ((p).x >= 0.0 && (p).y >= 0.0 && \ 542 | (p).z >= 0.0 && (p).a >= 0.0 && (p).b >= 0.0 && (p).c >= 0.0 && \ 543 | (p).u >= 0.0 && (p).v >= 0.0 && (p).w >= 0.0) \ 544 | ) 545 | 546 | /* */ 547 | 548 | /* */ 549 | 550 | /******************************************************************************* 551 | * 552 | * Represents one joint of the machine, including configuration, status and 553 | * command transfer. 554 | * 555 | ******************************************************************************/ 556 | 557 | #ifdef JOINT_DECL_NONE 558 | /* 559 | * NOTE: You have to define the joints compatible to the minimum settings 560 | */ 561 | #elif defined JOINT_DECL_FULL 562 | 563 | /** 564 | * Type of joint 565 | */ 566 | typedef enum 567 | { 568 | JOINT_TYPE_NONE = 0, /* Not defined */ 569 | JOINT_AXIS_TYPE_LINEAR, /* Linear axis, unit will be meters */ 570 | JOINT_AXIS_TYPE_ROTARY /* Polar axis, unit will be degrees */ 571 | } joint_axis_type_t; 572 | 573 | /** 574 | * Feedback type of a joint 575 | */ 576 | typedef enum 577 | { 578 | /* E.g. stepper without encoder */ 579 | JOINT_POSITION_FEEDBACK_NONE = 0, 580 | /* Incremental encoder, requires homing for all except axis free motion */ 581 | JOINT_POSITION_FEEDBACK_INCREMENTAL, 582 | /* Incremental encoder, requires no homing */ 583 | JOINT_POSITION_FEEDBACK_ABSOLUTE 584 | } joint_position_feedback_t; 585 | 586 | /** 587 | * I/O logic of various joint inputs/outputs, e.g. limit switches, index ... 588 | */ 589 | typedef enum 590 | { 591 | JOINT_LOGIC_ACTIVE_HIGH = 0x00, 592 | JOINT_LOGIC_ACTIVE_LOW = 0x01, /* used with xor */ 593 | JOINT_LOGIC_DISABLED 594 | } joint_logic_t; 595 | 596 | /** 597 | * Bit field of joint-related 598 | */ 599 | typedef union 600 | { 601 | uint16_t all; 602 | struct 603 | { 604 | /* The joint/axis is enabled */ 605 | unsigned enabled : 1; 606 | /* The amplifier is enabled */ 607 | unsigned amplifier_enabled : 1; 608 | /* The joint is homed */ 609 | unsigned homed : 1; 610 | /* The trajectory planer is at the goal position */ 611 | unsigned trajectory_done : 1; 612 | /* The joint is in the position tolerance of the goal position */ 613 | unsigned in_position : 1; 614 | /* The joint does not move (in standstill velocity tolerance) */ 615 | unsigned standstill : 1; 616 | /* The joint amplifier has an error */ 617 | unsigned amplifier_fault : 1; 618 | /* The joint encoder has an error */ 619 | unsigned encoder_fault : 1; 620 | /* The home switch is activated (also depends on config) */ 621 | unsigned home_index : 1; 622 | /* The positive limit switch is activated (depends also on config) */ 623 | unsigned positive_limit : 1; 624 | /* The negative limit switch is activated (depends also on config) */ 625 | unsigned negative_limit : 1; 626 | /* Fill up remaining bits */ 627 | unsigned reserved : 5; 628 | } bits; 629 | } joint_status_t; 630 | 631 | typedef struct 632 | { 633 | /* Joint type, linear or polar/rotary */ 634 | joint_axis_type_t type; 635 | /* The type of feedback */ 636 | joint_position_feedback_t feedback_type; 637 | /* The positive software limit in meters/degrees */ 638 | real_t positive_software_limit; 639 | /* The negative software limit in meters/degrees */ 640 | real_t negative_software_limit; 641 | /* Scales the axis position (normally increments) to meters/degrees */ 642 | real_t position_to_meters_scaler; 643 | /* The maximum velocity the joint can do in m/s / degrees/s */ 644 | real_t max_velocity; 645 | /* The maximum acceleration the joint can do in m/s / degrees/s */ 646 | real_t max_acceleration; 647 | /* Max trailing error at speed=0 in meters/degrees */ 648 | real_t max_trailing_error_min; 649 | /* Max trailing error at speed=max_velocity in meters/degrees */ 650 | real_t max_trailing_error_max; 651 | /* The deceleration used for ESTOP full break. It can exceed the max_acceleration. */ 652 | real_t estop_deceleration; 653 | /* Max position deviation at standstill to say the measured joint position is */ 654 | /* accurate enough, in m/s / degrees/s */ 655 | real_t in_position_tolerance; 656 | /* Max speed deviation at standstill to say the joint does not move anymore, */ 657 | /* m/s / degrees/s */ 658 | real_t standstill_tolerance; 659 | /* 1 = a limit switch exists and has to be respected */ 660 | joint_logic_t positive_hardware_limit; 661 | /* 1 = a limit switch exists and has to be respected */ 662 | joint_logic_t negative_hardware_limit; 663 | /* If not, a limit switch will be used OR, if not limit switch, the mechanical */ 664 | /* joint endpoint (measured using trailing error) */ 665 | joint_logic_t home_index; 666 | } joint_config_t; 667 | 668 | typedef struct 669 | { 670 | /* Axis configuration */ 671 | joint_config_t config; 672 | /* State of the joint, see joint_state_t */ 673 | joint_status_t status; 674 | /* The interpolated position of the trajectory generator (for all kinds of trajgen) */ 675 | real_t command_position; 676 | /* The interpolated velocity of the trajectory generator (for all kinds of trajgen) */ 677 | real_t command_velocity; 678 | /* The interpolated acceleration of the trajectory generator (for all kinds of trajgen) */ 679 | real_t command_acceleration; 680 | /* The scaled position measured with the encoders */ 681 | real_t position; 682 | /* Position captured by a latch operation, e.g. homing index */ 683 | real_t latched_position; 684 | } joint_t; 685 | 686 | #else 687 | 688 | /** 689 | * Minimum joint definition required to use the trajgen 690 | */ 691 | typedef struct 692 | { 693 | struct { real_t max_velocity, max_acceleration; } config; 694 | real_t command_position, command_velocity, command_acceleration, position; 695 | } joint_t; 696 | 697 | #endif 698 | 699 | /* */ 700 | 701 | /* */ 702 | 703 | /******************************************************************************* 704 | * 705 | * 706 | * Main trajectory generator error types and definitions. 707 | * 708 | * 709 | ******************************************************************************/ 710 | 711 | /** 712 | * Error declarations for the trajgen itself. However, because this is the "master" 713 | * File of the generator, it will return error codes defined in trajgen_error.h, 714 | * which includes joint, code and error source. 715 | */ 716 | enum 717 | { 718 | /* Main controller */ 719 | TRAJ_ERROR_OK = 0, 720 | TRAJ_ERROR_ERROR, 721 | TRAJ_ERROR_NULL_POINTER, 722 | TRAJ_ERROR_NUMERIC, 723 | TRAJ_ERROR_CONFIG_LAST_USED_JOINT_INVALID, 724 | TRAJ_ERROR_CONFIG_INTERPOLATION_RATE_INVALID, 725 | TRAJ_ERROR_CONFIG_OVERRIDE_INVALID, 726 | TRAJ_ERROR_CONFIG_COMPILE_SETTING_INVALID, 727 | TRAJ_ERROR_INVALID_STATE, /* The motion state is not valid (Internal problem) */ 728 | TRAJ_ERROR_INVALID_SWITCHING_STATE, /* The operating mode to set is invalid */ 729 | TRAJ_ERROR_INVALID_JOINT_NO, 730 | 731 | /* Kinematics */ 732 | KINEMATICS_ERR_ERROR, 733 | KINEMATICS_ERR_NULL_POINTER, 734 | KINEMATICS_ERR_INIT_FUNCTION_NULL, 735 | KINEMATICS_ERR_FORWARD_FAILED, 736 | KINEMATICS_ERR_INVERSE_FAILED, 737 | KINEMATICS_ERR_RESET_FAILED, 738 | 739 | /* Interpolators */ 740 | INTERPOLATOR_ERROR, 741 | INTERPOLATOR_ERROR_INIT_ARG_INVALID, 742 | INTERPOLATOR_ERROR_QUEUE_FULL, 743 | INTERPOLATOR_ERROR_OFFSET_IP_NULLPOINTER, 744 | INTERPOLATOR_ERROR_INTERPOLATE_ARG_NULLPOINTER, 745 | INTERPOLATOR_ERROR_NOT_RESET, 746 | INTERPOLATOR_ERROR_NULLPOINTER, 747 | 748 | /* Coordinated planer */ 749 | TP_ERR_ERROR, 750 | TP_ERR_TP_NULL_POINTER, 751 | TP_ERR_ABORTING, 752 | TP_ERR_QUEUE_PUT_FAILED, 753 | TP_ERR_INVALID_PARAM, 754 | TP_ERR_QUEUE_FULL, 755 | TP_ERR_QUEUE_TO_MANY_ELEMENTS_TO_REMOVE, 756 | TP_ERR_INVALID_MOTION_TYPE, 757 | TP_ERR_INVALID_SPEED, 758 | TP_ERR_INVALID_ACCEL, 759 | TP_ERR_INVALID_POSE, 760 | TP_ERR_SEGMENT_LENGTH_ZERO, 761 | TP_ERR_INVALID_SAMPLE_INTERVAL, 762 | TP_ERR_ALREADY_MOVING, 763 | TP_ERR_UNIT_VECTOR_CALC_INVALID_TYPE, 764 | TP_ERR_REF_POSITION_INVALIDATED_DURING_MOTION, 765 | 766 | /* Joint planers */ 767 | TRAJGEN_FREE_ERROR_ERROR, 768 | TRAJGEN_FREE_ERROR_INIT_NULLPOINTER, 769 | TRAJGEN_FREE_ERROR_INIT_INVALID_MAX_ACCEL, 770 | TRAJGEN_FREE_ERROR_INIT_INVALID_MAX_VELOCITY, 771 | TRAJGEN_FREE_ERROR_INIT_INVALID_SAMPLE_INTERVAL, 772 | TRAJGEN_FREE_ERROR_INVALID_ACCELERATION, 773 | 774 | /* Manual operation planer */ 775 | TG_MAN_ERROR_ERROR, 776 | TG_MAN_ERROR_INIT_NULLPOINTER, 777 | TG_MAN_ERROR_INIT_INVALID_MAX_ACCEL, 778 | TG_MAN_ERROR_INIT_INVALID_MAX_VELOCITY, 779 | TG_MAN_ERROR_INIT_INVALID_SAMPLE_INTERVAL, 780 | TG_MAN_ERROR_INVALID_ACCELERATION 781 | }; 782 | 783 | enum { KINEMATICS_ERR_OK=0 }; 784 | enum { INTERPOLATOR_OK=0 }; 785 | enum { TP_ERR_OK=0 }; 786 | enum { TRAJGEN_FREE_ERROR_OK=0 }; 787 | enum { TG_MAN_ERROR_OK=0 }; 788 | 789 | 790 | typedef uint16_t trajgen_error_t; 791 | #define kinematics_error_t trajgen_error_t 792 | #define trajgen_coord_error_t trajgen_error_t 793 | #define interpolator_error_t trajgen_error_t 794 | #define interpolator_error_t trajgen_error_t 795 | #define trajgen_jointtg_error_t trajgen_error_t 796 | #define trajgen_man_error_t trajgen_error_t 797 | 798 | /** 799 | * Main error type 800 | */ 801 | typedef union 802 | { 803 | int16_t errnom; 804 | struct 805 | { 806 | unsigned code : 12; 807 | unsigned joint : 4; 808 | } sel; 809 | } trajgen_error_details_t; 810 | 811 | /** 812 | * Writes a text version of a trajgen error into *errstr 813 | * @param uint16_t errnum 814 | * @param char* errstr 815 | * @param unsigned length 816 | */ 817 | extern const char* trajgen_errstr(uint16_t errnum, char* errstr, unsigned length); 818 | 819 | /* */ 820 | 821 | /* */ 822 | 823 | /******************************************************************** 824 | 825 | * Cyclic Cubic interpolation, used to interpolate joint command position for 826 | * the closed loop controllers between the way points produced by the trajectory 827 | * generators. This allows to enhance the system performance, especially when 828 | * high filter sample rates are required. 829 | * 830 | ********************************************************************/ 831 | 832 | /** 833 | * Cubic interpolator 834 | */ 835 | typedef struct 836 | { 837 | uint16_t n; /* Specifies number of queued points */ 838 | real_t T; /* The trajectory planer sample time */ 839 | real_t Ti; /* The time between two interpolated points */ 840 | real_t t; /* Current time, reset on push */ 841 | real_t x0, x1, x2, x3;/* Buffer for the interpolation, used like a FIFO */ 842 | real_t w0, w1; /* Way points */ 843 | real_t v0, v1; /* Velocities at the way points */ 844 | real_t a, b, c, d; /* Coefficients polynomial, a*x^3 + b*x^2 + c*x + d */ 845 | } interpolator_t; 846 | 847 | 848 | #ifdef EXPORT_INTERPOLATORS 849 | /** 850 | * Initialise the interpolator. 851 | */ 852 | extern interpolator_error_t interpolator_init(interpolator_t*, real_t sample_interval, 853 | unsigned interpolation_rate); 854 | 855 | /** 856 | * Reset the interpolator, clear the queue to the specified position value. 857 | */ 858 | extern interpolator_error_t interpolator_reset(interpolator_t*, real_t initial_position); 859 | /** 860 | * Performs one interpolation tick. All pointer argument must refer to existing 861 | * variables (not NULL). 862 | */ 863 | extern interpolator_error_t interpolator_interpolate(interpolator_t*, real_t *x, 864 | real_t *v, real_t *a, real_t *j); 865 | /** 866 | * Push a new position into the fifo. That is only allowed when the fifo is not full. 867 | */ 868 | extern interpolator_error_t interpolator_push(interpolator_t*, real_t point); 869 | /** 870 | * Returns nonzero if the fifo is not full. You must push a value before the 871 | * next interpolation step, or the last value in the fifo will be pushed 872 | * automatically. 873 | */ 874 | extern bool_t interpolator_need_update(interpolator_t*); 875 | 876 | #endif 877 | /* */ 878 | 879 | /* */ 880 | 881 | /******************************************************************************* 882 | * 883 | * Calculates the position and direction of the end effector (where the tool is 884 | * mounted) in three dimensional space based in the position of all relevant 885 | * machine joints and vice versa. Several kinematic machine models are available 886 | * and implementing own models is possible. 887 | * 888 | ******************************************************************************/ 889 | 890 | typedef struct 891 | { 892 | /** 893 | * The reset kinematics function sets all its arguments to their proper 894 | * values at the known initial position. When called, these should be set, 895 | * when known, to initial values, e.g., from an INI file. If the home 896 | * kinematics can accept arbitrary starting points, these initial values 897 | * should be used. 898 | */ 899 | kinematics_error_t(*reset)(pose_t *pose, real_t *joint_positions[]); 900 | 901 | /** 902 | * The forward kinematics take joint values and determine world coordinates, 903 | * given forward kinematics flags to resolve any ambiguities. The inverse 904 | * flags are set to indicate their value appropriate to the joint values 905 | * passed in. 906 | */ 907 | kinematics_error_t(*forward)(real_t *joint_positions[], pose_t *pose); 908 | 909 | /** 910 | * The inverse kinematics take world coordinates and determine joint values, 911 | * given the inverse kinematics flags to resolve any ambiguities. The forward 912 | * are set to indicate their value appropriate to the world coordinates 913 | * passed in. 914 | */ 915 | kinematics_error_t(*inverse)(pose_t *pose, real_t *joint_positions[]); 916 | 917 | } kinematics_t; 918 | 919 | extern kinematics_error_t kinematics_initialize(kinematics_t *kin, kinematics_t 920 | device_kinematics); 921 | 922 | /* */ 923 | 924 | /* */ 925 | 926 | /******************************************************************** 927 | * 928 | * Planer for coordinated queued motion with blending, i/o synchronisation and 929 | * motion synchronisation. 930 | * 931 | ********************************************************************/ 932 | 933 | /** 934 | * Naming of motion task types (typedef is int'ed for bit packing) 935 | */ 936 | #define trajgen_sg_motion_type_t int 937 | enum 938 | { 939 | SG_INVALID = 0x00, 940 | SG_LINEAR = 0x01, 941 | SG_CIRCULAR = 0x02, 942 | SG_CURVE = 0x03, 943 | SG_END_OF_MOTION 944 | }; 945 | 946 | /** 947 | * Type naming of motion synchronisation (typedef is int'ed for bit packing) 948 | */ 949 | #define trajgen_coord_motion_sync_type_t int 950 | enum 951 | { 952 | TP_SYNC_NONE = 0x00, 953 | TP_SYNC_VELOCITY = 0x01, 954 | TP_SYNC_POSITION = 0x02 955 | }; 956 | 957 | /** 958 | * Type for specification of synchronised motion (motion dependent on external 959 | * positions/velocities). 960 | */ 961 | typedef struct 962 | { 963 | /** 964 | * Scales from the reference position/velocity (e.g. spindle position/speed) 965 | * to the scalar position/velocity of the trajectory when multiplied. 966 | */ 967 | real_t scaler; 968 | /* 969 | * Position sync: The current (input) reference position 970 | * Velocity sync: The current (input) reference velocity 971 | */ 972 | real_t reference; 973 | /* INTERNAL USE: Accumulates the position-synchronized segment lengths of 974 | * all surpassed segments for position sync. 975 | */ 976 | real_t i_offset; 977 | /** 978 | * The type of synchronisation, for subsequent motion, default is TP_SYNC_NONE 979 | */ 980 | trajgen_coord_motion_sync_type_t type: 4; 981 | /** 982 | * INTERNAL USE: Saves if the position offset does not require to be updated 983 | * before starting a position synced motion. 984 | */ 985 | bool_t i_isset: 1; 986 | } trajgen_coord_motion_sync_t; 987 | 988 | /** 989 | * Coordinated planer configuration 990 | */ 991 | typedef struct 992 | { 993 | /* Sample interval (cycle interval time) */ 994 | real_t sample_interval; 995 | /* Max velocity allowed by machine constraints (normally set in a config file) */ 996 | real_t max_velocity; 997 | /* Max acceleration allowed by machine constraints (normally set in a config file) */ 998 | real_t max_acceleration; 999 | } trajgen_coord_config_t; 1000 | 1001 | /** 1002 | * Mask type for synchronised digital I/O states (bit field) 1003 | */ 1004 | typedef uint16_t trajgen_coord_syncdio_t; 1005 | 1006 | /** 1007 | * Synchronised digital I/Os to set/reset when the next queued task is shifted. 1008 | */ 1009 | typedef struct 1010 | { 1011 | /* the bits to be set to HIGH */ 1012 | trajgen_coord_syncdio_t set; 1013 | /* the bits to be set to LOW */ 1014 | trajgen_coord_syncdio_t clear; 1015 | } trajgen_coord_syncdio_cmd_t; 1016 | 1017 | /** 1018 | * Task data specialisation for line motion 1019 | */ 1020 | typedef struct 1021 | { 1022 | pose_line_t xyz, abc, uvw; 1023 | } trajgen_sg_line_t; 1024 | 1025 | /** 1026 | * Task data specialisation for circle motion 1027 | */ 1028 | typedef struct 1029 | { 1030 | pose_circle_t xyz; 1031 | pose_line_t abc, uvw; 1032 | } trajgen_sg_arc_t; 1033 | 1034 | /** 1035 | * Task data specialisation for curve motion 1036 | */ 1037 | typedef struct 1038 | { 1039 | #ifdef WITH_CURVES 1040 | pose_curve_t xyz; 1041 | #else 1042 | pose_line_t xyz; 1043 | #endif 1044 | pose_line_t abc, uvw; 1045 | } trajgen_sg_curve_t; 1046 | 1047 | /** 1048 | * Coordinated planer task task type, used in the queue. 1049 | */ 1050 | typedef struct 1051 | { 1052 | union { 1053 | trajgen_sg_line_t line; /* Specific line data */ 1054 | trajgen_sg_arc_t arc; /* Specific arc/circle/helix data */ 1055 | trajgen_sg_curve_t curve; /* Specific curve/nurbs data */ 1056 | } coords; 1057 | 1058 | /** 1059 | * segment's serial number 1060 | */ 1061 | uint32_t id; 1062 | /** 1063 | * The scalar path length that the pose moved already in this segment, values 1064 | * are from 0..target 1065 | */ 1066 | real_t progress; 1067 | /** 1068 | * Scalar length of the path segment 1069 | */ 1070 | real_t length; 1071 | /** 1072 | * Speed set by the user speed (requested speed) 1073 | */ 1074 | real_t v; 1075 | /** 1076 | * Acceleration set by the user (requested acceleration) 1077 | */ 1078 | real_t a; 1079 | /** 1080 | * Internally calculated (keep track of current step (vel * cycle_time) ) 1081 | */ 1082 | real_t current_velocity; 1083 | /** 1084 | * during the blend at the end of this move, stay within this distance from 1085 | * the path 1086 | */ 1087 | real_t blending_tolerance; 1088 | /** 1089 | * velocity below which we should start blending 1090 | */ 1091 | real_t blending_velocity; 1092 | /** 1093 | * synched DIO's for this move. what to turn on/off 1094 | */ 1095 | trajgen_coord_syncdio_cmd_t sync_dio; 1096 | /** 1097 | * SG_LINEAR (coords.line) or 1098 | */ 1099 | trajgen_sg_motion_type_t motion_type: 4; 1100 | /** 1101 | * The type of synchronisation, default is TP_SYNC_NONE 1102 | */ 1103 | trajgen_coord_motion_sync_type_t sync_type: 4; 1104 | /** 1105 | * this motion is being executed 1106 | */ 1107 | bool_t is_active: 1; 1108 | /** 1109 | * segment is being blended into following segment 1110 | */ 1111 | bool_t is_blending: 1; 1112 | /** 1113 | * Feed scale, etc, enable bits for this move 1114 | */ 1115 | bool_t override_enabled: 1; 1116 | } trajgen_sg_t; 1117 | 1118 | typedef struct 1119 | { 1120 | /* ptr to the tcs */ 1121 | trajgen_sg_t queue[TG_QUEUE_SIZE]; 1122 | /* size of queue */ 1123 | uint16_t size; 1124 | /* number of tcs now in queue */ 1125 | uint16_t num_elements; 1126 | /* indices to next to get, next to put */ 1127 | uint16_t start, end; 1128 | } trajgen_queue_t; 1129 | 1130 | typedef struct 1131 | { 1132 | real_t override; 1133 | pose_t last_position; 1134 | } trajgen_coord_internals_t; 1135 | 1136 | typedef struct 1137 | { 1138 | /* Static configuration, set during initialisation */ 1139 | trajgen_coord_config_t config; 1140 | /* Last error that occurred, only valid if a function does not return 0 */ 1141 | trajgen_coord_error_t last_error; 1142 | /* The buffer containing queued segments */ 1143 | trajgen_queue_t queue; 1144 | /* Defines for subsequent motion where spindle synchronisation is required */ 1145 | trajgen_coord_motion_sync_t sync; 1146 | /* Will be added to (only) the next pushed segments and transferred to 1147 | * sync_dio_current when the elements are processed */ 1148 | trajgen_coord_syncdio_cmd_t sync_dio_command; 1149 | /* Contains the actual digital i/o word that has to be transferred to the HAL 1150 | * or hardware port register */ 1151 | trajgen_coord_syncdio_t sync_dio_current; 1152 | /* Internally used current values, DO NOT MODIFY */ 1153 | trajgen_coord_internals_t _i; 1154 | /* number of motions blending */ 1155 | uint16_t num_queued_active; 1156 | /* Id of the actual queued segment, can be used to identify the program line 1157 | * in gnc programs */ 1158 | uint32_t sg_id; 1159 | /* Id of the next segment that is pushed into the queue */ 1160 | uint32_t next_sg_id; 1161 | /* Subsequent motions: stay within this distance of the programmed path during 1162 | * blends */ 1163 | real_t blending_tolerance; 1164 | /* Actual position/pose of the trajectory */ 1165 | pose_t pose; 1166 | /* End position of the last queued segment */ 1167 | pose_t end_pose; 1168 | /* Velocity scaling, normal value is 1.0 (means 100% = specified speed) */ 1169 | real_t override; 1170 | /* Abort in progress */ 1171 | bool_t is_aborting; 1172 | /* Motion paused */ 1173 | bool_t is_pausing; 1174 | /* Queue empty and goal position reached */ 1175 | bool_t is_done; 1176 | /* Subsequent motion: Flags defining which speed overrides are allowed */ 1177 | uint8_t override_enabled; 1178 | } trajgen_coord_t; 1179 | 1180 | #ifdef EXPORT_COORD_TG 1181 | /** 1182 | * Initialise the planer 1183 | */ 1184 | extern trajgen_coord_error_t trajgen_coord_init(trajgen_coord_t*, trajgen_coord_config_t config); 1185 | /** 1186 | * Reset all mutable states, clear queue, but not the config. 1187 | */ 1188 | extern trajgen_coord_error_t trajgen_coord_reset(trajgen_coord_t*); 1189 | /** 1190 | * Set the current pose. Use this for initialising or if axes moved manually. 1191 | */ 1192 | extern trajgen_coord_error_t trajgen_coord_set_position(trajgen_coord_t*, pose_t pos); 1193 | /** 1194 | * Move a line 1195 | */ 1196 | extern trajgen_coord_error_t trajgen_coord_add_line(trajgen_coord_t*, pose_t end, 1197 | real_t velocity, real_t acceleration); 1198 | /** 1199 | * Move a circle 1200 | */ 1201 | extern trajgen_coord_error_t trajgen_coord_add_arc(trajgen_coord_t*, pose_t end, 1202 | pose_position_t center, pose_vector_t normal, unsigned n_turns, real_t velocity, 1203 | real_t acceleration); 1204 | /** 1205 | * Move a curve/nurbs segment 1206 | */ 1207 | extern trajgen_coord_error_t trajgen_coord_add_curve(trajgen_coord_t* tp, 1208 | pose_t end, pose_vector_t start_ctrl_point, pose_vector_t end_ctrl_point, 1209 | real_t velocity, real_t acceleration); 1210 | /** 1211 | * Pause motion 1212 | */ 1213 | extern trajgen_coord_error_t trajgen_coord_pause(trajgen_coord_t*); 1214 | /** 1215 | * Resume from pause 1216 | */ 1217 | extern trajgen_coord_error_t trajgen_coord_resume(trajgen_coord_t*); 1218 | /** 1219 | * Stop motion, clear the queue 1220 | */ 1221 | extern trajgen_coord_error_t trajgen_coord_abort(trajgen_coord_t*); 1222 | /** 1223 | * Tick: has to be called with the configured sample frequency. 1224 | */ 1225 | extern trajgen_coord_error_t trajgen_coord_tick(trajgen_coord_t*); 1226 | /** 1227 | * Returns nonzero if the queue is full, so you have to wait until pushing 1228 | * next motion tasks. 1229 | */ 1230 | extern bool_t trajgen_coord_is_queue_full(trajgen_coord_t*); 1231 | /** 1232 | * Returns nonzero if all motion has finished and the planer pose reached the 1233 | * destination position. 1234 | */ 1235 | extern bool_t trajgen_coord_is_done(trajgen_coord_t*); 1236 | /** 1237 | * Returns the current pose 1238 | */ 1239 | extern pose_t trajgen_coord_get_position(trajgen_coord_t*); 1240 | /** 1241 | * Returns the ID of the actually processed task 1242 | */ 1243 | extern uint32_t trajgen_coord_get_current_id(trajgen_coord_t*); 1244 | /** 1245 | * Returns the number of currently queued tasks. 1246 | */ 1247 | extern uint16_t trajgen_coord_get_num_queued(trajgen_coord_t*); 1248 | /** 1249 | * Returns maximum size of the queue 1250 | */ 1251 | extern uint16_t trajgen_coord_get_queue_size(trajgen_coord_t*); 1252 | /** 1253 | * Returns returns the number of currently processed tasks. That can be greater 1254 | * one e.g. when blending. 1255 | */ 1256 | extern uint16_t trajgen_coord_get_num_queued_active(trajgen_coord_t*); 1257 | 1258 | #endif 1259 | /* */ 1260 | 1261 | /* */ 1262 | 1263 | /******************************************************************** 1264 | * 1265 | * Simple trajectory planner for one joint. When enabled, corrects the joint 1266 | * position based on a given command position and command velocity (maximum 1267 | * velocity when override == 1.0). 1268 | * 1269 | * Can be used for absolute motion, relative motion, position correction ( 1270 | * (e.g. mouse or jogwheel). 1271 | * 1272 | ********************************************************************/ 1273 | 1274 | typedef struct 1275 | { 1276 | /* Maximum acceleration of this joint for the joint trajgen */ 1277 | real_t max_acceleration; 1278 | /* Maximim positive and negative velocity */ 1279 | real_t max_velocity; 1280 | /* Sample interval, seconds */ 1281 | real_t sample_interval; 1282 | } trajgen_jointtg_config_t; 1283 | 1284 | typedef struct 1285 | { 1286 | /* The last error that occurred, only valid if a function returns nonzero */ 1287 | trajgen_jointtg_error_t last_error; 1288 | /* Configuration */ 1289 | trajgen_jointtg_config_t config; 1290 | /* Internal actual speed */ 1291 | real_t velocity; 1292 | /* Actual position (also returned by trajgen_jointtg_get_position()) */ 1293 | real_t position; 1294 | /* The position where the tp shall move to */ 1295 | real_t command_position; 1296 | /* The maximum velocity of this move */ 1297 | real_t command_velocity; 1298 | /* Enabled flag */ 1299 | bool_t is_enabled; 1300 | /* Pause flag */ 1301 | bool_t is_pause; 1302 | /* Done flag */ 1303 | bool_t is_done; 1304 | } trajgen_jointtg_t; 1305 | 1306 | #ifdef EXPORT_JOINT_TG 1307 | 1308 | /** 1309 | * Initializes the joint generator 1310 | * @param trajgen_jointtg_t *tp 1311 | * @param trajgen_jointtg_config_t config 1312 | * @return trajgen_jointtg_error_t 1313 | */ 1314 | extern trajgen_jointtg_error_t trajgen_jointtg_initialize(trajgen_jointtg_t *tp, 1315 | trajgen_jointtg_config_t config); 1316 | 1317 | /** 1318 | * Resets the manual operation generator except the configuration. 1319 | * @return trajgen_jointtg_error_t 1320 | */ 1321 | extern trajgen_jointtg_error_t trajgen_jointtg_reset(trajgen_jointtg_t *tp); 1322 | 1323 | /** 1324 | * Aborts the motion (force velocity to 0.0) 1325 | * @return 1326 | */ 1327 | extern trajgen_jointtg_error_t trajgen_jointtg_set_enabled(trajgen_jointtg_t *tp, 1328 | bool_t enabled); 1329 | 1330 | /** 1331 | * Input a new command position for the joint 1332 | * @return trajgen_jointtg_error_t 1333 | */ 1334 | extern trajgen_jointtg_error_t trajgen_jointtg_set_command_position(trajgen_jointtg_t *tp, 1335 | real_t position); 1336 | 1337 | /** 1338 | * Input a new velocity for the joint 1339 | * @return trajgen_jointtg_error_t 1340 | */ 1341 | extern trajgen_jointtg_error_t trajgen_jointtg_set_command_velocity(trajgen_jointtg_t *tp, 1342 | real_t velocity); 1343 | 1344 | /** 1345 | * Returns the last generated pose (output) 1346 | * @return pose_t 1347 | */ 1348 | extern real_t trajgen_jointtg_get_current_position(trajgen_jointtg_t *tp); 1349 | 1350 | /** 1351 | * Sets the internal position. Used to update when the amplifiers/closed loop 1352 | * controls are off 1353 | * @param pose_t position 1354 | * @return pose_t 1355 | */ 1356 | extern trajgen_jointtg_error_t trajgen_jointtg_set_current_position(trajgen_jointtg_t *tp, 1357 | real_t position); 1358 | 1359 | /** 1360 | * Calculates a new position from the command velocity 1361 | * @return pose_t 1362 | */ 1363 | extern trajgen_jointtg_error_t trajgen_jointtg_tick(trajgen_jointtg_t *tp); 1364 | 1365 | /** 1366 | * Returns true (nonzero) if the command velocity and the actual total velocity 1367 | * is zero. 1368 | * @return bool_t 1369 | */ 1370 | extern bool_t trajgen_jointtg_is_done(trajgen_jointtg_t *tp); 1371 | 1372 | #endif 1373 | /* */ 1374 | 1375 | /* */ 1376 | 1377 | /******************************************************************** 1378 | * 1379 | * Trajectory generator coordinated motion based on input velocities 1380 | * (e.g. Joystick). Output is a pose, which has to be converted to the individual 1381 | * joint positions using inverse kinematics. 1382 | * 1383 | ********************************************************************/ 1384 | 1385 | typedef struct 1386 | { 1387 | /* Maximum positive and negative velocity for all axes */ 1388 | pose_t max_velocity; 1389 | /* Maximum acceleration for each axis */ 1390 | pose_t max_acceleration; 1391 | /* Sample interval, seconds */ 1392 | real_t sample_interval; 1393 | } trajgen_man_config_t; 1394 | 1395 | typedef struct 1396 | { 1397 | /* The last error that occurred, only valid if a function returns nonzero */ 1398 | trajgen_man_error_t last_error; 1399 | /* Configuration, used to init */ 1400 | trajgen_man_config_t config; 1401 | /* The (input) velocities that the endeffector/axes shall move with */ 1402 | pose_t velocity; 1403 | /* Internal actual speed */ 1404 | pose_t current_velocity; 1405 | /* Actual position (also returned by trajgen_man_get_position()) */ 1406 | pose_t pose; 1407 | /* Enabled flag */ 1408 | bool_t is_enabled; 1409 | } trajgen_man_t; 1410 | 1411 | #ifdef EXPORT_MANUAL_TG 1412 | /** 1413 | * Initializes the manual operation generator 1414 | * @param trajgen_man_t *tp 1415 | * @param trajgen_man_config_t config 1416 | * @return trajgen_man_error_t 1417 | */ 1418 | extern trajgen_man_error_t trajgen_man_initialize(trajgen_man_t *mtp, trajgen_man_t *_tp, 1419 | trajgen_man_config_t config); 1420 | 1421 | /** 1422 | * Resets the manual operation generator except the configuration. 1423 | * @return trajgen_man_error_t 1424 | */ 1425 | extern trajgen_man_error_t trajgen_man_reset(trajgen_man_t *mtp); 1426 | 1427 | /** 1428 | * Aborts the motion (force velocity to 0.0) 1429 | * @return 1430 | */ 1431 | extern trajgen_man_error_t trajgen_man_abort(trajgen_man_t *mtp); 1432 | 1433 | /** 1434 | * Input a new velocity for every axis 1435 | * @return trajgen_man_error_t 1436 | */ 1437 | extern trajgen_man_error_t trajgen_man_set_velocity(trajgen_man_t *mtp, pose_t velocity); 1438 | 1439 | /** 1440 | * Returns the last generated pose (output) 1441 | * @return pose_t 1442 | */ 1443 | extern pose_t trajgen_man_get_position(trajgen_man_t *mtp); 1444 | 1445 | /** 1446 | * Sets the internal position. Used to update when the amplifiers/closed loop 1447 | * controls are off 1448 | * @param pose_t position 1449 | * @return pose_t 1450 | */ 1451 | extern trajgen_man_error_t trajgen_man_set_position(trajgen_man_t *mtp, pose_t position); 1452 | 1453 | /** 1454 | * Calculates a new position from the command velocity 1455 | * @return pose_t 1456 | */ 1457 | extern trajgen_man_error_t trajgen_man_tick(trajgen_man_t *mtp); 1458 | 1459 | /** 1460 | * Returns true (nonzero) if the command velocity and the actual total velocity 1461 | * is zero. 1462 | * @return bool_t 1463 | */ 1464 | extern bool_t trajgen_man_is_done(trajgen_man_t *mtp); 1465 | 1466 | #endif 1467 | /* */ 1468 | 1469 | /* */ 1470 | 1471 | /** 1472 | * Operating mode state, including switching between modes 1473 | */ 1474 | typedef enum 1475 | { 1476 | TRAJ_STATE_DISABLED = 0, /* Controller disabled, implicitly stops all motion */ 1477 | TRAJ_STATE_JOINT, /* Free joint motion */ 1478 | TRAJ_STATE_COORDINATED, /* Coordinated motion, requires homing */ 1479 | TRAJ_STATE_MAN, /* Manual operation, requires homing */ 1480 | TRAJ_______INTERNAL_STATES, 1481 | TRAJ_STATE_OK_TO_SWITCH, 1482 | TRAJ_STATE_DISABLING, 1483 | TRAJ_STATE_JOINT_ENTER, 1484 | TRAJ_STATE_JOINT_LEAVE, 1485 | TRAJ_STATE_COORDINATED_ENTER, 1486 | TRAJ_STATE_COORDINATED_LEAVE, 1487 | TRAJ_STATE_TG_MAN_ENTER, 1488 | TRAJ_STATE_TG_MAN_LEAVE 1489 | } trajgen_state_t; 1490 | 1491 | /** 1492 | * Configuration 1493 | */ 1494 | typedef struct 1495 | { 1496 | /* Own machine kinematics or NULL for "identity" (x=x,y=y, ...) */ 1497 | kinematics_t kinematics_functions; 1498 | /* Max (axes, not joints) accelerations for manual operation, but corresponds */ 1499 | /* to joints if the machine type is cartesian */ 1500 | pose_t max_manual_accelerations; 1501 | /* Max (axes, not joints) velocities for manual operation, but corresponds to */ 1502 | /* joints if the machine type is cartesian */ 1503 | pose_t max_manual_velocities; 1504 | /* Maximum trajectory velocity of the end effector ("tool tip"/"pose") */ 1505 | real_t max_coord_velocity; 1506 | /* Maximum trajectory acceleration of the end effector ("tool tip"/"pose") */ 1507 | real_t max_coord_acceleration; 1508 | /* The interval time of the positioning, equals 1/sample rate */ 1509 | real_t sample_interval; 1510 | /* The number of used joints, NOTE: it corresponds to the LAST USED JOINT + 1) */ 1511 | /* if there are inactive joints between, they still need to be calculated. */ 1512 | uint8_t number_of_used_joints; 1513 | /* The trajectory generators will run interpolation_rate times slower than the */ 1514 | /* positioning. Position between are interpolated. */ 1515 | uint8_t interpolation_rate; 1516 | /* All configurations for the joint planers */ 1517 | trajgen_jointtg_config_t joints[TG_MAX_JOINTS]; 1518 | } trajgen_config_t; 1519 | 1520 | /** 1521 | * Joint definition 1522 | */ 1523 | typedef struct 1524 | { 1525 | /* Interpolator data */ 1526 | interpolator_t interpolator; 1527 | /* The trajgen for this axis */ 1528 | trajgen_jointtg_t tg; 1529 | /* Uninterpolated positions */ 1530 | real_t planer_position; 1531 | /* The interpolated position */ 1532 | real_t position; 1533 | /* The interpolated velocity */ 1534 | real_t velocity; 1535 | /* The interpolated acceleration */ 1536 | real_t acceleration; 1537 | /* The interpolated jerk */ 1538 | real_t jerk; 1539 | } trajgen_joint_t; 1540 | 1541 | /** 1542 | * Internal state variables 1543 | */ 1544 | typedef struct 1545 | { 1546 | uint32_t tick; 1547 | real_t* joint_positions[TG_MAX_JOINTS]; 1548 | real_t* planer_positions[TG_MAX_JOINTS]; 1549 | } trajgen_internals_t; 1550 | 1551 | /** 1552 | * Main trajgen type 1553 | */ 1554 | typedef struct 1555 | { 1556 | /* Configuration of the trajectory generator */ 1557 | trajgen_config_t config; 1558 | /* The last error occurred, only valid if a function returns nonzero */ 1559 | trajgen_error_details_t last_error; 1560 | /* The actual operating state of the trajectory generator */ 1561 | trajgen_state_t state; 1562 | /* The externally requested */ 1563 | trajgen_state_t requested_state; 1564 | /* The joint data that the planer needs */ 1565 | trajgen_joint_t joints[TG_MAX_JOINTS]; 1566 | /* Instance of the coordinated motion planer */ 1567 | trajgen_coord_t coord_planer; 1568 | /* Instance of the manual operation planer */ 1569 | trajgen_man_t man_planer; 1570 | /* Internal state variables, NO NOT MODIFY */ 1571 | trajgen_internals_t _internals; 1572 | /* The kinematics data structure */ 1573 | kinematics_t kinematics; 1574 | /* The pose of the last planer calculation, not the interpolated one */ 1575 | pose_t planer_pose; 1576 | /* Specifies if the planed motion is done */ 1577 | bool_t is_done; 1578 | } trajgen_t; 1579 | 1580 | 1581 | /** 1582 | * Initialises the trajectory generator and all sub systems. This function 1583 | * MUST BE CALLED BEFORE ANY OTHER OPERATIONS with the trajgen, as internal 1584 | * pointer require initialisation. 1585 | * @param trajgen_t *trajectory_generator_data_location 1586 | * @param trajgen_config_t config 1587 | * @return trajgen_error_t 1588 | */ 1589 | extern trajgen_error_t trajgen_initialize(trajgen_t *trajectory_generator_data_location, 1590 | trajgen_config_t config); 1591 | 1592 | /** 1593 | * Resets the internal state, except the configuration 1594 | * @return trajgen_error_t 1595 | */ 1596 | extern trajgen_error_t trajgen_reset(); 1597 | 1598 | /** 1599 | * Sets the new requested trajgen state. 1600 | * @param trajgen_state_t state 1601 | * @return trajgen_error_t 1602 | */ 1603 | extern trajgen_error_t trajgen_switch_state(trajgen_state_t state); 1604 | 1605 | /** 1606 | * Abort the current generator, decelerate to standstill. After aborting you 1607 | * should wait for done state. 1608 | * @return trajgen_error_t 1609 | */ 1610 | extern trajgen_error_t trajgen_abort(); 1611 | 1612 | /** 1613 | * Called frequently with the sample period tg.config.sample_interval. 1614 | * @return trajgen_error_t 1615 | */ 1616 | extern trajgen_error_t trajgen_tick(); 1617 | 1618 | /** 1619 | * Returns the current tick counter of the TG, which is increased every time 1620 | * trajgen_tick() is called. 1621 | * @return uint32_t 1622 | */ 1623 | extern uint32_t trajgen_get_tick(); 1624 | 1625 | /** 1626 | * Writes build information into the *s. 1627 | * @param char* s 1628 | * @param unsigned length 1629 | * @return const char* 1630 | */ 1631 | extern const char* trajgen_info(char* s, unsigned length); 1632 | 1633 | /** 1634 | * Coordinated planer accessors 1635 | * APPLY COORDINATED MOTION PLANER ONLY. See trajgen_coord functions for 1636 | * documentation. 1637 | */ 1638 | extern bool_t trajgen_queue_full(); 1639 | extern uint32_t trajgen_current_id(); 1640 | extern uint16_t trajgen_num_queued(); 1641 | extern uint16_t trajgen_queue_size(); 1642 | extern trajgen_error_t trajgen_pause(); 1643 | extern trajgen_error_t trajgen_resume(); 1644 | extern trajgen_error_t trajgen_add_line(pose_t end, real_t velocity, real_t acceleration); 1645 | extern trajgen_error_t trajgen_add_arc(pose_t end, pose_position_t center, pose_vector_t 1646 | normal, unsigned n_turns, real_t velocity, real_t acceleration); 1647 | extern trajgen_coord_error_t trajgen_add_curve(pose_t end, pose_vector_t start_ctrl_point, 1648 | pose_vector_t end_ctrl_point, real_t velocity, real_t acceleration); 1649 | 1650 | /* */ 1651 | 1652 | #ifdef __cplusplus 1653 | } 1654 | #endif 1655 | #endif 1656 | 1657 | -------------------------------------------------------------------------------- /rm501.c: -------------------------------------------------------------------------------- 1 | /* 2 | * rm501.c - Mitsubishi RM-501 Movemaster II Robot Simulator 3 | * 4 | * Copyright (C) 2013-2025 Jakob Flierl 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation: 9 | * version 2.1 of the License. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, 19 | * MA 02110-1301 USA 20 | */ 21 | 22 | #define PROGRAM_VERSION "0.0.2" 23 | 24 | //#define HAVE_SDL // working 25 | #ifdef HAVE_SDL 26 | #define HAVE_AUDIO // working, requires HAVE_SDL 27 | #define HAVE_JOYSTICK // working, requires HAVE_SDL 28 | //#define HAVE_PNG // working, requires HAVE_SDL 29 | #define ENABLE_FPS_LIMIT // working, requires HAVE_SDL 30 | #ifdef ENABLE_FPS_LIMIT 31 | #define DEFAULT_FPS 30 32 | #endif 33 | #endif 34 | 35 | //#define HAVE_SPACENAV // working 36 | //#define HAVE_HAL // partially working 37 | //#define HAVE_NCURSES // unfinished 38 | //#define HAVE_SERIAL // unfinished 39 | //#define HAVE_MQTT // unfinished 40 | //#define HAVE_ZMQ // unfinished 41 | //#define HAVE_TRAJGEN // unfinished 42 | 43 | #include 44 | #include 45 | #include // EXIT_SUCCESS 46 | #include // for bool, true, false 47 | #include 48 | #include 49 | #include // memcpy() 50 | #include // sigaction(), sigsuspend(), sig*() 51 | #ifdef __MINW32__ 52 | #define _USE_MATH_DEFINES 1 53 | #endif 54 | #include 55 | 56 | #ifdef HAVE_ZMQ 57 | #include 58 | #endif 59 | 60 | #ifdef HAVE_MQTT 61 | #include "mqtt.h" 62 | #endif 63 | 64 | #ifdef HAVE_SDL 65 | 66 | #ifdef _WIN32 67 | #include 68 | #endif 69 | #include 70 | 71 | #include 72 | #include 73 | #include 74 | #include 75 | #ifndef GL_BGR 76 | #define GL_BGR 0x80E0 77 | #endif 78 | #ifndef GL_BGRA 79 | #define GL_BGRA 0x80E1 80 | #endif 81 | #endif 82 | 83 | #ifdef HAVE_NCURSES 84 | #include 85 | #include 86 | #include 87 | #endif 88 | 89 | #ifdef HAVE_SPACENAV 90 | #include 91 | #include 92 | #include 93 | #include 94 | #include 95 | #endif 96 | 97 | #ifdef HAVE_SERIAL 98 | #include 99 | #endif 100 | 101 | #ifdef HAVE_SDL 102 | #ifdef HAVE_PNG 103 | #include "png.h" 104 | #include "png.c" 105 | #endif 106 | #endif 107 | 108 | #ifdef HAVE_TRAJGEN 109 | #include "trajgen.h" 110 | #endif 111 | 112 | volatile int done = 0; // SIGINT or user exit requested 113 | 114 | #ifdef HAVE_HAL 115 | int do_hal = 0; 116 | #include "hal.h" 117 | #define MODULE_NAME "rm501" 118 | char *modname = MODULE_NAME; 119 | 120 | typedef struct 121 | { 122 | hal_float_t *axis[5]; 123 | } hal_t; 124 | 125 | int hal_comp_id; 126 | hal_t *hal_pos_data; 127 | #endif 128 | 129 | #ifdef HAVE_SDL 130 | int view_mode = 0; 131 | int width = 640, height = 480; 132 | TTF_Font *sdl_font; 133 | 134 | SDL_Window *sdl_window; 135 | SDL_GLContext sdl_glcontext; 136 | SDL_Renderer *sdl_renderer; 137 | 138 | #ifdef HAVE_AUDIO 139 | int do_audio = 0; 140 | double MIDDLE_C[5]; 141 | int middle_c_dist[5]; 142 | #endif 143 | #endif 144 | 145 | #ifdef HAVE_NCURSES 146 | WINDOW *menubar, *messagebar; 147 | #endif 148 | 149 | #ifdef HAVE_SDL 150 | #ifdef HAVE_PNG 151 | SDL_Surface *png_shot; 152 | #endif 153 | #endif 154 | 155 | #ifdef HAVE_JOYSTICK 156 | SDL_Joystick *joy = NULL; 157 | const int JOYSTICK_DEAD_ZONE = 2500; 158 | #endif 159 | 160 | #ifdef HAVE_ZMQ 161 | int do_zmq = 0; 162 | #endif 163 | 164 | #ifdef HAVE_MQTT 165 | int do_mqtt = 0; 166 | #endif 167 | 168 | #ifdef HAVE_TRAJGEN 169 | int do_trajgen_test = 0; 170 | real_t tg_speed; 171 | real_t tg_accel; 172 | real_t tg_blending; 173 | unsigned tg_n_joints; 174 | trajgen_t tg_tg; 175 | trajgen_config_t tg_cfg; 176 | pose_t tg_last_pose; 177 | char tg_err_str[1024]; 178 | 179 | double rnd() 180 | { 181 | return ((double)rand()) / RAND_MAX; // NOLINT(cert-msc30-c, cert-msc50-cpp) 182 | } 183 | 184 | double roundd(double v, unsigned digits) 185 | { 186 | for (unsigned i = 0; i < digits; i++) 187 | v *= 10.; 188 | v = round(v); 189 | for (unsigned i = 0; i < digits; i++) 190 | v /= 10.; 191 | return v; 192 | } 193 | 194 | double randpos(double max) 195 | { 196 | return roundd(max * ((rnd() * 2) - 1), 2); 197 | } 198 | 199 | void tg_echk(unsigned errnom) 200 | { 201 | if (errnom) 202 | { 203 | fprintf(stderr, "tg error: %s\n", trajgen_errstr(tg_tg.last_error.errnom, tg_err_str, 1024)); 204 | exit(1); // XXX 205 | } 206 | } 207 | 208 | void tg_init(real_t max_speed, real_t max_accel, real_t sample_frequency, 209 | unsigned interpolation_rate, unsigned max_joints) 210 | { 211 | memset(tg_err_str, 0, sizeof(tg_err_str)); 212 | memset(&tg_last_pose, 0, sizeof(pose_t)); 213 | memset(&tg_tg, 0, sizeof(trajgen_t)); 214 | memset(&tg_cfg, 0, sizeof(trajgen_config_t)); 215 | 216 | tg_n_joints = max_joints > TG_MAX_JOINTS ? TG_MAX_JOINTS : max_joints; 217 | memset(&tg_cfg, 0, sizeof(trajgen_config_t)); 218 | tg_cfg.number_of_used_joints = tg_n_joints; 219 | tg_cfg.max_coord_velocity = max_speed; 220 | tg_cfg.max_coord_acceleration = max_accel; 221 | tg_cfg.sample_interval = 1.0 / sample_frequency; 222 | tg_cfg.interpolation_rate = interpolation_rate; 223 | pose_set_all(&tg_cfg.max_manual_velocities, max_speed); 224 | pose_set_all(&tg_cfg.max_manual_accelerations, max_accel); 225 | for (unsigned i = 0; i < tg_n_joints; i++) 226 | { 227 | tg_cfg.joints[i].max_velocity = max_speed; 228 | tg_cfg.joints[i].max_acceleration = max_speed; 229 | } 230 | tg_echk(trajgen_initialize(&tg_tg, tg_cfg)); 231 | } 232 | 233 | void tg_line(double x, double y, double z) 234 | { 235 | pose_t p; 236 | p.x = isnan(x) ? tg_last_pose.x : x; 237 | p.y = isnan(y) ? tg_last_pose.y : y; 238 | p.z = isnan(z) ? tg_last_pose.z : z; 239 | p.a = tg_last_pose.a; 240 | p.b = tg_last_pose.b; 241 | p.c = tg_last_pose.c; 242 | p.u = tg_last_pose.u; 243 | p.v = tg_last_pose.v; 244 | p.w = tg_last_pose.w; 245 | 246 | tg_last_pose = p; 247 | 248 | fprintf(stderr, "# line {x:%.6g, y:%.6g, z:%.6g, v:%.6g, a:%.6g, tol:%.6g }\n", p.x, p.y, p.z, tg_speed, tg_accel, tg_blending); 249 | tg_echk(trajgen_add_line(p, tg_speed, tg_accel)); 250 | } 251 | 252 | #endif // HAVE_TRAJGEN 253 | 254 | typedef struct 255 | { 256 | double d1, d5, a2, a3; // dh parameters 257 | 258 | struct joint_s 259 | { 260 | double pos; 261 | double vel; 262 | double min; 263 | double max; 264 | } j[5]; // joints 265 | 266 | struct cart_s 267 | { 268 | double vel; 269 | } cart[6]; // cartesian coordinates 270 | 271 | double t[16]; // tool matrix 272 | int err; // error a to joint number if inverse kinematics fails 273 | char msg[2048]; // opt. message from inverse kinematics 274 | 275 | #ifdef PROJ3 276 | int proj3counter; 277 | #endif 278 | 279 | bool grip; 280 | 281 | } bot_t; 282 | 283 | #ifdef HAVE_AUDIO 284 | short middle_c_gen() 285 | { 286 | short out = 0; 287 | 288 | #define AXES 5 289 | for (int i = 0; i < AXES; i++) 290 | { 291 | double volume = 0.05; 292 | middle_c_dist[i]++; /* Take one sample */ 293 | int MIDDLE_C_SAMPLES = (int) (44100 / MIDDLE_C[i]); 294 | if (middle_c_dist[i] >= MIDDLE_C_SAMPLES) 295 | middle_c_dist[i] = 0; 296 | 297 | /* Low? */ 298 | if (middle_c_dist[i] < MIDDLE_C_SAMPLES / 2) 299 | out += (short)(-32768 * volume / AXES); 300 | else 301 | /* High */ 302 | out += (short)(32767 * volume / AXES); 303 | } 304 | #undef AXES 305 | return out; 306 | } 307 | 308 | void fill_audio(void *data, Uint8 *stream, int len) 309 | { 310 | /* Cast */ 311 | short* buff = (short*)stream; 312 | len /= 2; /* Because we're now using shorts */ 313 | /* Square */ 314 | for (int i = 0; i < len; i += 2) 315 | { 316 | buff[i] = middle_c_gen(); /* Left */ 317 | buff[i + 1] = buff[i]; /* Right, same as left */ 318 | } 319 | } 320 | 321 | void open_audio() 322 | { 323 | SDL_AudioSpec as; 324 | /* Fill out what we want */ 325 | as.freq = 44100; 326 | as.format = AUDIO_S16SYS; 327 | as.channels = 2; 328 | as.samples = 1024; 329 | as.callback = fill_audio; 330 | /* Get it */ 331 | SDL_OpenAudio(&as, NULL); 332 | /* Go! */ 333 | SDL_PauseAudio(0); 334 | } 335 | 336 | void close_audio() 337 | { 338 | SDL_CloseAudio(); 339 | } 340 | #endif // HAVE_AUDIO 341 | 342 | #define rad2deg(rad) ((rad) * (180.0 / M_PI)) 343 | #define deg2rad(deg) ((deg) * (M_PI / 180.0)) 344 | #define sq(x) ((x) * (x)) 345 | 346 | void rotate_m_axyz(double *mat, double angle, double x, double y, double z) 347 | { 348 | double m[16] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; 349 | double s = sin(deg2rad(angle)); 350 | double c = cos(deg2rad(angle)); 351 | 352 | double mag = sqrt(sq(x) + sq(y) + sq(z)); 353 | if (mag <= 1.0e-4) 354 | { 355 | return; 356 | } // no rotation, leave mat as-is 357 | x /= mag; 358 | y /= mag; 359 | z /= mag; 360 | 361 | double one_c = 1.0 - c; 362 | #define M(row, col) m[col * 4 + row] 363 | M(0, 0) = (one_c * sq(x)) + c; 364 | M(0, 1) = (one_c * x * y) - z * s; 365 | M(0, 2) = (one_c * z * x) + y * s; 366 | 367 | M(1, 0) = (one_c * x * y) + z * s; 368 | M(1, 1) = (one_c * sq(y)) + c; 369 | M(1, 2) = (one_c * y * z) - x * s; 370 | 371 | M(2, 0) = (one_c * z * x) - y * s; 372 | M(2, 1) = (one_c * y * z) + x * s; 373 | M(2, 2) = (one_c * sq(z)) + c; 374 | #undef M 375 | 376 | for (int i = 0; i < 4; i++) 377 | { 378 | #define A(row, col) mat[(col << 2) + row] 379 | #define B(row, col) m[(col << 2) + row] 380 | double ai0 = A(i, 0), ai1 = A(i, 1), ai2 = A(i, 2), ai3 = A(i, 3); 381 | A(i, 0) = ai0 * B(0, 0) + ai1 * B(1, 0) + ai2 * B(2, 0) + ai3 * B(3, 0); 382 | A(i, 1) = ai0 * B(0, 1) + ai1 * B(1, 1) + ai2 * B(2, 1) + ai3 * B(3, 1); 383 | A(i, 2) = ai0 * B(0, 2) + ai1 * B(1, 2) + ai2 * B(2, 2) + ai3 * B(3, 2); 384 | A(i, 3) = ai0 * B(0, 3) + ai1 * B(1, 3) + ai2 * B(2, 3) + ai3 * B(3, 3); 385 | #undef A 386 | #undef B 387 | } 388 | } 389 | 390 | #define RPY_P_FUZZ (0.000001) 391 | 392 | int pmMatRpyConvert(double m[], double *r, double *p, double *y) 393 | { 394 | *p = atan2(-m[2], sqrt(sq(m[0]) + sq(m[1]))); 395 | 396 | if (fabs(*p - (2 * M_PI)) < RPY_P_FUZZ) 397 | { 398 | *r = atan2(m[4], m[5]); 399 | *p = (2 * M_PI); 400 | *y = 0.0; 401 | } 402 | else if (fabs(*p + (2 * M_PI)) < RPY_P_FUZZ) 403 | { 404 | *r = -atan2(m[6], m[5]); 405 | *p = -(2 * M_PI); 406 | *y = 0.0; 407 | } 408 | else 409 | { 410 | *r = atan2(m[6], m[10]); 411 | *y = atan2(m[1], m[0]); 412 | } 413 | 414 | return 0; 415 | } 416 | 417 | int kins_fwd(bot_t *bot) 418 | { 419 | double tr1 = deg2rad(bot->j[0].pos); 420 | double tr2 = deg2rad(bot->j[1].pos); 421 | double tr3 = deg2rad(bot->j[2].pos); 422 | double tr4 = deg2rad(bot->j[3].pos); 423 | double tr5 = deg2rad(bot->j[4].pos); 424 | 425 | double C234 = cos(tr2 + tr3 + tr4); 426 | double S234 = sin(tr2 + tr3 + tr4); 427 | double C23 = cos(tr2 + tr3); 428 | double S23 = sin(tr2 + tr3); 429 | double S1 = sin(tr1); 430 | double C1 = cos(tr1); 431 | double S2 = sin(tr2); 432 | double C2 = cos(tr2); 433 | double S5 = sin(tr5); 434 | double C5 = cos(tr5); 435 | 436 | double px = C1 * (bot->d5 * S234 + bot->a3 * C23 + bot->a2 * C2); 437 | double py = S1 * (bot->d5 * S234 + bot->a3 * C23 + bot->a2 * C2); 438 | double pz = bot->d1 - bot->d5 * C234 + bot->a3 * S23 + bot->a2 * S2; 439 | 440 | bot->t[0] = C1 * C5 * C234 - S1 * S5; 441 | bot->t[1] = C5 * S234; 442 | bot->t[2] = -S1 * C5 * C234 - C1 * S5; 443 | bot->t[3] = 0; 444 | 445 | bot->t[4] = -C1 * S234; 446 | bot->t[5] = C234; 447 | bot->t[6] = S1 * S234; 448 | bot->t[7] = 0; 449 | 450 | bot->t[8] = S1 * C5 + C1 * C234 * S5; 451 | bot->t[9] = S5 * S234; 452 | bot->t[10] = C1 * C5 - S1 * C234 * S5; 453 | bot->t[11] = 0; 454 | 455 | bot->t[12] = px; 456 | bot->t[13] = pz; 457 | bot->t[14] = py; 458 | 459 | bot->t[15] = 1; 460 | 461 | return 0; 462 | } 463 | 464 | int kins_inv(bot_t *bot) 465 | { 466 | 467 | double nx = -bot->t[0], ny = bot->t[2]; 468 | double ox = bot->t[8], oy = -bot->t[10]; 469 | double ax = -bot->t[4], ay = bot->t[6], az = -bot->t[5]; 470 | 471 | double px = bot->t[12]; 472 | double pz = bot->t[13]; 473 | double py = bot->t[14]; 474 | 475 | double th1; 476 | 477 | if (py == 0 && px == 0) 478 | { // point on the Z0 axis 479 | if (ay == 0 && ax == 0) 480 | { // wrist pointing straight up/down 481 | th1 = 0; 482 | } 483 | else 484 | { 485 | th1 = atan2(ay, ax); 486 | } 487 | } 488 | else 489 | { 490 | th1 = atan2(py, px); 491 | } 492 | 493 | double c1 = cos(th1); 494 | double s1 = sin(th1); 495 | 496 | double t234 = atan2(c1 * ax + s1 * ay, -az); 497 | double c234 = cos(t234); 498 | double s234 = sin(t234); 499 | 500 | // joint 3 - elbow 501 | double tp1 = c1 * px + s1 * py - bot->d5 * s234; 502 | double tp2 = pz - bot->d1 + bot->d5 * c234; 503 | double c3 = (sq(tp1) + sq(tp2) - sq(bot->a3) - sq(bot->a2)) / (2 * bot->a2 * bot->a3); 504 | double s3 = -sqrt(1 - sq(c3)); 505 | double th3 = atan2(s3, c3); 506 | 507 | // joint 2 - shoulder 508 | double num = tp2 * (bot->a3 * c3 + bot->a2) - bot->a3 * s3 * tp1; 509 | double den = tp1 * (bot->a3 * c3 + bot->a2) + bot->a3 * s3 * tp2; 510 | double th2 = atan2(num, den); 511 | 512 | // joint 4 - pitch 513 | double th4 = (t234 - th3 - th2); 514 | 515 | // joint 5 - roll 516 | double th5 = atan2(s1 * nx - c1 * ny, s1 * ox - c1 * oy); 517 | 518 | char msg[5][256]; 519 | double th[] = {th1, th2, th3, th4, th5}; 520 | 521 | bot->err = 0; 522 | 523 | for (int i = 0; i < 5; i++) 524 | { 525 | #ifdef KINS_INV_IGNORE_LIMITS 526 | if (!isnan(th[i])) 527 | { 528 | #else 529 | if (!isnan(th[i]) && th[i] >= deg2rad(bot->j[i].min) && th[i] <= deg2rad(bot->j[i].max)) 530 | { 531 | #endif 532 | bot->j[i].pos = rad2deg(th[i]); 533 | } 534 | else 535 | { 536 | bot->err |= (1 << i); 537 | } 538 | 539 | // pretty print results 540 | 541 | if (isnan(th[i])) 542 | { 543 | snprintf(msg[i], sizeof(msg[i]), "%7s ", "nan"); 544 | } 545 | else if (th[i] < deg2rad(bot->j[i].min)) 546 | { 547 | snprintf(msg[i], sizeof(msg[i]), "%7.2fv", rad2deg(th[i])); 548 | } 549 | else if (th[i] > deg2rad(bot->j[i].max)) 550 | { 551 | snprintf(msg[i], sizeof(msg[i]), "%7.2f^", rad2deg(th[i])); 552 | } 553 | else 554 | { 555 | snprintf(msg[i], sizeof(msg[i]), "%7.2f ", rad2deg(th[i])); 556 | } 557 | } 558 | 559 | snprintf(bot->msg, sizeof(bot->msg), "kin_inv(%d): %s %s %s %s %s", bot->err, msg[0], msg[1], msg[2], msg[3], msg[4]); 560 | 561 | return bot->err; 562 | } 563 | 564 | #ifdef HAVE_SDL 565 | void cross(float th, float l) 566 | { 567 | glLineWidth(th); 568 | glBegin(GL_LINES); 569 | glColor3f(1, 0, 0); 570 | glVertex3f(0, 0, 0); 571 | glVertex3f(l, 0, 0); 572 | glColor3f(0, 1, 0); 573 | glVertex3f(0, 0, 0); 574 | glVertex3f(0, l, 0); 575 | glColor3f(0, 0, 1); 576 | glVertex3f(0, 0, 0); 577 | glVertex3f(0, 0, l); 578 | glEnd(); 579 | glLineWidth(1); 580 | } 581 | #endif 582 | 583 | #ifdef HAVE_SDL 584 | 585 | unsigned int power_two_floor(unsigned int val) 586 | { 587 | unsigned int power = 2, nextVal = power * 2; 588 | while ((nextVal *= 2) <= val) 589 | power *= 2; 590 | return power * 2; 591 | } 592 | 593 | void text(int x, int y, TTF_Font *font, const char *format, ...) 594 | { 595 | char buffer[256]; 596 | va_list args; 597 | 598 | if (!font) 599 | { 600 | return; 601 | } 602 | 603 | va_start(args, format); 604 | vsnprintf(buffer, 255, format, args); 605 | 606 | SDL_Color col = {255, 255, 255}; 607 | SDL_Surface *msg = TTF_RenderText_Blended(font, buffer, col); 608 | GLuint tex; 609 | int texture_format; 610 | 611 | glEnable(GL_TEXTURE_2D); 612 | glGenTextures(1, &tex); 613 | glBindTexture(GL_TEXTURE_2D, tex); 614 | 615 | // Avoid mipmap filtering 616 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); 617 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); 618 | 619 | unsigned int w = power_two_floor(msg->w) * 2; 620 | unsigned int h = power_two_floor(msg->h) * 2; 621 | 622 | // Create a surface to the correct size in RGB format, and copy the old image 623 | SDL_Surface *s = SDL_CreateRGBSurface(0, w, h, 32, 0x00ff0000, 0x0000ff00, 0x000000ff, 0xff000000); 624 | SDL_BlitSurface(msg, NULL, s, NULL); 625 | 626 | int colors = s->format->BytesPerPixel; 627 | if (colors == 4) 628 | { // alpha 629 | if (s->format->Rmask == 0x000000ff) 630 | texture_format = GL_RGBA; 631 | else 632 | texture_format = GL_BGRA; 633 | } 634 | else 635 | { // no alpha 636 | if (s->format->Rmask == 0x000000ff) 637 | texture_format = GL_RGB; 638 | else 639 | texture_format = GL_BGR; 640 | } 641 | 642 | glTexImage2D(GL_TEXTURE_2D, 0, colors, w, h, 0, texture_format, 643 | GL_UNSIGNED_BYTE, s->pixels); 644 | 645 | glBegin(GL_QUADS); 646 | { 647 | int z = 1; 648 | glTexCoord2d(0, 0); 649 | glVertex3f(x, y, z); 650 | glTexCoord2d(1, 0); 651 | glVertex3f(x + s->w, y, z); 652 | glTexCoord2d(1, 1); 653 | glVertex3f(x + s->w, y + s->h, z); 654 | glTexCoord2d(0, 1); 655 | glVertex3f(x, y + s->h, z); 656 | } 657 | glEnd(); 658 | 659 | glDisable(GL_TEXTURE_2D); 660 | 661 | glDeleteTextures(1, &tex); 662 | SDL_FreeSurface(s); 663 | SDL_FreeSurface(msg); 664 | 665 | va_end(args); 666 | } 667 | 668 | void text_matrix(int x, int y, double m[]) 669 | { 670 | for (int i = 0; i < 4; i++) 671 | { 672 | for (int j = 0; j < 4; j++) 673 | { 674 | text(x + 75 * i, y + TTF_FontHeight(sdl_font) * j, sdl_font, "%8.2f", m[4 * i + j]); 675 | } 676 | } 677 | } 678 | 679 | void cube(GLfloat dSize, int wire) 680 | { 681 | float size = dSize * 0.5; 682 | 683 | #define V(a, b, c) glVertex3d(a size, b size, c size); 684 | #define N(a, b, c) glNormal3d(a, b, c); 685 | 686 | if (wire) 687 | { 688 | glBegin(GL_LINE_LOOP); 689 | N(1.0, 0.0, 0.0); 690 | V(+, -, +); 691 | V(+, -, -); 692 | V(+, +, -); 693 | V(+, +, +); 694 | glEnd(); 695 | glBegin(GL_LINE_LOOP); 696 | N(0.0, 1.0, 0.0); 697 | V(+, +, +); 698 | V(+, +, -); 699 | V(-, +, -); 700 | V(-, +, +); 701 | glEnd(); 702 | glBegin(GL_LINE_LOOP); 703 | N(0.0, 0.0, 1.0); 704 | V(+, +, +); 705 | V(-, +, +); 706 | V(-, -, +); 707 | V(+, -, +); 708 | glEnd(); 709 | glBegin(GL_LINE_LOOP); 710 | N(-1.0, 0.0, 0.0); 711 | V(-, -, +); 712 | V(-, +, +); 713 | V(-, +, -); 714 | V(-, -, -); 715 | glEnd(); 716 | glBegin(GL_LINE_LOOP); 717 | N(0.0, -1.0, 0.0); 718 | V(-, -, +); 719 | V(-, -, -); 720 | V(+, -, -); 721 | V(+, -, +); 722 | glEnd(); 723 | glBegin(GL_LINE_LOOP); 724 | N(0.0, 0.0, -1.0); 725 | V(-, -, -); 726 | V(-, +, -); 727 | V(+, +, -); 728 | V(+, -, -); 729 | glEnd(); 730 | } 731 | else 732 | { 733 | glBegin(GL_QUADS); 734 | N(1.0, 0.0, 0.0); 735 | V(+, -, +); 736 | V(+, -, -); 737 | V(+, +, -); 738 | V(+, +, +); 739 | N(0.0, 1.0, 0.0); 740 | V(+, +, +); 741 | V(+, +, -); 742 | V(-, +, -); 743 | V(-, +, +); 744 | N(0.0, 0.0, 1.0); 745 | V(+, +, +); 746 | V(-, +, +); 747 | V(-, -, +); 748 | V(+, -, +); 749 | N(-1.0, 0.0, 0.0); 750 | V(-, -, +); 751 | V(-, +, +); 752 | V(-, +, -); 753 | V(-, -, -); 754 | N(0.0, -1.0, 0.0); 755 | V(-, -, +); 756 | V(-, -, -); 757 | V(+, -, -); 758 | V(+, -, +); 759 | N(0.0, 0.0, -1.0); 760 | V(-, -, -); 761 | V(-, +, -); 762 | V(+, +, -); 763 | V(+, -, -); 764 | glEnd(); 765 | } 766 | #undef V 767 | #undef N 768 | } 769 | 770 | GLUquadricObj *create_quadric(int wire) 771 | { 772 | GLUquadricObj *qobj = gluNewQuadric(); 773 | if (!wire) 774 | { 775 | gluQuadricDrawStyle(qobj, GLU_FILL); 776 | } 777 | else 778 | { 779 | gluQuadricDrawStyle(qobj, GLU_LINE); 780 | } 781 | return qobj; 782 | } 783 | 784 | void cylinder(int wire, double BASE, double TOP, double HEIGHT, double SLICES, double STACKS) 785 | { 786 | GLUquadricObj *QUAD = create_quadric(wire); 787 | 788 | gluCylinder(QUAD, BASE, TOP, HEIGHT, SLICES, STACKS); 789 | 790 | if (!wire) 791 | { 792 | glRotatef(180, 1, 0, 0); 793 | gluDisk(QUAD, 0.0, BASE, SLICES, 1); 794 | glRotatef(180, 1, 0, 0); 795 | glTranslatef(0.0, 0.0, HEIGHT); 796 | gluDisk(QUAD, 0.0, TOP, SLICES, 1); 797 | } 798 | 799 | gluDeleteQuadric(QUAD); 800 | } 801 | 802 | void draw_bot(int wire, const bot_t *bot) 803 | { 804 | glPushMatrix(); // 1 805 | 806 | glTranslatef(0.0, bot->d1, 0.0); 807 | 808 | cross(4, 1); 809 | 810 | if (!wire) 811 | { 812 | glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); 813 | } 814 | else 815 | { 816 | glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); 817 | } 818 | 819 | // link1 - base 820 | 821 | glColor3f(0.25, 0.25, 0.25); 822 | 823 | glPushMatrix(); // 2 824 | glTranslatef(-0.5, -1.875, 0); 825 | glScalef(2, 0.75, 2); 826 | cube(1.0, wire); 827 | glPopMatrix(); // 1 828 | 829 | glPushMatrix(); // 2 830 | glTranslatef(0, -1.75, 0); 831 | glScalef(1 - 0.05, 1.0 - 0.05, 1.25 - 0.05); 832 | cube(1.0, wire); 833 | glPopMatrix(); // 1 834 | 835 | glPushMatrix(); // 2 836 | glTranslatef(0.0, -1.175 - 0.65 / 2, 0); 837 | glRotatef(-90, 1, 0, 0); 838 | glScalef(1.0, 1.0, 1.0); 839 | glColor3f(0.5, 0.5, 0.5); 840 | 841 | cylinder(wire, 0.35, 0.35, 0.65, 16, 1); 842 | 843 | glPopMatrix(); // 1 844 | 845 | // link2 - body 846 | 847 | glColor3f(1.0, 0.44, 0.176); 848 | 849 | glPushMatrix(); // 2 850 | glTranslatef(0, 0.0, 0.0); 851 | glRotatef(bot->j[0].pos, 0.0, 1.0, 0.0); 852 | glTranslatef(0, 0.0, 0.0); 853 | 854 | glPushMatrix(); // 3 855 | glTranslatef(-0.75, -0.45, 0.0); 856 | glScalef(2.5, 0.9, 1.2); 857 | cube(1.001, wire); 858 | glPopMatrix(); // 2 859 | 860 | glPushMatrix(); // 3 861 | glRotatef(14, 0.0, 0.0, 1.0); 862 | glTranslatef(-0.975, 0.05, 0.0); 863 | glScalef(1.9, 0.9, 1.2); 864 | cube(0.999, wire); 865 | glPopMatrix(); // 2 866 | 867 | glPushMatrix(); // 3 868 | glTranslatef(0.0, 0, -1.2 / 2); 869 | glRotatef(-90, 0.0, 0.0, 1.0); 870 | glScalef(1.0, 1.0, 1.0); 871 | cylinder(wire, 0.5, 0.5, 1.2, 16, 1); 872 | glPopMatrix(); // 2 873 | 874 | if (!wire) 875 | { 876 | glPushMatrix(); // 3 877 | glTranslatef(0.0, 0, -1.4 / 2); 878 | glRotatef(-90, 0.0, 0.0, 1.0); 879 | glScalef(1.0, 1.0, 1.0); 880 | cylinder(wire, 0.15, 0.15, 1.4, 16, 1); 881 | glPopMatrix(); // 2 882 | } 883 | 884 | glColor3f(1.0, 0.44, 0.176); 885 | 886 | glPushMatrix(); // 3 887 | glTranslatef(0.0, 0, -bot->d5 / 2); 888 | glRotatef(-90, 0.0, 0.0, 1.0); 889 | glScalef(1.0, 1.0, 1.0); 890 | cylinder(wire, 0.35, 0.35, bot->d5, 16, 1); 891 | glPopMatrix(); // 2 892 | 893 | // link3 - upperarm 894 | 895 | glColor3f(1.0, 0.44, 0.176); 896 | 897 | glRotatef(bot->j[1].pos, 0.0, 0.0, 1.0); 898 | 899 | glPushMatrix(); // 3 900 | glTranslatef(1.1, 0.0, 0.0); 901 | glScalef(bot->a2, 1.0, 1.0); 902 | cube(1.0, wire); 903 | glPopMatrix(); // 2 904 | 905 | if (!wire) 906 | { 907 | glPushMatrix(); // 3 908 | glTranslatef(0, 0, -0.5); 909 | glRotatef(-90, 0.0, 0.0, 1.0); 910 | glScalef(1.0, 1.0, 1.0); 911 | cylinder(wire, 0.5, 0.5, 1, 16, 1); 912 | glPopMatrix(); // 2 913 | 914 | glPushMatrix(); // 3 915 | glTranslatef(bot->a2, 0, -0.5); 916 | glRotatef(-90, 0.0, 0.0, 1.0); 917 | glScalef(1.0, 1.0, 1.0); 918 | cylinder(wire, 0.5, 0.5, 1, 16, 1); 919 | glPopMatrix(); // 2 920 | 921 | glPushMatrix(); // 3 922 | glTranslatef(bot->a2, 0, -1.1 / 2); 923 | glRotatef(-90, 0.0, 0.0, 1.0); 924 | glScalef(1.0, 1.0, 1.0); 925 | cylinder(wire, 0.25, 0.25, 1.1, 16, 1); 926 | glPopMatrix(); // 2 927 | 928 | glPushMatrix(); // 3 929 | glTranslatef(bot->a2, 0, -1.2 / 2); 930 | glRotatef(-90, 0.0, 0.0, 1.0); 931 | glScalef(1.0, 1.0, 1.0); 932 | cylinder(wire, 0.15, 0.15, 1.2, 16, 1); 933 | glPopMatrix(); // 2 934 | } 935 | 936 | // link4 - forearm 937 | 938 | glTranslatef(bot->a2, 0, 0); 939 | glRotatef(bot->j[2].pos, 0.0, 0.0, 1.0); 940 | 941 | glPushMatrix(); // 3 942 | glTranslatef(0.6, 0.0, 0.0); 943 | glScalef(1.2, 0.8, 0.9); 944 | cube(1.0, wire); 945 | glPopMatrix(); // 2 946 | 947 | glPushMatrix(); // 3 948 | glTranslatef(0, 0, -0.45); 949 | glRotatef(-90, 0.0, 0.0, 1.0); 950 | glScalef(1.0, 1.0, 1.0); 951 | cylinder(wire, 0.4, 0.4, 0.9, 16, 1); 952 | glPopMatrix(); // 2 953 | 954 | glPushMatrix(); // 3 955 | glTranslatef(1.2, 0, -0.45); 956 | glRotatef(-90, 0.0, 0.0, 1.0); 957 | glScalef(1.0, 1.0, 1.0); 958 | cylinder(wire, 0.4, 0.4, 0.9, 16, 1); 959 | glPopMatrix(); // 2 960 | 961 | // link5 - wrist pitch 962 | 963 | glColor3f(0.25, 0.25, 0.25); 964 | 965 | glTranslatef(bot->a3, 0, 0); 966 | glRotatef(bot->j[3].pos, 0.0, 0.0, 1.0); 967 | 968 | glPushMatrix(); // 3 969 | glTranslatef(0, 0, -0.6); 970 | glRotatef(-90, 0.0, 0.0, 1.0); 971 | cylinder(wire, 0.3, 0.3, 1.2, 22, 1); 972 | glPopMatrix(); // 2 973 | 974 | glColor3f(0.5, 0.5, 0.5); 975 | 976 | // link6 - hand 977 | 978 | glColor3f(0.25, 0.25, 0.25); 979 | 980 | glRotatef(bot->j[4].pos, 0.0, 1.0, 0.0); 981 | 982 | glPushMatrix(); // 3 983 | glRotatef(90, 1.0, 0.0, 0.0); 984 | cylinder(wire, 0.13, 0.13, 0.5, 22, 1); 985 | glPopMatrix(); // 2 986 | 987 | glColor3f(0.15, 0.15, 0.15); 988 | 989 | glPushMatrix(); // 3 990 | glTranslatef(0.0, -0.7, 0.0); 991 | glScalef(0.5, 0.4, 0.4); 992 | cube(1.0, wire); 993 | glPopMatrix(); // 2 994 | 995 | glPushMatrix(); // 3 996 | glTranslatef(0.0, -1.02, 0.0); 997 | glScalef(0.8, 0.25, 0.4); 998 | cube(1.0, wire); 999 | glPopMatrix(); // 2 1000 | 1001 | glTranslatef(0, -bot->d5, 0); 1002 | 1003 | double grip = bot->grip ? 0.25 : 0.05; 1004 | 1005 | glPushMatrix(); // 3 1006 | glTranslatef(grip, 0.0, 0.0); 1007 | glScalef(0.1, 0.4, 0.25); 1008 | cube(1.0, wire); 1009 | glPopMatrix(); // 2 1010 | 1011 | glPushMatrix(); // 3 1012 | glTranslatef(-grip, 0.0, 0.0); 1013 | glScalef(0.1, 0.4, 0.25); 1014 | cube(1.0, wire); 1015 | glPopMatrix(); // 2 1016 | 1017 | glDisable(GL_LIGHTING); 1018 | glDisable(GL_COLOR_MATERIAL); 1019 | cross(4, 0.5); 1020 | 1021 | glPopMatrix(); // 1 1022 | 1023 | glPopMatrix(); // 0 1024 | } 1025 | 1026 | void scene(const bot_t *bot_fwd, const bot_t *bot_inv) 1027 | { 1028 | glEnable(GL_LIGHTING); 1029 | glEnable(GL_COLOR_MATERIAL); 1030 | 1031 | // table 1032 | 1033 | glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); 1034 | 1035 | glColor3f(0.001, 0.001, 0.001); 1036 | 1037 | for (int x = -50; x <= 50; x += 5) 1038 | { 1039 | glPushMatrix(); 1040 | glTranslatef(-0.5 + x * .1, 0, 0); 1041 | glScalef(.05, .01, 10.0); 1042 | cube(1.0, 0); 1043 | glPopMatrix(); 1044 | glPushMatrix(); 1045 | glTranslatef(-0.5, 0, x * .1); 1046 | glScalef(10.0, .01, .05); 1047 | cube(1.0, 0); 1048 | glPopMatrix(); 1049 | } 1050 | 1051 | draw_bot(0, bot_fwd); 1052 | draw_bot(1, bot_inv); 1053 | } 1054 | 1055 | void draw_hud(bot_t *bot) 1056 | { 1057 | glMatrixMode(GL_MODELVIEW); 1058 | 1059 | glViewport(0, 0, width, height); 1060 | 1061 | glMatrixMode(GL_PROJECTION); 1062 | 1063 | glPushMatrix(); 1064 | glLoadIdentity(); 1065 | glOrtho(0.0, width, height, 0.0, -1.0, 10.0); 1066 | glMatrixMode(GL_MODELVIEW); 1067 | glLoadIdentity(); 1068 | 1069 | glClear(GL_DEPTH_BUFFER_BIT); 1070 | glColor3f(1, 0.2f, 0.2f); 1071 | 1072 | glEnable(GL_BLEND); 1073 | glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 1074 | 1075 | glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); 1076 | 1077 | glColor3ub(25, 200, 25); 1078 | 1079 | text(15, 10, sdl_font, "AXIS POS VEL"); 1080 | 1081 | text(15, 10 + 1 * TTF_FontHeight(sdl_font), sdl_font, "X: %8.2f %8.3f", bot->t[12], bot->cart[0].vel); 1082 | text(15, 10 + 2 * TTF_FontHeight(sdl_font), sdl_font, "Y: %8.2f %8.3f", bot->t[13], bot->cart[1].vel); 1083 | text(15, 10 + 3 * TTF_FontHeight(sdl_font), sdl_font, "Z: %8.2f %8.3f", bot->t[14], bot->cart[2].vel); 1084 | 1085 | double r, p, y; 1086 | pmMatRpyConvert(bot->t, &r, &p, &y); 1087 | 1088 | text(15, 10 + 5 * TTF_FontHeight(sdl_font), sdl_font, "R: %8.2f %8.3f", rad2deg(r), bot->cart[5].vel); 1089 | text(15, 10 + 6 * TTF_FontHeight(sdl_font), sdl_font, "P: %8.2f %8.3f", rad2deg(p), bot->cart[3].vel); 1090 | text(15, 10 + 7 * TTF_FontHeight(sdl_font), sdl_font, "Y: %8.2f %8.3f", rad2deg(y), bot->cart[4].vel); 1091 | 1092 | text(15, 10 + 9 * TTF_FontHeight(sdl_font), sdl_font, "GRIP: %s", bot->grip ? "Open" : "Closed"); 1093 | 1094 | text(15, 10 + 11 * TTF_FontHeight(sdl_font), sdl_font, "JOINT ANGLE VEL"); 1095 | /* 1096 | int i; 1097 | for (i = 0; i < 5; i++) { 1098 | text(15, 10+(i+1)*TTF_FontHeight(sdl_font), sdl_font, 1099 | "%d: %8.2f %8.3f", i+1, bot->j[i].pos, bot->j[i].vel); 1100 | }*/ 1101 | 1102 | text(15, 10 + 12 * TTF_FontHeight(sdl_font), sdl_font, "%d: %8.2f %8.3f", 1, bot->j[0].pos, bot->j[0].vel); 1103 | text(15, 10 + 13 * TTF_FontHeight(sdl_font), sdl_font, "%d: %8.2f %8.3f", 2, bot->j[1].pos, bot->j[1].vel); 1104 | text(15, 10 + 14 * TTF_FontHeight(sdl_font), sdl_font, "%d: %8.2f %8.3f", 3, bot->j[2].pos, bot->j[2].vel); 1105 | text(15, 10 + 15 * TTF_FontHeight(sdl_font), sdl_font, "%d: %8.2f %8.3f", 4, bot->j[3].pos - 90.0, bot->j[3].vel); 1106 | text(15, 10 + 16 * TTF_FontHeight(sdl_font), sdl_font, "%d: %8.2f %8.3f", 5, bot->j[4].pos, bot->j[4].vel); 1107 | 1108 | text(width - 370, 10, sdl_font, "TOOL"); 1109 | text_matrix(width - 320, 10, bot->t); 1110 | 1111 | if (strlen(bot->msg)) 1112 | { 1113 | text(15, height - TTF_FontHeight(sdl_font) * 1.5, sdl_font, bot->msg); 1114 | } 1115 | 1116 | glMatrixMode(GL_PROJECTION); 1117 | glPopMatrix(); 1118 | 1119 | glMatrixMode(GL_MODELVIEW); 1120 | } 1121 | 1122 | void display(const bot_t *bot_fwd, bot_t *bot_inv) 1123 | { 1124 | SDL_GetWindowSize(sdl_window, &width, &height); 1125 | 1126 | glClearColor(0.0f, 0.0f, 0.0f, 0.0f); 1127 | glClearDepth(1.0); 1128 | 1129 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 1130 | 1131 | glLoadIdentity(); 1132 | 1133 | if (view_mode == 0) 1134 | { 1135 | glViewport(0, 0, width, height); 1136 | } 1137 | else 1138 | { 1139 | glViewport(0, 0, width / 2, height / 2); 1140 | } 1141 | 1142 | glPushMatrix(); 1143 | glTranslatef(0, 0, -10); 1144 | glRotatef(15, 1, 0, 0); // glRotatef(rotate_x, 0, 1, 0); 1145 | glTranslatef(0, -2, 0); 1146 | 1147 | scene(bot_fwd, bot_inv); 1148 | 1149 | glPopMatrix(); 1150 | 1151 | glMatrixMode(GL_PROJECTION); 1152 | 1153 | if (view_mode != 0) 1154 | { 1155 | glPushMatrix(); 1156 | 1157 | glLoadIdentity(); 1158 | #define DIM 4.5 1159 | if (height > width) 1160 | { 1161 | glOrtho(-DIM, DIM, -DIM * (height / (width * 1.0)), DIM * (height / (width * 1.0)), -DIM * 2, DIM * 2); 1162 | } 1163 | else 1164 | { 1165 | glOrtho(-DIM * (width / (height * 1.0)), DIM * (width / (height * 1.0)), -DIM, DIM, -DIM * 2, DIM * 2); 1166 | } 1167 | #undef DIM 1168 | glMatrixMode(GL_MODELVIEW); 1169 | 1170 | glViewport(0, height / 2 + 1, width / 2 + 1, height / 2); 1171 | glPushMatrix(); 1172 | glTranslatef(0, -3, 0); 1173 | glRotatef(90, 0, -1, 0); 1174 | 1175 | scene(bot_fwd, bot_inv); 1176 | 1177 | glPopMatrix(); 1178 | 1179 | glViewport(width / 2 + 1, height / 2 + 1, width / 2, height / 2); 1180 | glPushMatrix(); 1181 | glTranslatef(0, -3, 0); 1182 | scene(bot_fwd, bot_inv); 1183 | glPopMatrix(); 1184 | 1185 | glViewport(width / 2 + 1, 0, width / 2, height / 2); 1186 | glPushMatrix(); 1187 | // glTranslatef( -2,0,0); 1188 | glRotatef(90, 1, 0, 0); 1189 | scene(bot_fwd, bot_inv); 1190 | glPopMatrix(); 1191 | 1192 | glMatrixMode(GL_PROJECTION); 1193 | } 1194 | 1195 | glPopMatrix(); 1196 | 1197 | draw_hud(bot_inv); 1198 | 1199 | glEnd(); 1200 | 1201 | SDL_GL_SwapWindow(sdl_window); 1202 | } 1203 | #endif 1204 | 1205 | int do_kins_fwd = 1; 1206 | int do_kins_inv = 0; 1207 | 1208 | void jog_joint(bot_t *bot, int i, double amount) 1209 | { 1210 | double tmp = bot->j[i].pos + amount; 1211 | 1212 | if (amount > 0) 1213 | { 1214 | bot->j[i].pos = fmin(tmp, bot->j[i].max); 1215 | do_kins_fwd = 1; 1216 | } 1217 | else 1218 | { 1219 | bot->j[i].pos = fmax(tmp, bot->j[i].min); 1220 | do_kins_fwd = 1; 1221 | } 1222 | } 1223 | 1224 | void move_tool(bot_t *bot, int i, double amount) 1225 | { 1226 | bot->t[12 + i] += amount; 1227 | do_kins_inv = 1; 1228 | } 1229 | 1230 | void rotate_tool(bot_t *bot, double a, double x, double y, double z) 1231 | { 1232 | rotate_m_axyz(bot->t, a, x, y, z); 1233 | do_kins_inv = 1; 1234 | } 1235 | 1236 | #ifdef HAVE_SPACENAV 1237 | 1238 | typedef struct 1239 | { 1240 | int fd; 1241 | int pos[6]; 1242 | int key[2]; 1243 | } spacenav_t; 1244 | 1245 | #define test_bit(bit, array) (array[bit / 8] & (1 << (bit % 8))) 1246 | 1247 | int spacenav_open(void) 1248 | { 1249 | char fname[20]; 1250 | struct input_id id; 1251 | 1252 | int i = 0; 1253 | while (i < 64) 1254 | { 1255 | snprintf(fname, sizeof(fname), "/dev/input/event%d", i++); 1256 | int fd = open(fname, O_RDWR | O_NONBLOCK); 1257 | if (fd > 0) 1258 | { 1259 | ioctl(fd, EVIOCGID, &id); 1260 | 1261 | if (id.vendor == 0x046d && (id.product == 0xc626 || id.product == 0xc623 || id.product == 0xc603)) 1262 | { 1263 | // fprintf(stderr, "Using device: %s\n", fname); 1264 | return fd; 1265 | } 1266 | 1267 | close(fd); 1268 | } 1269 | } 1270 | 1271 | return 0; 1272 | } 1273 | 1274 | void spacenav_close(spacenav_t *s) 1275 | { 1276 | if (s->fd) 1277 | { 1278 | close(s->fd); 1279 | } 1280 | } 1281 | 1282 | void spacenav_read(spacenav_t *s) 1283 | { 1284 | struct input_event ev; 1285 | 1286 | int n; 1287 | while ((n = read(s->fd, &ev, sizeof(struct input_event))) > 0) 1288 | { 1289 | // fprintf(stderr, "spacenav_read %d bytes.\n", n); 1290 | 1291 | if (n >= sizeof(struct input_event)) 1292 | { 1293 | switch (ev.type) 1294 | { 1295 | case EV_KEY: 1296 | s->key[ev.code] = ev.value; // fprintf(stderr, "Key %d pressed %d.\n", ev.code, ev.value); 1297 | break; 1298 | case EV_REL: 1299 | s->pos[ev.code] = ev.value; // fprintf(stderr, "REL %d %d\n", ev.code, ev.value); 1300 | break; 1301 | case EV_ABS: 1302 | s->pos[ev.code] = ev.value; // fprintf(stderr, "ABS %d %d\n", ev.code, ev.value); 1303 | break; 1304 | default: 1305 | break; 1306 | } 1307 | } 1308 | } 1309 | } 1310 | 1311 | #endif 1312 | 1313 | int update_model(bot_t *bot_fwd, bot_t *bot_inv, int do_kins_fwd, int do_kins_inv) 1314 | { 1315 | int err = 0; 1316 | if (do_kins_inv) 1317 | { 1318 | err = kins_inv(bot_inv); 1319 | if (bot_inv->err == 0) 1320 | { 1321 | kins_fwd(bot_inv); 1322 | memcpy(bot_fwd, bot_inv, sizeof(bot_t)); 1323 | } 1324 | else 1325 | { 1326 | memcpy(bot_inv, bot_fwd, sizeof(bot_t)); 1327 | } 1328 | } 1329 | else if (do_kins_fwd) 1330 | { 1331 | kins_fwd(bot_fwd); 1332 | memcpy(bot_inv, bot_fwd, sizeof(bot_t)); 1333 | } 1334 | 1335 | return err; 1336 | } 1337 | 1338 | #ifdef HAVE_SDL 1339 | #include "icons/rm501.cdata" 1340 | void set_window_icon(SDL_Window *sdl_window) 1341 | { 1342 | SDL_RWops *z = SDL_RWFromMem(rm501_png, sizeof(rm501_png)); 1343 | SDL_Surface *sdl_window_icon_surface = IMG_Load_RW(z, 1); 1344 | if (sdl_window_icon_surface == NULL) 1345 | { 1346 | fprintf(stderr, "Error: unable to load icons/rm501.png: %s.\n", SDL_GetError()); 1347 | exit(EXIT_FAILURE); 1348 | } 1349 | SDL_SetWindowIcon(sdl_window, sdl_window_icon_surface); 1350 | SDL_FreeSurface(sdl_window_icon_surface); 1351 | } 1352 | 1353 | #ifdef HAVE_PNG 1354 | void screenshot(int x, int y, const char *filename) 1355 | { 1356 | unsigned char pixels[width * height * 6]; 1357 | SDL_Surface *infoSurface = SDL_GetWindowSurface(sdl_window); 1358 | SDL_Surface *sdl_screenshot = SDL_CreateRGBSurfaceFrom(pixels, infoSurface->w, infoSurface->h, infoSurface->format->BitsPerPixel, infoSurface->w * infoSurface->format->BytesPerPixel, infoSurface->format->Rmask, infoSurface->format->Gmask, infoSurface->format->Bmask, infoSurface->format->Amask); 1359 | 1360 | SDL_RenderReadPixels(sdl_renderer, &infoSurface->clip_rect, infoSurface->format->format, pixels, infoSurface->w * infoSurface->format->BytesPerPixel); 1361 | SDL_SavePNG(sdl_screenshot, filename); 1362 | SDL_FreeSurface(sdl_screenshot); 1363 | SDL_FreeSurface(infoSurface); 1364 | infoSurface = NULL; 1365 | } 1366 | #endif 1367 | 1368 | SDL_Rect toggle_fake_fullscreen(SDL_Rect old_bounds) 1369 | { 1370 | if (SDL_GetWindowFlags(sdl_window) & SDL_WINDOW_BORDERLESS) 1371 | { 1372 | SDL_SetWindowBordered(sdl_window, SDL_TRUE); 1373 | SDL_SetWindowSize(sdl_window, old_bounds.w, old_bounds.h); 1374 | SDL_SetWindowPosition(sdl_window, old_bounds.x, old_bounds.y); 1375 | return old_bounds; 1376 | } 1377 | else 1378 | { 1379 | SDL_Rect cur_bounds; 1380 | SDL_GetWindowPosition(sdl_window, &cur_bounds.x, &cur_bounds.y); 1381 | SDL_GetWindowSize(sdl_window, &cur_bounds.w, &cur_bounds.h); 1382 | 1383 | int idx = SDL_GetWindowDisplayIndex(sdl_window); 1384 | SDL_Rect bounds; 1385 | SDL_GetDisplayBounds(idx, &bounds); 1386 | SDL_SetWindowBordered(sdl_window, SDL_FALSE); 1387 | // SDL_SetWindowPosition(sdl_window, bounds.x, bounds.y); 1388 | SDL_SetWindowSize(sdl_window, bounds.w, bounds.h); 1389 | 1390 | return cur_bounds; 1391 | } 1392 | } 1393 | #endif 1394 | 1395 | void handle_signal(int signal) 1396 | { 1397 | #ifndef __MINGW32__ 1398 | sigset_t pending; 1399 | // Find out which signal we're handling 1400 | switch (signal) 1401 | { 1402 | case SIGHUP: 1403 | case SIGUSR1: 1404 | break; 1405 | case SIGINT: 1406 | done = 1; 1407 | default: 1408 | return; 1409 | } 1410 | 1411 | sigpending(&pending); 1412 | if (sigismember(&pending, SIGHUP)) 1413 | { 1414 | } 1415 | if (sigismember(&pending, SIGUSR1)) 1416 | { 1417 | } 1418 | #endif 1419 | } 1420 | 1421 | #ifdef HAVE_MQTT 1422 | 1423 | #define RM501_SCALAR 150.0 1424 | 1425 | coord_t bot2coord(bot_t *bot) 1426 | { 1427 | // convert bot to coord 1428 | double r = 0, p = 0, y = 0; 1429 | pmMatRpyConvert(bot->t, &y, &r, &p); 1430 | coord_t coord = { 1431 | bot->t[14] * RM501_SCALAR, 1432 | bot->t[12] * RM501_SCALAR, 1433 | bot->t[13] * RM501_SCALAR, 1434 | rad2deg(p) + 90.0, 1435 | rad2deg(r), // bot_inv.j[4].pos 1436 | bot->grip}; 1437 | return coord; 1438 | } 1439 | 1440 | void coord2bot(bot_t *bot, coord_t coord) 1441 | { 1442 | // convert coord to bot 1443 | bot_t bot_aux = *bot; 1444 | bot_aux.t[14] = coord.x / RM501_SCALAR; 1445 | bot_aux.t[12] = coord.y / RM501_SCALAR; 1446 | bot_aux.t[13] = coord.z / RM501_SCALAR; 1447 | 1448 | for (int i = 0; i < 10; i++) 1449 | { 1450 | double r = 0, p = 0, y = 0; 1451 | pmMatRpyConvert(bot_aux.t, &y, &r, &p); 1452 | p = rad2deg(p) - 90.0; 1453 | r = rad2deg(r); 1454 | // pitch 1455 | rotate_m_axyz(bot_aux.t, coord.pitch - p, sin(deg2rad(bot_aux.j[0].pos)), 0, cos(deg2rad(bot_aux.j[0].pos))); 1456 | // roll 1457 | rotate_m_axyz(bot_aux.t, coord.roll - r, 0, 1, 0); 1458 | 1459 | kins_inv(&bot_aux); 1460 | // bot_aux.j[4].pos=r; 1461 | // kins_fwd(&bot_aux); 1462 | } 1463 | 1464 | bot_aux.grip = coord.grip; 1465 | // return results 1466 | *bot = bot_aux; 1467 | } 1468 | #endif 1469 | 1470 | int main(int argc, char *argv[]) 1471 | { 1472 | 1473 | #ifndef __MINGW32__ 1474 | struct sigaction sa; 1475 | sa.sa_handler = &handle_signal; 1476 | sa.sa_flags = SA_RESTART; 1477 | sigfillset(&sa.sa_mask); 1478 | 1479 | if (sigaction(SIGHUP, &sa, NULL) == -1) 1480 | { 1481 | perror("Error: cannot handle SIGHUP"); 1482 | } 1483 | 1484 | if (sigaction(SIGUSR1, &sa, NULL) == -1) 1485 | { 1486 | perror("Error: cannot handle SIGUSR1"); 1487 | } 1488 | 1489 | if (sigaction(SIGINT, &sa, NULL) == -1) 1490 | { 1491 | perror("Error: cannot handle SIGINT"); 1492 | } 1493 | #endif 1494 | 1495 | #ifdef ENABLE_FPS_LIMIT 1496 | unsigned int ft = 0, frames; 1497 | #endif 1498 | 1499 | bot_t bot_fwd, bot_inv; 1500 | 1501 | #ifdef HAVE_SDL 1502 | int do_sdl = 1; 1503 | bool do_fullscreen = false; 1504 | 1505 | SDL_Event ev; 1506 | const Uint8 *keys = SDL_GetKeyboardState(0); 1507 | 1508 | int sdlk_tab_pressed = 0; 1509 | 1510 | int sdl_flags = SDL_WINDOW_SHOWN | SDL_WINDOW_RESIZABLE | SDL_WINDOW_OPENGL; 1511 | SDL_DisplayMode sdl_displaymode; 1512 | 1513 | SDL_Rect cur_bounds; 1514 | #endif 1515 | 1516 | #ifdef HAVE_NCURSES 1517 | int do_curses = 0; 1518 | #endif 1519 | 1520 | int do_help = 0; 1521 | int do_version = 0; 1522 | int verbose = 0; 1523 | int rw_mode = 0; 1524 | 1525 | int i = 0; 1526 | while (++i < argc) 1527 | { 1528 | #define OPTION_SET(longopt, shortopt) (strcmp(argv[i], longopt) == 0 || strcmp(argv[i], shortopt) == 0) 1529 | #define OPTION_VALUE ((i + 1 < argc) ? (argv[i + 1]) : (NULL)) 1530 | #define OPTION_VALUE_PROCESSED (i++) 1531 | if (OPTION_SET("--help", "-h")) 1532 | { 1533 | do_help = 1; 1534 | } 1535 | else if (OPTION_SET("--version", "-v")) 1536 | { 1537 | do_version = 1; 1538 | #ifdef HAVE_SDL 1539 | } 1540 | else if (OPTION_SET("--fullscreen", "-f")) 1541 | { 1542 | do_fullscreen = true; 1543 | } 1544 | else if (OPTION_SET("--no-sdl", "-s")) 1545 | { 1546 | do_sdl = 0; 1547 | #endif 1548 | #ifdef HAVE_AUDIO 1549 | } 1550 | else if (OPTION_SET("--audio", "-a")) 1551 | { 1552 | do_audio = 1; 1553 | #endif 1554 | #ifdef HAVE_HAL 1555 | } 1556 | else if (OPTION_SET("--hal", "-l")) 1557 | { 1558 | do_hal = 1; 1559 | #endif 1560 | #ifdef HAVE_TRAJGEN 1561 | } 1562 | else if (OPTION_SET("--trajgen-test", "-t")) 1563 | { 1564 | do_trajgen_test = 1; 1565 | #endif 1566 | #ifdef HAVE_NCURSES 1567 | } 1568 | else if (OPTION_SET("--curses", "-c")) 1569 | { 1570 | do_curses = 1; 1571 | #endif 1572 | #ifdef HAVE_ZMQ 1573 | } 1574 | else if (OPTION_SET("--zmq", "-z")) 1575 | { 1576 | do_zmq = 1; 1577 | #endif 1578 | #ifdef HAVE_MQTT 1579 | } 1580 | else if (OPTION_SET("--mqtt", "-m")) 1581 | { 1582 | do_mqtt = 1; 1583 | } 1584 | else if (OPTION_SET("--rw", "-w")) 1585 | { 1586 | do_mqtt = 1; 1587 | rw_mode = 1; 1588 | #endif 1589 | } 1590 | else if (OPTION_SET("--verbose", "-x")) 1591 | { 1592 | verbose++; 1593 | } 1594 | else 1595 | { 1596 | fprintf(stderr, "Unknown option: %s\n", argv[i]); 1597 | do_help = 1; 1598 | } 1599 | } 1600 | 1601 | if (do_version) 1602 | { 1603 | fprintf(stdout, "%s %s\n", argv[0], PROGRAM_VERSION); 1604 | return EXIT_SUCCESS; 1605 | } 1606 | 1607 | if (do_help) 1608 | { 1609 | fprintf(stdout, "Usage: %s [OPTIONS]\n\n" 1610 | " Where [OPTIONS] are zero or more of the following:\n\n" 1611 | #ifdef HAVE_SDL 1612 | " [-s|--no-sdl] Hide SDL window\n" 1613 | " [-f|--fullscreen] SDL window fullscreen mode\n" 1614 | #endif 1615 | #ifdef HAVE_AUDIO 1616 | " [-a|--audio] Motor motion audio\n" 1617 | #endif 1618 | #ifdef HAVE_NCURSES 1619 | " [-c|--curses] Curses text mode\n" 1620 | #endif 1621 | #ifdef HAVE_HAL 1622 | " [-l|--hal] HAL mode\n" 1623 | #endif 1624 | #ifdef HAVE_TRAJGEN 1625 | " [-t|--trajgen-test] Do trajgen test\n" 1626 | #endif 1627 | #ifdef HAVE_ZMQ 1628 | " [-z|--zmq] ZMQ server mode\n" 1629 | #endif 1630 | #ifdef HAVE_MQTT 1631 | " [-m|--mqtt] MQTT client mode\n" 1632 | " [-w|--rw] MQTT write mode\n" 1633 | #endif 1634 | " [-x|--verbose] Show verbose information\n\n" 1635 | " [-h|--help] Show help information\n\n" 1636 | " [-v|--version] Show version number\n\n" 1637 | "Report bugs to Jakob Flierl \n" 1638 | "Website and manual: https://github.com/koppi/rm501\n" 1639 | "\n", 1640 | argv[0]); 1641 | return EXIT_SUCCESS; 1642 | } 1643 | 1644 | #ifdef HAVE_HAL 1645 | if (do_hal) 1646 | { 1647 | // initialize component 1648 | hal_comp_id = hal_init(modname); 1649 | if (hal_comp_id < 1) 1650 | { 1651 | fprintf(stderr, "%s: ERROR: hal_init failed\n", modname); 1652 | goto fail0; 1653 | } 1654 | 1655 | // allocate hal memory 1656 | hal_pos_data = hal_malloc(sizeof(hal_t)); 1657 | if (hal_pos_data == NULL) 1658 | { 1659 | fprintf(stderr, "%s: ERROR: unable to allocate HAL shared memory\n", modname); 1660 | goto fail1; 1661 | } 1662 | 1663 | // register pins 1664 | for (i = 0; i < 5; i++) 1665 | { 1666 | if (hal_pin_float_newf(HAL_OUT, &(hal_pos_data->axis[i]), hal_comp_id, "%s.%d.pos-cmd", MODULE_NAME, i) != 0) 1667 | { 1668 | fprintf(stderr, "%s: ERROR: unable to register hal pin %s.%d.pos-cmd\n", modname, MODULE_NAME, i); 1669 | goto fail1; 1670 | } 1671 | } 1672 | } 1673 | #endif 1674 | 1675 | #ifdef HAVE_ZMQ 1676 | if (do_zmq && verbose >= 1) 1677 | { 1678 | int major, minor, patch; 1679 | zmq_version(&major, &minor, &patch); 1680 | fprintf(stderr, "Using 0MQ version %d.%d.%d\n", major, minor, patch); 1681 | } 1682 | #endif 1683 | 1684 | #ifdef HAVE_MQTT 1685 | if (do_mqtt) 1686 | { 1687 | mqtt_handler_init(); 1688 | } 1689 | #endif 1690 | 1691 | #ifdef HAVE_NCURSES 1692 | if (do_curses) 1693 | { 1694 | initscr(); 1695 | nonl(); 1696 | cbreak(); 1697 | noecho(); 1698 | keypad(stdscr, 1); 1699 | timeout(0); // getch non-blocking 1700 | if (has_colors()) 1701 | { 1702 | start_color(); 1703 | init_pair(1, COLOR_WHITE, COLOR_BLUE); 1704 | init_pair(2, COLOR_BLUE, COLOR_WHITE); 1705 | init_pair(3, COLOR_RED, COLOR_WHITE); 1706 | } 1707 | curs_set(0); 1708 | 1709 | struct winsize w; 1710 | ioctl(STDOUT_FILENO, TIOCGWINSZ, &w); 1711 | 1712 | menubar = subwin(stdscr, 1, w.ws_col, 0, 0); 1713 | messagebar = subwin(stdscr, 1, w.ws_col - 1, w.ws_row - 1, 1); 1714 | wbkgd(menubar, COLOR_PAIR(2)); 1715 | wbkgd(messagebar, COLOR_PAIR(2)); 1716 | 1717 | werase(menubar); 1718 | wrefresh(menubar); 1719 | 1720 | werase(messagebar); 1721 | wrefresh(messagebar); 1722 | 1723 | wprintw(menubar, "Mitsubishi RM-501 Movemaster II Robot Simulator"); 1724 | wprintw(messagebar, "Status: DISCONNECTED, OFFLINE"); 1725 | } 1726 | #endif 1727 | 1728 | #ifdef HAVE_SPACENAV 1729 | spacenav_t sn = {0}; 1730 | 1731 | sn.fd = spacenav_open(); 1732 | #endif 1733 | 1734 | #ifdef HAVE_SDL 1735 | if (do_sdl) 1736 | { 1737 | 1738 | if (verbose >= 1) 1739 | { 1740 | SDL_version linked; 1741 | SDL_GetVersion(&linked); 1742 | SDL_version compiled; 1743 | SDL_VERSION(&compiled); 1744 | fprintf(stderr, "Compiled with SDL version %d.%d.%d.\n", compiled.major, compiled.minor, compiled.patch); 1745 | fprintf(stderr, "Linked against SDL version %d.%d.%d.\n", linked.major, linked.minor, linked.patch); 1746 | } 1747 | 1748 | #ifdef HAVE_JOYSTICK 1749 | SDL_SetHint(SDL_HINT_JOYSTICK_ALLOW_BACKGROUND_EVENTS, "1"); 1750 | #endif 1751 | if (SDL_Init(SDL_INIT_EVERYTHING) < 0) 1752 | { 1753 | fprintf(stderr, "Unable to initialise SDL: %s\n", SDL_GetError()); 1754 | exit(EXIT_FAILURE); 1755 | } 1756 | 1757 | #ifdef HAVE_AUDIO 1758 | if (do_audio) 1759 | { 1760 | open_audio(); 1761 | } 1762 | #endif 1763 | 1764 | #ifdef HAVE_JOYSTICK 1765 | if (SDL_NumJoysticks() < 1) 1766 | { 1767 | // printf( "Warning: No joysticks connected!\n" ); 1768 | } 1769 | else 1770 | { 1771 | joy = SDL_JoystickOpen(0); 1772 | if (joy == NULL) 1773 | { 1774 | printf("Warning: Unable to open game controller! SDL Error: %s\n", SDL_GetError()); 1775 | } 1776 | } 1777 | #endif 1778 | 1779 | if (TTF_Init() < 0) 1780 | { 1781 | fprintf(stderr, "Unable to initialise SDL_ttf.\n"); 1782 | exit(EXIT_FAILURE); 1783 | } 1784 | 1785 | char *sdl_font_file = NULL; 1786 | #if defined(__HAIKU__) 1787 | sdl_font_file = "/Haiku/system/data/fonts/ttfonts/DejaVuSansMono.ttf"; 1788 | #elif defined(__FreeBSD__) 1789 | sdl_font_file = "/usr/local/share/fonts/dejavu/DejaVuSansMono.ttf"; 1790 | //#elif defined(linux) // Alpine Linux 1791 | // sdl_font_file = "/usr/share/fonts/ttf-dejavu/DejaVuSansMono.ttf"; 1792 | #elif defined(__linux__) 1793 | sdl_font_file = "/usr/share/fonts/truetype/dejavu/DejaVuSansMono.ttf"; 1794 | #elif defined(__MINGW32__) 1795 | sdl_font_file = "doc/DejaVuSansMono.ttf"; 1796 | #else 1797 | #warning "Please set your font path in the source code." 1798 | #endif 1799 | 1800 | sdl_font = TTF_OpenFont(sdl_font_file, 15); 1801 | 1802 | if (!sdl_font) 1803 | { 1804 | fprintf(stderr, "%s: %s\n", SDL_GetError(), sdl_font_file); 1805 | exit(EXIT_FAILURE); 1806 | } 1807 | 1808 | if (SDL_GetCurrentDisplayMode(0, &sdl_displaymode) != 0) 1809 | { 1810 | fprintf(stderr, "Could not get display mode for video display #%d: %s", 0, SDL_GetError()); 1811 | exit(EXIT_FAILURE); 1812 | } 1813 | 1814 | /* 1815 | if (sdl_flags & SDL_WINDOW_FULLSCREEN) { 1816 | width = sdl_displaymode.w; 1817 | height = sdl_displaymode.h; 1818 | }*/ 1819 | 1820 | SDL_SetHint(SDL_HINT_RENDER_DRIVER, "opengl"); 1821 | //SDL_SetHint(SDL_HINT_RENDER_DRIVER, "opengles2"); 1822 | 1823 | sdl_window = SDL_CreateWindow("Mitsubishi RM-501 Movemaster II Robot Simulator", 1824 | SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, 1825 | width, height, sdl_flags); 1826 | 1827 | SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1); 1828 | SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE, 24); 1829 | 1830 | sdl_glcontext = SDL_GL_CreateContext(sdl_window); 1831 | 1832 | sdl_renderer = SDL_CreateRenderer(sdl_window, -1, SDL_RENDERER_ACCELERATED); 1833 | 1834 | set_window_icon(sdl_window); 1835 | 1836 | SDL_RenderClear(sdl_renderer); 1837 | 1838 | if (do_fullscreen) 1839 | { 1840 | cur_bounds = toggle_fake_fullscreen(cur_bounds); 1841 | } 1842 | 1843 | //SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1); 1844 | //glEnable(GL_DEBUG_OUTPUT); 1845 | 1846 | { 1847 | GLfloat pos[4] = {3., 5., 2., 1.}; 1848 | GLfloat white[4] = {0.75, 0.75, 0.75, 1.}; 1849 | GLfloat black[4] = {0., 0., 0., 0.}; 1850 | 1851 | glDisable(GL_LIGHTING); 1852 | glEnable(GL_LIGHT1); 1853 | glLightfv(GL_LIGHT1, GL_POSITION, pos); 1854 | glLightfv(GL_LIGHT1, GL_DIFFUSE, white); 1855 | glLightfv(GL_LIGHT1, GL_SPECULAR, black); 1856 | 1857 | glEnable(GL_COLOR_MATERIAL); 1858 | glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE); 1859 | glMaterialfv(GL_FRONT, GL_SPECULAR, black); 1860 | } 1861 | 1862 | glEnable(GL_TEXTURE_2D); 1863 | 1864 | glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST); 1865 | 1866 | glEnable(GL_LINE_SMOOTH); 1867 | glHint(GL_LINE_SMOOTH_HINT, GL_NICEST); 1868 | 1869 | glEnable(GL_POLYGON_SMOOTH); 1870 | glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST); 1871 | 1872 | glEnable(GL_BLEND); 1873 | glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 1874 | 1875 | glDepthFunc(GL_LEQUAL); 1876 | glEnable(GL_DEPTH_TEST); 1877 | glShadeModel(GL_SMOOTH); 1878 | 1879 | glMatrixMode(GL_PROJECTION); 1880 | 1881 | glLoadIdentity(); 1882 | gluPerspective(45, width / (height * 1.0), 0.1, 500); 1883 | 1884 | glMatrixMode(GL_MODELVIEW); 1885 | #endif 1886 | } 1887 | 1888 | bot_fwd.d1 = 2.3; 1889 | bot_fwd.d5 = 1.3; 1890 | bot_fwd.a2 = 2.2; 1891 | bot_fwd.a3 = 1.6; 1892 | 1893 | bot_fwd.j[0].min = -120; 1894 | bot_fwd.j[0].max = 180; 1895 | 1896 | bot_fwd.j[1].min = -30; 1897 | bot_fwd.j[1].max = 100; 1898 | 1899 | bot_fwd.j[2].min = -100; 1900 | bot_fwd.j[2].max = 0; 1901 | 1902 | bot_fwd.j[3].min = -15; 1903 | bot_fwd.j[3].max = 195; 1904 | 1905 | bot_fwd.j[4].min = -360; 1906 | bot_fwd.j[4].max = 360; 1907 | 1908 | bot_fwd.j[0].pos = 0; 1909 | bot_fwd.j[1].pos = 90; 1910 | bot_fwd.j[2].pos = -90; 1911 | bot_fwd.j[3].pos = 0; 1912 | bot_fwd.j[4].pos = 0; 1913 | 1914 | memcpy(&bot_inv, &bot_fwd, sizeof(bot_t)); 1915 | kins_inv(&bot_inv); 1916 | kins_fwd(&bot_inv); 1917 | kins_fwd(&bot_fwd); 1918 | 1919 | #ifdef HAVE_SDL 1920 | if (do_sdl) 1921 | { 1922 | #ifdef ENABLE_FPS_LIMIT 1923 | frames = 0; 1924 | ft = SDL_GetTicks(); 1925 | #endif 1926 | } 1927 | #endif 1928 | 1929 | #ifdef HAVE_HAL 1930 | if (do_hal) 1931 | { 1932 | hal_ready(hal_comp_id); 1933 | } 1934 | #endif 1935 | 1936 | #ifdef HAVE_TRAJGEN 1937 | tg_init(1000, 25000, 1000.0, 1, 3); 1938 | tg_echk(trajgen_switch_state(TRAJ_STATE_COORDINATED)); 1939 | while (!tg_tg.is_done) 1940 | tg_echk(trajgen_tick()); 1941 | tg_speed = 100; 1942 | tg_accel = 5000; 1943 | tg_blending = 15; 1944 | tg_line(1.6, 3.2, 0.0); 1945 | while (!tg_tg.is_done) 1946 | tg_echk(trajgen_tick()); 1947 | #endif 1948 | 1949 | while (!done) 1950 | { 1951 | #ifdef HAVE_MQTT 1952 | coord_t coord = bot2coord(&bot_inv); 1953 | bot_t bot_aux = bot_inv; 1954 | 1955 | if (mqtt_periodic_callback(&coord, rw_mode)) 1956 | { 1957 | // an update arrived from mqtt 1958 | 1959 | int try = 100; 1960 | do 1961 | { 1962 | // try to convert multiple times until result is good 1963 | coord2bot(&bot_aux, coord); 1964 | 1965 | bot_fwd = bot_aux; 1966 | bot_inv = bot_aux; 1967 | update_model(&bot_fwd, &bot_inv, 1, 1); 1968 | bot_aux = bot_inv; 1969 | } while (!coord_equal(coord, bot2coord(&bot_aux), EPSILON) && --try > 0); 1970 | } 1971 | #endif 1972 | 1973 | static double old_pos[5]; 1974 | 1975 | for (i = 0; i < 5; i++) 1976 | { 1977 | old_pos[i] = bot_fwd.j[i].pos; 1978 | } 1979 | 1980 | static double old_pos_cart_fwd[6]; 1981 | old_pos_cart_fwd[0] = bot_fwd.t[12]; 1982 | old_pos_cart_fwd[1] = bot_fwd.t[13]; 1983 | old_pos_cart_fwd[2] = bot_fwd.t[14]; 1984 | double y, r, p; 1985 | pmMatRpyConvert(bot_fwd.t, &y, &r, &p); 1986 | old_pos_cart_fwd[3] = r; 1987 | old_pos_cart_fwd[4] = p; 1988 | old_pos_cart_fwd[5] = y; 1989 | 1990 | static double old_pos_cart_inv[6]; 1991 | old_pos_cart_inv[0] = bot_inv.t[12]; 1992 | old_pos_cart_inv[1] = bot_inv.t[13]; 1993 | old_pos_cart_inv[2] = bot_inv.t[14]; 1994 | pmMatRpyConvert(bot_inv.t, &y, &r, &p); 1995 | old_pos_cart_inv[3] = r; 1996 | old_pos_cart_inv[4] = p; 1997 | old_pos_cart_inv[5] = y; 1998 | 1999 | #ifdef HAVE_SDL 2000 | if (do_sdl) 2001 | { 2002 | SDL_PollEvent(&ev); 2003 | 2004 | if (ev.type == SDL_QUIT || 2005 | (ev.type == SDL_KEYDOWN && ev.key.keysym.sym == SDLK_ESCAPE)) 2006 | done = 1; 2007 | 2008 | if (ev.type == SDL_KEYUP && ev.key.keysym.sym == SDLK_F11) 2009 | { 2010 | cur_bounds = toggle_fake_fullscreen(cur_bounds); 2011 | } 2012 | 2013 | if (ev.type == SDL_KEYUP && ev.key.keysym.sym == SDLK_TAB) 2014 | { 2015 | sdlk_tab_pressed = 0; 2016 | } 2017 | 2018 | double d = 0.05; 2019 | double cnt = 1.00; 2020 | 2021 | if (keys[SDL_SCANCODE_LCTRL] || keys[SDL_SCANCODE_RCTRL]) 2022 | { 2023 | d *= 2; 2024 | cnt *= 2; 2025 | } 2026 | 2027 | if (keys[SDL_SCANCODE_Q]) 2028 | { 2029 | jog_joint(&bot_fwd, 0, cnt); 2030 | } 2031 | if (keys[SDL_SCANCODE_W]) 2032 | { 2033 | jog_joint(&bot_fwd, 1, cnt); 2034 | } 2035 | if (keys[SDL_SCANCODE_E]) 2036 | { 2037 | jog_joint(&bot_fwd, 2, cnt); 2038 | } 2039 | if (keys[SDL_SCANCODE_R]) 2040 | { 2041 | jog_joint(&bot_fwd, 3, cnt); 2042 | } 2043 | if (keys[SDL_SCANCODE_T]) 2044 | { 2045 | jog_joint(&bot_fwd, 4, cnt); 2046 | } 2047 | 2048 | if (keys[SDL_SCANCODE_A]) 2049 | { 2050 | jog_joint(&bot_fwd, 0, -cnt); 2051 | } 2052 | if (keys[SDL_SCANCODE_S]) 2053 | { 2054 | jog_joint(&bot_fwd, 1, -cnt); 2055 | } 2056 | if (keys[SDL_SCANCODE_D]) 2057 | { 2058 | jog_joint(&bot_fwd, 2, -cnt); 2059 | } 2060 | if (keys[SDL_SCANCODE_F]) 2061 | { 2062 | jog_joint(&bot_fwd, 3, -cnt); 2063 | } 2064 | if (keys[SDL_SCANCODE_G]) 2065 | { 2066 | jog_joint(&bot_fwd, 4, -cnt); 2067 | } 2068 | 2069 | if (keys[SDL_SCANCODE_O]) 2070 | { 2071 | bot_fwd.grip = true; 2072 | bot_inv.grip = true; 2073 | } 2074 | if (keys[SDL_SCANCODE_L]) 2075 | { 2076 | bot_fwd.grip = false; 2077 | bot_inv.grip = false; 2078 | } 2079 | 2080 | if (!keys[SDL_SCANCODE_LSHIFT] && !keys[SDL_SCANCODE_RSHIFT]) 2081 | { 2082 | if (keys[SDL_SCANCODE_LEFT]) 2083 | { 2084 | move_tool(&bot_inv, 0, -d); 2085 | } 2086 | if (keys[SDL_SCANCODE_RIGHT]) 2087 | { 2088 | move_tool(&bot_inv, 0, d); 2089 | } 2090 | if (keys[SDL_SCANCODE_I]) 2091 | { 2092 | move_tool(&bot_inv, 1, d); 2093 | } 2094 | if (keys[SDL_SCANCODE_K]) 2095 | { 2096 | move_tool(&bot_inv, 1, -d); 2097 | } 2098 | if (keys[SDL_SCANCODE_UP]) 2099 | { 2100 | move_tool(&bot_inv, 2, d); 2101 | } 2102 | if (keys[SDL_SCANCODE_DOWN]) 2103 | { 2104 | move_tool(&bot_inv, 2, -d); 2105 | } 2106 | } 2107 | else 2108 | { 2109 | if (keys[SDL_SCANCODE_UP]) 2110 | { 2111 | rotate_tool(&bot_inv, -cnt, sin(deg2rad(bot_inv.j[0].pos)), 0, cos(deg2rad(bot_inv.j[0].pos))); 2112 | } 2113 | if (keys[SDL_SCANCODE_DOWN]) 2114 | { 2115 | rotate_tool(&bot_inv, cnt, sin(deg2rad(bot_inv.j[0].pos)), 0, cos(deg2rad(bot_inv.j[0].pos))); 2116 | } 2117 | if (keys[SDL_SCANCODE_LEFT]) 2118 | { 2119 | rotate_tool(&bot_inv, -cnt, 0, 1, 0); 2120 | } 2121 | if (keys[SDL_SCANCODE_RIGHT]) 2122 | { 2123 | rotate_tool(&bot_inv, cnt, 0, 1, 0); 2124 | } 2125 | } 2126 | 2127 | if (ev.type == SDL_KEYDOWN) 2128 | { 2129 | 2130 | switch (ev.key.keysym.sym) 2131 | { 2132 | case SDLK_TAB: 2133 | if (!sdlk_tab_pressed) 2134 | { 2135 | if (view_mode == 0) 2136 | { 2137 | view_mode = 1; 2138 | } 2139 | else 2140 | { 2141 | view_mode = 0; 2142 | } 2143 | sdlk_tab_pressed = 1; 2144 | } 2145 | break; 2146 | case SDLK_F1: 2147 | bot_fwd.j[0].pos = 0; 2148 | bot_fwd.j[1].pos = 45; 2149 | bot_fwd.j[2].pos = -45; 2150 | bot_fwd.j[3].pos = 0; 2151 | bot_fwd.j[4].pos = 0; 2152 | do_kins_fwd = 1; 2153 | break; 2154 | default: 2155 | break; 2156 | } 2157 | } 2158 | #ifdef HAVE_JOYSTICK 2159 | if (ev.type == SDL_JOYAXISMOTION) 2160 | { 2161 | #define JOY_SCALE (1.0 / (32768.0 * 2.0)) 2162 | if (ev.jaxis.which == 0) 2163 | { 2164 | // X-axis motion 2165 | if (ev.jaxis.axis == 0) 2166 | { 2167 | // Left of dead zone 2168 | if (ev.jaxis.value < -JOYSTICK_DEAD_ZONE) 2169 | { 2170 | move_tool(&bot_inv, 0, (double)(d * ev.jaxis.value * JOY_SCALE)); 2171 | } 2172 | // Right of dead zone 2173 | else if (ev.jaxis.value > JOYSTICK_DEAD_ZONE) 2174 | { 2175 | move_tool(&bot_inv, 0, (double)(d * ev.jaxis.value * JOY_SCALE)); 2176 | } 2177 | } // Y-axis motion 2178 | else if (ev.jaxis.axis == 1) 2179 | { 2180 | // Left of dead zone 2181 | if (ev.jaxis.value < -JOYSTICK_DEAD_ZONE || ev.jaxis.value > JOYSTICK_DEAD_ZONE) 2182 | { 2183 | move_tool(&bot_inv, 2, (double)(-d * ev.jaxis.value * JOY_SCALE)); 2184 | } 2185 | } // Z-axis motion 2186 | else if (ev.jaxis.axis == 3) 2187 | { 2188 | // Left of dead zone 2189 | if (ev.jaxis.value < -JOYSTICK_DEAD_ZONE || ev.jaxis.value > JOYSTICK_DEAD_ZONE) 2190 | { 2191 | move_tool(&bot_inv, 1, (double)(-d * ev.jaxis.value * JOY_SCALE)); 2192 | } 2193 | } 2194 | } 2195 | } 2196 | #endif 2197 | } 2198 | #endif 2199 | 2200 | #ifdef HAVE_SPACENAV 2201 | if (sn.fd) 2202 | { 2203 | spacenav_read(&sn); 2204 | 2205 | #define JOG_MIN 15 2206 | #define JOG_SPEED 0.0001 2207 | #define JOG_SPEED_ROT 0.008 2208 | if (abs(sn.pos[0]) > JOG_MIN) 2209 | { 2210 | move_tool(&bot_inv, 0, sn.pos[0] * JOG_SPEED); 2211 | } 2212 | 2213 | if (abs(sn.pos[1]) > JOG_MIN) 2214 | { 2215 | move_tool(&bot_inv, 2, -sn.pos[1] * JOG_SPEED); 2216 | } 2217 | 2218 | if (abs(sn.pos[2]) > JOG_MIN) 2219 | { 2220 | move_tool(&bot_inv, 1, -sn.pos[2] * JOG_SPEED); 2221 | } 2222 | 2223 | if (abs(sn.pos[5]) > JOG_MIN) 2224 | { 2225 | rotate_tool(&bot_inv, -sn.pos[5] * JOG_SPEED_ROT, 0, 1, 0); 2226 | } 2227 | 2228 | #ifdef SPACENAV_JOG_A 2229 | if (abs(sn.pos[4]) > JOG_MIN) 2230 | { 2231 | rotate_tool(&bot_inv, -sn.pos[4] * JOG_SPEED_ROT, 2232 | sin(deg2rad(bot_inv.j[0].pos)), 0, cos(deg2rad(bot_inv.j[0].pos))); 2233 | } 2234 | #endif 2235 | if (sn.key[0]) 2236 | { 2237 | done = 1; 2238 | } 2239 | } 2240 | #endif 2241 | 2242 | #ifdef HAVE_TRAJGEN 2243 | if (do_trajgen_test) 2244 | { 2245 | trajgen_tick(); 2246 | 2247 | double oldx = bot_inv.t[12]; 2248 | double oldy = bot_inv.t[13]; 2249 | double oldz = bot_inv.t[14]; 2250 | 2251 | double dx = tg_tg.joints[0].position - oldx; 2252 | double dy = tg_tg.joints[1].position - oldy; 2253 | double dz = tg_tg.joints[2].position - oldz; 2254 | 2255 | move_tool(&bot_inv, 0, dx); 2256 | move_tool(&bot_inv, 1, dy); 2257 | move_tool(&bot_inv, 2, dz); 2258 | 2259 | if (trajgen_num_queued() < 2) 2260 | { 2261 | tg_line(2.8 + randpos(0.5), 1.0 + randpos(1), randpos(2)); 2262 | } 2263 | } 2264 | #endif 2265 | 2266 | update_model(&bot_fwd, &bot_inv, do_kins_fwd, do_kins_inv); 2267 | 2268 | if (do_kins_inv || do_kins_fwd) 2269 | { 2270 | memcpy(&bot_inv, &bot_fwd, sizeof(bot_t)); 2271 | kins_inv(&bot_inv); 2272 | kins_fwd(&bot_inv); 2273 | kins_fwd(&bot_fwd); 2274 | } 2275 | 2276 | do_kins_fwd = 0; 2277 | do_kins_inv = 0; 2278 | 2279 | // update joint velocity values 2280 | for (i = 0; i < 5; i++) 2281 | { 2282 | bot_fwd.j[i].vel = bot_fwd.j[i].pos - old_pos[i]; 2283 | bot_inv.j[i].vel = bot_inv.j[i].pos - old_pos[i]; 2284 | } 2285 | // update cartesian velocity values 2286 | bot_fwd.cart[0].vel = bot_fwd.t[12] - old_pos_cart_fwd[0]; 2287 | bot_fwd.cart[1].vel = bot_fwd.t[13] - old_pos_cart_fwd[1]; 2288 | bot_fwd.cart[2].vel = bot_fwd.t[14] - old_pos_cart_fwd[2]; 2289 | pmMatRpyConvert(bot_fwd.t, &y, &r, &p); 2290 | bot_fwd.cart[3].vel = r - old_pos_cart_fwd[3]; 2291 | bot_fwd.cart[4].vel = p - old_pos_cart_fwd[4]; 2292 | bot_fwd.cart[5].vel = y - old_pos_cart_fwd[5]; 2293 | 2294 | bot_inv.cart[0].vel = bot_inv.t[12] - old_pos_cart_inv[0]; 2295 | bot_inv.cart[1].vel = bot_inv.t[13] - old_pos_cart_inv[1]; 2296 | bot_inv.cart[2].vel = bot_inv.t[14] - old_pos_cart_inv[2]; 2297 | pmMatRpyConvert(bot_inv.t, &y, &r, &p); 2298 | bot_inv.cart[3].vel = r - old_pos_cart_inv[3]; 2299 | bot_inv.cart[4].vel = p - old_pos_cart_inv[4]; 2300 | bot_inv.cart[5].vel = y - old_pos_cart_inv[5]; 2301 | 2302 | #ifdef HAVE_SDL 2303 | if (do_sdl) 2304 | { 2305 | display(&bot_fwd, &bot_inv); 2306 | SDL_RenderPresent(sdl_renderer); 2307 | // screenshot(0, 0, "screenshot.png"); 2308 | #ifdef HAVE_AUDIO 2309 | if (do_audio) 2310 | { 2311 | for (i = 0; i < 5; i++) 2312 | { 2313 | MIDDLE_C[i] = fabs(bot_fwd.j[i].vel * 261.626); 2314 | } 2315 | } 2316 | #endif // HAVE_AUDIO 2317 | } 2318 | #endif // HAVE_SDL 2319 | 2320 | #ifdef HAVE_NCURSES 2321 | if (do_curses) 2322 | { 2323 | int key = getch(); 2324 | 2325 | if (key == 'q') 2326 | { 2327 | done = 1; 2328 | } 2329 | 2330 | move(8, 1); 2331 | printw("x: %7.2f", bot_inv.t[12]); 2332 | move(9, 1); 2333 | printw("y: %7.2f", bot_inv.t[13]); 2334 | move(10, 1); 2335 | printw("z: %7.2f", bot_inv.t[14]); 2336 | 2337 | double r, p, y; 2338 | 2339 | pmMatRpyConvert(bot_inv.t, &r, &p, &y); 2340 | 2341 | move(12, 1); 2342 | printw("a: %7.2f", rad2deg(r)); 2343 | move(13, 1); 2344 | printw("b: %7.2f", rad2deg(p)); 2345 | move(14, 1); 2346 | printw("c: %7.2f", rad2deg(y)); 2347 | 2348 | int i; 2349 | for (i = 0; i < 5; i++) 2350 | { 2351 | move(18, 1 + i * 11); 2352 | printw("%7.2f", bot_inv.j[i].pos); 2353 | } 2354 | 2355 | move(20, 1); 2356 | if (strlen(bot_inv.msg)) 2357 | { 2358 | printw("%s", bot_inv.msg); 2359 | } 2360 | else 2361 | { 2362 | printw(" "); 2363 | } 2364 | 2365 | touchwin(stdscr); 2366 | refresh(); 2367 | } 2368 | #endif 2369 | 2370 | #ifdef HAVE_HAL 2371 | if (do_hal) 2372 | { 2373 | for (i = 0; i < 5; i++) 2374 | { 2375 | *(hal_pos_data->axis[i]) = bot_fwd.j[i].pos; 2376 | } 2377 | } 2378 | #endif 2379 | 2380 | #ifdef HAVE_SDL 2381 | if (do_sdl) 2382 | { 2383 | #ifdef ENABLE_FPS_LIMIT 2384 | while (frames * 1000.0 / ((float)(SDL_GetTicks() - ft + 1)) > (float)(DEFAULT_FPS)) 2385 | { 2386 | SDL_Delay(10); 2387 | } 2388 | frames++; 2389 | #endif 2390 | } 2391 | #endif 2392 | } // while ! done 2393 | 2394 | #ifdef HAVE_SDL 2395 | if (do_sdl) 2396 | { 2397 | SDL_RenderClear(sdl_renderer); 2398 | 2399 | if (sdl_font) 2400 | { 2401 | TTF_CloseFont(sdl_font); 2402 | } 2403 | 2404 | #ifdef HAVE_JOYSTICK 2405 | if (joy) 2406 | { 2407 | SDL_JoystickClose(joy); 2408 | joy = NULL; 2409 | } 2410 | #endif 2411 | 2412 | TTF_Quit(); 2413 | 2414 | SDL_DestroyRenderer(sdl_renderer); 2415 | SDL_GL_DeleteContext(sdl_glcontext); 2416 | SDL_DestroyWindow(sdl_window); 2417 | 2418 | #ifdef HAVE_AUDIO 2419 | if (do_audio) 2420 | { 2421 | close_audio(); 2422 | } 2423 | #endif 2424 | 2425 | SDL_Quit(); 2426 | } 2427 | #endif 2428 | 2429 | #ifdef HAVE_SPACENAV 2430 | spacenav_close(&sn); 2431 | #endif 2432 | 2433 | #ifdef HAVE_NCURSES 2434 | delwin(menubar); 2435 | delwin(messagebar); 2436 | endwin(); 2437 | #endif 2438 | 2439 | #ifdef HAVE_HAL 2440 | fail1: 2441 | if (do_hal) 2442 | { 2443 | hal_exit(hal_comp_id); 2444 | } 2445 | fail0: 2446 | #endif 2447 | 2448 | #ifdef HAVE_MQTT 2449 | if (do_mqtt) 2450 | { 2451 | mqtt_handler_close(); 2452 | } 2453 | #endif 2454 | 2455 | return 0; 2456 | } 2457 | --------------------------------------------------------------------------------