| @@ -0,0 +1,16 @@ | ||
| 1. create a new build-directory | ||
| (it is always better not to build in the | ||
| src) | ||
| mkdir <builddir> | ||
| 2. cd <builddir> | ||
| 3. ccmake .. | ||
| 4. adapt CMAKE_INSTALL_PREFIX to desired installation directory | ||
| 5. If you want to build the tests: enable BUILD_TESTS | ||
| 6. If you want to build the python bindings: enable PYTHON_BINDING, you need sip 4.5 for this to work | ||
| 7 Generate (press g) | ||
| 8. make | ||
| 9. To execute the tests: make check | ||
| 10. To create to API-documentation: make docs (in <builddir>) | ||
| 11. To install: make install [DESTDIR=...] | ||
| 12.To uninstall: make uninstall | ||
|
|
| @@ -0,0 +1,17 @@ | ||
| # - Config file for the orocos-kdl package | ||
| # It defines the following variables | ||
| # orocos_kdl_INCLUDE_DIRS - include directories for Orocos KDL | ||
| # orocos_kdl_LIBRARIES - libraries to link against for Orocos KDL | ||
| # orocos_kdl_PKGCONFIG_DIR - directory containing the .pc pkgconfig files | ||
|
|
||
| # Compute paths | ||
| set(orocos_kdl_INCLUDE_DIRS "${CMAKE_CURRENT_LIST_DIR}/../../../include;@Boost_INCLUDE_DIRS@;@Eigen_INCLUDE_DIR@") | ||
|
|
||
| if(NOT TARGET orocos-kdl) | ||
| include("${CMAKE_CURRENT_LIST_DIR}/OrocosKDLTargets.cmake") | ||
| endif() | ||
|
|
||
| set(orocos_kdl_LIBRARIES orocos-kdl) | ||
|
|
||
| # where the .pc pkgconfig files are installed | ||
| set(orocos_kdl_PKGCONFIG_DIR "${CMAKE_CURRENT_LIST_DIR}/../../../lib/pkgconfig") |
| @@ -0,0 +1,11 @@ | ||
| set(PACKAGE_VERSION "@KDL_VERSION@") | ||
|
|
||
| # Check whether the requested PACKAGE_FIND_VERSION is compatible | ||
| if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") | ||
| set(PACKAGE_VERSION_COMPATIBLE FALSE) | ||
| else() | ||
| set(PACKAGE_VERSION_COMPATIBLE TRUE) | ||
| if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") | ||
| set(PACKAGE_VERSION_EXACT TRUE) | ||
| endif() | ||
| endif() |
| @@ -0,0 +1,8 @@ | ||
| Kinematics and Dynamics Library: | ||
|
|
||
| Orocos project to supply RealTime usable kinematics and dynamics code, | ||
| it contains code for rigid body kinematics calculations and | ||
| representations for kinematic structures and their inverse and forward | ||
| kinematic solvers. | ||
|
|
||
|
|
| @@ -0,0 +1,22 @@ | ||
| IF(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") | ||
| MESSAGE(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"") | ||
| ENDIF(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") | ||
|
|
||
| FILE(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) | ||
| STRING(REGEX REPLACE "\n" ";" files "${files}") | ||
| FOREACH(file ${files}) | ||
| MESSAGE(STATUS "Uninstalling \"${file}\"") | ||
| IF(EXISTS "${file}") | ||
| EXEC_PROGRAM( | ||
| "@CMAKE_COMMAND@" ARGS "-E remove \"${file}\"" | ||
| OUTPUT_VARIABLE rm_out | ||
| RETURN_VALUE rm_retval | ||
| ) | ||
| IF("${rm_retval}" STREQUAL 0) | ||
| ELSE("${rm_retval}" STREQUAL 0) | ||
| MESSAGE(FATAL_ERROR "Problem when removing \"${file}\"") | ||
| ENDIF("${rm_retval}" STREQUAL 0) | ||
| ELSE(EXISTS "${file}") | ||
| MESSAGE(STATUS "File \"${file}\" does not exist.") | ||
| ENDIF(EXISTS "${file}") | ||
| ENDFOREACH(file) |
| @@ -0,0 +1,47 @@ | ||
| # - Macro to check whether the STL containers support incomplete types | ||
| # The issue at stake is discussed here in depth: | ||
| # http://www.drdobbs.com/the-standard-librarian-containers-of-inc/184403814 | ||
| # | ||
| # Empirically libstdc++ and MSVC++ support containers of incomplete types | ||
| # whereas libc++ does not. | ||
| # | ||
| # The result is returned in HAVE_STL_CONTAINER_INCOMPLETE_TYPES | ||
| # | ||
| # Copyright 2014 Brian Jensen <Jensen dot J dot Brian at gmail dot com> | ||
| # Author: Brian Jensen <Jensen dot J dot Brian at gmail dot com> | ||
| # | ||
|
|
||
| macro(CHECK_STL_CONTAINERS) | ||
| INCLUDE(CheckCXXSourceCompiles) | ||
| SET(CMAKE_REQUIRED_FLAGS) | ||
| CHECK_CXX_SOURCE_COMPILES(" | ||
| #include <string> | ||
| #include <map> | ||
| #include <vector> | ||
| class TreeElement; | ||
| typedef std::map<std::string, TreeElement> SegmentMap; | ||
| class TreeElement | ||
| { | ||
| TreeElement(const std::string& name): number(0) {} | ||
| public: | ||
| int number; | ||
| SegmentMap::const_iterator parent; | ||
| std::vector<SegmentMap::const_iterator> children; | ||
| static TreeElement Root(std::string& name) | ||
| { | ||
| return TreeElement(name); | ||
| } | ||
| }; | ||
| int main() | ||
| { | ||
| return 0; | ||
| } | ||
| " | ||
| HAVE_STL_CONTAINER_INCOMPLETE_TYPES) | ||
|
|
||
| endmacro(CHECK_STL_CONTAINERS) |
| @@ -0,0 +1,37 @@ | ||
| # - Macro to provide an option dependent on other options. | ||
| # This macro presents an option to the user only if a set of other | ||
| # conditions are true. When the option is not presented a default | ||
| # value is used, but any value set by the user is preserved for when | ||
| # the option is presented again. | ||
| # Example invocation: | ||
| # DEPENDENT_OPTION(USE_FOO "Use Foo" ON | ||
| # "USE_BAR;NOT USE_ZOT" OFF) | ||
| # If USE_BAR is true and USE_ZOT is false, this provides an option called | ||
| # USE_FOO that defaults to ON. Otherwise, it sets USE_FOO to OFF. If | ||
| # the status of USE_BAR or USE_ZOT ever changes, any value for the | ||
| # USE_FOO option is saved so that when the option is re-enabled it | ||
| # retains its old value. | ||
| MACRO(DEPENDENT_OPTION option doc default depends force) | ||
| IF(${option}_ISSET MATCHES "^${option}_ISSET$") | ||
| SET(${option}_AVAILABLE 1) | ||
| FOREACH(d ${depends}) | ||
| STRING(REGEX REPLACE " +" ";" CMAKE_DEPENDENT_OPTION_DEP "${d}") | ||
| IF(${CMAKE_DEPENDENT_OPTION_DEP}) | ||
| ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) | ||
| SET(${option}_AVAILABLE 0) | ||
| ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) | ||
| ENDFOREACH(d) | ||
| IF(${option}_AVAILABLE) | ||
| OPTION(${option} "${doc}" "${default}") | ||
| SET(${option} "${${option}}" CACHE BOOL "${doc}" FORCE) | ||
| ELSE(${option}_AVAILABLE) | ||
| IF(${option} MATCHES "^${option}$") | ||
| ELSE(${option} MATCHES "^${option}$") | ||
| SET(${option} "${${option}}" CACHE INTERNAL "${doc}") | ||
| ENDIF(${option} MATCHES "^${option}$") | ||
| SET(${option} ${force}) | ||
| ENDIF(${option}_AVAILABLE) | ||
| ELSE(${option}_ISSET MATCHES "^${option}_ISSET$") | ||
| SET(${option} "${${option}_ISSET}") | ||
| ENDIF(${option}_ISSET MATCHES "^${option}_ISSET$") | ||
| ENDMACRO(DEPENDENT_OPTION) |
| @@ -0,0 +1,89 @@ | ||
| # - Try to find Eigen3 lib | ||
| # | ||
| # This module supports requiring a minimum version, e.g. you can do | ||
| # find_package(Eigen3 3.1.2) | ||
| # to require version 3.1.2 or newer of Eigen3. | ||
| # | ||
| # Once done this will define | ||
| # | ||
| # EIGEN3_FOUND - system has eigen lib with correct version | ||
| # EIGEN3_INCLUDE_DIR - the eigen include directory | ||
| # EIGEN3_VERSION - eigen version | ||
| # | ||
| # This module reads hints about search locations from | ||
| # the following environment variables: | ||
| # | ||
| # EIGEN3_ROOT | ||
| # EIGEN3_ROOT_DIR | ||
|
|
||
| # Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org> | ||
| # Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr> | ||
| # Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com> | ||
| # Redistribution and use is allowed according to the terms of the 2-clause BSD license. | ||
|
|
||
| if(NOT Eigen3_FIND_VERSION) | ||
| if(NOT Eigen3_FIND_VERSION_MAJOR) | ||
| set(Eigen3_FIND_VERSION_MAJOR 2) | ||
| endif(NOT Eigen3_FIND_VERSION_MAJOR) | ||
| if(NOT Eigen3_FIND_VERSION_MINOR) | ||
| set(Eigen3_FIND_VERSION_MINOR 91) | ||
| endif(NOT Eigen3_FIND_VERSION_MINOR) | ||
| if(NOT Eigen3_FIND_VERSION_PATCH) | ||
| set(Eigen3_FIND_VERSION_PATCH 0) | ||
| endif(NOT Eigen3_FIND_VERSION_PATCH) | ||
|
|
||
| set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") | ||
| endif(NOT Eigen3_FIND_VERSION) | ||
|
|
||
| macro(_eigen3_check_version) | ||
| file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) | ||
|
|
||
| string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") | ||
| set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") | ||
| string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") | ||
| set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") | ||
| string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") | ||
| set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") | ||
|
|
||
| set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) | ||
| if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) | ||
| set(EIGEN3_VERSION_OK FALSE) | ||
| else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) | ||
| set(EIGEN3_VERSION_OK TRUE) | ||
| endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) | ||
|
|
||
| if(NOT EIGEN3_VERSION_OK) | ||
|
|
||
| message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " | ||
| "but at least version ${Eigen3_FIND_VERSION} is required") | ||
| endif(NOT EIGEN3_VERSION_OK) | ||
| endmacro(_eigen3_check_version) | ||
|
|
||
| if (EIGEN3_INCLUDE_DIR) | ||
|
|
||
| # in cache already | ||
| _eigen3_check_version() | ||
| set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) | ||
|
|
||
| else (EIGEN3_INCLUDE_DIR) | ||
|
|
||
| find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library | ||
| HINTS | ||
| ENV EIGEN3_ROOT | ||
| ENV EIGEN3_ROOT_DIR | ||
| PATHS | ||
| ${CMAKE_INSTALL_PREFIX}/include | ||
| ${KDE4_INCLUDE_DIR} | ||
| PATH_SUFFIXES eigen3 eigen | ||
| ) | ||
|
|
||
| if(EIGEN3_INCLUDE_DIR) | ||
| _eigen3_check_version() | ||
| endif(EIGEN3_INCLUDE_DIR) | ||
|
|
||
| include(FindPackageHandleStandardArgs) | ||
| find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) | ||
|
|
||
| mark_as_advanced(EIGEN3_INCLUDE_DIR) | ||
|
|
||
| endif(EIGEN3_INCLUDE_DIR) |
| @@ -0,0 +1,133 @@ | ||
| ## FindPkgConfig.cmake | ||
| ## by Albert Strasheim <http://students . ee . sun . ac . za/~albert/> | ||
| ## and Alex Brooks (a.brooks at acfr . usyd . edu . au) | ||
| ## | ||
| ## This module finds packages using pkg-config, which retrieves | ||
| ## information about packages from special metadata files. | ||
| ## | ||
| ## See http://www . freedesktop . org/Software/pkgconfig/ | ||
| ## | ||
| ## ------------------------------------------------------------------- | ||
| ## | ||
| ## Usage: | ||
| ## | ||
| ## INCLUDE( ${CMAKE_ROOT}/Modules/FindPkgConfig.cmake) | ||
| ## | ||
| ## IF ( CMAKE_PKGCONFIG_EXECUTABLE ) | ||
| ## | ||
| ## # Find all the librtk stuff with pkg-config | ||
| ## PKGCONFIG( "librtk >= 2.0" HAVE_RTK RTK_INCLUDE_DIRS RTK_DEFINES RTK_LINK_DIRS RTK_LIBS ) | ||
| ## | ||
| ## ELSE ( CMAKE_PKGCONFIG_EXECUTABLE ) | ||
| ## | ||
| ## # Can't find pkg-config -- have to find librtk somehow else | ||
| ## | ||
| ## ENDIF ( CMAKE_PKGCONFIG_EXECUTABLE ) | ||
| ## | ||
| ## | ||
| ## Notes: | ||
| ## | ||
| ## You can set the PKG_CONFIG_PATH environment variable to tell | ||
| ## pkg-config where to search for .pc files. See pkg-config(1) for | ||
| ## more information. | ||
| ## | ||
| # | ||
| # FIXME: IF(WIN32) pkg-config --msvc-syntax ENDIF(WIN32) ??? | ||
| # | ||
| # FIXME: Parsing of pkg-config output is specific to gnu-style flags | ||
| # | ||
|
|
||
| FIND_PROGRAM(CMAKE_PKGCONFIG_EXECUTABLE pkg-config) | ||
| MARK_AS_ADVANCED(CMAKE_PKGCONFIG_EXECUTABLE) | ||
|
|
||
| ######################################## | ||
|
|
||
| MACRO(PKGCONFIG_PARSE_FLAGS FLAGS INCLUDES DEFINES) | ||
|
|
||
| #MESSAGE("DEBUG: FLAGS: ${FLAGS}") | ||
|
|
||
| STRING(REGEX MATCHALL " -I[^ ]*" ${INCLUDES} "${FLAGS}") | ||
| STRING(REGEX REPLACE " -I" "" ${INCLUDES} "${${INCLUDES}}") | ||
| #MESSAGE("DEBUG: INCLUDES: ${${INCLUDES}}") | ||
|
|
||
| STRING(REGEX REPLACE " -I[^ ]*" "" ${DEFINES} "${FLAGS}") | ||
| #MESSAGE("DEBUG: DEFINES: ${${DEFINES}}") | ||
|
|
||
| ENDMACRO(PKGCONFIG_PARSE_FLAGS) | ||
|
|
||
| ######################################## | ||
|
|
||
| MACRO(PKGCONFIG_PARSE_LIBS LIBS LINKDIRS LINKLIBS) | ||
|
|
||
| #MESSAGE("DEBUG: LIBS: ${LIBS}") | ||
|
|
||
| STRING(REGEX MATCHALL " -L[^ ]*" ${LINKDIRS} "${LIBS}") | ||
| STRING(REGEX REPLACE " -L" "" ${LINKDIRS} "${${LINKDIRS}}") | ||
| #MESSAGE("DEBUG: LINKDIRS: ${${LINKDIRS}}") | ||
|
|
||
| STRING(REGEX MATCHALL " -l[^ ]*" ${LINKLIBS} "${LIBS}") | ||
| STRING(REGEX REPLACE " -l" "" ${LINKLIBS} "${${LINKLIBS}}") | ||
| #MESSAGE("DEBUG: LINKLIBS: ${${LINKLIBS}}") | ||
|
|
||
| ENDMACRO(PKGCONFIG_PARSE_LIBS) | ||
|
|
||
| ######################################## | ||
|
|
||
| MACRO(PKGCONFIG LIBRARY FOUND INCLUDE_DIRS DEFINES LINKDIRS LINKLIBS) | ||
|
|
||
| SET(${FOUND} 0) | ||
|
|
||
| # alexm: why print it twice? once here, and once when it's found/not found | ||
| # MESSAGE("-- Looking for ${LIBRARY}") | ||
|
|
||
| IF(CMAKE_PKGCONFIG_EXECUTABLE) | ||
| # MESSAGE("DEBUG: pkg-config executable found") | ||
|
|
||
| EXEC_PROGRAM(${CMAKE_PKGCONFIG_EXECUTABLE} | ||
| ARGS "'${LIBRARY}'" | ||
| OUTPUT_VARIABLE PKGCONFIG_OUTPUT | ||
| RETURN_VALUE PKGCONFIG_RETURN) | ||
|
|
||
| IF(NOT PKGCONFIG_RETURN) | ||
|
|
||
| # set C_FLAGS and CXX_FLAGS | ||
| EXEC_PROGRAM(${CMAKE_PKGCONFIG_EXECUTABLE} | ||
| ARGS "--cflags '${LIBRARY}'" | ||
| OUTPUT_VARIABLE CMAKE_PKGCONFIG_C_FLAGS) | ||
|
|
||
| #SET(CMAKE_PKGCONFIG_CXX_FLAGS "${CMAKE_PKGCONFIG_C_FLAGS}") | ||
| PKGCONFIG_PARSE_FLAGS(" ${CMAKE_PKGCONFIG_C_FLAGS}" ${INCLUDE_DIRS} ${DEFINES} ) | ||
|
|
||
| # set LIBRARIES | ||
| EXEC_PROGRAM(${CMAKE_PKGCONFIG_EXECUTABLE} | ||
| ARGS "--libs '${LIBRARY}'" | ||
| OUTPUT_VARIABLE CMAKE_PKGCONFIG_LIBRARIES) | ||
| PKGCONFIG_PARSE_LIBS (" ${CMAKE_PKGCONFIG_LIBRARIES}" ${LINKDIRS} ${LINKLIBS} ) | ||
|
|
||
| SET(${FOUND} 1) | ||
| MESSAGE("-- Looking for ${LIBRARY} -- found") | ||
|
|
||
| ELSE(NOT PKGCONFIG_RETURN) | ||
| MESSAGE("-- Looking for ${LIBRARY} -- not found") | ||
|
|
||
| SET(CMAKE_PKGCONFIG_C_FLAGS "") | ||
| SET(CMAKE_PKGCONFIG_CXX_FLAGS "") | ||
| SET(CMAKE_PKGCONFIG_LIBRARIES "") | ||
| SET(${INCLUDE_DIRS} "") | ||
| SET(${DEFINES} "") | ||
| SET(${LINKDIRS} "") | ||
| SET(${LINKLIBS} "") | ||
|
|
||
| ENDIF(NOT PKGCONFIG_RETURN) | ||
|
|
||
| ELSE(CMAKE_PKGCONFIG_EXECUTABLE) | ||
| MESSAGE("-- pkg-config executable NOT FOUND") | ||
| ENDIF(CMAKE_PKGCONFIG_EXECUTABLE) | ||
|
|
||
| #MESSAGE("Have ${LIBRARY} : ${${FOUND}}") | ||
| #MESSAGE("${LIBRARY} include dirs: ${${INCLUDE_DIRS}}") | ||
| #MESSAGE("${LIBRARY} defines : ${${DEFINES}}") | ||
| #MESSAGE("${LIBRARY} link dirs : ${${LINKDIRS}}") | ||
| #MESSAGE("${LIBRARY} link libs : ${${LINKLIBS}}") | ||
|
|
||
| ENDMACRO(PKGCONFIG) |
| @@ -0,0 +1,6 @@ | ||
| The Debian Package kdl | ||
| ---------------------------- | ||
|
|
||
| Comments regarding the Package | ||
|
|
||
| -- Leopold Palomo Avellaneda <lepalom@wol.es> Wed, 24 Oct 2007 13:20:52 +0200 |
| @@ -0,0 +1,6 @@ | ||
| kdl for Debian | ||
| -------------- | ||
|
|
||
| <possible notes regarding this package - if none, delete this file> | ||
|
|
||
| -- Leopold Palomo Avellaneda <lepalom@wol.es> Wed, 24 Oct 2007 13:20:52 +0200 |
| @@ -0,0 +1,11 @@ | ||
| orocos-kdl (1.0.0) UNRELEASED; urgency=low | ||
|
|
||
| * First major number release | ||
|
|
||
| -- Ruben Smits <ruben.smits@mech.kuleuven.be> Mon, 29 Sep 2008 12:23:37 +0200 | ||
|
|
||
| orocos-kdl (0.99.0) UNRELEASED; urgency=low | ||
|
|
||
| * Initial release. | ||
|
|
||
| -- Ruben Smits <ruben.smits@mech.kuleuven.be> Wed, 07 May 2008 13:49:51 +0200 |
| @@ -0,0 +1 @@ | ||
| 5 |
| @@ -0,0 +1,39 @@ | ||
| Source: orocos-kdl | ||
| Priority: extra | ||
| Maintainer: Ruben Smits <ruben.smits@mech.kuleuven.be> | ||
| Build-Depends: debhelper (>= 5), cmake (>=2.6.0), pkg-config, python-all-dev(>=2.3.5-11), python-sip4-dev (>=4.4.5), python-sip4, python-support, sip4, libeigen2-dev | ||
| Standards-Version: 3.7.2 | ||
| Section: libs | ||
|
|
||
| Package: liborocos-kdl-dev | ||
| Section: libdevel | ||
| Architecture: any | ||
| Depends: liborocos-kdl (= ${Source-Version}) | ||
| Description: Kinematics and Dynamics Library development files | ||
| Orocos project to supply RealTime usable kinematics and dynamics code, | ||
| it contains code for rigid body kinematics calculations and | ||
| representations for kinematic structures and their inverse and forward | ||
| kinematic solvers. | ||
|
|
||
| Package: liborocos-kdl | ||
| Section: libs | ||
| Architecture: any | ||
| Depends: ${shlibs:Depends} | ||
| Description: Kinematics and Dynamics Library runtime | ||
| Orocos project to supply RealTime usable kinematics and dynamics code, | ||
| it contains code for rigid body kinematics calculations and | ||
| representations for kinematic structures and their inverse and forward | ||
| kinematic solvers. | ||
|
|
||
| Package: python-orocos-kdl | ||
| Section: libs | ||
| Architecture: any | ||
| Depends: ${python:Depends}, python, liborocos-kdl | ||
| XS-Python-Version: current | ||
| XB-Python-Version:${python:Versions} | ||
| Description: Kinematics and Dynamics Library python binding | ||
| Orocos project to supply RealTime usable kinematics and dynamics code, | ||
| it contains code for rigid body kinematics calculations and | ||
| representations for kinematic structures and their inverse and forward | ||
| kinematic solvers. | ||
|
|
| @@ -0,0 +1,26 @@ | ||
| This is kdl, written and maintained by the orocos development team <orocos-dev@lists.mech.kuleuven.be> | ||
| on Wed, 24 Oct 2007 13:20:52 +0200. | ||
|
|
||
| The original source can always be found at: | ||
| http://www.orocos.org/kdl | ||
|
|
||
| Copyright Holder: Orocos Dev Team | ||
|
|
||
| License: | ||
|
|
||
| This program is free software; you can redistribute it and/or modify | ||
| it under the terms of the GNU General Public License as published by | ||
| the Free Software Foundation; either version 2 of the License, or | ||
| (at your option) any later version. | ||
|
|
||
| This program is distributed in the hope that it will be useful, | ||
| but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
| GNU General Public License for more details. | ||
|
|
||
| You should have received a copy of the GNU General Public License | ||
| along with this package; if not, write to the Free Software | ||
| Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA | ||
|
|
||
| On Debian systems, the complete text of the GNU General | ||
| Public License can be found in `/usr/share/common-licenses/GPL'. |
| @@ -0,0 +1,2 @@ | ||
| CMakeLists.txt | ||
| README |
| @@ -0,0 +1,22 @@ | ||
| Document: kdl | ||
| Title: Debian kdl Manual | ||
| Author: <insert document author here> | ||
| Abstract: This manual describes what kdl is | ||
| and how it can be used to | ||
| manage online manuals on Debian systems. | ||
| Section: unknown | ||
|
|
||
| Format: debiandoc-sgml | ||
| Files: /usr/share/doc/kdl/kdl.sgml.gz | ||
|
|
||
| Format: postscript | ||
| Files: /usr/share/doc/kdl/kdl.ps.gz | ||
|
|
||
| Format: text | ||
| Files: /usr/share/doc/kdl/kdl.text.gz | ||
|
|
||
| Format: HTML | ||
| Index: /usr/share/doc/kdl/html/index.html | ||
| Files: /usr/share/doc/kdl/html/*.html | ||
| @@ -0,0 +1,6 @@ | ||
| usr/include/kdl/*.hpp | ||
| usr/include/kdl/*.inl | ||
| usr/include/kdl/utilities/*.hpp | ||
| usr/include/kdl/utilities/*.h | ||
| usr/lib/liborocos-kdl.so | ||
| usr/lib/pkgconfig/orocos-kdl.pc |
| @@ -0,0 +1 @@ | ||
| usr/lib/liborocos-kdl.so.* |
| @@ -0,0 +1,59 @@ | ||
| .\" Hey, EMACS: -*- nroff -*- | ||
| .\" First parameter, NAME, should be all caps | ||
| .\" Second parameter, SECTION, should be 1-8, maybe w/ subsection | ||
| .\" other parameters are allowed: see man(7), man(1) | ||
| .TH KDL SECTION "octubre 24, 2007" | ||
| .\" Please adjust this date whenever revising the manpage. | ||
| .\" | ||
| .\" Some roff macros, for reference: | ||
| .\" .nh disable hyphenation | ||
| .\" .hy enable hyphenation | ||
| .\" .ad l left justify | ||
| .\" .ad b justify to both left and right margins | ||
| .\" .nf disable filling | ||
| .\" .fi enable filling | ||
| .\" .br insert line break | ||
| .\" .sp <n> insert n+1 empty lines | ||
| .\" for manpage-specific macros, see man(7) | ||
| .SH NAME | ||
| kdl \- program to do something | ||
| .SH SYNOPSIS | ||
| .B kdl | ||
| .RI [ options ] " files" ... | ||
| .br | ||
| .B bar | ||
| .RI [ options ] " files" ... | ||
| .SH DESCRIPTION | ||
| This manual page documents briefly the | ||
| .B kdl | ||
| and | ||
| .B bar | ||
| commands. | ||
| .PP | ||
| .\" TeX users may be more comfortable with the \fB<whatever>\fP and | ||
| .\" \fI<whatever>\fP escape sequences to invode bold face and italics, | ||
| .\" respectively. | ||
| \fBkdl\fP is a program that... | ||
| .SH OPTIONS | ||
| These programs follow the usual GNU command line syntax, with long | ||
| options starting with two dashes (`-'). | ||
| A summary of options is included below. | ||
| For a complete description, see the Info files. | ||
| .TP | ||
| .B \-h, \-\-help | ||
| Show summary of options. | ||
| .TP | ||
| .B \-v, \-\-version | ||
| Show version of program. | ||
| .SH SEE ALSO | ||
| .BR bar (1), | ||
| .BR baz (1). | ||
| .br | ||
| The programs are documented fully by | ||
| .IR "The Rise and Fall of a Fooish Bar" , | ||
| available via the Info system. | ||
| .SH AUTHOR | ||
| kdl was written by <upstream author>. | ||
| .PP | ||
| This manual page was written by Leopold Palomo Avellaneda <lepalom@wol.es>, | ||
| for the Debian project (but may be used by others). |
| @@ -0,0 +1 @@ | ||
| usr/lib/python/site-packages/PyKDL.so |
| @@ -0,0 +1,125 @@ | ||
| #!/usr/bin/make -f | ||
| # -*- makefile -*- | ||
| # Sample debian/rules that uses debhelper. | ||
| # This file was originally written by Joey Hess and Craig Small. | ||
| # As a special exception, when this file is copied by dh-make into a | ||
| # dh-make output file, you may use that output file without restriction. | ||
| # This special exception was added by Craig Small in version 0.37 of dh-make. | ||
|
|
||
| # Uncomment this to turn on verbose mode. | ||
| #export DH_VERBOSE=1 | ||
|
|
||
|
|
||
| # These are used for cross-compiling and for saving the configure script | ||
| # from having to guess our platform (since we know it already) | ||
| DEB_HOST_GNU_TYPE ?= $(shell dpkg-architecture -qDEB_HOST_GNU_TYPE) | ||
| DEB_BUILD_GNU_TYPE ?= $(shell dpkg-architecture -qDEB_BUILD_GNU_TYPE) | ||
|
|
||
|
|
||
| CFLAGS = -Wall -g | ||
|
|
||
| ifneq (,$(findstring noopt,$(DEB_BUILD_OPTIONS))) | ||
| CFLAGS += -O0 | ||
| else | ||
| CFLAGS += -O2 | ||
| endif | ||
|
|
||
| # allow parallel build using recommended approach from | ||
| # http://www.de.debian.org/doc/debian-policy/ch-source.html@s-debianrules-options | ||
| # | ||
| # For Bash type: | ||
| # | ||
| # export DEB_BUILD_OPTIONS="parallel=2" | ||
| # svn-buildpackage ... | ||
| # | ||
| ifneq (,$(filter parallel=%,$(DEB_BUILD_OPTIONS))) | ||
| NUMJOBS=$(patsubst parallel=%,%,$(filter parallel=%,$(DEB_BUILD_OPTIONS))) | ||
| MAKE_FLAGS += -j$(NUMJOBS) | ||
| endif | ||
|
|
||
| # shared library versions, option 1 | ||
| #version=2.0.5 | ||
| #major=2 | ||
| version=`grep orocos-kdl debian/changelog | head -1 | awk '{print substr($$2,2,length($$2)-2)}'` | ||
| major=`grep orocos-kdl debian/changelog | head -1 | awk '{print substr($$2,2,3)}'` | ||
| # option 2, assuming the library is created as src/.libs/libfoo.so.2.0.5 or so | ||
| #version=`ls src/.libs/lib*.so.* | \ | ||
| # awk '{if (match($$0,/[0-9]+\.[0-9]+\.[0-9]+$$/)) print substr($$0,RSTART)}'` | ||
| #major=`ls src/.libs/lib*.so.* | \ | ||
| # awk '{if (match($$0,/\.so\.[0-9]+$$/)) print substr($$0,RSTART+4)}'` | ||
|
|
||
| configure: configure-stamp | ||
| configure-stamp: | ||
| mkdir -p dbuild | ||
| cd dbuild;\ | ||
| cmake .. \ | ||
| -DCMAKE_INSTALL_PREFIX=/usr \ | ||
| -DPYTHON_BINDINGS=ON \ | ||
| -DBUILD_MODELS=ON \ | ||
| -DCMAKE_BUILD_TYPE="Release"\ | ||
| -DPYTHON_SITE_PACKAGES_DIR=lib/python/site-packages | ||
| dh_testdir | ||
| touch configure-stamp | ||
|
|
||
| build: build-stamp | ||
| build-stamp: configure-stamp | ||
| cd dbuild;$(MAKE) | ||
| dh_testdir | ||
| touch build-stamp | ||
|
|
||
| clean: | ||
| dh_testdir | ||
| dh_testroot | ||
| rm -f build-stamp* | ||
| rm -f configure-stamp* | ||
|
|
||
| # Add here commands to clean up after the build process. | ||
| -rm -rf build* install* | ||
|
|
||
| dh_clean | ||
|
|
||
| install: build | ||
| cd dbuild;$(MAKE) install DESTDIR=$(CURDIR)/debian/tmp | ||
| dh_testdir | ||
| dh_testroot | ||
| dh_installdirs | ||
|
|
||
| # Build architecture-independent files here. | ||
| binary-indep: build install | ||
| # We have nothing to do by default. | ||
|
|
||
| # Build architecture-dependent files here. | ||
| binary-arch: build install | ||
| dh_testdir | ||
| dh_testroot | ||
| dh_installchangelogs | ||
| dh_installdocs | ||
| dh_installexamples | ||
| dh_install --sourcedir=debian/tmp --list-missing | ||
| # dh_installmenu | ||
| # dh_installdebconf | ||
| # dh_installlogrotate | ||
| # dh_installemacsen | ||
| # dh_installpam | ||
| # dh_installmime | ||
| # dh_installinit | ||
| # dh_installcron | ||
| # dh_installinfo | ||
| dh_installman | ||
| dh_link | ||
| dh_strip | ||
| dh_compress | ||
| dh_fixperms | ||
| # dh_perl | ||
| dh_pysupport | ||
| # dh_pycentral | ||
| # dh_python | ||
| dh_makeshlibs | ||
| dh_installdeb | ||
| dh_shlibdeps | ||
| dh_gencontrol | ||
| dh_md5sums | ||
| dh_builddeb | ||
|
|
||
| binary: binary-indep binary-arch | ||
| .PHONY: build clean binary-indep binary-arch binary install configure |
| @@ -0,0 +1 @@ | ||
| shlibs:Depends=libc6 (>= 2.3.5-1), libgcc1 (>= 1:4.1.1-12), libstdc++6 (>= 4.1.1-12) |
| @@ -0,0 +1,4 @@ | ||
|
|
||
| CONFIGURE_FILE("${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in" "${CMAKE_CURRENT_BINARY_DIR}/Doxyfile" IMMEDIATE @ONLY) | ||
| ADD_CUSTOM_TARGET(docs "doxygen" "Doxyfile") | ||
| INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/kdl.tag DESTINATION share/doc/liborocos-kdl/ OPTIONAL) # only installs if found. |
| @@ -0,0 +1,242 @@ | ||
| # Doxyfile 1.4.6 | ||
|
|
||
| #--------------------------------------------------------------------------- | ||
| # Project related configuration options | ||
| #--------------------------------------------------------------------------- | ||
| PROJECT_NAME = KDL | ||
| PROJECT_NUMBER = @KDL_VERSION@ | ||
| OUTPUT_DIRECTORY = ./api | ||
| CREATE_SUBDIRS = NO | ||
| OUTPUT_LANGUAGE = English | ||
| USE_WINDOWS_ENCODING = NO | ||
| BRIEF_MEMBER_DESC = YES | ||
| REPEAT_BRIEF = YES | ||
| ABBREVIATE_BRIEF = "The $name class" \ | ||
| "The $name widget" \ | ||
| "The $name file" \ | ||
| is \ | ||
| provides \ | ||
| specifies \ | ||
| contains \ | ||
| represents \ | ||
| a \ | ||
| an \ | ||
| the | ||
| ALWAYS_DETAILED_SEC = NO | ||
| INLINE_INHERITED_MEMB = YES | ||
| FULL_PATH_NAMES = YES | ||
| STRIP_FROM_PATH = @PROJ_SOURCE_DIR@ | ||
| STRIP_FROM_INC_PATH = @PROJ_SOURCE_DIR@ | ||
| SHORT_NAMES = NO | ||
| JAVADOC_AUTOBRIEF = YES | ||
| MULTILINE_CPP_IS_BRIEF = NO | ||
| DETAILS_AT_TOP = NO | ||
| INHERIT_DOCS = YES | ||
| SEPARATE_MEMBER_PAGES = NO | ||
| TAB_SIZE = 8 | ||
| ALIASES = | ||
| OPTIMIZE_OUTPUT_FOR_C = NO | ||
| OPTIMIZE_OUTPUT_JAVA = NO | ||
| BUILTIN_STL_SUPPORT = YES | ||
| DISTRIBUTE_GROUP_DOC = NO | ||
| SUBGROUPING = YES | ||
| #--------------------------------------------------------------------------- | ||
| # Build related configuration options | ||
| #--------------------------------------------------------------------------- | ||
| EXTRACT_ALL = YES | ||
| EXTRACT_PRIVATE = YES | ||
| EXTRACT_STATIC = YES | ||
| EXTRACT_LOCAL_CLASSES = YES | ||
| EXTRACT_LOCAL_METHODS = NO | ||
| HIDE_UNDOC_MEMBERS = NO | ||
| HIDE_UNDOC_CLASSES = NO | ||
| HIDE_FRIEND_COMPOUNDS = NO | ||
| HIDE_IN_BODY_DOCS = NO | ||
| INTERNAL_DOCS = NO | ||
| CASE_SENSE_NAMES = YES | ||
| HIDE_SCOPE_NAMES = NO | ||
| SHOW_INCLUDE_FILES = YES | ||
| INLINE_INFO = YES | ||
| SORT_MEMBER_DOCS = YES | ||
| SORT_BRIEF_DOCS = NO | ||
| SORT_BY_SCOPE_NAME = NO | ||
| GENERATE_TODOLIST = YES | ||
| GENERATE_TESTLIST = YES | ||
| GENERATE_BUGLIST = YES | ||
| GENERATE_DEPRECATEDLIST= YES | ||
| ENABLED_SECTIONS = | ||
| MAX_INITIALIZER_LINES = 30 | ||
| SHOW_USED_FILES = YES | ||
| SHOW_DIRECTORIES = NO | ||
| FILE_VERSION_FILTER = | ||
| #--------------------------------------------------------------------------- | ||
| # configuration options related to warning and progress messages | ||
| #--------------------------------------------------------------------------- | ||
| QUIET = NO | ||
| WARNINGS = YES | ||
| WARN_IF_UNDOCUMENTED = YES | ||
| WARN_IF_DOC_ERROR = YES | ||
| WARN_NO_PARAMDOC = NO | ||
| WARN_FORMAT = "$file:$line: $text" | ||
| WARN_LOGFILE = | ||
| #--------------------------------------------------------------------------- | ||
| # configuration options related to the input files | ||
| #--------------------------------------------------------------------------- | ||
| INPUT = @PROJ_SOURCE_DIR@/src \ | ||
| @PROJ_SOURCE_DIR@/src/bindings/rtt \ | ||
| @PROJ_SOURCE_DIR@/src/utilities/svd_eigen_Macie.hpp | ||
| FILE_PATTERNS = *.cpp \ | ||
| *.cxx \ | ||
| *.h \ | ||
| *.hpp \ | ||
| *.inl | ||
|
|
||
| RECURSIVE = NO | ||
| EXCLUDE = | ||
|
|
||
| EXCLUDE_SYMLINKS = NO | ||
| EXCLUDE_PATTERNS = *.svn* CMake* | ||
| EXAMPLE_PATH = | ||
| EXAMPLE_PATTERNS = * | ||
| EXAMPLE_RECURSIVE = NO | ||
| IMAGE_PATH = | ||
| INPUT_FILTER = | ||
| FILTER_PATTERNS = | ||
| FILTER_SOURCE_FILES = NO | ||
| #--------------------------------------------------------------------------- | ||
| # configuration options related to source browsing | ||
| #--------------------------------------------------------------------------- | ||
| SOURCE_BROWSER = NO | ||
| INLINE_SOURCES = NO | ||
| STRIP_CODE_COMMENTS = YES | ||
| REFERENCED_BY_RELATION = YES | ||
| REFERENCES_RELATION = YES | ||
| USE_HTAGS = NO | ||
| VERBATIM_HEADERS = YES | ||
| #--------------------------------------------------------------------------- | ||
| # configuration options related to the alphabetical class index | ||
| #--------------------------------------------------------------------------- | ||
| ALPHABETICAL_INDEX = YES | ||
| COLS_IN_ALPHA_INDEX = 4 | ||
| IGNORE_PREFIX = | ||
| #--------------------------------------------------------------------------- | ||
| # configuration options related to the HTML output | ||
| #--------------------------------------------------------------------------- | ||
| GENERATE_HTML = YES | ||
| HTML_OUTPUT = html | ||
| HTML_FILE_EXTENSION = .html | ||
| HTML_HEADER = | ||
| HTML_FOOTER = | ||
| HTML_STYLESHEET = | ||
| HTML_ALIGN_MEMBERS = YES | ||
| GENERATE_HTMLHELP = NO | ||
| CHM_FILE = | ||
| HHC_LOCATION = | ||
| GENERATE_CHI = NO | ||
| BINARY_TOC = NO | ||
| TOC_EXPAND = NO | ||
| DISABLE_INDEX = NO | ||
| ENUM_VALUES_PER_LINE = 4 | ||
| GENERATE_TREEVIEW = NO | ||
| TREEVIEW_WIDTH = 250 | ||
| #--------------------------------------------------------------------------- | ||
| # configuration options related to the LaTeX output | ||
| #--------------------------------------------------------------------------- | ||
| GENERATE_LATEX = NO | ||
| LATEX_OUTPUT = latex | ||
| LATEX_CMD_NAME = latex | ||
| MAKEINDEX_CMD_NAME = makeindex | ||
| COMPACT_LATEX = NO | ||
| PAPER_TYPE = a4wide | ||
| EXTRA_PACKAGES = | ||
| LATEX_HEADER = | ||
| PDF_HYPERLINKS = NO | ||
| USE_PDFLATEX = NO | ||
| LATEX_BATCHMODE = NO | ||
| LATEX_HIDE_INDICES = NO | ||
| #--------------------------------------------------------------------------- | ||
| # configuration options related to the RTF output | ||
| #--------------------------------------------------------------------------- | ||
| GENERATE_RTF = NO | ||
| RTF_OUTPUT = rtf | ||
| COMPACT_RTF = NO | ||
| RTF_HYPERLINKS = NO | ||
| RTF_STYLESHEET_FILE = | ||
| RTF_EXTENSIONS_FILE = | ||
| #--------------------------------------------------------------------------- | ||
| # configuration options related to the man page output | ||
| #--------------------------------------------------------------------------- | ||
| GENERATE_MAN = NO | ||
| MAN_OUTPUT = man | ||
| MAN_EXTENSION = .3 | ||
| MAN_LINKS = NO | ||
| #--------------------------------------------------------------------------- | ||
| # configuration options related to the XML output | ||
| #--------------------------------------------------------------------------- | ||
| GENERATE_XML = NO | ||
| XML_OUTPUT = xml | ||
| XML_SCHEMA = | ||
| XML_DTD = | ||
| XML_PROGRAMLISTING = YES | ||
| #--------------------------------------------------------------------------- | ||
| # configuration options for the AutoGen Definitions output | ||
| #--------------------------------------------------------------------------- | ||
| GENERATE_AUTOGEN_DEF = NO | ||
| #--------------------------------------------------------------------------- | ||
| # configuration options related to the Perl module output | ||
| #--------------------------------------------------------------------------- | ||
| GENERATE_PERLMOD = NO | ||
| PERLMOD_LATEX = NO | ||
| PERLMOD_PRETTY = YES | ||
| PERLMOD_MAKEVAR_PREFIX = | ||
| #--------------------------------------------------------------------------- | ||
| # Configuration options related to the preprocessor | ||
| #--------------------------------------------------------------------------- | ||
| ENABLE_PREPROCESSING = YES | ||
| MACRO_EXPANSION = NO | ||
| EXPAND_ONLY_PREDEF = NO | ||
| SEARCH_INCLUDES = YES | ||
| INCLUDE_PATH = | ||
| INCLUDE_FILE_PATTERNS = | ||
| PREDEFINED = | ||
| EXPAND_AS_DEFINED = | ||
| SKIP_FUNCTION_MACROS = YES | ||
| #--------------------------------------------------------------------------- | ||
| # Configuration::additions related to external references | ||
| #--------------------------------------------------------------------------- | ||
| TAGFILES = @OROCOS_INSTALL@/share/doc/liborocos-rtt/rtt.tag=../../../../rtt/@RTT_VVERSION@/api/html | ||
| GENERATE_TAGFILE = kdl.tag | ||
| ALLEXTERNALS = NO | ||
| EXTERNAL_GROUPS = YES | ||
| PERL_PATH = /usr/bin/perl | ||
| #--------------------------------------------------------------------------- | ||
| # Configuration options related to the dot tool | ||
| #--------------------------------------------------------------------------- | ||
| CLASS_DIAGRAMS = NO | ||
| HIDE_UNDOC_RELATIONS = YES | ||
| HAVE_DOT = YES | ||
| CLASS_GRAPH = YES | ||
| COLLABORATION_GRAPH = YES | ||
| GROUP_GRAPHS = YES | ||
| UML_LOOK = NO | ||
| TEMPLATE_RELATIONS = YES | ||
| INCLUDE_GRAPH = YES | ||
| INCLUDED_BY_GRAPH = YES | ||
| CALL_GRAPH = NO | ||
| GRAPHICAL_HIERARCHY = YES | ||
| DIRECTORY_GRAPH = YES | ||
| DOT_IMAGE_FORMAT = png | ||
| DOT_PATH = | ||
| DOTFILE_DIRS = | ||
| MAX_DOT_GRAPH_WIDTH = 1024 | ||
| MAX_DOT_GRAPH_HEIGHT = 1024 | ||
| MAX_DOT_GRAPH_DEPTH = 1000 | ||
| DOT_TRANSPARENT = NO | ||
| DOT_MULTI_TARGETS = NO | ||
| GENERATE_LEGEND = YES | ||
| DOT_CLEANUP = YES | ||
| #--------------------------------------------------------------------------- | ||
| # Configuration::additions related to the search engine | ||
| #--------------------------------------------------------------------------- | ||
| SEARCHENGINE = YES | ||
| USE_MATHJAX = YES |
| @@ -0,0 +1,70 @@ | ||
| Goal of the tests in KDL : | ||
| ========================== | ||
|
|
||
| All numerical calculations that are non-trivial should be tested by | ||
| a testprogram. This is important because these kinds of errors are difficult to trace | ||
| when they occur in an application. | ||
|
|
||
| The tests are run automatically when using "make check". | ||
|
|
||
| Test programs should give a short message on std::cerr and a long log of their activities on std::cout. | ||
| Test programs should exit with a non-zero status number if an error occurred. | ||
|
|
||
| (make check > log , if you want the log into a file.) | ||
|
|
||
|
|
||
|
|
||
|
|
||
| Description of the tests in KDL: | ||
| ================================ | ||
|
|
||
| framestest.cpp : | ||
| - consistency of the objects Frame,Vector,Twist and Wrench and | ||
| all of their mathematical operations. | ||
| - e.g. R*R.Inverse() == Frame::Identity, or (R*(R*v))=(R*R)*v | ||
| - many other equalities are checked. | ||
|
|
||
| frameveltest.cpp doubleveltest.cpp : | ||
| - similar consistency checks on framestest, but for FrameVel,VectorVel,TwistVel, WrenchVel, | ||
| doubleVel, FrameAcc,VectorAcc,TwistAcc,WrenchAcc, doubleAcc and all of their mathematical | ||
| operations. | ||
|
|
||
| jacobianframetests.cpp jacobiandoubletests.cpp jacobiantests.hpp : | ||
| - tests the Jacobian<X> classes for X=Frame,Vector,Wrench,Twist,double | ||
| and ALL unary and binary operations on Jacobian<X>. | ||
| - The checks are done by generating random instances of these classes | ||
| and checking the Jacobian with numerical differentiation. | ||
|
|
||
|
|
||
| iotest.cpp: | ||
| - tests the I/O by output of a kinematic family and reading it back in. | ||
|
|
||
|
|
||
| serialchaintest.cpp : | ||
| - The following routines for testing are provided : | ||
| - CompareFamilies(kinematicfamily1, kinematicfamily2) | ||
| compares jnt2cartpos transformation for two kf's. | ||
| - TestForwardAndInverse | ||
| checks whether jnt2cartpos and cartpos2jnt are consistent. | ||
| - TestForwardPosAndJac | ||
| compares jnt2cartpos and jnt2jac by numerical differentiation. | ||
| - TestCartVelAndJac | ||
| checks whether jnt2cartvel and jnt2jac are consistent. | ||
| - TestCartVelAndInverse | ||
| checks whether jnt2cartvel and cartvel2jnt are consistent. | ||
|
|
||
| - These routines are used on : | ||
| - SerialChain | ||
| - A SerialChain with joint index offset and a LinearTransmission. | ||
| - ZXXZXZ (featherstone) kinematic family | ||
| - A SerialChain requested from the ZXXZXZ (featherstone) kinematic family. | ||
|
|
||
| featherstonetest.cpp : | ||
| - tests the transformations for the featherstone kinematic family for all 8 configurations. | ||
| and tests the set/getConfiguration(enum) set/getConfiguration(jointvalues) routines. | ||
| - transformations tested are : Jnt2CartPos and CartPos2Jnt. | ||
|
|
||
|
|
||
|
|
||
| treetest.cpp : | ||
| - work in progress.. |
| @@ -0,0 +1,261 @@ | ||
| \documentclass[a4paper,10pt]{report} | ||
|
|
||
| \title{User Manual \\ Kinematics \& Dynamics Library} | ||
| \author{Ruben Smits, Herman Bruyninckx} | ||
|
|
||
| \usepackage{listings} | ||
| \usepackage{amsmath} | ||
| \usepackage{url} | ||
| \usepackage{todo} | ||
| \usepackage{hyperref} | ||
|
|
||
| \begin{document} | ||
| \lstset{language=c++} | ||
|
|
||
| \maketitle | ||
|
|
||
| \chapter{Introduction to KDL} | ||
| \label{cha:introduction-kdl} | ||
|
|
||
| \section{What is KDL?} | ||
| \label{sec:what-kdl} | ||
| Kinematics \& Dynamics Library is a c++ library that offers | ||
|
|
||
| \begin{itemize} | ||
| \item classes for geometric primitives and their operations | ||
| \item classes for kinematic descriptions of chains (robots) | ||
| \item (realtime) kinematic solvers for these kinematic chains | ||
| \end{itemize} | ||
|
|
||
| \section{Getting support - the Orocos community} | ||
| \label{sec:gett-supp-oroc} | ||
|
|
||
| \begin{itemize} | ||
| \item This document! | ||
| \item The website : \url{http://www.orocos.org/kdl}. | ||
| \item The mailinglist. \todo{add link to mailinglist} | ||
| \end{itemize} | ||
|
|
||
|
|
||
| \section{Getting Orocos KDL} | ||
| \label{sec:getting-orocos-kdl} | ||
|
|
||
| First off all you need to successfully install the KDL-library. This is | ||
| explained in the \textbf{Installation Manual}, it can be found on | ||
| \url{http://www.orocos.org/kdl/Installation_Manual} | ||
|
|
||
|
|
||
| \chapter{Tutorial} | ||
| \label{cha:tutorial} | ||
|
|
||
| \paragraph{Important remarks} | ||
| \label{sec:important-remarks} | ||
| \begin{itemize} | ||
| \item All geometric primitives and there operations can be used in | ||
| realtime. None of the operations lead to dynamic memory allocations | ||
| and all of the operations are deterministic in time. | ||
| \item All values are in [m] for translational components and [rad] for | ||
| rotational components. | ||
| \end{itemize} | ||
|
|
||
| \section{Geometric Primitives} | ||
| \label{sec:geometric-primitives} | ||
|
|
||
| \paragraph{Headers} | ||
| \label{sec:headers} | ||
|
|
||
| \begin{itemize} | ||
| \item \lstinline$frames.hpp$: Definition of all geometric primitives | ||
| and there transformations/operators. | ||
| \item \lstinline$frames_io.hpp$: Definition of the input/output | ||
| operators. | ||
| \end{itemize} | ||
|
|
||
|
|
||
| The following primitives are available: | ||
| \begin{itemize} | ||
| \item Vector~\ref{sec:vector} | ||
| \item Rotation~\ref{sec:rotation} | ||
| \item Frame~\ref{sec:frame} | ||
| \item Twist~\ref{sec:twist} | ||
| \item Wrench~\ref{sec:wrench} | ||
| \end{itemize} | ||
|
|
||
| \subsection{Vector} | ||
| \label{sec:vector} | ||
| Represents the 3D position of a point/object. It contains | ||
| three values, representing the X,Y,Z position of the point/object with | ||
| respect to the reference frame: | ||
|
|
||
| $\mathrm{Vector} = \left[\begin{array}{c} x \\ y \\ z \end{array} \right]$ | ||
|
|
||
| \paragraph{Creating Vectors} | ||
| \label{sec:creating-vectors} | ||
| There are different ways to create a vector: | ||
| \begin{lstlisting} | ||
| Vector v1; //The default constructor, X-Y-Z are initialized to zero | ||
| Vector v2(x,y,z); //X-Y-Z are initialized with the given values | ||
| Vector v3(v2); //The copy constructor | ||
| Vector v4=Vector::Zero(); //All values are set to zero | ||
| \end{lstlisting} | ||
|
|
||
| \paragraph{Get/Set individual elements} | ||
| \label{sec:gets-indiv-elem} | ||
| The operators \lstinline$[]$ and \lstinline$()$ use indices from 0..2, | ||
| index checking is enabled/disabled by the DEBUG/NDEBUG definitions: | ||
| \begin{lstlisting} | ||
| v1[0]=v2[1];//copy y value of v2 to x value of v1 | ||
| v2(1)=v3(3);//copy z value of v3 to y value of v2 | ||
| v3.x( v4.y() );//copy y value of v4 to x value of v3 | ||
| \end{lstlisting} | ||
|
|
||
| \paragraph{Multiply/Divide with a scalar} | ||
| \label{sec:mult-with-scal} | ||
| You can multiply or divide a Vector with a double using the operator | ||
| \lstinline$*$ and \lstinline$/$: | ||
| \begin{lstlisting} | ||
| v2=2*v1; | ||
| v3=v1/2; | ||
| \end{lstlisting} | ||
|
|
||
| \paragraph{Add and subtract vectors} | ||
| \label{sec:add-subtract-vectors} | ||
| You can add or substract a vector with another vector: | ||
| \begin{lstlisting} | ||
| v2+=v1; | ||
| v3-=v1; | ||
| v4=v1+v2; | ||
| v5=v2-v3; | ||
| \end{lstlisting} | ||
|
|
||
| \paragraph{Cross and scalar product} | ||
| \label{sec:cross-scalar-product} | ||
| You can calculate the cross product of two vectors, which results in | ||
| new vector or calculate the scalar(dot) product of two vectors: | ||
| \begin{lstlisting} | ||
| v3=v1*v2; //Cross product | ||
| double a=dot(v1,v2)//Scalar product | ||
| \end{lstlisting} | ||
|
|
||
| \paragraph{Resetting} | ||
| \label{sec:resetting} | ||
| You can reset the values of a vector to zero: | ||
| \begin{lstlisting} | ||
| SetToZero(v1); | ||
| \end{lstlisting} | ||
|
|
||
| \paragraph{Comparing vectors} | ||
| \label{sec:comparing-vectors} | ||
| With or without giving an accuracy: | ||
| \begin{lstlisting} | ||
| v1==v2; | ||
| v2!=v3; | ||
| Equal(v3,v4,eps);//with accuracy eps | ||
| \end{lstlisting} | ||
|
|
||
| \subsection{Rotation} | ||
| \label{sec:rotation} | ||
|
|
||
| Represents the 3D rotation of an object wrt the reference | ||
| frame. Internally it is represented by a 3x3 matrix which is a | ||
| non-minimal representation: | ||
|
|
||
| $\mathrm{Rotation} = | ||
| \left[\begin{array}{ccc} | ||
| Xx&Yx&Zx\\ | ||
| Xy&Yy&Zy\\ | ||
| Xz&Yz&Zz | ||
| \end{array}\right] | ||
| $ | ||
|
|
||
| \paragraph{Creating Rotations} | ||
| \label{sec:creating-rotations} | ||
|
|
||
| \subparagraph{Safe ways to create a Rotation} | ||
| \label{sec:safe-ways-create} | ||
|
|
||
| The following result always in consistent Rotations. This means the | ||
| rows/columns are always normalized and orthogonal: | ||
|
|
||
| \begin{lstlisting} | ||
| Rotation r1; //The default constructor, initializes to an 3x3 identity matrix | ||
| Rotation r1 = Rotation::Identity();//Identity Rotation = zero rotation | ||
| Rotation r2 = Rotation::RPY(roll,pitch,yaw); //Rotation build out off Roll-Pitch-Yaw angles | ||
| Rotation r3 = Rotation::EulerZYZ(alpha,beta,gamma); //Rotation build out off Euler Z-Y-Z angles | ||
| Rotation r4 = Rotation::EulerZYX(alpha,beta,gamma); //Rotation build out off Euler Z-Y-X angles | ||
| Rotation r5 = Rotation::Rot(vector,angle); //Rotation build out off an equivalent axis(vector) and an angle. | ||
| \end{lstlisting} | ||
|
|
||
| \subparagraph{Other ways} | ||
| \label{sec:other-ways} | ||
| The following should be used with care, they can result in | ||
| inconsistent rotation matrices, since there is no checking if | ||
| columns/rows are normalized or orthogonal | ||
| \begin{lstlisting} | ||
| Rotation r6( Xx,Yx,Zx,Xy,Yy,Zy,Xz,Yz,Zz);//Give each individual element (Column-Major) | ||
| Rotation r7(vectorX,vectorY,vectorZ);//Give each individual column | ||
| \end{lstlisting} | ||
|
|
||
| \paragraph{Getting values} | ||
| \label{sec:getting-values} | ||
| Individual values, the indices go from 0..2: | ||
|
|
||
| \subsection{Frame} | ||
| \label{sec:frame} | ||
|
|
||
| \subsection{Twist} | ||
| \label{sec:twist} | ||
|
|
||
| \subsection{Wrench} | ||
| \label{sec:wrench} | ||
|
|
||
|
|
||
| \section{Kinematic Structures} | ||
| \label{sec:kinematic-structures} | ||
|
|
||
| \subsection{Joint} | ||
| \label{sec:joint} | ||
|
|
||
| \subsection{Segment} | ||
| \label{sec:segment} | ||
|
|
||
| \subsection{Chain} | ||
| \label{sec:chain} | ||
|
|
||
| \section{Kinematic Solvers} | ||
| \label{sec:kinematic-solvers} | ||
|
|
||
| \subsection{Forward position kinematics} | ||
| \label{sec:forw-posit-kinem} | ||
|
|
||
| \subsection{Forward velocity kinematics} | ||
| \label{sec:forw-veloc-kinem} | ||
|
|
||
| \subsection{Jacobian calculation} | ||
| \label{sec:jacobian-calculation} | ||
|
|
||
| \subsection{Inverse velocity kinematics} | ||
| \label{sec:inverse-veloc-kinem} | ||
|
|
||
| \subsection{Inverse position kinematics} | ||
| \label{sec:inverse-posit-kinem} | ||
|
|
||
| \section{Motion Specification Primitives} | ||
| \label{sec:moti-spec-prim} | ||
|
|
||
| \subsection{Path} | ||
| \label{sec:path} | ||
|
|
||
| \subsection{Velocity profile} | ||
| \label{sec:velocity-profile} | ||
|
|
||
| \subsection{Trajectory} | ||
| \label{sec:trajectory} | ||
|
|
||
|
|
||
| \end{document} | ||
|
|
||
| %%% Local Variables: | ||
| %%% mode: latex | ||
| %%% TeX-master: t | ||
| %%% End: |
| @@ -0,0 +1,14 @@ | ||
| IF(ENABLE_EXAMPLES) | ||
| INCLUDE_DIRECTORIES(${PROJ_SOURCE_DIR}/src ${PROJ_SOURCE_DIR}/models ${PROJ_BINARY_DIR}/src) | ||
|
|
||
| add_executable(geometry geometry.cpp ) | ||
| TARGET_LINK_LIBRARIES(geometry orocos-kdl) | ||
|
|
||
| add_executable(trajectory_example trajectory_example.cpp ) | ||
| TARGET_LINK_LIBRARIES(trajectory_example orocos-kdl) | ||
|
|
||
| add_executable(chainiksolverpos_lma_demo chainiksolverpos_lma_demo.cpp ) | ||
| TARGET_LINK_LIBRARIES(chainiksolverpos_lma_demo orocos-kdl orocos-kdl-models) | ||
|
|
||
| ENDIF(ENABLE_EXAMPLES) | ||
|
|
| @@ -0,0 +1,3 @@ | ||
| To build one of the examples do: | ||
|
|
||
| g++ -I<KDL_INCLUDE_DIR> -L<KDL_LIB_DIR> -lorocos-kdl <example.cpp> -o <example> |
| @@ -0,0 +1,218 @@ | ||
| /** | ||
| \file chainiksolverpos_lma_demo.cpp | ||
| \brief Test program for inverse position kinematics | ||
| results with 1 million inv pos kin. | ||
| <code> | ||
| #times successful 999992 | ||
| #times -1 result 0 | ||
| #times -2 result 5 | ||
| #times -3 result 3 | ||
| average number of iter 16.6437 | ||
| min. nr of iter 13 | ||
| max. nr of iter 500 | ||
| min. difference after solving 3.86952e-12 | ||
| max. difference after solving 4.79339e-05 | ||
| min. trans. difference after solving 3.86952e-12 | ||
| max. trans. difference after solving 4.79339e-05 | ||
| min. rot. difference after solving 0 | ||
| max. rot. difference after solving 0.000261335 | ||
| elapsed time 199.14 | ||
| estimate of average time per invposkin (ms)0.19914 | ||
| estimate of longest time per invposkin (ms) 5.98245 | ||
| estimate of shortest time per invposkin (ms) 0.155544 | ||
| </code> | ||
| */ | ||
|
|
||
| /************************************************************************** | ||
| begin : May 2011 | ||
| copyright : (C) 2011 Erwin Aertbelien | ||
| email : firstname.lastname@mech.kuleuven.ac.be | ||
| History (only major changes)( AUTHOR-Description ) : | ||
| *************************************************************************** | ||
| * This library is free software; you can redistribute it and/or * | ||
| * modify it under the terms of the GNU Lesser General Public * | ||
| * License as published by the Free Software Foundation; either * | ||
| * version 2.1 of the License, or (at your option) any later version. * | ||
| * * | ||
| * This library is distributed in the hope that it will be useful, * | ||
| * but WITHOUT ANY WARRANTY; without even the implied warranty of * | ||
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * | ||
| * Lesser General Public License for more details. * | ||
| * * | ||
| * You should have received a copy of the GNU Lesser General Public * | ||
| * License along with this library; if not, write to the Free Software * | ||
| * Foundation, Inc., 59 Temple Place, * | ||
| * Suite 330, Boston, MA 02111-1307 USA * | ||
| * * | ||
| ***************************************************************************/ | ||
|
|
||
| #include <iostream> | ||
| #include <frames_io.hpp> | ||
| #include <models.hpp> | ||
| #include <chainiksolverpos_lma.hpp> | ||
| #include <chainfksolverpos_recursive.hpp> | ||
| #include <boost/timer.hpp> | ||
|
|
||
| /** | ||
| * tests the inverse kinematics on the given kinematic chain for a | ||
| * large number of times and provides statistics on the result. | ||
| * \TODO provide other examples. | ||
| */ | ||
| void test_inverseposkin(KDL::Chain& chain) { | ||
| using namespace KDL; | ||
| using namespace std; | ||
| boost::timer timer; | ||
| int num_of_trials = 1000000; | ||
| int total_number_of_iter = 0; | ||
| int n = chain.getNrOfJoints(); | ||
| int nrofresult_ok = 0; | ||
| int nrofresult_minus1=0; | ||
| int nrofresult_minus2=0; | ||
| int nrofresult_minus3=0; | ||
| int min_num_of_iter = 10000000; | ||
| int max_num_of_iter = 0; | ||
| double min_diff = 1E10; | ||
| double max_diff = 0; | ||
| double min_trans_diff = 1E10; | ||
| double max_trans_diff = 0; | ||
| double min_rot_diff = 1E10; | ||
| double max_rot_diff = 0; | ||
| Eigen::Matrix<double,6,1> L; | ||
| L(0)=1;L(1)=1;L(2)=1; | ||
| L(3)=0.01;L(4)=0.01;L(5)=0.01; | ||
| ChainFkSolverPos_recursive fwdkin(chain); | ||
| ChainIkSolverPos_LMA solver(chain,L); | ||
| JntArray q(n); | ||
| JntArray q_init(n); | ||
| JntArray q_sol(n); | ||
| for (int trial=0;trial<num_of_trials;++trial) { | ||
| q.data.setRandom(); | ||
| q.data *= M_PI; | ||
| q_init.data.setRandom(); | ||
| q_init.data *= M_PI; | ||
| Frame pos_goal,pos_reached; | ||
| fwdkin.JntToCart(q,pos_goal); | ||
| //solver.compute_fwdpos(q.data); | ||
| //pos_goal = solver.T_base_head; | ||
| int retval; | ||
| retval = solver.CartToJnt(q_init,pos_goal,q_sol); | ||
| switch (retval) { | ||
| case 0: | ||
| nrofresult_ok++; | ||
| break; | ||
| case -1: | ||
| nrofresult_minus1++; | ||
| break; | ||
| case -2: | ||
| nrofresult_minus2++; | ||
| break; | ||
| case -3: | ||
| nrofresult_minus3++; | ||
| break; | ||
| } | ||
| if (retval !=0) { | ||
| cout << "-------------- failed ----------------------------" << endl; | ||
| cout << "pos " << pos_goal << endl; | ||
| cout << "reached pos " << solver.T_base_head << endl; | ||
| cout << "TF from pos to head \n" << pos_goal.Inverse()*solver.T_base_head << endl; | ||
| cout << "gradient " << solver.grad.transpose() << endl; | ||
| cout << "q " << q.data.transpose()/M_PI*180.0 << endl; | ||
| cout << "q_sol " << q_sol.data.transpose()/M_PI*180.0 << endl; | ||
| cout << "q_init " << q_init.data.transpose()/M_PI*180.0 << endl; | ||
| cout << "return value " << retval << endl; | ||
| cout << "last #iter " << solver.lastNrOfIter << endl; | ||
| cout << "last diff " << solver.lastDifference << endl; | ||
| cout << "jacobian of goal values "; | ||
| solver.display_jac(q); | ||
| std::cout << "jacobian of solved values "; | ||
| solver.display_jac(q_sol); | ||
|
|
||
| } | ||
| total_number_of_iter += solver.lastNrOfIter; | ||
| if (solver.lastNrOfIter > max_num_of_iter) max_num_of_iter = solver.lastNrOfIter; | ||
| if (solver.lastNrOfIter < min_num_of_iter) min_num_of_iter = solver.lastNrOfIter; | ||
| if (retval!=-3) { | ||
| if (solver.lastDifference > max_diff) max_diff = solver.lastDifference; | ||
| if (solver.lastDifference < min_diff) min_diff = solver.lastDifference; | ||
|
|
||
| if (solver.lastTransDiff > max_trans_diff) max_trans_diff = solver.lastTransDiff; | ||
| if (solver.lastTransDiff < min_trans_diff) min_trans_diff = solver.lastTransDiff; | ||
|
|
||
| if (solver.lastRotDiff > max_trans_diff) max_rot_diff = solver.lastRotDiff; | ||
| if (solver.lastRotDiff < min_trans_diff) min_rot_diff = solver.lastRotDiff; | ||
| } | ||
| fwdkin.JntToCart(q_sol,pos_reached); | ||
| } | ||
| cout << "------------------ statistics ------------------------------"<<endl; | ||
| cout << "#times successful " << nrofresult_ok << endl; | ||
| cout << "#times -1 result " << nrofresult_minus1 << endl; | ||
| cout << "#times -2 result " << nrofresult_minus2 << endl; | ||
| cout << "#times -3 result " << nrofresult_minus3 << endl; | ||
| cout << "average number of iter " << (double)total_number_of_iter/(double)num_of_trials << endl; | ||
| cout << "min. nr of iter " << min_num_of_iter << endl; | ||
| cout << "max. nr of iter " << max_num_of_iter << endl; | ||
| cout << "min. difference after solving " << min_diff << endl; | ||
| cout << "max. difference after solving " << max_diff << endl; | ||
| cout << "min. trans. difference after solving " << min_trans_diff << endl; | ||
| cout << "max. trans. difference after solving " << max_trans_diff << endl; | ||
| cout << "min. rot. difference after solving " << min_rot_diff << endl; | ||
| cout << "max. rot. difference after solving " << max_rot_diff << endl; | ||
| double el = timer.elapsed(); | ||
| cout << "elapsed time " << el << endl; | ||
| cout << "estimate of average time per invposkin (ms)" << el/num_of_trials*1000 << endl; | ||
| cout << "estimate of longest time per invposkin (ms) " << el/total_number_of_iter*max_num_of_iter *1000 << endl; | ||
| cout << "estimate of shortest time per invposkin (ms) " << el/total_number_of_iter*min_num_of_iter *1000 << endl; | ||
| } | ||
|
|
||
| int main(int argc,char* argv[]) { | ||
| std::cout << | ||
| " This example generates random joint positions, applies a forward kinematic transformation,\n" | ||
| << " and calls ChainIkSolverPos_LMA on the resulting pose. In this way we are sure that\n" | ||
| << " the resulting pose is reachable. However, some percentage of the trials will be at\n" | ||
| << " near singular position, where it is more difficult to achieve convergence and accuracy\n" | ||
| << " The initial position given to the algorithm is also a random joint position\n" | ||
| << " This routine asks for an accuracy of 10 micrometer, one order of magnitude better than a\n" | ||
| << " a typical industrial robot.\n" | ||
| << " This routine can take more then 6 minutes to execute. It then gives statistics on execution times\n" | ||
| << " and failures.\n" | ||
| << " Typically when executed 1 000 000 times, you will still see some small amount of failures\n" | ||
| << " Typically these failures are in the neighbourhoud of singularities. Most failures of type -2 still\n" | ||
| << " reach an accuracy better than 1E-4.\n" | ||
| << " This is much better than ChainIkSolverPos_NR, which fails a few times per 100 trials.\n"; | ||
| using namespace KDL; | ||
| Chain chain; | ||
| chain = KDL::Puma560(); | ||
| //chain = KDL::KukaLWR_DHnew(); | ||
|
|
||
| ChainIkSolverPos_LMA solver(chain); | ||
|
|
||
| test_inverseposkin(chain); | ||
|
|
||
| return 0; | ||
| } | ||
|
|
||
| /** results with 1 million inv pos kin. | ||
| #times successful 999992 | ||
| #times -1 result 0 | ||
| #times -2 result 5 | ||
| #times -3 result 3 | ||
| average number of iter 16.6437 | ||
| min. nr of iter 13 | ||
| max. nr of iter 500 | ||
| min. difference after solving 3.86952e-12 | ||
| max. difference after solving 4.79339e-05 | ||
| min. trans. difference after solving 3.86952e-12 | ||
| max. trans. difference after solving 4.79339e-05 | ||
| min. rot. difference after solving 0 | ||
| max. rot. difference after solving 0.000261335 | ||
| elapsed time 199.14 | ||
| estimate of average time per invposkin (ms)0.19914 | ||
| estimate of longest time per invposkin (ms) 5.98245 | ||
| estimate of shortest time per invposkin (ms) 0.155544 | ||
| */ | ||
|
|
| @@ -0,0 +1,154 @@ | ||
| #include <frames.hpp> | ||
| #include <frames_io.hpp> | ||
|
|
||
| int main() | ||
| { | ||
|
|
||
| //Creating Vectors | ||
| KDL::Vector v1;//Default constructor | ||
| KDL::Vector v2(1.0,2.0,3.0);//Most used constructor | ||
| KDL::Vector v3(v2);//Copy constructor | ||
| KDL::Vector v4 = KDL::Vector::Zero();//Static member | ||
|
|
||
| //Use operator << to print the values of your vector | ||
| std::cout<<"v1 ="<<v1<<std::endl; | ||
| std::cout<<"v2 = "<<v2<<std::endl; | ||
| std::cout<<"v3 = "<<v3<<std::endl; | ||
| std::cout<<"v4 = "<<v4<<std::endl; | ||
|
|
||
| //Get/Set values of a vector | ||
| v1[0]=4.0; | ||
| v1[1]=5.0; | ||
| v1[2]=6.0; | ||
| v2(0)=7.0; | ||
| v2(1)=8.0; | ||
| v2(2)=9.0; | ||
| v3.x(10.0); | ||
| v3.y(11.0); | ||
| v3.z(12.0); | ||
|
|
||
| std::cout<<"v1: "<<v1[0]<<", "<<v1[1]<<", "<<v1[2]<<std::endl; | ||
| std::cout<<"v2: "<<v2(0)<<", "<<v2(1)<<", "<<v2(2)<<std::endl; | ||
| std::cout<<"v3: "<<v3.x()<<", "<<v3.y()<<", "<<v3.z()<<std::endl; | ||
|
|
||
| //double - vector operators | ||
| std::cout<<"2*v2 = "<<2*v2<<std::endl; | ||
| std::cout<<"v1*2 = "<<v1*2<<std::endl; | ||
| std::cout<<"v1/2 = "<<v1/2<<std::endl; | ||
|
|
||
| //vector - vector operators | ||
| std::cout<<"v1+v2 = "<<v1+v2<<std::endl; | ||
| std::cout<<"v3-v1 = "<<v3-v1<<std::endl; | ||
|
|
||
| v3-=v1; | ||
| v2+=v1; | ||
| std::cout<<"v3-=v1; v3 = "<<v3<<std::endl; | ||
| std::cout<<"v2+=v1; v2 = "<<v2<<std::endl; | ||
|
|
||
| //cross and dot product between two vectors | ||
| std::cout<<"cross(v1,v2) = "<<v1*v2<<std::endl; | ||
| std::cout<<"dot(v1,v2) = "<<dot(v1,v2)<<std::endl; | ||
|
|
||
| //Inversing the sign of a vector | ||
| v1=-v2; | ||
| std::cout<<"v1=-v2; v1="<<v1<<std::endl; | ||
| v1.ReverseSign(); | ||
| std::cout<<"v1.ReverseSign(); v1 = "<<v1<<std::endl; | ||
|
|
||
| //Equal operators | ||
| std::cout<<"v1==v2 ? "<<(v1==v2)<<std::endl; | ||
| std::cout<<"v1!=v2 ? "<<(v1!=v2)<<std::endl; | ||
| std::cout<<"Equal(v1,v2,1e-6) ? "<<Equal(v1,v2,1e-6)<<std::endl; | ||
|
|
||
| //Calculating the norm and normalising your vector | ||
| std::cout<<"norm(v3): "<<v3.Norm()<<std::endl; | ||
| v3.Normalize(); | ||
| std::cout<<"Normalize(v3)"<<v3<<std::endl; | ||
|
|
||
| //Setting your vector to zero | ||
| SetToZero(v1); | ||
| std::cout<<"SetToZero(v1); v1 = "<<v1<<std::endl; | ||
|
|
||
|
|
||
| //Creating Rotations: | ||
| //Default constructor | ||
| KDL::Rotation r1; | ||
| //Creating a rotation matrix out of three unit vectors Vx, Vy, | ||
| //Vz. Be careful, these vectors should be normalised and | ||
| //orthogonal. Otherwise this can result in an inconsistent | ||
| //rotation matrix | ||
| KDL::Rotation r2(KDL::Vector(0,0,1), | ||
| KDL::Vector(0,-1,0), | ||
| KDL::Vector(-1,0,0)); | ||
| //Creating a rotation matrix out of 9 values, Be careful, these | ||
| //values can result in an inconsistent rotation matrix if the | ||
| //resulting rows/columns are not orthogonal/normalized | ||
| KDL::Rotation r3(0,0,-1,1,0,0,0,-1,0); | ||
| //Creating an Identity rotation matrix | ||
| KDL::Rotation r4=KDL::Rotation::Identity(); | ||
| //Creating a Rotation matrix from a rotation around X | ||
| KDL::Rotation r5=KDL::Rotation::RotX(M_PI/3); | ||
| //Creating a Rotation matrix from a rotation around Y | ||
| KDL::Rotation r6=KDL::Rotation::RotY(M_PI/3); | ||
| //Creating a Rotation matrix from a rotation around Z | ||
| KDL::Rotation r7=KDL::Rotation::RotZ(M_PI/3); | ||
| //Creating a Rotation matrix from a rotation around a arbitrary | ||
| //vector, the vector should not be normalised | ||
| KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),M_PI/4); | ||
| //Creating a Rotation matrix from a rotation around a arbitrary | ||
| //vector, the vector should be normalised | ||
| KDL::Rotation r9=KDL::Rotation::Rot2(KDL::Vector(0.4472,0.5477,0.7071), | ||
| M_PI/4); | ||
| //Creating a Rotation matrix from Euler ZYZ rotation angles | ||
| KDL::Rotation r10=KDL::Rotation::EulerZYZ(1.,2.,3.); | ||
| //Creating a Rotation matrix from Euler ZYX rotation angles | ||
| KDL::Rotation r11=KDL::Rotation::EulerZYX(1.,2.,3.); | ||
| //Creating a Rotation matrix from Roll-Pitch-Yaw rotation angles | ||
| KDL::Rotation r12=KDL::Rotation::RPY(1.,2.,3.); | ||
|
|
||
| //Printing the rotations: | ||
| std::cout<<"r1: "<<r1<<std::endl; | ||
| std::cout<<"r2: "<<r2<<std::endl; | ||
| std::cout<<"r3: "<<r3<<std::endl; | ||
| std::cout<<"r4: "<<r4<<std::endl; | ||
| std::cout<<"r5: "<<r5<<std::endl; | ||
| std::cout<<"r6: "<<r6<<std::endl; | ||
| std::cout<<"r7: "<<r7<<std::endl; | ||
| std::cout<<"r8: "<<r8<<std::endl; | ||
| std::cout<<"r9: "<<r9<<std::endl; | ||
| std::cout<<"r10: "<<r10<<std::endl; | ||
| std::cout<<"r11: "<<r11<<std::endl; | ||
| std::cout<<"r12: "<<r12<<std::endl; | ||
|
|
||
| //Getting information out of the rotation matrix: | ||
| //The individual elements | ||
| std::cout<<"r8(1,2): "<<r8(1,2)<<std::endl; | ||
| //The equivalent rotation vector; | ||
| std::cout<<"equiv rot vector of r11: "<<r11.GetRot()<<std::endl; | ||
| //The equivalent rotation vector and angle: | ||
| double angle=r10.GetRotAngle(v1); | ||
| std::cout<<"equiv rot vector of r10:"<<v1<<"and angle: "<<angle<<std::endl; | ||
| //The Euler ZYZ angles | ||
| double alfa,beta,gamma; | ||
| r9.GetEulerZYZ(alfa,beta,gamma); | ||
| std::cout<<"EulerZYZ: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl; | ||
| //The Euler ZYZ angles | ||
| r9.GetEulerZYX(alfa,beta,gamma); | ||
| std::cout<<"EulerZYX: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl; | ||
| //The Roll-Pitch-Yaw angles | ||
| r9.GetRPY(alfa,beta,gamma); | ||
| std::cout<<"Roll-Pitch-Yaw: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl; | ||
| //The underlying unitvector X | ||
| r8.UnitX(v1);//or | ||
| std::cout<<"UnitX of r8:"<<r8.UnitX()<<std::endl; | ||
| //The underlying unitvector Y | ||
| r8.UnitY(v1);//or | ||
| std::cout<<"Unity of r8:"<<r8.UnitY()<<std::endl; | ||
| //The underlying unitvector Z | ||
| r8.UnitZ(v1);//or | ||
| std::cout<<"UnitZ of r8:"<<r8.UnitZ()<<std::endl; | ||
|
|
||
|
|
||
|
|
||
|
|
||
| } |
| @@ -0,0 +1,14 @@ | ||
| function plotframe(T,scale) | ||
| if nargin <2 | ||
| scale=0.2; | ||
| end | ||
|
|
||
|
|
||
| origin=T*[0;0;0;1]; | ||
| x = T*[scale;0;0;1]; | ||
| y = T*[0;scale;0;1]; | ||
| z = T*[0;0;scale;1]; | ||
|
|
||
| line([origin(1) x(1)],[origin(2) x(2)],[origin(3) x(3)],'color','red','linewidth',2); | ||
| line([origin(1) y(1)],[origin(2) y(2)],[origin(3) y(3)],'color','green','linewidth',2); | ||
| line([origin(1) z(1)],[origin(2) z(2)],[origin(3) z(3)],'color','blue','linewidth',2); |
| @@ -0,0 +1,117 @@ | ||
| /** | ||
| * \file path_example.cpp | ||
| * An example to demonstrate the use of trajectory generation | ||
| * functions. | ||
| * | ||
| * There are is a matlab/octave file in the examples directory to visualise the results | ||
| * of this example program. (visualize_trajectory.m) | ||
| * | ||
| */ | ||
|
|
||
| #include <frames.hpp> | ||
| #include <frames_io.hpp> | ||
| #include <trajectory.hpp> | ||
| #include <trajectory_segment.hpp> | ||
| #include <trajectory_stationary.hpp> | ||
| #include <trajectory_composite.hpp> | ||
| #include <trajectory_composite.hpp> | ||
| #include <velocityprofile_trap.hpp> | ||
| #include <path_roundedcomposite.hpp> | ||
| #include <rotational_interpolation_sa.hpp> | ||
| #include <utilities/error.h> | ||
| #include <trajectory_composite.hpp> | ||
|
|
||
| int main(int argc,char* argv[]) { | ||
| using namespace KDL; | ||
| // Create the trajectory: | ||
| // use try/catch to catch any exceptions thrown. | ||
| // NOTE: exceptions will become obsolete in a future version. | ||
| try { | ||
| // Path_RoundedComposite defines the geometric path along | ||
| // which the robot will move. | ||
| // | ||
| Path_RoundedComposite* path = new Path_RoundedComposite(0.2,0.01,new RotationalInterpolation_SingleAxis()); | ||
| // The routines are now robust against segments that are parallel. | ||
| // When the routines are parallel, no rounding is needed, and no attempt is made | ||
| // add constructing a rounding arc. | ||
| // (It is still not possible when the segments are on top of each other) | ||
| // Note that you can only rotate in a deterministic way over an angle less then M_PI! | ||
| // With an angle == M_PI, you cannot predict over which side will be rotated. | ||
| // With an angle > M_PI, the routine will rotate over 2*M_PI-angle. | ||
| // If you need to rotate over a larger angle, you need to introduce intermediate points. | ||
| // So, there is a common use case for using parallel segments. | ||
| path->Add(Frame(Rotation::RPY(M_PI,0,0), Vector(-1,0,0))); | ||
| path->Add(Frame(Rotation::RPY(M_PI/2,0,0), Vector(-0.5,0,0))); | ||
| path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0))); | ||
| path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1))); | ||
| path->Add(Frame(Rotation::RPY(0,0.7,0), Vector(1.5,0.3,0))); | ||
| path->Add(Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0))); | ||
|
|
||
| // always call Finish() at the end, otherwise the last segment will not be added. | ||
| path->Finish(); | ||
|
|
||
| // Trajectory defines a motion of the robot along a path. | ||
| // This defines a trapezoidal velocity profile. | ||
| VelocityProfile* velpref = new VelocityProfile_Trap(0.5,0.1); | ||
| velpref->SetProfile(0,path->PathLength()); | ||
| Trajectory* traject = new Trajectory_Segment(path, velpref); | ||
|
|
||
|
|
||
| Trajectory_Composite* ctraject = new Trajectory_Composite(); | ||
| ctraject->Add(traject); | ||
| ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0)))); | ||
|
|
||
|
|
||
|
|
||
| // use the trajectory | ||
| double dt=0.1; | ||
| std::ofstream of("./trajectory.dat"); | ||
| for (double t=0.0; t <= traject->Duration(); t+= dt) { | ||
| Frame current_pose; | ||
| current_pose = traject->Pos(t); | ||
| for (int i=0;i<4;++i) | ||
| for (int j=0;j<4;++j) | ||
| of << current_pose(i,j) << "\t"; | ||
| of << "\n"; | ||
| // also velocities and accelerations are available ! | ||
| //traject->Vel(t); | ||
| //traject->Acc(t); | ||
| } | ||
| of.close(); | ||
|
|
||
| // you can get some meta-info on the path: | ||
| for (int segmentnr=0; segmentnr < path->GetNrOfSegments(); segmentnr++) { | ||
| double starts,ends; | ||
| Path::IdentifierType pathtype; | ||
| if (segmentnr==0) { | ||
| starts = 0.0; | ||
| } else { | ||
| starts = path->GetLengthToEndOfSegment(segmentnr-1); | ||
| } | ||
| ends = path->GetLengthToEndOfSegment(segmentnr); | ||
| pathtype = path->GetSegment(segmentnr)->getIdentifier(); | ||
| std::cout << "segment " << segmentnr << " runs from s="<<starts << " to s=" <<ends; | ||
| switch(pathtype) { | ||
| case Path::ID_CIRCLE: | ||
| std::cout << " circle"; | ||
| break; | ||
| case Path::ID_LINE: | ||
| std::cout << " line "; | ||
| break; | ||
| default: | ||
| std::cout << " unknown "; | ||
| break; | ||
| } | ||
| std::cout << std::endl; | ||
| } | ||
| std::cout << " trajectory written to the ./trajectory.dat file " << std::endl; | ||
|
|
||
| delete ctraject; | ||
| } catch(Error& error) { | ||
| std::cout <<"I encountered this error : " << error.Description() << std::endl; | ||
| std::cout << "with the following type " << error.GetType() << std::endl; | ||
| } | ||
|
|
||
| } | ||
|
|
||
|
|
| @@ -0,0 +1,41 @@ | ||
| % visualize.m the output of the trajectory_example file. | ||
| % this file runs with Matlab or octave | ||
|
|
||
| T=load('./trajectory.dat'); | ||
| figure(1); | ||
| clf; | ||
| plot3(0,0,0); | ||
| hold on; | ||
| for i=1:size(T,1) | ||
| tf = reshape(T(i,:),4,4)'; | ||
| plotframe(tf,0.03); | ||
| end | ||
| axis equal;grid on; | ||
|
|
||
| dt=0.1; | ||
| time = (1:length(T(:,4)) )*dt; | ||
|
|
||
| figure(2); | ||
| subplot(4,1,1); | ||
| plot(time,T(:,4)); | ||
| xlabel('time [s]'); | ||
| ylabel('position x [m]'); | ||
|
|
||
| subplot(4,1,2); | ||
| plot(time,T(:,8)); | ||
| xlabel('time [s]'); | ||
| ylabel('position y [m]'); | ||
|
|
||
| subplot(4,1,3); | ||
| plot(time,T(:,12)); | ||
| xlabel('time [s]'); | ||
| ylabel('position z [m]'); | ||
|
|
||
| subplot(4,1,4); | ||
| x=diff(T(:,4)); | ||
| y=diff(T(:,8)); | ||
| z=diff(T(:,12)); | ||
| v=sqrt(x.*x + y.*y + z.*z); | ||
| plot(time(1:end-1),v); | ||
| xlabel('time [s]'); | ||
| ylabel('path velocity'); |
| @@ -0,0 +1,22 @@ | ||
| <package> | ||
| <description brief="The Kinematics and Dynamics Library (latest)"> | ||
|
|
||
| This package contains a recent version of the Kinematics and Dynamics | ||
| Library (KDL), distributed by the Orocos Project. | ||
|
|
||
| </description> | ||
| <author>Ruben Smits, Erwin Aertbelien, Orocos Developers</author> | ||
| <license>LGPL</license> | ||
| <review status="reviewed" notes=""/> | ||
| <url>http://www.orocos.org/kdl</url> | ||
| <export> | ||
| <cpp cflags="-I${prefix}/install_dir/include `pkg-config --cflags eigen3`" lflags="-Wl,-rpath,${prefix}/install_dir/lib -L${prefix}/install_dir/lib -lorocos-kdl"/> | ||
| <doxymaker external="http://www.orocos.org/kdl" /> | ||
| </export> | ||
|
|
||
| <rosdep name="cppunit"/> | ||
| <rosdep name="pkg-config"/> | ||
| <rosdep name="eigen"/> | ||
|
|
||
| </package> | ||
|
|
| @@ -0,0 +1,45 @@ | ||
| OPTION(BUILD_MODELS "Build models for some well known robots" FALSE) | ||
|
|
||
| IF(BUILD_MODELS) | ||
|
|
||
| ADD_LIBRARY(orocos-kdl-models SHARED puma560.cpp kukaLWR_DHnew.cpp) | ||
| INCLUDE_DIRECTORIES(${PROJ_SOURCE_DIR}/src ${PROJ_BINARY_DIR}/src) | ||
| SET_TARGET_PROPERTIES( orocos-kdl-models PROPERTIES | ||
| SOVERSION "${KDL_VERSION_MAJOR}.${KDL_VERSION_MINOR}" | ||
| VERSION "${KDL_VERSION}" | ||
| COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS}" | ||
| INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}" | ||
| PUBLIC_HEADER models.hpp) | ||
| TARGET_LINK_LIBRARIES(orocos-kdl-models orocos-kdl) | ||
|
|
||
| export(TARGETS orocos-kdl-models APPEND | ||
| FILE "${PROJECT_BINARY_DIR}/OrocosKDLTargets.cmake") | ||
|
|
||
| INSTALL( TARGETS orocos-kdl-models | ||
| EXPORT OrocosKDLTargets | ||
| ARCHIVE DESTINATION lib${LIB_SUFFIX} | ||
| LIBRARY DESTINATION lib${LIB_SUFFIX} | ||
| PUBLIC_HEADER DESTINATION include/kdl | ||
| ) | ||
|
|
||
| ENDIF(BUILD_MODELS) | ||
|
|
||
| INCLUDE(CMakeDependentOption) | ||
| CMAKE_DEPENDENT_OPTION(BUILD_MODELS_DEMO "Build demo for some of the models" OFF "BUILD_MODELS" OFF) | ||
| IF(BUILD_MODELS_DEMO) | ||
| ADD_EXECUTABLE(p560test puma560test.cpp) | ||
| TARGET_LINK_LIBRARIES(p560test orocos-kdl-models) | ||
| SET_TARGET_PROPERTIES( p560test PROPERTIES | ||
| COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS}") | ||
|
|
||
| ADD_EXECUTABLE(kukaLWRtestDHnew kukaLWRtestDHnew.cpp) | ||
| TARGET_LINK_LIBRARIES(kukaLWRtestDHnew orocos-kdl-models) | ||
| SET_TARGET_PROPERTIES( kukaLWRtestDHnew PROPERTIES | ||
| COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS}") | ||
|
|
||
| ADD_EXECUTABLE(kukaLWRtestHCG kukaLWRtestHCG.cpp) | ||
| TARGET_LINK_LIBRARIES(kukaLWRtestHCG orocos-kdl-models) | ||
| SET_TARGET_PROPERTIES( kukaLWRtestHCG PROPERTIES | ||
| COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS}") | ||
|
|
||
| ENDIF(BUILD_MODELS_DEMO) |
| @@ -0,0 +1,83 @@ | ||
| // Copyright (C) 2009 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> | ||
|
|
||
| // Version: 1.0 | ||
| // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> | ||
| // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> | ||
| // URL: http://www.orocos.org/kdl | ||
|
|
||
| // This library is free software; you can redistribute it and/or | ||
| // modify it under the terms of the GNU Lesser General Public | ||
| // License as published by the Free Software Foundation; either | ||
| // version 2.1 of the License, or (at your option) any later version. | ||
|
|
||
| // This library is distributed in the hope that it will be useful, | ||
| // but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | ||
| // Lesser General Public License for more details. | ||
|
|
||
| // You should have received a copy of the GNU Lesser General Public | ||
| // License along with this library; if not, write to the Free Software | ||
| // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA | ||
|
|
||
| #include <chain.hpp> | ||
| #include "models.hpp" | ||
|
|
||
| namespace KDL{ | ||
| Chain KukaLWR_DHnew(){ | ||
| Chain kukaLWR_DHnew; | ||
|
|
||
| //joint 0 | ||
| kukaLWR_DHnew.addSegment(Segment(Joint(Joint::None), | ||
| Frame::DH_Craig1989(0.0, 0.0, 0.31, 0.0) | ||
| )); | ||
| //joint 1 | ||
| kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ), | ||
| Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0), | ||
| Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2, | ||
| Vector::Zero(), | ||
| RotationalInertia(0.0,0.0,0.0115343,0.0,0.0,0.0)))); | ||
|
|
||
| //joint 2 | ||
| kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ), | ||
| Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0), | ||
| Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0).Inverse()*RigidBodyInertia(2, | ||
| Vector(0.0,-0.3120511,-0.0038871), | ||
| RotationalInertia(-0.5471572,-0.0000302,-0.5423253,0.0,0.0,0.0018828)))); | ||
|
|
||
| //joint 3 | ||
| kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ), | ||
| Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0), | ||
| Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2, | ||
| Vector(0.0,-0.0015515,0.0), | ||
| RotationalInertia(0.0063507,0.0,0.0107804,0.0,0.0,-0.0005147)))); | ||
|
|
||
| //joint 4 | ||
| kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ), | ||
| Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0), | ||
| Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0).Inverse()*RigidBodyInertia(2, | ||
| Vector(0.0,0.5216809,0.0), | ||
| RotationalInertia(-1.0436952,0.0,-1.0392780,0.0,0.0,0.0005324)))); | ||
|
|
||
| //joint 5 | ||
| kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ), | ||
| Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0), | ||
| Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2, | ||
| Vector(0.0,0.0119891,0.0), | ||
| RotationalInertia(0.0036654,0.0,0.0060429,0.0,0.0,0.0004226)))); | ||
|
|
||
| //joint 6 | ||
| kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ), | ||
| Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0), | ||
| Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2, | ||
| Vector(0.0,0.0080787,0.0), | ||
| RotationalInertia(0.0010431,0.0,0.0036376,0.0,0.0,0.0000101)))); | ||
| //joint 7 | ||
| kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ), | ||
| Frame::Identity(), | ||
| RigidBodyInertia(2, | ||
| Vector::Zero(), | ||
| RotationalInertia(0.000001,0.0,0.0001203,0.0,0.0,0.0)))); | ||
| return kukaLWR_DHnew; | ||
| } | ||
|
|
||
| } |
| @@ -0,0 +1,173 @@ | ||
| #include <chain.hpp> | ||
| #include "models.hpp" | ||
| #include <frames_io.hpp> | ||
| #include <kinfam_io.hpp> | ||
|
|
||
| #include <chainfksolverpos_recursive.hpp> | ||
| #include <chainidsolver_recursive_newton_euler.hpp> | ||
|
|
||
| using namespace KDL; | ||
| using namespace std; | ||
|
|
||
| void outputLine( double, double, double, double, double, double, double); | ||
| int getInputs(JntArray&, JntArray&, JntArray&, int&); | ||
|
|
||
| int main(int argc , char** argv){ | ||
|
|
||
| Chain kLWR=KukaLWR_DHnew(); | ||
|
|
||
| JntArray q(kLWR.getNrOfJoints()); | ||
| JntArray qdot(kLWR.getNrOfJoints()); | ||
| JntArray qdotdot(kLWR.getNrOfJoints()); | ||
| JntArray tau(kLWR.getNrOfJoints()); | ||
| Wrenches f(kLWR.getNrOfSegments()); | ||
| int linenum; //number of experiment= number of line | ||
| getInputs(q, qdot,qdotdot,linenum); | ||
|
|
||
| ChainFkSolverPos_recursive fksolver(kLWR); | ||
| Frame T; | ||
| ChainIdSolver_RNE idsolver(kLWR,Vector(0.0,0.0,-9.81)); | ||
|
|
||
| fksolver.JntToCart(q,T); | ||
| idsolver.CartToJnt(q,qdot,qdotdot,f,tau); | ||
|
|
||
| std::cout<<"pose: \n"<<T<<std::endl; | ||
| std::cout<<"tau: "<<tau<<std::endl; | ||
|
|
||
| //write file: code based on example 14.4, c++ how to program, Deitel and Deitel, book p 708 | ||
| ofstream outPoseFile("poseResultaat.dat",ios::app); | ||
| if(!outPoseFile) | ||
| { | ||
| cerr << "File poseResultaat could not be opened" <<endl; | ||
| exit(1); | ||
| } | ||
| outPoseFile << "linenumber=experimentnr= "<< linenum << "\n"; | ||
| outPoseFile << T << "\n \n"; | ||
| outPoseFile.close(); | ||
|
|
||
| ofstream outTauFile("tauResultaat.dat",ios::app); | ||
| if(!outTauFile) | ||
| { | ||
| cerr << "File tauResultaat could not be opened" <<endl; | ||
| exit(1); | ||
| } | ||
| outTauFile << setiosflags( ios::left) << setw(10) << linenum; | ||
| outTauFile << tau << "\n"; | ||
| outTauFile.close(); | ||
| } | ||
|
|
||
|
|
||
|
|
||
| int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr) | ||
| { | ||
| //cout << " q" << _q<< "\n"; | ||
|
|
||
| //declaration | ||
| //int linenr; //line =experiment number | ||
| int counter; | ||
|
|
||
| //initialisation | ||
| counter=0; | ||
|
|
||
| //ask which experiment number= line number in files | ||
| cout << "Give experiment number= line number in files \n ?"; | ||
| cin >> linenr; | ||
|
|
||
| //read files: code based on example 14.8, c++ how to program, Deitel and Deitel, book p 712 | ||
|
|
||
| /* | ||
| *READING Q = joint positions | ||
| */ | ||
|
|
||
| ifstream inQfile("interpreteerbaar/q", ios::in); | ||
|
|
||
| if (!inQfile) | ||
| { | ||
| cerr << "File q could not be opened \n"; | ||
| exit(1); | ||
| } | ||
|
|
||
| //print headers | ||
| cout << setiosflags( ios::left) << setw(15) << "_q(0)" << setw(15) << "_q(1)" << setw(15) << "_q(2)" << setw(15) << "_q(3)" << setw(15) << "_q(4)" << setw(15) << "_q(5)" << setw(15) << "_q(6)" << " \n" ; | ||
|
|
||
| while(!inQfile.eof()) | ||
| { | ||
| //read out a line of the file | ||
| inQfile >> _q(0) >> _q(1) >> _q(2) >> _q(3) >> _q(4) >> _q(5) >> _q(6); | ||
| counter++; | ||
| if(counter==linenr) | ||
| { | ||
| outputLine( _q(0), _q(1), _q(2), _q(3), _q(4), _q(5), _q(6)); | ||
| break; | ||
| } | ||
|
|
||
| } | ||
| inQfile.close(); | ||
|
|
||
| /* | ||
| *READING Qdot = joint velocities | ||
| */ | ||
| counter=0;//reset counter | ||
| ifstream inQdotfile("interpreteerbaar/qdot", ios::in); | ||
|
|
||
| if (!inQdotfile) | ||
| { | ||
| cerr << "File qdot could not be opened \n"; | ||
| exit(1); | ||
| } | ||
|
|
||
| //print headers | ||
| cout << setiosflags( ios::left) << setw(15) << "_qdot(0)" << setw(15) << "_qdot(1)" << setw(15) << "_qdot(2)" << setw(15) << "_qdot(3)" << setw(15) << "_qdot(4)" << setw(15) << "_qdot(5)" << setw(15) << "_qdot(6)" << " \n" ; | ||
|
|
||
| while(!inQdotfile.eof()) | ||
| { | ||
| //read out a line of the file | ||
| inQdotfile >> _qdot(0) >> _qdot(1) >> _qdot(2) >> _qdot(3) >> _qdot(4) >> _qdot(5) >> _qdot(6) ; | ||
| counter++; | ||
| if(counter==linenr) | ||
| { | ||
| outputLine( _qdot(0), _qdot(1), _qdot(2), _qdot(3), _qdot(4), _qdot(5), _qdot(6)); | ||
| break; | ||
| } | ||
|
|
||
| } | ||
| inQdotfile.close(); | ||
|
|
||
| /* | ||
| *READING Qdotdot = joint accelerations | ||
| */ | ||
| counter=0;//reset counter | ||
| ifstream inQdotdotfile("interpreteerbaar/qddot", ios::in); | ||
|
|
||
| if (!inQdotdotfile) | ||
| { | ||
| cerr << "File qdotdot could not be opened \n"; | ||
| exit(1); | ||
| } | ||
|
|
||
| //print headers | ||
| cout << setiosflags( ios::left) << setw(15) << "_qdotdot(0)" << setw(15) << "_qdotdot(1)" << setw(15) << "_qdotdot(2)" << setw(15) << "_qdotdot(3)" << setw(15) << "_qdotdot(4)" << setw(15) << "_qdotdot(5)" << setw(15) << "_qdotdot(6)" << " \n" ; | ||
|
|
||
| while(!inQdotdotfile.eof()) | ||
| { | ||
| //read out a line of the file | ||
| inQdotdotfile >> _qdotdot(0) >> _qdotdot(1) >> _qdotdot(2) >> _qdotdot(3) >> _qdotdot(4) >> _qdotdot(5) >> _qdotdot(6); | ||
| counter++; | ||
| if(counter==linenr) | ||
| { | ||
| outputLine(_qdotdot(0), _qdotdot(1), _qdotdot(2), _qdotdot(3), _qdotdot(4), _qdotdot(5), _qdotdot(6) ); | ||
| break; | ||
| } | ||
|
|
||
| } | ||
| inQdotdotfile.close(); | ||
|
|
||
|
|
||
| return 0; | ||
| } | ||
|
|
||
| void outputLine( double x1, double x2, double x3, double x4, double x5, double x6, double x7) | ||
| { | ||
| cout << setiosflags(ios::left) << setiosflags(ios::fixed | ios::showpoint) <<setw(15) | ||
| << x1 << setw(15) << x2 <<setw(15) <<setw(15) << x3 <<setw(15) << x4 <<setw(15) << x5 <<setw(15) << x6 <<setw(15) << x7 <<"\n"; | ||
| } |
| @@ -0,0 +1,197 @@ | ||
| #include <chain.hpp> | ||
| #include "models.hpp" | ||
| #include <frames_io.hpp> | ||
| #include <kinfam_io.hpp> //know how to print different types on screen | ||
|
|
||
| #include <chainfksolverpos_recursive.hpp> | ||
| #include <chainidsolver_recursive_newton_euler.hpp> | ||
| #include <jntspaceinertiamatrix.hpp> | ||
| #include <chaindynparam.hpp> | ||
|
|
||
| using namespace KDL; | ||
| using namespace std; | ||
|
|
||
| void outputLine( double, double, double, double, double, double, double); | ||
| int getInputs(JntArray&, JntArray&, JntArray&, int&); | ||
|
|
||
| int main(int argc , char** argv){ | ||
|
|
||
| Chain kLWR=KukaLWR_DHnew(); | ||
|
|
||
| JntArray q(kLWR.getNrOfJoints()); | ||
| JntArray qdot(kLWR.getNrOfJoints()); | ||
| JntArray qdotdot(kLWR.getNrOfJoints()); | ||
| JntArray tau(kLWR.getNrOfJoints()); | ||
| JntArray tauHCGa(kLWR.getNrOfJoints()); | ||
| JntArray tauHCG(kLWR.getNrOfJoints()); | ||
| JntArray C(kLWR.getNrOfJoints()); //coriolis matrix | ||
| JntArray G(kLWR.getNrOfJoints()); //gravity matrix | ||
| Wrenches f(kLWR.getNrOfSegments()); | ||
| Vector grav(0.0,0.0,-9.81); | ||
| JntSpaceInertiaMatrix H(kLWR.getNrOfJoints()); //inertiamatrix H=square matrix of size= number of joints | ||
| ChainDynParam chaindynparams(kLWR,grav); | ||
|
|
||
| int linenum; //number of experiment= number of line | ||
| //read out inputs from files | ||
| getInputs(q, qdot,qdotdot,linenum); | ||
|
|
||
| //calculation of torques with kukaLWRDH_new.cpp (dynamic model) | ||
| ChainFkSolverPos_recursive fksolver(kLWR); | ||
| Frame T; | ||
| ChainIdSolver_RNE idsolver(kLWR,grav); | ||
|
|
||
| fksolver.JntToCart(q,T); | ||
| idsolver.CartToJnt(q,qdot,qdotdot,f,tau); | ||
|
|
||
| std::cout<<"pose (with dynamic model): \n"<<T<<std::endl; | ||
| std::cout<<"tau (with dynamic model): \n"<<tau<<std::endl; | ||
|
|
||
| //calculation of the HCG matrices | ||
| chaindynparams.JntToMass(q,H); | ||
| chaindynparams.JntToCoriolis(q,qdot,C); | ||
| chaindynparams.JntToGravity(q,G); | ||
|
|
||
| //calculation of the torques with the HCG matrices | ||
| Multiply(H, qdotdot, tauHCG); //H*qdotdot | ||
| Add(tauHCG,C,tauHCGa); //tauHCGa=H*qdotdot+C | ||
| Add(tauHCGa,G,tauHCG); //tauHCG=H*qdotdot+C+G | ||
|
|
||
| std::cout<<"H= \n"<<H<<"\n C = \n "<<C<<"\n G= \n"<<G<<" \n tau (with HCG)= \n"<< tauHCG <<std::endl; | ||
|
|
||
| //write file: code based on example 14.4, c++ how to program, Deitel and Deitel, book p 708 | ||
| ofstream outPoseFile("poseResultaat.dat",ios::app); | ||
| if(!outPoseFile) | ||
| { | ||
| cerr << "File poseResultaat could not be opened" <<endl; | ||
| exit(1); | ||
| } | ||
| outPoseFile << "linenumber=experimentnr= "<< linenum << "\n"; | ||
| outPoseFile << T << "\n \n"; | ||
| outPoseFile.close(); | ||
|
|
||
| ofstream outTauFile("tauResultaat.dat",ios::app); | ||
| if(!outTauFile) | ||
| { | ||
| cerr << "File tauResultaat could not be opened" <<endl; | ||
| exit(1); | ||
| } | ||
| outTauFile << setiosflags( ios::left) << setw(10) << linenum; | ||
| outTauFile << tau << "\n"; | ||
| outTauFile.close(); | ||
| } | ||
|
|
||
|
|
||
|
|
||
| int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr) | ||
| { | ||
| //cout << " q" << _q<< "\n"; | ||
|
|
||
| //declaration | ||
| //int linenr; //line =experiment number | ||
| int counter; | ||
|
|
||
| //initialisation | ||
| counter=0; | ||
|
|
||
| //ask which experiment number= line number in files | ||
| cout << "Give experiment number= line number in files \n ?"; | ||
| cin >> linenr; | ||
|
|
||
| //read files: code based on example 14.8, c++ how to program, Deitel and Deitel, book p 712 | ||
|
|
||
| /* | ||
| *READING Q = joint positions | ||
| */ | ||
|
|
||
| ifstream inQfile("interpreteerbaar/q", ios::in); | ||
|
|
||
| if (!inQfile) | ||
| { | ||
| cerr << "File q could not be opened \n"; | ||
| exit(1); | ||
| } | ||
|
|
||
| //print headers | ||
| cout << setiosflags( ios::left) << setw(15) << "_q(0)" << setw(15) << "_q(1)" << setw(15) << "_q(2)" << setw(15) << "_q(3)" << setw(15) << "_q(4)" << setw(15) << "_q(5)" << setw(15) << "_q(6)" << " \n" ; | ||
|
|
||
| while(!inQfile.eof()) | ||
| { | ||
| //read out a line of the file | ||
| inQfile >> _q(0) >> _q(1) >> _q(2) >> _q(3) >> _q(4) >> _q(5) >> _q(6); | ||
| counter++; | ||
| if(counter==linenr) | ||
| { | ||
| outputLine( _q(0), _q(1), _q(2), _q(3), _q(4), _q(5), _q(6)); | ||
| break; | ||
| } | ||
|
|
||
| } | ||
| inQfile.close(); | ||
|
|
||
| /* | ||
| *READING Qdot = joint velocities | ||
| */ | ||
| counter=0;//reset counter | ||
| ifstream inQdotfile("interpreteerbaar/qdot", ios::in); | ||
|
|
||
| if (!inQdotfile) | ||
| { | ||
| cerr << "File qdot could not be opened \n"; | ||
| exit(1); | ||
| } | ||
|
|
||
| //print headers | ||
| cout << setiosflags( ios::left) << setw(15) << "_qdot(0)" << setw(15) << "_qdot(1)" << setw(15) << "_qdot(2)" << setw(15) << "_qdot(3)" << setw(15) << "_qdot(4)" << setw(15) << "_qdot(5)" << setw(15) << "_qdot(6)" << " \n" ; | ||
|
|
||
| while(!inQdotfile.eof()) | ||
| { | ||
| //read out a line of the file | ||
| inQdotfile >> _qdot(0) >> _qdot(1) >> _qdot(2) >> _qdot(3) >> _qdot(4) >> _qdot(5) >> _qdot(6) ; | ||
| counter++; | ||
| if(counter==linenr) | ||
| { | ||
| outputLine( _qdot(0), _qdot(1), _qdot(2), _qdot(3), _qdot(4), _qdot(5), _qdot(6)); | ||
| break; | ||
| } | ||
|
|
||
| } | ||
| inQdotfile.close(); | ||
|
|
||
| /* | ||
| *READING Qdotdot = joint accelerations | ||
| */ | ||
| counter=0;//reset counter | ||
| ifstream inQdotdotfile("interpreteerbaar/qddot", ios::in); | ||
|
|
||
| if (!inQdotdotfile) | ||
| { | ||
| cerr << "File qdotdot could not be opened \n"; | ||
| exit(1); | ||
| } | ||
|
|
||
| //print headers | ||
| cout << setiosflags( ios::left) << setw(15) << "_qdotdot(0)" << setw(15) << "_qdotdot(1)" << setw(15) << "_qdotdot(2)" << setw(15) << "_qdotdot(3)" << setw(15) << "_qdotdot(4)" << setw(15) << "_qdotdot(5)" << setw(15) << "_qdotdot(6)" << " \n" ; | ||
|
|
||
| while(!inQdotdotfile.eof()) | ||
| { | ||
| //read out a line of the file | ||
| inQdotdotfile >> _qdotdot(0) >> _qdotdot(1) >> _qdotdot(2) >> _qdotdot(3) >> _qdotdot(4) >> _qdotdot(5) >> _qdotdot(6); | ||
| counter++; | ||
| if(counter==linenr) | ||
| { | ||
| outputLine(_qdotdot(0), _qdotdot(1), _qdotdot(2), _qdotdot(3), _qdotdot(4), _qdotdot(5), _qdotdot(6) ); | ||
| break; | ||
| } | ||
|
|
||
| } | ||
| inQdotdotfile.close(); | ||
|
|
||
|
|
||
| return 0; | ||
| } | ||
|
|
||
| void outputLine( double x1, double x2, double x3, double x4, double x5, double x6, double x7) | ||
| { | ||
| cout << setiosflags(ios::left) << setiosflags(ios::fixed | ios::showpoint) <<setw(15) | ||
| << x1 << setw(15) << x2 <<setw(15) <<setw(15) << x3 <<setw(15) << x4 <<setw(15) << x5 <<setw(15) << x6 <<setw(15) << x7 <<"\n"; | ||
| } |
| @@ -0,0 +1,35 @@ | ||
| // Copyright (C) 2009 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> | ||
|
|
||
| // Version: 1.0 | ||
| // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> | ||
| // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> | ||
| // URL: http://www.orocos.org/kdl | ||
|
|
||
| // This library is free software; you can redistribute it and/or | ||
| // modify it under the terms of the GNU Lesser General Public | ||
| // License as published by the Free Software Foundation; either | ||
| // version 2.1 of the License, or (at your option) any later version. | ||
|
|
||
| // This library is distributed in the hope that it will be useful, | ||
| // but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | ||
| // Lesser General Public License for more details. | ||
|
|
||
| // You should have received a copy of the GNU Lesser General Public | ||
| // License along with this library; if not, write to the Free Software | ||
| // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA | ||
|
|
||
| #ifndef MODELS_HPP | ||
| #define MODELS_HPP | ||
|
|
||
| namespace KDL{ | ||
|
|
||
| class Chain; | ||
|
|
||
| Chain Puma560(); | ||
| Chain KukaLWR(); | ||
| Chain KukaLWRsegment(); | ||
| Chain KukaLWR_DHnew(); | ||
|
|
||
| } | ||
| #endif |
| @@ -0,0 +1,54 @@ | ||
| // Copyright (C) 2009 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> | ||
|
|
||
| // Version: 1.0 | ||
| // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> | ||
| // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> | ||
| // URL: http://www.orocos.org/kdl | ||
|
|
||
| // This library is free software; you can redistribute it and/or | ||
| // modify it under the terms of the GNU Lesser General Public | ||
| // License as published by the Free Software Foundation; either | ||
| // version 2.1 of the License, or (at your option) any later version. | ||
|
|
||
| // This library is distributed in the hope that it will be useful, | ||
| // but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | ||
| // Lesser General Public License for more details. | ||
|
|
||
| // You should have received a copy of the GNU Lesser General Public | ||
| // License along with this library; if not, write to the Free Software | ||
| // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA | ||
|
|
||
| #include <chain.hpp> | ||
| #include "models.hpp" | ||
|
|
||
| namespace KDL{ | ||
| Chain Puma560(){ | ||
| Chain puma560; | ||
| puma560.addSegment(Segment()); | ||
| puma560.addSegment(Segment(Joint(Joint::RotZ), | ||
| Frame::DH(0.0,M_PI_2,0.0,0.0), | ||
| RigidBodyInertia(0,Vector::Zero(),RotationalInertia(0,0.35,0,0,0,0)))); | ||
| puma560.addSegment(Segment(Joint(Joint::RotZ), | ||
| Frame::DH(0.4318,0.0,0.0,0.0), | ||
| RigidBodyInertia(17.4,Vector(-.3638,.006,.2275),RotationalInertia(0.13,0.524,0.539,0,0,0)))); | ||
| puma560.addSegment(Segment()); | ||
| puma560.addSegment(Segment(Joint(Joint::RotZ), | ||
| Frame::DH(0.0203,-M_PI_2,0.15005,0.0), | ||
| RigidBodyInertia(4.8,Vector(-.0203,-.0141,.070),RotationalInertia(0.066,0.086,0.0125,0,0,0)))); | ||
| puma560.addSegment(Segment(Joint(Joint::RotZ), | ||
| Frame::DH(0.0,M_PI_2,0.4318,0.0), | ||
| RigidBodyInertia(0.82,Vector(0,.019,0),RotationalInertia(1.8e-3,1.3e-3,1.8e-3,0,0,0)))); | ||
| puma560.addSegment(Segment()); | ||
| puma560.addSegment(Segment()); | ||
| puma560.addSegment(Segment(Joint(Joint::RotZ), | ||
| Frame::DH(0.0,-M_PI_2,0.0,0.0), | ||
| RigidBodyInertia(0.34,Vector::Zero(),RotationalInertia(.3e-3,.4e-3,.3e-3,0,0,0)))); | ||
| puma560.addSegment(Segment(Joint(Joint::RotZ), | ||
| Frame::DH(0.0,0.0,0.0,0.0), | ||
| RigidBodyInertia(0.09,Vector(0,0,.032),RotationalInertia(.15e-3,0.15e-3,.04e-3,0,0,0)))); | ||
| puma560.addSegment(Segment()); | ||
| return puma560; | ||
| } | ||
|
|
||
| } |
| @@ -0,0 +1,67 @@ | ||
| #include <chain.hpp> | ||
| #include "models.hpp" | ||
| #include <frames_io.hpp> | ||
| #include <kinfam_io.hpp> | ||
|
|
||
| #include <chainfksolverpos_recursive.hpp> | ||
| #include <chainidsolver_recursive_newton_euler.hpp> | ||
|
|
||
| using namespace KDL; | ||
|
|
||
| int main(int argc , char** argv){ | ||
|
|
||
| Chain p560=Puma560(); | ||
| //Chain p560; | ||
| // p560.addSegment(Segment(Joint(Joint::RotX),Frame::Identity(),RigidBodyInertia(1.0,Vector(0.0,1.0,.0),RotationalInertia(1.0,2.0,3.0)))); | ||
| // p560.addSegment(Segment(Joint(Joint::RotY),Frame(Rotation::Identity(),Vector(0,2,0)),RigidBodyInertia(1.0,Vector(1.0,0.0,.0),RotationalInertia(1.0,2.0,3,4,5,6)))); | ||
| // p560.addSegment(Segment(Joint(Joint::RotZ),Frame(Rotation::Identity(),Vector(2,0,0)),RigidBodyInertia(1.0,Vector(0.0,0.0,1),RotationalInertia(1.0,2.0,3,4,5,6)))); | ||
|
|
||
| JntArray q(p560.getNrOfJoints()); | ||
| JntArray qdot(p560.getNrOfJoints()); | ||
| JntArray qdotdot(p560.getNrOfJoints()); | ||
| JntArray tau(p560.getNrOfJoints()); | ||
| Wrenches f(p560.getNrOfSegments()); | ||
|
|
||
| for(unsigned int i=0;i<p560.getNrOfJoints();i++){ | ||
| q(i)=0.0; | ||
| qdot(i)=0.0; | ||
| qdotdot(i)=0.0; | ||
|
|
||
| //if(i<2) | ||
| //{ | ||
| std::cout << "give q(" << i+1 << ")\n" << std::endl; | ||
| std::cin >> q(i); | ||
| std::cout << "give qdot(" << i+1 << ")\n" << std::endl; | ||
| std::cin >> qdot(i); | ||
| std::cout << "give qdotdot(" << i << ")\n" << std::endl; | ||
| std::cin >> qdotdot(i); | ||
| //} | ||
|
|
||
| } | ||
|
|
||
| ChainFkSolverPos_recursive fksolver(p560); | ||
| Frame T; | ||
| ChainIdSolver_RNE idsolver(p560,Vector(0.0,0.0,-9.81)); | ||
|
|
||
| //#include <time.h> | ||
| //time_t before,after; | ||
| //time(&before); | ||
| //unsigned int k=0; | ||
| //for(k=0;k<1e7;k++) | ||
| fksolver.JntToCart(q,T); | ||
| //time(&after); | ||
| //std::cout<<"elapsed time for FK: "<<difftime(after,before)<<" seconds for "<<k<<" iterations"<<std::endl; | ||
| //std::cout<<"time per iteration for FK: "<<difftime(after,before)/k<<" seconds."<<std::endl; | ||
| //time(&before); | ||
| //for(k=0;k<1e7;k++) | ||
| idsolver.CartToJnt(q,qdot,qdotdot,f,tau); | ||
| //time(&after); | ||
| //std::cout<<"elapsed time for ID: "<<difftime(after,before)<<" seconds for "<<k<<" iterations"<<std::endl; | ||
| //std::cout<<"time per iteration for ID: "<<difftime(after,before)/k<<" seconds."<<std::endl; | ||
|
|
||
| std::cout<<T<<std::endl; | ||
| std::cout<<"tau: "<<tau<<std::endl; | ||
|
|
||
|
|
||
| } | ||
|
|
| @@ -0,0 +1,24 @@ | ||
| <package> | ||
| <name>orocos_kdl</name> | ||
| <version>1.4.0</version> | ||
| <description> | ||
| This package contains a recent version of the Kinematics and Dynamics | ||
| Library (KDL), distributed by the Orocos Project. | ||
| </description> | ||
| <maintainer email="ruben@intermodalics.eu">Ruben Smits</maintainer> | ||
| <url>http://wiki.ros.org/orocos_kdl</url> | ||
| <license>LGPL</license> | ||
|
|
||
| <buildtool_depend>cmake</buildtool_depend> | ||
| <build_depend>eigen</build_depend> | ||
|
|
||
| <run_depend>catkin</run_depend> | ||
| <run_depend>eigen</run_depend> | ||
| <run_depend>pkg-config</run_depend> | ||
|
|
||
| <test_depend>cppunit</test_depend> | ||
| <export> | ||
| <build_type>cmake</build_type> | ||
| </export> | ||
|
|
||
| </package> |
| @@ -0,0 +1,146 @@ | ||
| FILE( GLOB_RECURSE KDL_SRCS [^.]*.cpp [^.]*.cxx) | ||
| FILE( GLOB KDL_HPPS [^.]*.hpp [^.]*.inl) | ||
|
|
||
| FILE( GLOB UTIL_HPPS utilities/[^.]*.h utilities/[^.]*.hpp) | ||
|
|
||
| INCLUDE(CheckCXXSourceCompiles) | ||
| SET(CMAKE_REQUIRED_FLAGS) | ||
| CHECK_CXX_SOURCE_COMPILES(" | ||
| #include <string> | ||
| #include <map> | ||
| #include <vector.hpp> | ||
| class TreeElement; | ||
| typedef std::map<std::string, TreeElement> SegmentMap; | ||
| class TreeElement | ||
| { | ||
| TreeElement(const std::string& name): number(0) {} | ||
| public: | ||
| int number; | ||
| SegmentMap::const_iterator parent; | ||
| std::vector<SegmentMap::const_iterator> children; | ||
| static TreeElement Root(std::string& name) | ||
| { | ||
| return TreeElement(name); | ||
| } | ||
| }; | ||
| int main() | ||
| { | ||
| return 0; | ||
| } | ||
| " | ||
| HAVE_STL_CONTAINER_INCOMPLETE_TYPES) | ||
|
|
||
| if(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) | ||
| SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT Off) | ||
| ELSE(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) | ||
| SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT On) | ||
| ENDIF(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) | ||
|
|
||
| SET(KDL_USE_NEW_TREE_INTERFACE ${KDL_USE_NEW_TREE_INTERFACE_DEFAULT} CACHE BOOL "Use the new KDL Tree interface") | ||
|
|
||
| #Sanity check, inform the user | ||
| IF(NOT HAVE_STL_CONTAINER_INCOMPLETE_TYPES AND NOT KDL_USE_NEW_TREE_INTERFACE) | ||
| MESSAGE(WARNING "You have chosen to use the current Tree Interface, but your platform doesn't support containers of " | ||
| "incomplete types, this configuration is likely invalid") | ||
| ENDIF() | ||
|
|
||
| #In Windows (Visual Studio) it is necessary to specify the postfix | ||
| #of the debug library name and no symbols are exported by kdl, | ||
| #so it is necessary to compile it as a static library | ||
| IF(MSVC) | ||
| SET(CMAKE_DEBUG_POSTFIX "d") | ||
| SET(LIB_TYPE STATIC) | ||
| ELSE(MSVC) | ||
| SET(LIB_TYPE SHARED) | ||
| ENDIF(MSVC) | ||
|
|
||
| CONFIGURE_FILE(config.h.in config.h @ONLY) | ||
|
|
||
| #### Settings for rpath | ||
| IF(${CMAKE_MINIMUM_REQUIRED_VERSION} VERSION_GREATER "2.8.12") | ||
| MESSAGE(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed.") | ||
| ENDIF() | ||
| IF(NOT (CMAKE_VERSION VERSION_LESS 2.8.12)) | ||
| IF(NOT MSVC) | ||
| #add the option to disable RPATH | ||
| OPTION(OROCOSKDL_ENABLE_RPATH "Enable RPATH during installation" TRUE) | ||
| MARK_AS_ADVANCED(OROCOSKDL_ENABLE_RPATH) | ||
| ENDIF(NOT MSVC) | ||
|
|
||
| IF(OROCOSKDL_ENABLE_RPATH) | ||
| #Configure RPATH | ||
| SET(CMAKE_MACOSX_RPATH TRUE) #enable RPATH on OSX. This also suppress warnings on CMake >= 3.0 | ||
| # when building, don't use the install RPATH already | ||
| # (but later on when installing) | ||
| SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) | ||
| #build directory by default is built with RPATH | ||
| SET(CMAKE_SKIP_BUILD_RPATH FALSE) | ||
|
|
||
| #This is relative RPATH for libraries built in the same project | ||
| #I assume that the directory is | ||
| # - install_dir/something for binaries | ||
| # - install_dir/lib for libraries | ||
| LIST(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir) | ||
| IF("${isSystemDir}" STREQUAL "-1") | ||
| FILE(RELATIVE_PATH _rel_path "${CMAKE_INSTALL_PREFIX}/bin" "${CMAKE_INSTALL_PREFIX}/lib") | ||
| IF (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") | ||
| SET(CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}") | ||
| ELSE() | ||
| SET(CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}") | ||
| ENDIF() | ||
| ENDIF("${isSystemDir}" STREQUAL "-1") | ||
| # add the automatically determined parts of the RPATH | ||
| # which point to directories outside the build tree to the install RPATH | ||
| SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) #very important! | ||
| ENDIF() | ||
| ENDIF() | ||
| #####end RPATH | ||
|
|
||
| ADD_LIBRARY(orocos-kdl ${LIB_TYPE} ${KDL_SRCS} config.h) | ||
|
|
||
| SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES | ||
| SOVERSION "${KDL_VERSION_MAJOR}.${KDL_VERSION_MINOR}" | ||
| VERSION "${KDL_VERSION}" | ||
| COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS}" | ||
| PUBLIC_HEADER "${KDL_HPPS};${CMAKE_CURRENT_BINARY_DIR}/config.h" | ||
| ) | ||
|
|
||
| #### Settings for rpath disabled (back-compatibility) | ||
| IF(${CMAKE_MINIMUM_REQUIRED_VERSION} VERSION_GREATER "2.8.12") | ||
| MESSAGE(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed.") | ||
| ENDIF() | ||
| IF(CMAKE_VERSION VERSION_LESS 2.8.12) | ||
| SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES | ||
| INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}") | ||
| ELSE() | ||
| IF(NOT OROCOSKDL_ENABLE_RPATH) | ||
| SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES | ||
| INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}") | ||
| ENDIF() | ||
| ENDIF() | ||
| #####end RPATH | ||
|
|
||
| # Needed so that the generated config.h can be used | ||
| INCLUDE_DIRECTORIES(${CMAKE_CURRENT_BINARY_DIR}) | ||
| TARGET_LINK_LIBRARIES(orocos-kdl ${Boost_LIBRARIES}) | ||
|
|
||
| INSTALL(TARGETS orocos-kdl | ||
| EXPORT OrocosKDLTargets | ||
| ARCHIVE DESTINATION lib${LIB_SUFFIX} | ||
| LIBRARY DESTINATION lib${LIB_SUFFIX} | ||
| PUBLIC_HEADER DESTINATION include/kdl | ||
| ) | ||
|
|
||
| INSTALL(FILES ${UTIL_HPPS} DESTINATION include/kdl/utilities) | ||
|
|
||
| # Orocos convention: | ||
| CONFIGURE_FILE( kdl.pc.in ${CMAKE_CURRENT_BINARY_DIR}/orocos-kdl.pc @ONLY) | ||
| CONFIGURE_FILE( kdl.pc.in ${CMAKE_CURRENT_BINARY_DIR}/orocos_kdl.pc @ONLY) | ||
|
|
||
| INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/orocos-kdl.pc DESTINATION lib${LIB_SUFFIX}/pkgconfig) | ||
| INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/orocos_kdl.pc DESTINATION lib${LIB_SUFFIX}/pkgconfig) |
| @@ -0,0 +1,15 @@ | ||
| Coding standards : | ||
| ================= | ||
|
|
||
| - Standard naming for header file and cpp file : hpp and cpp | ||
| - KDL namespace and references. | ||
|
|
||
| Directories: | ||
| ============ | ||
|
|
||
| utilities: utility classes for KDL, should not be included separately | ||
| by a user | ||
|
|
||
| experimental: preliminary code snippets. | ||
|
|
||
| bindings: code for binding KDL with other libraries or coding-languages |
| @@ -0,0 +1,10 @@ | ||
| /** | ||
| * \brief This file contains ToDo's that do not belong in some other file. | ||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
| **/ | ||
|
|
| @@ -0,0 +1,109 @@ | ||
| // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> | ||
|
|
||
| // Version: 1.0 | ||
| // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> | ||
| // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> | ||
| // URL: http://www.orocos.org/kdl | ||
|
|
||
| // This library is free software; you can redistribute it and/or | ||
| // modify it under the terms of the GNU Lesser General Public | ||
| // License as published by the Free Software Foundation; either | ||
| // version 2.1 of the License, or (at your option) any later version. | ||
|
|
||
| // This library is distributed in the hope that it will be useful, | ||
| // but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | ||
| // Lesser General Public License for more details. | ||
|
|
||
| // You should have received a copy of the GNU Lesser General Public | ||
| // License along with this library; if not, write to the Free Software | ||
| // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA | ||
|
|
||
| #include "articulatedbodyinertia.hpp" | ||
|
|
||
| #include <Eigen/Core> | ||
|
|
||
| using namespace Eigen; | ||
|
|
||
| namespace KDL{ | ||
|
|
||
| ArticulatedBodyInertia::ArticulatedBodyInertia(const RigidBodyInertia& rbi) | ||
| { | ||
| this->M=Matrix3d::Identity()*rbi.m; | ||
| this->I=Map<const Matrix3d>(rbi.I.data); | ||
| this->H << 0,-rbi.h[2],rbi.h[1], | ||
| rbi.h[2],0,-rbi.h[0], | ||
| -rbi.h[1],rbi.h[0],0; | ||
| } | ||
|
|
||
| ArticulatedBodyInertia::ArticulatedBodyInertia(double m, const Vector& c, const RotationalInertia& Ic) | ||
| { | ||
| *this = RigidBodyInertia(m,c,Ic); | ||
| } | ||
|
|
||
| ArticulatedBodyInertia::ArticulatedBodyInertia(const Matrix3d& M, const Matrix3d& H, const Matrix3d& I) | ||
| { | ||
| this->M=M; | ||
| this->I=I; | ||
| this->H=H; | ||
| } | ||
|
|
||
| ArticulatedBodyInertia operator*(double a,const ArticulatedBodyInertia& I){ | ||
| return ArticulatedBodyInertia(a*I.M,a*I.H,a*I.I); | ||
| } | ||
|
|
||
| ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia, const ArticulatedBodyInertia& Ib){ | ||
| return ArticulatedBodyInertia(Ia.M+Ib.M,Ia.H+Ib.H,Ia.I+Ib.I); | ||
| } | ||
|
|
||
| ArticulatedBodyInertia operator+(const RigidBodyInertia& Ia, const ArticulatedBodyInertia& Ib){ | ||
| return ArticulatedBodyInertia(Ia)+Ib; | ||
| } | ||
| ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia, const ArticulatedBodyInertia& Ib){ | ||
| return ArticulatedBodyInertia(Ia.M-Ib.M,Ia.H-Ib.H,Ia.I-Ib.I); | ||
| } | ||
|
|
||
| ArticulatedBodyInertia operator-(const RigidBodyInertia& Ia, const ArticulatedBodyInertia& Ib){ | ||
| return ArticulatedBodyInertia(Ia)-Ib; | ||
| } | ||
|
|
||
| Wrench operator*(const ArticulatedBodyInertia& I,const Twist& t){ | ||
| Wrench result; | ||
| Vector3d::Map(result.force.data)=I.M*Vector3d::Map(t.vel.data)+I.H.transpose()*Vector3d::Map(t.rot.data); | ||
| Vector3d::Map(result.torque.data)=I.I*Vector3d::Map(t.rot.data)+I.H*Vector3d::Map(t.vel.data); | ||
| return result; | ||
| } | ||
|
|
||
| ArticulatedBodyInertia operator*(const Frame& T,const ArticulatedBodyInertia& I){ | ||
| Frame X=T.Inverse(); | ||
| //mb=ma | ||
| //hb=R*(h-m*r) | ||
| //Ib = R(Ia+r x h x + (h-m*r) x r x)R' | ||
| Map<Matrix3d> E(X.M.data); | ||
| Matrix3d rcross; | ||
| rcross << 0,-X.p[2],X.p[1], | ||
| X.p[2],0,-X.p[0], | ||
| -X.p[1],X.p[0],0; | ||
|
|
||
| Matrix3d HrM=I.H-rcross*I.M; | ||
| return ArticulatedBodyInertia(E*I.M*E.transpose(),E*HrM*E.transpose(),E*(I.I-rcross*I.H.transpose()+HrM*rcross)*E.transpose()); | ||
| } | ||
|
|
||
| ArticulatedBodyInertia operator*(const Rotation& M,const ArticulatedBodyInertia& I){ | ||
| Map<const Matrix3d> E(M.data); | ||
| return ArticulatedBodyInertia(E.transpose()*I.M*E,E.transpose()*I.H*E,E.transpose()*I.I*E); | ||
| } | ||
|
|
||
| ArticulatedBodyInertia ArticulatedBodyInertia::RefPoint(const Vector& p){ | ||
| //mb=ma | ||
| //hb=R*(h-m*r) | ||
| //Ib = R(Ia+r x h x + (h-m*r) x r x)R' | ||
| Matrix3d rcross; | ||
| rcross << 0,-p[2],p[1], | ||
| p[2],0,-p[0], | ||
| -p[1],p[0],0; | ||
|
|
||
| Matrix3d HrM=this->H-rcross*this->M; | ||
| return ArticulatedBodyInertia(this->M,HrM,this->I-rcross*this->H.transpose()+HrM*rcross); | ||
| } | ||
| }//namespace |