pax_global_header00006660000000000000000000000064132670477460014532gustar00rootroot0000000000000052 comment=29c4dfe1eda4c5cc033d5040cd3953043ca0b7ec orocos-kdl-1.4.0/000077500000000000000000000000001326704774600136105ustar00rootroot00000000000000orocos-kdl-1.4.0/.travis.yml000066400000000000000000000017601326704774600157250ustar00rootroot00000000000000language: cpp compiler: - gcc - clang env: - OROCOS_KDL_BUILD_TYPE=Debug - OROCOS_KDL_BUILD_TYPE=Release before_script: - sudo apt-get -qq update - sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev #build orocos_kdl - cd orocos_kdl - mkdir build - cd build - cmake -DENABLE_TESTS:BOOL=ON -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./.. # compile and install orocos_kdl - make - sudo make install - cd ../.. #build python bindings - cd python_orocos_kdl - mkdir build - cd build - cmake -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./.. #compile and install python bindings - make - sudo make install - export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib - sudo ldconfig - cd ../.. script: #execute orocos_kdl tests - cd orocos_kdl/build - make check - cd ../.. #execute python bindings tests - cd python_orocos_kdl/build - python ../tests/PyKDLtest.py orocos-kdl-1.4.0/orocos_kdl/000077500000000000000000000000001326704774600157465ustar00rootroot00000000000000orocos-kdl-1.4.0/orocos_kdl/CMakeLists.txt000066400000000000000000000102341326704774600205060ustar00rootroot00000000000000# # Test CMake version # CMAKE_MINIMUM_REQUIRED(VERSION 2.6) #MARK_AS_ADVANCED( FORCE CMAKE_BACKWARDS_COMPATIBILITY ) ################################################### # # # Start project customization section # # # ################################################### PROJECT(orocos_kdl) SET( KDL_VERSION 1.4.0) STRING( REGEX MATCHALL "[0-9]+" KDL_VERSIONS ${KDL_VERSION} ) LIST( GET KDL_VERSIONS 0 KDL_VERSION_MAJOR) LIST( GET KDL_VERSIONS 1 KDL_VERSION_MINOR) LIST( GET KDL_VERSIONS 2 KDL_VERSION_PATCH) MESSAGE( STATUS "Orocos KDL version ${VERSION} (${KDL_VERSION_MAJOR}.${KDL_VERSION_MINOR}.${KDL_VERSION_PATCH})" ) SET( PROJ_SOURCE_DIR ${orocos_kdl_SOURCE_DIR} ) SET( PROJ_BINARY_DIR ${orocos_kdl_BINARY_DIR} ) IF(NOT CMAKE_INSTALL_PREFIX) SET( CMAKE_INSTALL_PREFIX /usr/local/ CACHE PATH "Installation directory" FORCE) MESSAGE( STATUS "Setting installation directory to ${CMAKE_INSTALL_PREFIX}" ) ENDIF(NOT CMAKE_INSTALL_PREFIX) SET(CMAKE_VERSION "${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}.${CMAKE_PATCH_VERSION}") IF ( NOT CMAKE_BUILD_TYPE ) SET(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build, options are: None(CMAKE_CXX_FLAGS or CMAKE_C_FLAGS used) Debug Release RelWithDebInfo MinSizeRel." FORCE) MESSAGE( STATUS "Setting build type to '${CMAKE_BUILD_TYPE}'" ) ELSE ( NOT CMAKE_BUILD_TYPE ) MESSAGE( STATUS "Build type set to '${CMAKE_BUILD_TYPE}' by user." ) ENDIF ( NOT CMAKE_BUILD_TYPE ) SET( KDL_CFLAGS "") find_package(Eigen 3 QUIET) if(NOT Eigen_FOUND) include(${PROJ_SOURCE_DIR}/config/FindEigen3.cmake) set(Eigen_INCLUDE_DIR "${EIGEN3_INCLUDE_DIR}") endif() include_directories(${Eigen_INCLUDE_DIR}) SET(KDL_CFLAGS "${KDL_CFLAGS} -I${Eigen_INCLUDE_DIR}") # Check the platform STL containers capabilities include(config/CheckSTLContainers.cmake) CHECK_STL_CONTAINERS() # Set the default option appropriately 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) # Allow the user to select the Tree API version to use set(KDL_USE_NEW_TREE_INTERFACE ${KDL_USE_NEW_TREE_INTERFACE_DEFAULT} CACHE BOOL "Use the new KDL Tree interface") # The new interface requires the use of shared pointers if(KDL_USE_NEW_TREE_INTERFACE) # We need shared_ptr from boost since not all compilers are c++11 capable find_package(Boost REQUIRED) include_directories(${Boost_INCLUDE_DIRS}) set(KDL_INCLUDE_DIRS ${Boost_INCLUDE_DIRS}) endif(KDL_USE_NEW_TREE_INTERFACE) INCLUDE (${PROJ_SOURCE_DIR}/config/DependentOption.cmake) OPTION(ENABLE_TESTS OFF "Enable building of tests") IF( ENABLE_TESTS ) # If not in standard paths, set CMAKE_xxx_PATH's in environment, eg. # export CMAKE_INCLUDE_PATH=/opt/local/include # export CMAKE_LIBRARY_PATH=/opt/local/lib FIND_LIBRARY(CPPUNIT cppunit) SET(CPPUNIT ${CPPUNIT} "dl") FIND_PATH(CPPUNIT_HEADERS cppunit/TestRunner.h) IF ( CPPUNIT AND CPPUNIT_HEADERS) MESSAGE( STATUS "-- Looking for Cppunit - found") ELSE ( CPPUNIT AND CPPUNIT_HEADERS ) MESSAGE( FATAL_ERROR "-- Looking for Cppunit - not found") ENDIF ( CPPUNIT AND CPPUNIT_HEADERS ) ENDIF(ENABLE_TESTS ) OPTION(ENABLE_EXAMPLES OFF "Enable building of examples") ADD_SUBDIRECTORY( doc ) ADD_SUBDIRECTORY( src ) ADD_SUBDIRECTORY( tests ) ADD_SUBDIRECTORY( models ) ADD_SUBDIRECTORY( examples ) export(TARGETS orocos-kdl FILE "${PROJECT_BINARY_DIR}/OrocosKDLTargets.cmake") export(PACKAGE orocos_kdl) set(KDL_INCLUDE_DIRS ${KDL_INCLUDE_DIRS} ${Eigen_INCLUDE_DIR}) CONFIGURE_FILE(KDLConfig.cmake.in ${PROJECT_BINARY_DIR}/orocos_kdl-config.cmake @ONLY) CONFIGURE_FILE(KDLConfigVersion.cmake.in ${PROJECT_BINARY_DIR}/orocos_kdl-config-version.cmake @ONLY) INSTALL(FILES ${PROJECT_BINARY_DIR}/orocos_kdl-config.cmake DESTINATION share/orocos_kdl/cmake) INSTALL(FILES ${PROJECT_BINARY_DIR}/orocos_kdl-config-version.cmake DESTINATION share/orocos_kdl/cmake) INSTALL(EXPORT OrocosKDLTargets DESTINATION share/orocos_kdl/cmake) INSTALL(FILES package.xml DESTINATION share/orocos_kdl) orocos-kdl-1.4.0/orocos_kdl/COPYING000066400000000000000000000574771326704774600170250ustar00rootroot00000000000000 GNU LESSER GENERAL PUBLIC LICENSE Version 2.1, February 1999 Copyright (C) 1991, 1999 Free Software Foundation, Inc. 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. [This is the first released version of the Lesser GPL. It also counts as the successor of the GNU Library Public License, version 2, hence the version number 2.1.] Preamble The licenses for most software are designed to take away your freedom to share and change it. By contrast, the GNU General Public Licenses are intended to guarantee your freedom to share and change free software--to make sure the software is free for all its users. This license, the Lesser General Public License, applies to some specially designated software packages--typically libraries--of the Free Software Foundation and other authors who decide to use it. You can use it too, but we suggest you first think carefully about whether this license or the ordinary General Public License is the better strategy to use in any particular case, based on the explanations below. When we speak of free software, we are referring to freedom of use, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for this service if you wish); that you receive source code or can get it if you want it; that you can change the software and use pieces of it in new free programs; and that you are informed that you can do these things. To protect your rights, we need to make restrictions that forbid distributors to deny you these rights or to ask you to surrender these rights. These restrictions translate to certain responsibilities for you if you distribute copies of the library or if you modify it. For example, if you distribute copies of the library, whether gratis or for a fee, you must give the recipients all the rights that we gave you. You must make sure that they, too, receive or can get the source code. If you link other code with the library, you must provide complete object files to the recipients, so that they can relink them with the library after making changes to the library and recompiling it. And you must show them these terms so they know their rights. We protect your rights with a two-step method: (1) we copyright the library, and (2) we offer you this license, which gives you legal permission to copy, distribute and/or modify the library. To protect each distributor, we want to make it very clear that there is no warranty for the free library. Also, if the library is modified by someone else and passed on, the recipients should know that what they have is not the original version, so that the original author's reputation will not be affected by problems that might be introduced by others. Finally, software patents pose a constant threat to the existence of any free program. We wish to make sure that a company cannot effectively restrict the users of a free program by obtaining a restrictive license from a patent holder. Therefore, we insist that any patent license obtained for a version of the library must be consistent with the full freedom of use specified in this license. Most GNU software, including some libraries, is covered by the ordinary GNU General Public License. This license, the GNU Lesser General Public License, applies to certain designated libraries, and is quite different from the ordinary General Public License. We use this license for certain libraries in order to permit linking those libraries into non-free programs. When a program is linked with a library, whether statically or using a shared library, the combination of the two is legally speaking a combined work, a derivative of the original library. The ordinary General Public License therefore permits such linking only if the entire combination fits its criteria of freedom. The Lesser General Public License permits more lax criteria for linking other code with the library. We call this license the "Lesser" General Public License because it does Less to protect the user's freedom than the ordinary General Public License. It also provides other free software developers Less of an advantage over competing non-free programs. These disadvantages are the reason we use the ordinary General Public License for many libraries. However, the Lesser license provides advantages in certain special circumstances. For example, on rare occasions, there may be a special need to encourage the widest possible use of a certain library, so that it becomes a de-facto standard. To achieve this, non-free programs must be allowed to use the library. A more frequent case is that a free library does the same job as widely used non-free libraries. In this case, there is little to gain by limiting the free library to free software only, so we use the Lesser General Public License. In other cases, permission to use a particular library in non-free programs enables a greater number of people to use a large body of free software. For example, permission to use the GNU C Library in non-free programs enables many more people to use the whole GNU operating system, as well as its variant, the GNU/Linux operating system. Although the Lesser General Public License is Less protective of the users' freedom, it does ensure that the user of a program that is linked with the Library has the freedom and the wherewithal to run that program using a modified version of the Library. The precise terms and conditions for copying, distribution and modification follow. Pay close attention to the difference between a "work based on the library" and a "work that uses the library". The former contains code derived from the library, whereas the latter must be combined with the library in order to run. GNU LESSER GENERAL PUBLIC LICENSE TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 0. This License Agreement applies to any software library or other program which contains a notice placed by the copyright holder or other authorized party saying it may be distributed under the terms of this Lesser General Public License (also called "this License"). Each licensee is addressed as "you". A "library" means a collection of software functions and/or data prepared so as to be conveniently linked with application programs (which use some of those functions and data) to form executables. The "Library", below, refers to any such software library or work which has been distributed under these terms. A "work based on the Library" means either the Library or any derivative work under copyright law: that is to say, a work containing the Library or a portion of it, either verbatim or with modifications and/or translated straightforwardly into another language. (Hereinafter, translation is included without limitation in the term "modification".) "Source code" for a work means the preferred form of the work for making modifications to it. For a library, complete source code means all the source code for all modules it contains, plus any associated interface definition files, plus the scripts used to control compilation and installation of the library. Activities other than copying, distribution and modification are not covered by this License; they are outside its scope. The act of running a program using the Library is not restricted, and output from such a program is covered only if its contents constitute a work based on the Library (independent of the use of the Library in a tool for writing it). Whether that is true depends on what the Library does and what the program that uses the Library does. 1. You may copy and distribute verbatim copies of the Library's complete source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice and disclaimer of warranty; keep intact all the notices that refer to this License and to the absence of any warranty; and distribute a copy of this License along with the Library. You may charge a fee for the physical act of transferring a copy, and you may at your option offer warranty protection in exchange for a fee. 2. You may modify your copy or copies of the Library or any portion of it, thus forming a work based on the Library, and copy and distribute such modifications or work under the terms of Section 1 above, provided that you also meet all of these conditions: a) The modified work must itself be a software library. b) You must cause the files modified to carry prominent notices stating that you changed the files and the date of any change. c) You must cause the whole of the work to be licensed at no charge to all third parties under the terms of this License. d) If a facility in the modified Library refers to a function or a table of data to be supplied by an application program that uses the facility, other than as an argument passed when the facility is invoked, then you must make a good faith effort to ensure that, in the event an application does not supply such function or table, the facility still operates, and performs whatever part of its purpose remains meaningful. (For example, a function in a library to compute square roots has a purpose that is entirely well-defined independent of the application. Therefore, Subsection 2d requires that any application-supplied function or table used by this function must be optional: if the application does not supply it, the square root function must still compute square roots.) These requirements apply to the modified work as a whole. If identifiable sections of that work are not derived from the Library, and can be reasonably considered independent and separate works in themselves, then this License, and its terms, do not apply to those sections when you distribute them as separate works. But when you distribute the same sections as part of a whole which is a work based on the Library, the distribution of the whole must be on the terms of this License, whose permissions for other licensees extend to the entire whole, and thus to each and every part regardless of who wrote it. Thus, it is not the intent of this section to claim rights or contest your rights to work written entirely by you; rather, the intent is to exercise the right to control the distribution of derivative or collective works based on the Library. In addition, mere aggregation of another work not based on the Library with the Library (or with a work based on the Library) on a volume of a storage or distribution medium does not bring the other work under the scope of this License. 3. You may opt to apply the terms of the ordinary GNU General Public License instead of this License to a given copy of the Library. To do this, you must alter all the notices that refer to this License, so that they refer to the ordinary GNU General Public License, version 2, instead of to this License. (If a newer version than version 2 of the ordinary GNU General Public License has appeared, then you can specify that version instead if you wish.) Do not make any other change in these notices. Once this change is made in a given copy, it is irreversible for that copy, so the ordinary GNU General Public License applies to all subsequent copies and derivative works made from that copy. This option is useful when you wish to copy part of the code of the Library into a program that is not a library. 4. You may copy and distribute the Library (or a portion or derivative of it, under Section 2) in object code or executable form under the terms of Sections 1 and 2 above provided that you accompany it with the complete corresponding machine-readable source code, which must be distributed under the terms of Sections 1 and 2 above on a medium customarily used for software interchange. If distribution of object code is made by offering access to copy from a designated place, then offering equivalent access to copy the source code from the same place satisfies the requirement to distribute the source code, even though third parties are not compelled to copy the source along with the object code. 5. A program that contains no derivative of any portion of the Library, but is designed to work with the Library by being compiled or linked with it, is called a "work that uses the Library". Such a work, in isolation, is not a derivative work of the Library, and therefore falls outside the scope of this License. However, linking a "work that uses the Library" with the Library creates an executable that is a derivative of the Library (because it contains portions of the Library), rather than a "work that uses the library". The executable is therefore covered by this License. Section 6 states terms for distribution of such executables. When a "work that uses the Library" uses material from a header file that is part of the Library, the object code for the work may be a derivative work of the Library even though the source code is not. Whether this is true is especially significant if the work can be linked without the Library, or if the work is itself a library. The threshold for this to be true is not precisely defined by law. If such an object file uses only numerical parameters, data structure layouts and accessors, and small macros and small inline functions (ten lines or less in length), then the use of the object file is unrestricted, regardless of whether it is legally a derivative work. (Executables containing this object code plus portions of the Library will still fall under Section 6.) Otherwise, if the work is a derivative of the Library, you may distribute the object code for the work under the terms of Section 6. Any executables containing that work also fall under Section 6, whether or not they are linked directly with the Library itself. 6. As an exception to the Sections above, you may also combine or link a "work that uses the Library" with the Library to produce a work containing portions of the Library, and distribute that work under terms of your choice, provided that the terms permit modification of the work for the customer's own use and reverse engineering for debugging such modifications. You must give prominent notice with each copy of the work that the Library is used in it and that the Library and its use are covered by this License. You must supply a copy of this License. If the work during execution displays copyright notices, you must include the copyright notice for the Library among them, as well as a reference directing the user to the copy of this License. Also, you must do one of these things: a) Accompany the work with the complete corresponding machine-readable source code for the Library including whatever changes were used in the work (which must be distributed under Sections 1 and 2 above); and, if the work is an executable linked with the Library, with the complete machine-readable "work that uses the Library", as object code and/or source code, so that the user can modify the Library and then relink to produce a modified executable containing the modified Library. (It is understood that the user who changes the contents of definitions files in the Library will not necessarily be able to recompile the application to use the modified definitions.) b) Use a suitable shared library mechanism for linking with the Library. A suitable mechanism is one that (1) uses at run time a copy of the library already present on the user's computer system, rather than copying library functions into the executable, and (2) will operate properly with a modified version of the library, if the user installs one, as long as the modified version is interface-compatible with the version that the work was made with. c) Accompany the work with a written offer, valid for at least three years, to give the same user the materials specified in Subsection 6a, above, for a charge no more than the cost of performing this distribution. d) If distribution of the work is made by offering access to copy from a designated place, offer equivalent access to copy the above specified materials from the same place. e) Verify that the user has already received a copy of these materials or that you have already sent this user a copy. For an executable, the required form of the "work that uses the Library" must include any data and utility programs needed for reproducing the executable from it. However, as a special exception, the materials to be distributed need not include anything that is normally distributed (in either source or binary form) with the major components (compiler, kernel, and so on) of the operating system on which the executable runs, unless that component itself accompanies the executable. It may happen that this requirement contradicts the license restrictions of other proprietary libraries that do not normally accompany the operating system. Such a contradiction means you cannot use both them and the Library together in an executable that you distribute. 7. You may place library facilities that are a work based on the Library side-by-side in a single library together with other library facilities not covered by this License, and distribute such a combined library, provided that the separate distribution of the work based on the Library and of the other library facilities is otherwise permitted, and provided that you do these two things: a) Accompany the combined library with a copy of the same work based on the Library, uncombined with any other library facilities. This must be distributed under the terms of the Sections above. b) Give prominent notice with the combined library of the fact that part of it is a work based on the Library, and explaining where to find the accompanying uncombined form of the same work. 8. You may not copy, modify, sublicense, link with, or distribute the Library except as expressly provided under this License. Any attempt otherwise to copy, modify, sublicense, link with, or distribute the Library is void, and will automatically terminate your rights under this License. However, parties who have received copies, or rights, from you under this License will not have their licenses terminated so long as such parties remain in full compliance. 9. You are not required to accept this License, since you have not signed it. However, nothing else grants you permission to modify or distribute the Library or its derivative works. These actions are prohibited by law if you do not accept this License. Therefore, by modifying or distributing the Library (or any work based on the Library), you indicate your acceptance of this License to do so, and all its terms and conditions for copying, distributing or modifying the Library or works based on it. 10. Each time you redistribute the Library (or any work based on the Library), the recipient automatically receives a license from the original licensor to copy, distribute, link with or modify the Library subject to these terms and conditions. You may not impose any further restrictions on the recipients' exercise of the rights granted herein. You are not responsible for enforcing compliance by third parties with this License. 11. If, as a consequence of a court judgment or allegation of patent infringement or for any other reason (not limited to patent issues), conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot distribute so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not distribute the Library at all. For example, if a patent license would not permit royalty-free redistribution of the Library by all those who receive copies directly or indirectly through you, then the only way you could satisfy both it and this License would be to refrain entirely from distribution of the Library. If any portion of this section is held invalid or unenforceable under any particular circumstance, the balance of the section is intended to apply, and the section as a whole is intended to apply in other circumstances. It is not the purpose of this section to induce you to infringe any patents or other property right claims or to contest validity of any such claims; this section has the sole purpose of protecting the integrity of the free software distribution system which is implemented by public license practices. Many people have made generous contributions to the wide range of software distributed through that system in reliance on consistent application of that system; it is up to the author/donor to decide if he or she is willing to distribute software through any other system and a licensee cannot impose that choice. This section is intended to make thoroughly clear what is believed to be a consequence of the rest of this License. 12. If the distribution and/or use of the Library is restricted in certain countries either by patents or by copyrighted interfaces, the original copyright holder who places the Library under this License may add an explicit geographical distribution limitation excluding those countries, so that distribution is permitted only in or among countries not thus excluded. In such case, this License incorporates the limitation as if written in the body of this License. 13. The Free Software Foundation may publish revised and/or new versions of the Lesser General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns. Each version is given a distinguishing version number. If the Library specifies a version number of this License which applies to it and "any later version", you have the option of following the terms and conditions either of that version or of any later version published by the Free Software Foundation. If the Library does not specify a license version number, you may choose any version ever published by the Free Software Foundation. 14. If you wish to incorporate parts of the Library into other free programs whose distribution conditions are incompatible with these, write to the author to ask for permission. For software which is copyrighted by the Free Software Foundation, write to the Free Software Foundation; we sometimes make exceptions for this. Our decision will be guided by the two goals of preserving the free status of all derivatives of our free software and of promoting the sharing and reuse of software generally. NO WARRANTY 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. END OF TERMS AND CONDITIONS orocos-kdl-1.4.0/orocos_kdl/INSTALL000066400000000000000000000010571326704774600170020ustar00rootroot000000000000001. create a new build-directory (it is always better not to build in the src) mkdir 2. cd 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 ) 11. To install: make install [DESTDIR=...] 12.To uninstall: make uninstall orocos-kdl-1.4.0/orocos_kdl/KDLConfig.cmake.in000066400000000000000000000012541326704774600211170ustar00rootroot00000000000000# - 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") orocos-kdl-1.4.0/orocos_kdl/KDLConfigVersion.cmake.in000066400000000000000000000005671326704774600224730ustar00rootroot00000000000000set(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()orocos-kdl-1.4.0/orocos_kdl/README000066400000000000000000000004011326704774600166210ustar00rootroot00000000000000Kinematics 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. orocos-kdl-1.4.0/orocos_kdl/cmake_uninstall.cmake.in000066400000000000000000000015541326704774600225330ustar00rootroot00000000000000IF(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)orocos-kdl-1.4.0/orocos_kdl/config/000077500000000000000000000000001326704774600172135ustar00rootroot00000000000000orocos-kdl-1.4.0/orocos_kdl/config/CheckSTLContainers.cmake000066400000000000000000000024671326704774600236540ustar00rootroot00000000000000# - 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 # Author: Brian Jensen # macro(CHECK_STL_CONTAINERS) INCLUDE(CheckCXXSourceCompiles) SET(CMAKE_REQUIRED_FLAGS) CHECK_CXX_SOURCE_COMPILES(" #include #include #include class TreeElement; typedef std::map SegmentMap; class TreeElement { TreeElement(const std::string& name): number(0) {} public: int number; SegmentMap::const_iterator parent; std::vector children; static TreeElement Root(std::string& name) { return TreeElement(name); } }; int main() { return 0; } " HAVE_STL_CONTAINER_INCOMPLETE_TYPES) endmacro(CHECK_STL_CONTAINERS) orocos-kdl-1.4.0/orocos_kdl/config/DependentOption.cmake000066400000000000000000000032701326704774600233160ustar00rootroot00000000000000# - 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) orocos-kdl-1.4.0/orocos_kdl/config/FindEigen3.cmake000066400000000000000000000061611326704774600221340ustar00rootroot00000000000000# - 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, # Copyright (c) 2008, 2009 Gael Guennebaud, # Copyright (c) 2009 Benoit Jacob # 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) orocos-kdl-1.4.0/orocos_kdl/config/FindPkgConfig.cmake000066400000000000000000000100311326704774600226600ustar00rootroot00000000000000## FindPkgConfig.cmake ## by Albert Strasheim ## 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) orocos-kdl-1.4.0/orocos_kdl/debian/000077500000000000000000000000001326704774600171705ustar00rootroot00000000000000orocos-kdl-1.4.0/orocos_kdl/debian/README000066400000000000000000000002451326704774600200510ustar00rootroot00000000000000The Debian Package kdl ---------------------------- Comments regarding the Package -- Leopold Palomo Avellaneda Wed, 24 Oct 2007 13:20:52 +0200 orocos-kdl-1.4.0/orocos_kdl/debian/README.Debian000066400000000000000000000002641326704774600212330ustar00rootroot00000000000000kdl for Debian -------------- -- Leopold Palomo Avellaneda Wed, 24 Oct 2007 13:20:52 +0200 orocos-kdl-1.4.0/orocos_kdl/debian/changelog000066400000000000000000000004601326704774600210420ustar00rootroot00000000000000orocos-kdl (1.0.0) UNRELEASED; urgency=low * First major number release -- Ruben Smits Mon, 29 Sep 2008 12:23:37 +0200 orocos-kdl (0.99.0) UNRELEASED; urgency=low * Initial release. -- Ruben Smits Wed, 07 May 2008 13:49:51 +0200 orocos-kdl-1.4.0/orocos_kdl/debian/compat000066400000000000000000000000021326704774600203660ustar00rootroot000000000000005 orocos-kdl-1.4.0/orocos_kdl/debian/control000066400000000000000000000027521326704774600206010ustar00rootroot00000000000000Source: orocos-kdl Priority: extra Maintainer: Ruben Smits 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. orocos-kdl-1.4.0/orocos_kdl/debian/copyright000066400000000000000000000020711326704774600211230ustar00rootroot00000000000000This is kdl, written and maintained by the orocos development team 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'. orocos-kdl-1.4.0/orocos_kdl/debian/docs000066400000000000000000000000261326704774600200410ustar00rootroot00000000000000CMakeLists.txt README orocos-kdl-1.4.0/orocos_kdl/debian/kdl.doc-base.EX000066400000000000000000000007331326704774600216570ustar00rootroot00000000000000Document: kdl Title: Debian kdl Manual Author: 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 orocos-kdl-1.4.0/orocos_kdl/debian/liborocos-kdl-dev.install000066400000000000000000000002431326704774600240760ustar00rootroot00000000000000usr/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 orocos-kdl-1.4.0/orocos_kdl/debian/liborocos-kdl.install000066400000000000000000000000331326704774600233170ustar00rootroot00000000000000usr/lib/liborocos-kdl.so.* orocos-kdl-1.4.0/orocos_kdl/debian/manpage.1.ex000066400000000000000000000033071326704774600213000ustar00rootroot00000000000000.\" 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 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\fP and .\" \fI\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 . .PP This manual page was written by Leopold Palomo Avellaneda , for the Debian project (but may be used by others). orocos-kdl-1.4.0/orocos_kdl/debian/python-orocos-kdl.install000066400000000000000000000000461326704774600241530ustar00rootroot00000000000000usr/lib/python/site-packages/PyKDL.so orocos-kdl-1.4.0/orocos_kdl/debian/rules000077500000000000000000000062001326704774600202460ustar00rootroot00000000000000#!/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 orocos-kdl-1.4.0/orocos_kdl/debian/substvars000066400000000000000000000001251326704774600211450ustar00rootroot00000000000000shlibs:Depends=libc6 (>= 2.3.5-1), libgcc1 (>= 1:4.1.1-12), libstdc++6 (>= 4.1.1-12) orocos-kdl-1.4.0/orocos_kdl/doc/000077500000000000000000000000001326704774600165135ustar00rootroot00000000000000orocos-kdl-1.4.0/orocos_kdl/doc/CMakeLists.txt000066400000000000000000000004321326704774600212520ustar00rootroot00000000000000 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. orocos-kdl-1.4.0/orocos_kdl/doc/Doxyfile.in000066400000000000000000000217261326704774600206360ustar00rootroot00000000000000# 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 orocos-kdl-1.4.0/orocos_kdl/doc/tests.txt000066400000000000000000000053341326704774600204230ustar00rootroot00000000000000Goal 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 classes for X=Frame,Vector,Wrench,Twist,double and ALL unary and binary operations on Jacobian. - 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.. orocos-kdl-1.4.0/orocos_kdl/doc/tex/000077500000000000000000000000001326704774600173135ustar00rootroot00000000000000orocos-kdl-1.4.0/orocos_kdl/doc/tex/UserManual.tex000066400000000000000000000154221326704774600221150ustar00rootroot00000000000000\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: orocos-kdl-1.4.0/orocos_kdl/examples/000077500000000000000000000000001326704774600175645ustar00rootroot00000000000000orocos-kdl-1.4.0/orocos_kdl/examples/CMakeLists.txt000066400000000000000000000007721326704774600223320ustar00rootroot00000000000000IF(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) orocos-kdl-1.4.0/orocos_kdl/examples/README000066400000000000000000000001621326704774600204430ustar00rootroot00000000000000To build one of the examples do: g++ -I -L -lorocos-kdl -o orocos-kdl-1.4.0/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp000066400000000000000000000212531326704774600255130ustar00rootroot00000000000000/** \file chainiksolverpos_lma_demo.cpp \brief Test program for inverse position kinematics 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 */ /************************************************************************** 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 #include #include #include #include #include /** * 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 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 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 ------------------------------"< #include 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 ="< #include #include #include #include #include #include #include #include #include #include #include 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="< This package contains a recent version of the Kinematics and Dynamics Library (KDL), distributed by the Orocos Project. Ruben Smits, Erwin Aertbelien, Orocos Developers LGPL http://www.orocos.org/kdl orocos-kdl-1.4.0/orocos_kdl/models/000077500000000000000000000000001326704774600172315ustar00rootroot00000000000000orocos-kdl-1.4.0/orocos_kdl/models/CMakeLists.txt000066400000000000000000000032741326704774600217770ustar00rootroot00000000000000OPTION(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)orocos-kdl-1.4.0/orocos_kdl/models/kukaLWR_DHnew.cpp000066400000000000000000000066361326704774600223550ustar00rootroot00000000000000// Copyright (C) 2009 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 #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; } } orocos-kdl-1.4.0/orocos_kdl/models/kukaLWRtestDHnew.cpp000066400000000000000000000114621326704774600231070ustar00rootroot00000000000000#include #include "models.hpp" #include #include #include #include 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"<> 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) < #include "models.hpp" #include #include //know how to print different types on screen #include #include #include #include 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"<> 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) < // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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(); } #endiforocos-kdl-1.4.0/orocos_kdl/models/puma560.cpp000066400000000000000000000054001326704774600211310ustar00rootroot00000000000000// Copyright (C) 2009 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 #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; } } orocos-kdl-1.4.0/orocos_kdl/models/puma560test.cpp000066400000000000000000000044171326704774600220400ustar00rootroot00000000000000#include #include "models.hpp" #include #include #include #include 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> 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_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: "< orocos_kdl 1.4.0 This package contains a recent version of the Kinematics and Dynamics Library (KDL), distributed by the Orocos Project. Ruben Smits http://wiki.ros.org/orocos_kdl LGPL cmake eigen catkin eigen pkg-config cppunit cmake orocos-kdl-1.4.0/orocos_kdl/src/000077500000000000000000000000001326704774600165355ustar00rootroot00000000000000orocos-kdl-1.4.0/orocos_kdl/src/CMakeLists.txt000066400000000000000000000123421326704774600212770ustar00rootroot00000000000000FILE( 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 #include #include class TreeElement; typedef std::map SegmentMap; class TreeElement { TreeElement(const std::string& name): number(0) {} public: int number; SegmentMap::const_iterator parent; std::vector 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) orocos-kdl-1.4.0/orocos_kdl/src/README.txt000066400000000000000000000005451326704774600202370ustar00rootroot00000000000000Coding 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-languagesorocos-kdl-1.4.0/orocos_kdl/src/TODO.txt000066400000000000000000000001331326704774600200400ustar00rootroot00000000000000/** * \brief This file contains ToDo's that do not belong in some other file. **/ orocos-kdl-1.4.0/orocos_kdl/src/articulatedbodyinertia.cpp000066400000000000000000000100611326704774600237720ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 using namespace Eigen; namespace KDL{ ArticulatedBodyInertia::ArticulatedBodyInertia(const RigidBodyInertia& rbi) { this->M=Matrix3d::Identity()*rbi.m; this->I=Map(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 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 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 orocos-kdl-1.4.0/orocos_kdl/src/articulatedbodyinertia.hpp000066400000000000000000000122221326704774600240000ustar00rootroot00000000000000// Copyright (C) 2009 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_ARTICULATEDBODYINERTIA_HPP #define KDL_ARTICULATEDBODYINERTIA_HPP #include "frames.hpp" #include "rotationalinertia.hpp" #include "rigidbodyinertia.hpp" #include namespace KDL { /** * \brief 6D Inertia of a articulated body * * The inertia is defined in a certain reference point and a certain reference base. * The reference point does not have to coincide with the origin of the reference frame. */ class ArticulatedBodyInertia{ public: /** * This constructor creates a zero articulated body inertia matrix, */ ArticulatedBodyInertia(){ *this=ArticulatedBodyInertia::Zero(); } /** * This constructor creates a cartesian space articulated body inertia matrix, * the arguments is a rigid body inertia. */ ArticulatedBodyInertia(const RigidBodyInertia& rbi); /** * This constructor creates a cartesian space inertia matrix, * the arguments are the mass, the vector from the reference point to cog and the rotational inertia in the cog. */ explicit ArticulatedBodyInertia(double m, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero()); /** * Creates an inertia with zero mass, and zero RotationalInertia */ static inline ArticulatedBodyInertia Zero(){ return ArticulatedBodyInertia(Eigen::Matrix3d::Zero(),Eigen::Matrix3d::Zero(),Eigen::Matrix3d::Zero()); }; ~ArticulatedBodyInertia(){}; friend ArticulatedBodyInertia operator*(double a,const ArticulatedBodyInertia& I); friend ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib); friend ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib); friend ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib); friend ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib); friend Wrench operator*(const ArticulatedBodyInertia& I,const Twist& t); friend ArticulatedBodyInertia operator*(const Frame& T,const ArticulatedBodyInertia& I); friend ArticulatedBodyInertia operator*(const Rotation& R,const ArticulatedBodyInertia& I); /** * Reference point change with v the vector from the old to * the new point expressed in the current reference frame */ ArticulatedBodyInertia RefPoint(const Vector& p); ArticulatedBodyInertia(const Eigen::Matrix3d& M,const Eigen::Matrix3d& H,const Eigen::Matrix3d& I); Eigen::Matrix3d M; Eigen::Matrix3d H; Eigen::Matrix3d I; }; /** * Scalar product: I_new = double * I_old */ ArticulatedBodyInertia operator*(double a,const ArticulatedBodyInertia& I); /** * addition I: I_new = I_old1 + I_old2, make sure that I_old1 * and I_old2 are expressed in the same reference frame/point, * otherwise the result is worth nothing */ ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib); ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib); ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib); ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib); /** * calculate spatial momentum: h = I*v * make sure that the twist v and the inertia are expressed in the same reference frame/point */ Wrench operator*(const ArticulatedBodyInertia& I,const Twist& t); /** * Coordinate system transform Ia = T_a_b*Ib with T_a_b the frame from a to b. */ ArticulatedBodyInertia operator*(const Frame& T,const ArticulatedBodyInertia& I); /** * Reference frame orientation change Ia = R_a_b*Ib with R_a_b * the rotation of b expressed in a */ ArticulatedBodyInertia operator*(const Rotation& R,const ArticulatedBodyInertia& I); } #endif orocos-kdl-1.4.0/orocos_kdl/src/chain.cpp000066400000000000000000000043421326704774600203260ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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" namespace KDL { using namespace std; Chain::Chain(): nrOfJoints(0), nrOfSegments(0), segments(0) { } Chain::Chain(const Chain& in): nrOfJoints(0), nrOfSegments(0), segments(0) { for(unsigned int i=0;iaddSegment(in.getSegment(i)); } Chain& Chain::operator=(const Chain& arg) { nrOfJoints=0; nrOfSegments=0; segments.resize(0); for(unsigned int i=0;iaddSegment(chain.getSegment(i)); } const Segment& Chain::getSegment(unsigned int nr)const { return segments[nr]; } Segment& Chain::getSegment(unsigned int nr) { return segments[nr]; } Chain::~Chain() { } } orocos-kdl-1.4.0/orocos_kdl/src/chain.hpp000066400000000000000000000065111326704774600203330ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_CHAIN_HPP #define KDL_CHAIN_HPP #include "segment.hpp" #include namespace KDL { /** * \brief This class encapsulates a serial kinematic * interconnection structure. It is built out of segments. * * @ingroup KinematicFamily */ class Chain { private: unsigned int nrOfJoints; unsigned int nrOfSegments; public: std::vector segments; /** * The constructor of a chain, a new chain is always empty. * */ Chain(); Chain(const Chain& in); Chain& operator = (const Chain& arg); /** * Adds a new segment to the end of the chain. * * @param segment The segment to add */ void addSegment(const Segment& segment); /** * Adds a complete chain to the end of the chain * The added chain is copied. * * @param chain The chain to add */ void addChain(const Chain& chain); /** * Request the total number of joints in the chain.\n * Important: It is not the * same as the total number of segments since a segment does not * need to have a joint. This function is important when * creating a KDL::JntArray to use with this chain. * @return total nr of joints */ unsigned int getNrOfJoints()const {return nrOfJoints;}; /** * Request the total number of segments in the chain. * @return total number of segments */ unsigned int getNrOfSegments()const {return nrOfSegments;}; /** * Request the nr'd segment of the chain. There is no boundary * checking. * * @param nr the nr of the segment starting from 0 * * @return a constant reference to the nr'd segment */ const Segment& getSegment(unsigned int nr)const; /** * Request the nr'd segment of the chain. There is no boundary * checking. * * @param nr the nr of the segment starting from 0 * * @return a reference to the nr'd segment */ Segment& getSegment(unsigned int nr); virtual ~Chain(); }; }//end of namespace KDL #endif orocos-kdl-1.4.0/orocos_kdl/src/chaindynparam.cpp000066400000000000000000000112511326704774600220570ustar00rootroot00000000000000// Copyright (C) 2009 Dominick Vanthienen // Version: 1.0 // Author: Dominick Vanthienen // Maintainer: Ruben Smits // 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 "chaindynparam.hpp" #include "frames_io.hpp" #include namespace KDL { ChainDynParam::ChainDynParam(const Chain& _chain, Vector _grav): chain(_chain), nr(0), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), grav(_grav), jntarraynull(nj), chainidsolver_coriolis( chain, Vector::Zero()), chainidsolver_gravity( chain, grav), wrenchnull(ns,Wrench::Zero()), X(ns), S(ns), Ic(ns) { ag=-Twist(grav,Vector::Zero()); } void ChainDynParam::updateInternalDataStructures() { nj = chain.getNrOfJoints(); ns = chain.getNrOfSegments(); jntarraynull.resize(nj); chainidsolver_coriolis.updateInternalDataStructures(); chainidsolver_gravity.updateInternalDataStructures(); wrenchnull.resize(ns,Wrench::Zero()); X.resize(ns); S.resize(ns); Ic.resize(ns); } //calculate inertia matrix H int ChainDynParam::JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H) { if(nj != chain.getNrOfJoints() || ns != chain.getNrOfSegments()) return (error = E_NOT_UP_TO_DATE); //Check sizes when in debug mode if(q.rows()!=nj || H.rows()!=nj || H.columns()!=nj ) return (error = E_SIZE_MISMATCH); unsigned int k=0; double q_; //Sweep from root to leaf for(unsigned int i=0;i=0;i--) { if(i!=0) { //assumption that previous segment is parent Ic[i-1]=Ic[i-1]+X[i]*Ic[i]; } F=Ic[i]*S[i]; if(chain.getSegment(i).getJoint().getType()!=Joint::None) { H(k,k)=dot(S[i],F); j=k; //countervariable for the joints l=i; //countervariable for the segments while(l!=0) //go from leaf to root starting at i { //assumption that previous segment is parent F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l] l--; //go down a segment if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint { j--; H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment H(j,k)=H(k,j); } } k--; //this if-loop should be repeated nj times (k=nj-1 to k=0) } } return (error = E_NOERROR); } //calculate coriolis matrix C int ChainDynParam::JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis) { //make a null matrix with the size of q_dotdot and a null wrench SetToZero(jntarraynull); //the calculation of coriolis matrix C return chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis); } //calculate gravity matrix G int ChainDynParam::JntToGravity(const JntArray &q,JntArray &gravity) { //make a null matrix with the size of q_dotdot and a null wrench SetToZero(jntarraynull); //the calculation of coriolis matrix C return chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity); } ChainDynParam::~ChainDynParam() { } } orocos-kdl-1.4.0/orocos_kdl/src/chaindynparam.hpp000066400000000000000000000057131326704774600220720ustar00rootroot00000000000000// Copyright (C) 2009 Dominick Vanthienen // Version: 1.0 // Author: Dominick Vanthienen // Maintainer: Ruben Smits // 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 KDLCHAINDYNPARAM_HPP #define KDLCHAINDYNPARAM_HPP #include "chainidsolver_recursive_newton_euler.hpp" #include "articulatedbodyinertia.hpp" #include "jntspaceinertiamatrix.hpp" #include namespace KDL { /** * Implementation of a method to calculate the matrices H (inertia),C(coriolis) and G(gravitation) * for the calculation torques out of the pose and derivatives. * (inverse dynamics) * * The algorithm implementation for H is based on the book "Rigid Body * Dynamics Algorithms" of Roy Featherstone, 2008 * (ISBN:978-0-387-74314-1) See page 107 for the pseudo-code. * This algorithm is extended for the use of fixed joints * * It calculates the joint-space inertia matrix, given the motion of * the joints (q,qdot,qdotdot), external forces on the segments * (expressed in the segments reference frame) and the dynamical * parameters of the segments. */ class ChainDynParam : SolverI { public: ChainDynParam(const Chain& chain, Vector _grav); virtual ~ChainDynParam(); virtual int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis); virtual int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H); virtual int JntToGravity(const JntArray &q,JntArray &gravity); /// @copydoc KDL::SolverI::updateInternalDataStructures() virtual void updateInternalDataStructures(); private: const Chain& chain; int nr; unsigned int nj; unsigned int ns; Vector grav; Vector vectornull; JntArray jntarraynull; ChainIdSolver_RNE chainidsolver_coriolis; ChainIdSolver_RNE chainidsolver_gravity; std::vector wrenchnull; std::vector X; std::vector S; //std::vector I; std::vector > Ic; Wrench F; Twist ag; }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/chainfksolver.hpp000066400000000000000000000121561326704774600221110ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_CHAIN_FKSOLVER_HPP #define KDL_CHAIN_FKSOLVER_HPP #include "chain.hpp" #include "framevel.hpp" #include "frameacc.hpp" #include "jntarray.hpp" #include "jntarrayvel.hpp" #include "jntarrayacc.hpp" #include "solveri.hpp" namespace KDL { /** * \brief This abstract class encapsulates a * solver for the forward position kinematics for a KDL::Chain. * * @ingroup KinematicFamily */ //Forward definition class ChainFkSolverPos : public KDL::SolverI { public: /** * Calculate forward position kinematics for a KDL::Chain, * from joint coordinates to cartesian pose. * * @param q_in input joint coordinates * @param p_out reference to output cartesian pose * * @return if < 0 something went wrong */ virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1)=0; /** * Calculate forward position kinematics for a KDL::Chain, * from joint coordinates to cartesian pose. * * @param q_in input joint coordinates * @param p_out reference to a vector of output cartesian poses for all segments * * @return if < 0 something went wrong */ virtual int JntToCart(const JntArray& q_in, std::vector& p_out,int segmentNr=-1)=0; virtual void updateInternalDataStructures()=0; virtual ~ChainFkSolverPos(){}; }; /** * \brief This abstract class encapsulates a solver * for the forward velocity kinematics for a KDL::Chain. * * @ingroup KinematicFamily */ class ChainFkSolverVel : public KDL::SolverI { public: /** * Calculate forward position and velocity kinematics, from * joint coordinates to cartesian coordinates. * * @param q_in input joint coordinates (position and velocity) * @param out output cartesian coordinates (position and velocity) * * @return if < 0 something went wrong */ virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0; /** * Calculate forward position and velocity kinematics, from * joint coordinates to cartesian coordinates. * * @param q_in input joint coordinates (position and velocity) * @param out output cartesian coordinates for all segments (position and velocity) * * @return if < 0 something went wrong */ virtual int JntToCart(const JntArrayVel& q_in, std::vector& out,int segmentNr=-1)=0; virtual void updateInternalDataStructures()=0; virtual ~ChainFkSolverVel(){}; }; /** * \brief This abstract class encapsulates a solver * for the forward acceleration kinematics for a KDL::Chain. * * @ingroup KinematicFamily */ class ChainFkSolverAcc : public KDL::SolverI { public: /** * Calculate forward position, velocity and accelaration * kinematics, from joint coordinates to cartesian coordinates * * @param q_in input joint coordinates (position, velocity and * acceleration @param out output cartesian coordinates (position, velocity * and acceleration * * @return if < 0 something went wrong */ virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0; /** * Calculate forward position, velocity and accelaration * kinematics, from joint coordinates to cartesian coordinates * * @param q_in input joint coordinates (position, velocity and * acceleration @param out output cartesian coordinates (position, velocity * and acceleration for all segments * * @return if < 0 something went wrong */ virtual int JntToCart(const JntArrayAcc& q_in, std::vector& out,int segmentNr=-1)=0; virtual void updateInternalDataStructures()=0; virtual ~ChainFkSolverAcc()=0; }; }//end of namespace KDL #endif orocos-kdl-1.4.0/orocos_kdl/src/chainfksolverpos_recursive.cpp000066400000000000000000000066301326704774600247150ustar00rootroot00000000000000// Copyright (C) 2007 Francois Cauwe // Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "chainfksolverpos_recursive.hpp" #include namespace KDL { ChainFkSolverPos_recursive::ChainFkSolverPos_recursive(const Chain& _chain): chain(_chain) { } int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int seg_nr) { unsigned int segmentNr; if(seg_nr<0) segmentNr=chain.getNrOfSegments(); else segmentNr = seg_nr; p_out = Frame::Identity(); if(q_in.rows()!=chain.getNrOfJoints()) return (error = E_SIZE_MISMATCH); else if(segmentNr>chain.getNrOfSegments()) return (error = E_OUT_OF_RANGE); else{ int j=0; for(unsigned int i=0;i& p_out, int seg_nr) { unsigned int segmentNr; if(seg_nr<0) segmentNr=chain.getNrOfSegments(); else segmentNr = seg_nr; if(q_in.rows()!=chain.getNrOfJoints()) return -1; else if(segmentNr>chain.getNrOfSegments()) return -1; else if(p_out.size() != segmentNr) return -1; else if(segmentNr == 0) return -1; else{ int j=0; // Initialization if(chain.getSegment(0).getJoint().getType()!=Joint::None){ p_out[0] = chain.getSegment(0).pose(q_in(j)); j++; }else p_out[0] = chain.getSegment(0).pose(0.0); for(unsigned int i=1;i // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDLCHAINFKSOLVERPOS_RECURSIVE_HPP #define KDLCHAINFKSOLVERPOS_RECURSIVE_HPP #include "chainfksolver.hpp" namespace KDL { /** * Implementation of a recursive forward position kinematics * algorithm to calculate the position transformation from joint * space to Cartesian space of a general kinematic chain (KDL::Chain). * * @ingroup KinematicFamily */ class ChainFkSolverPos_recursive : public ChainFkSolverPos { public: ChainFkSolverPos_recursive(const Chain& chain); ~ChainFkSolverPos_recursive(); virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1); virtual int JntToCart(const JntArray& q_in, std::vector& p_out, int segmentNr=-1); virtual void updateInternalDataStructures() {}; private: const Chain& chain; }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/chainfksolvervel_recursive.cpp000066400000000000000000000102171326704774600246760ustar00rootroot00000000000000// Copyright (C) 2007 Francois Cauwe // Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "chainfksolvervel_recursive.hpp" namespace KDL { ChainFkSolverVel_recursive::ChainFkSolverVel_recursive(const Chain& _chain): chain(_chain) { } ChainFkSolverVel_recursive::~ChainFkSolverVel_recursive() { } int ChainFkSolverVel_recursive::JntToCart(const JntArrayVel& in,FrameVel& out,int seg_nr) { unsigned int segmentNr; if(seg_nr<0) segmentNr=chain.getNrOfSegments(); else segmentNr = seg_nr; out=FrameVel::Identity(); if(!(in.q.rows()==chain.getNrOfJoints()&&in.qdot.rows()==chain.getNrOfJoints())) return (error = E_SIZE_MISMATCH); else if(segmentNr>chain.getNrOfSegments()) return (error = E_OUT_OF_RANGE); else{ int j=0; for (unsigned int i=0;i& out,int seg_nr) { unsigned int segmentNr; if(seg_nr<0) segmentNr=chain.getNrOfSegments(); else segmentNr = seg_nr; if(!(in.q.rows()==chain.getNrOfJoints()&&in.qdot.rows()==chain.getNrOfJoints())) return -1; else if(segmentNr>chain.getNrOfSegments()) return -1; else if(out.size()!=segmentNr) return -1; else if(segmentNr == 0) return -1; else{ int j=0; // Initialization if(chain.getSegment(0).getJoint().getType()!=Joint::None){ out[0] = FrameVel(chain.getSegment(0).pose(in.q(0)), chain.getSegment(0).twist(in.q(0),in.qdot(0))); j++; }else out[0] = FrameVel(chain.getSegment(0).pose(0.0), chain.getSegment(0).twist(0.0,0.0)); for (unsigned int i=1;i // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_CHAIN_FKSOLVERVEL_RECURSIVE_HPP #define KDL_CHAIN_FKSOLVERVEL_RECURSIVE_HPP #include "chainfksolver.hpp" namespace KDL { /** * Implementation of a recursive forward position and velocity * kinematics algorithm to calculate the position and velocity * transformation from joint space to Cartesian space of a general * kinematic chain (KDL::Chain). * * @ingroup KinematicFamily */ class ChainFkSolverVel_recursive : public ChainFkSolverVel { public: ChainFkSolverVel_recursive(const Chain& chain); ~ChainFkSolverVel_recursive(); virtual int JntToCart(const JntArrayVel& q_in,FrameVel& out,int segmentNr=-1); virtual int JntToCart(const JntArrayVel& q_in,std::vector& out,int segmentNr=-1); virtual void updateInternalDataStructures() {}; private: const Chain& chain; }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/chainidsolver.hpp000066400000000000000000000040021326704774600220740ustar00rootroot00000000000000// Copyright (C) 2009 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_CHAIN_IDSOLVER_HPP #define KDL_CHAIN_IDSOLVER_HPP #include "chain.hpp" #include "frames.hpp" #include "jntarray.hpp" #include "solveri.hpp" namespace KDL { typedef std::vector Wrenches; /** * \brief This abstract class encapsulates the inverse * dynamics solver for a KDL::Chain. * */ class ChainIdSolver : public KDL::SolverI { public: /** * Calculate inverse dynamics, from joint positions, velocity, acceleration, external forces * to joint torques/forces. * * @param q input joint positions * @param q_dot input joint velocities * @param q_dotdot input joint accelerations * * @param torque output joint torques * * @return if < 0 something went wrong */ virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques)=0; // Need functions to return the manipulator mass, coriolis and gravity matrices - Lagrangian Formulation. }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp000066400000000000000000000100151326704774600267450ustar00rootroot00000000000000// Copyright (C) 2009 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "chainidsolver_recursive_newton_euler.hpp" #include "frames_io.hpp" namespace KDL{ ChainIdSolver_RNE::ChainIdSolver_RNE(const Chain& chain_,Vector grav): chain(chain_),nj(chain.getNrOfJoints()),ns(chain.getNrOfSegments()), X(ns),S(ns),v(ns),a(ns),f(ns) { ag=-Twist(grav,Vector::Zero()); } void ChainIdSolver_RNE::updateInternalDataStructures() { nj = chain.getNrOfJoints(); ns = chain.getNrOfSegments(); X.resize(ns); S.resize(ns); v.resize(ns); a.resize(ns); f.resize(ns); } int ChainIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques) { if(nj != chain.getNrOfJoints() || ns != chain.getNrOfSegments()) return (error = E_NOT_UP_TO_DATE); //Check sizes when in debug mode if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns) return (error = E_SIZE_MISMATCH); unsigned int j=0; //Sweep from root to leaf for(unsigned int i=0;i=0;i--){ if(chain.getSegment(i).getJoint().getType()!=Joint::None) torques(j--)=dot(S[i],f[i]); if(i!=0) f[i-1]=f[i-1]+X[i]*f[i]; } return (error = E_NOERROR); } }//namespace orocos-kdl-1.4.0/orocos_kdl/src/chainidsolver_recursive_newton_euler.hpp000066400000000000000000000060621326704774600267610ustar00rootroot00000000000000// Copyright (C) 2009 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_CHAIN_IKSOLVER_RECURSIVE_NEWTON_EULER_HPP #define KDL_CHAIN_IKSOLVER_RECURSIVE_NEWTON_EULER_HPP #include "chainidsolver.hpp" namespace KDL{ /** * \brief Recursive newton euler inverse dynamics solver * * The algorithm implementation is based on the book "Rigid Body * Dynamics Algorithms" of Roy Featherstone, 2008 * (ISBN:978-0-387-74314-1) See page 96 for the pseudo-code. * * It calculates the torques for the joints, given the motion of * the joints (q,qdot,qdotdot), external forces on the segments * (expressed in the segments reference frame) and the dynamical * parameters of the segments. */ class ChainIdSolver_RNE : public ChainIdSolver{ public: /** * Constructor for the solver, it will allocate all the necessary memory * \param chain The kinematic chain to calculate the inverse dynamics for, an internal copy will be made. * \param grav The gravity vector to use during the calculation. */ ChainIdSolver_RNE(const Chain& chain,Vector grav); ~ChainIdSolver_RNE(){}; /** * Function to calculate from Cartesian forces to joint torques. * Input parameters; * \param q The current joint positions * \param q_dot The current joint velocities * \param q_dotdot The current joint accelerations * \param f_ext The external forces (no gravity) on the segments * Output parameters: * \param torques the resulting torques for the joints */ int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques); /// @copydoc KDL::SolverI::updateInternalDataStructures virtual void updateInternalDataStructures(); private: const Chain& chain; unsigned int nj; unsigned int ns; std::vector X; std::vector S; std::vector v; std::vector a; std::vector f; Twist ag; }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/chainidsolver_vereshchagin.cpp000066400000000000000000000425671326704774600246370ustar00rootroot00000000000000// Copyright (C) 2009, 2011 // Version: 1.0 // Author: Ruben Smits, Herman Bruyninckx, Azamat Shakhimardanov // Maintainer: Ruben Smits, Azamat Shakhimardanov // 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 "chainidsolver_vereshchagin.hpp" #include "frames_io.hpp" #include "utilities/svd_eigen_HH.hpp" namespace KDL { using namespace Eigen; ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin(const Chain& chain_, Twist root_acc, unsigned int _nc) : chain(chain_), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), nc(_nc), results(ns + 1, segment_info(nc)) { acc_root = root_acc; //Provide the necessary memory for computing the inverse of M0 nu_sum.resize(nc); M_0_inverse.resize(nc, nc); Um = MatrixXd::Identity(nc, nc); Vm = MatrixXd::Identity(nc, nc); Sm = VectorXd::Ones(nc); tmpm = VectorXd::Ones(nc); } void ChainIdSolver_Vereshchagin::updateInternalDataStructures() { ns = chain.getNrOfSegments(); results.resize(ns+1,segment_info(nc)); } int ChainIdSolver_Vereshchagin::CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques) { nj = chain.getNrOfJoints(); if(ns != chain.getNrOfSegments()) return (error = E_NOT_UP_TO_DATE); //Check sizes always if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || torques.rows() != nj || f_ext.size() != ns) return (error = E_SIZE_MISMATCH); if (alfa.columns() != nc || beta.rows() != nc) return (error = E_SIZE_MISMATCH); //do an upward recursion for position and velocities this->initial_upwards_sweep(q, q_dot, q_dotdot, f_ext); //do an inward recursion for inertia, forces and constraints this->downwards_sweep(alfa, torques); //Solve for the constraint forces this->constraint_calculation(beta); //do an upward recursion to propagate the result this->final_upwards_sweep(q_dotdot, torques); return (error = E_NOERROR); } void ChainIdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const JntArray &qdot, const JntArray &qdotdot, const Wrenches& f_ext) { //if (q.rows() != nj || qdot.rows() != nj || qdotdot.rows() != nj || f_ext.size() != ns) // return -1; unsigned int j = 0; F_total = Frame::Identity(); for (unsigned int i = 0; i < ns; i++) { //Express everything in the segments reference frame (body coordinates) //which is at the segments tip, i.e. where the next joint is attached. //Calculate segment properties: X,S,vj,cj const Segment& segment = chain.getSegment(i); segment_info& s = results[i + 1]; //The pose between the joint root and the segment tip (tip expressed in joint root coordinates) s.F = segment.pose(q(j)); //X pose of each link in link coord system F_total = F_total * s.F; //X pose of the each link in root coord system s.F_base = F_total; //X pose of the each link in root coord system for getter functions //The velocity due to the joint motion of the segment expressed in the segments reference frame (tip) Twist vj = s.F.M.Inverse(segment.twist(q(j), qdot(j))); //XDot of each link //Twist aj = s.F.M.Inverse(segment.twist(q(j), qdotdot(j))); //XDotDot of each link //The unit velocity due to the joint motion of the segment expressed in the segments reference frame (tip) s.Z = s.F.M.Inverse(segment.twist(q(j), 1.0)); //Put Z in the joint root reference frame: s.Z = s.F * s.Z; //The total velocity of the segment expressed in the segments reference frame (tip) if (i != 0) { s.v = s.F.Inverse(results[i].v) + vj; // recursive velocity of each link in segment frame //s.A=s.F.Inverse(results[i].A)+aj; s.A = s.F.M.Inverse(results[i].A); } else { s.v = vj; s.A = s.F.M.Inverse(acc_root); } //c[i] = cj + v[i]xvj (remark: cj=0, since our S is not time dependent in local coordinates) //The velocity product acceleration //std::cout << i << " Initial upward" << s.v << std::endl; s.C = s.v*vj; //This is a cross product: cartesian space BIAS acceleration in local link coord. //Put C in the joint root reference frame s.C = s.F * s.C; //+F_total.M.Inverse(acc_root)); //The rigid body inertia of the segment, expressed in the segments reference frame (tip) s.H = segment.getInertia(); //wrench of the rigid body bias forces and the external forces on the segment (in body coordinates, tip) //external forces are taken into account through s.U. Wrench FextLocal = F_total.M.Inverse() * f_ext[i]; s.U = s.v * (s.H * s.v) - FextLocal; //f_ext[i]; if (segment.getJoint().getType() != Joint::None) j++; } } void ChainIdSolver_Vereshchagin::downwards_sweep(const Jacobian& alfa, const JntArray &torques) { int j = nj - 1; for (int i = ns; i >= 0; i--) { //Get a handle for the segment we are working on. segment_info& s = results[i]; //For segment N, //tilde is in the segment refframe (tip, not joint root) //without tilde is at the joint root (the childs tip!!!) //P_tilde is the articulated body inertia //R_tilde is the sum of external and coriolis/centrifugal forces //M is the (unit) acceleration energy already generated at link i //G is the (unit) magnitude of the constraint forces at link i //E are the (unit) constraint forces due to the constraints if (i == (int)ns) { s.P_tilde = s.H; s.R_tilde = s.U; s.M.setZero(); s.G.setZero(); //changeBase(alfa_N,F_total.M.Inverse(),alfa_N2); for (unsigned int r = 0; r < 3; r++) for (unsigned int c = 0; c < nc; c++) { //copy alfa constrain force matrix in E~ s.E_tilde(r, c) = alfa(r + 3, c); s.E_tilde(r + 3, c) = alfa(r, c); } //Change the reference frame of alfa to the segmentN tip frame //F_Total holds end effector frame, if done per segment bases then constraints could be extended to all segments Rotation base_to_end = F_total.M.Inverse(); for (unsigned int c = 0; c < nc; c++) { Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)), Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c))); col = base_to_end*col; s.E_tilde.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data); } } else { //For all others: //Everything should expressed in the body coordinates of segment i segment_info& child = results[i + 1]; //Copy PZ into a vector so we can do matrix manipulations, put torques above forces Vector6d vPZ; vPZ << Vector3d::Map(child.PZ.torque.data), Vector3d::Map(child.PZ.force.data); Matrix6d PZDPZt; PZDPZt.noalias() = vPZ * vPZ.transpose(); PZDPZt /= child.D; //equation a) (see Vereshchagin89) PZDPZt=[I,H;H',M] //Azamat:articulated body inertia as in Featherstone (7.19) s.P_tilde = s.H + child.P - ArticulatedBodyInertia(PZDPZt.bottomRightCorner<3,3>(), PZDPZt.topRightCorner<3,3>(), PZDPZt.topLeftCorner<3,3>()); //equation b) (see Vereshchagin89) //Azamat: bias force as in Featherstone (7.20) s.R_tilde = s.U + child.R + child.PC + (child.PZ / child.D) * child.u; //equation c) (see Vereshchagin89) s.E_tilde = child.E; //Azamat: equation (c) right side term s.E_tilde.noalias() -= (vPZ * child.EZ.transpose()) / child.D; //equation d) (see Vereshchagin89) s.M = child.M; //Azamat: equation (d) right side term s.M.noalias() -= (child.EZ * child.EZ.transpose()) / child.D; //equation e) (see Vereshchagin89) s.G = child.G; Twist CiZDu = child.C + (child.Z / child.D) * child.u; Vector6d vCiZDu; vCiZDu << Vector3d::Map(CiZDu.rot.data), Vector3d::Map(CiZDu.vel.data); s.G.noalias() += child.E.transpose() * vCiZDu; } if (i != 0) { //Transform all results to joint root coordinates of segment i (== body coordinates segment i-1) //equation a) s.P = s.F * s.P_tilde; //equation b) s.R = s.F * s.R_tilde; //equation c), in matrix: torques above forces, so switch and switch back for (unsigned int c = 0; c < nc; c++) { Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)), Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c))); col = s.F*col; s.E.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data); } //needed for next recursion s.PZ = s.P * s.Z; s.D = dot(s.Z, s.PZ); s.PC = s.P * s.C; //u=(Q-Z(R+PC)=sum of external forces along the joint axes, //R are the forces coming from the children, //Q is taken zero (do we need to take the previous calculated torques? //projection of coriolis and centrepital forces into joint subspace (0 0 Z) s.totalBias = -dot(s.Z, s.R + s.PC); s.u = torques(j) + s.totalBias; //Matrix form of Z, put rotations above translations Vector6d vZ; vZ << Vector3d::Map(s.Z.rot.data), Vector3d::Map(s.Z.vel.data); s.EZ.noalias() = s.E.transpose() * vZ; if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) j--; } } } void ChainIdSolver_Vereshchagin::constraint_calculation(const JntArray& beta) { //equation f) nu = M_0_inverse*(beta_N - E0_tilde`*acc0 - G0) //M_0_inverse, always nc*nc symmetric matrix //std::cout<<"M0: "< axis torque/force double constraint_torque = -dot(s.Z, constraint_force); //The result should be the torque at this joint torques(j) = constraint_torque; //s.constAccComp = torques(j) / s.D; s.constAccComp = constraint_torque / s.D; s.nullspaceAccComp = s.u / s.D; //total joint space acceleration resulting from accelerations of parent joints, constraint forces and // nullspace forces. q_dotdot(j) = (s.nullspaceAccComp + parentAccComp + s.constAccComp); s.acc = s.F.Inverse(a_p + s.Z * q_dotdot(j) + s.C);//returns acceleration in link distal tip coordinates. For use needs to be transformed if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) j++; } } /* void ChainIdSolver_Vereshchagin::getLinkCartesianPose(Frames& x_base) { for (int i = 0; i < ns; i++) { x_base[i] = results[i + 1].F_base; } return; } void ChainIdSolver_Vereshchagin::getLinkCartesianVelocity(Twists& xDot_base) { for (int i = 0; i < ns; i++) { xDot_base[i] = results[i + 1].F_base.M * results[i + 1].v; } return; } void ChainIdSolver_Vereshchagin::getLinkCartesianAcceleration(Twists& xDotDot_base) { for (int i = 0; i < ns; i++) { xDotDot_base[i] = results[i + 1].F_base.M * results[i + 1].acc; //std::cout << "XDotDot_base[i] " << xDotDot_base[i] << std::endl; } return; } void ChainIdSolver_Vereshchagin::getLinkPose(Frames& x_local) { for (int i = 0; i < ns; i++) { x_local[i] = results[i + 1].F; } return; } void ChainIdSolver_Vereshchagin::getLinkVelocity(Twists& xDot_local) { for (int i = 0; i < ns; i++) { xDot_local[i] = results[i + 1].v; } return; } void ChainIdSolver_Vereshchagin::getLinkAcceleration(Twists& xDotdot_local) { for (int i = 0; i < ns; i++) { xDotdot_local[i] = results[i + 1].acc; } return; } void ChainIdSolver_Vereshchagin::getJointBiasAcceleration(JntArray& bias_q_dotdot) { for (int i = 0; i < ns; i++) { //this is only force double tmp = results[i + 1].totalBias; //this is acceleration bias_q_dotdot(i) = tmp / results[i + 1].D; //s.totalBias = - dot(s.Z, s.R + s.PC); //std::cout << "totalBiasAccComponent" << i << ": " << bias_q_dotdot(i) << std::endl; //bias_q_dotdot(i) = s.totalBias/s.D } return; } void ChainIdSolver_Vereshchagin::getJointConstraintAcceleration(JntArray& constraint_q_dotdot) { for (int i = 0; i < ns; i++) { constraint_q_dotdot(i) = results[i + 1].constAccComp; //double tmp = results[i + 1].u; //s.u = torques(j) + s.totalBias; // std::cout << "s.constraintAccComp" << i << ": " << results[i+1].constAccComp << std::endl; //nullspace_q_dotdot(i) = s.u/s.D } return; } //Check the name it does not seem to be appropriate void ChainIdSolver_Vereshchagin::getJointNullSpaceAcceleration(JntArray& nullspace_q_dotdot) { for (int i = 0; i < ns; i++) { nullspace_q_dotdot(i) = results[i + 1].nullspaceAccComp; //double tmp = results[i + 1].u; //s.u = torques(j) + s.totalBias; //std::cout << "s.nullSpaceAccComp" << i << ": " << results[i+1].nullspaceAccComp << std::endl; //nullspace_q_dotdot(i) = s.u/s.D } return; } //This is not only a bias force energy but also includes generalized forces //change type of parameter G //this method should return array of G's void ChainIdSolver_Vereshchagin::getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G) { for (int i = 0; i < ns; i++) { G = results[i + 1].G; //double tmp = results[i + 1].u; //s.u = torques(j) + s.totalBias; //std::cout << "s.G " << i << ": " << results[i+1].G << std::endl; //nullspace_q_dotdot(i) = s.u/s.D } return; } //this method should return array of R's void ChainIdSolver_Vereshchagin::getLinkBiasForceMatrix(Wrenches& R_tilde) { for (int i = 0; i < ns; i++) { R_tilde[i] = results[i + 1].R_tilde; //Azamat: bias force as in Featherstone (7.20) //s.R_tilde = s.U + child.R + child.PC + child.PZ / child.D * child.u; std::cout << "s.R_tilde " << i << ": " << results[i + 1].R_tilde << std::endl; } return; } */ }//namespace orocos-kdl-1.4.0/orocos_kdl/src/chainidsolver_vereshchagin.hpp000066400000000000000000000174471326704774600246430ustar00rootroot00000000000000// Copyright (C) 2009, 2011 // Version: 1.0 // Author: Ruben Smits, Herman Bruyninckx, Azamat Shakhimardanov // Maintainer: Ruben Smits, Azamat Shakhimardanov // 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 KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP #define KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP #include "chainidsolver.hpp" #include "frames.hpp" #include "articulatedbodyinertia.hpp" #include namespace KDL { /** * \brief Dynamics calculations by constraints based on Vereshchagin 1989. * for a chain. This class creates instance of hybrid dynamics solver. * The solver calculates total joint space accelerations in a chain when a constraint force(s) is applied * to the chain's end-effector (task space/cartesian space). */ class ChainIdSolver_Vereshchagin : KDL::SolverI { typedef std::vector Twists; typedef std::vector Frames; typedef Eigen::Matrix Vector6d; typedef Eigen::Matrix Matrix6d; typedef Eigen::Matrix Matrix6Xd; public: /** * Constructor for the solver, it will allocate all the necessary memory * \param chain The kinematic chain to calculate the inverse dynamics for, an internal copy will be made. * \param root_acc The acceleration vector of the root to use during the calculation.(most likely contains gravity) * */ ChainIdSolver_Vereshchagin(const Chain& chain, Twist root_acc, unsigned int nc); ~ChainIdSolver_Vereshchagin() { }; /** * This method calculates joint space constraint torques and total joint space acceleration. * It returns 0 when it succeeds, otherwise -1 or -2 for unmatching matrix and array sizes. * Input parameters; * \param q The current joint positions * \param q_dot The current joint velocities * \param f_ext The external forces (no gravity, it is given in root acceleration) on the segments. * Output parameters: * \param q_dotdot The joint accelerations * \param torques the resulting constraint torques for the joints * * @return error/success code */ int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques); /// @copydoc KDL::SolverI::updateInternalDataStructures virtual void updateInternalDataStructures(); /* //Returns cartesian positions of links in base coordinates void getLinkCartesianPose(Frames& x_base); //Returns cartesian velocities of links in base coordinates void getLinkCartesianVelocity(Twists& xDot_base); //Returns cartesian acceleration of links in base coordinates void getLinkCartesianAcceleration(Twists& xDotDot_base); //Returns cartesian positions of links in link tip coordinates void getLinkPose(Frames& x_local); //Returns cartesian velocities of links in link tip coordinates void getLinkVelocity(Twists& xDot_local); //Returns cartesian acceleration of links in link tip coordinates void getLinkAcceleration(Twists& xDotdot_local); //Acceleration energy due to unit constraint forces at the end-effector void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M); //Acceleration energy due to arm configuration: bias force plus input joint torques void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G); void getLinkUnitForceMatrix(Matrix6Xd& E_tilde); void getLinkBiasForceMatrix(Wrenches& R_tilde); void getJointBiasAcceleration(JntArray &bias_q_dotdot); */ private: /** * This method calculates all cartesian space poses, twists, bias accelerations. * External forces are also taken into account in this outward sweep. */ void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext); /** * This method is a force balance sweep. It calculates articulated body inertias and bias forces. * Additionally, acceleration energies generated by bias forces and unit forces are calculated here. */ void downwards_sweep(const Jacobian& alfa, const JntArray& torques); /** * This method calculates constraint force magnitudes. * */ void constraint_calculation(const JntArray& beta); /** * This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together. * */ void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques); private: const Chain& chain; unsigned int nj; unsigned int ns; unsigned int nc; Twist acc_root; Jacobian alfa_N; Jacobian alfa_N2; Eigen::MatrixXd M_0_inverse; Eigen::MatrixXd Um; Eigen::MatrixXd Vm; JntArray beta_N; Eigen::VectorXd nu; Eigen::VectorXd nu_sum; Eigen::VectorXd Sm; Eigen::VectorXd tmpm; Wrench qdotdot_sum; Frame F_total; struct segment_info { Frame F; //local pose with respect to previous link in segments coordinates Frame F_base; // pose of a segment in root coordinates Twist Z; //Unit twist Twist v; //twist Twist acc; //acceleration twist Wrench U; //wrench p of the bias forces (in cartesian space) Wrench R; //wrench p of the bias forces Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form Twist C; //constraint Twist A; //constraint ArticulatedBodyInertia H; //I (expressed in 6*6 matrix) ArticulatedBodyInertia P; //I (expressed in 6*6 matrix) ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix) Wrench PZ; //vector U[i] = I_A[i]*S[i] Wrench PC; //vector E[i] = I_A[i]*c[i] double D; //vector D[i] = S[i]^T*U[i] Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints Matrix6Xd E_tilde; Eigen::MatrixXd M; //acceleration energy already generated at link i Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i Eigen::VectorXd EZ; //K[i] = Etiltde'*Z double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace double u; //vector u[i] = torques(i) - S[i]^T*(p_A[i] + I_A[i]*C[i]) in joint subspace. Azamat: In code u[i] = torques(i) - s[i].totalBias segment_info(unsigned int nc): D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0) { E.resize(6, nc); E_tilde.resize(6, nc); G.resize(nc); M.resize(nc, nc); EZ.resize(nc); E.setZero(); E_tilde.setZero(); M.setZero(); G.setZero(); EZ.setZero(); }; }; std::vector > results; }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/chainiksolver.hpp000066400000000000000000000143401326704774600221110ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_CHAIN_IKSOLVER_HPP #define KDL_CHAIN_IKSOLVER_HPP #include "chain.hpp" #include "frames.hpp" #include "framevel.hpp" #include "frameacc.hpp" #include "jntarray.hpp" #include "jntarrayvel.hpp" #include "jntarrayacc.hpp" #include "solveri.hpp" namespace KDL { /** * \brief This abstract class encapsulates the inverse * position solver for a KDL::Chain. * * @ingroup KinematicFamily */ class ChainIkSolverPos : public KDL::SolverI { public: /** * Calculate inverse position kinematics, from cartesian *coordinates to joint coordinates. * * @param q_init initial guess of the joint coordinates * @param p_in input cartesian coordinates * @param q_out output joint coordinates * * @return if < 0 something went wrong */ virtual int CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)=0; virtual ~ChainIkSolverPos(){}; virtual void updateInternalDataStructures()=0; }; /** * \brief This abstract class encapsulates the inverse * velocity solver for a KDL::Chain. * * @ingroup KinematicFamily */ class ChainIkSolverVel : public KDL::SolverI { public: /** * Calculate inverse velocity kinematics, from joint positions *and cartesian velocity to joint velocities. * * @param q_in input joint positions * @param v_in input cartesian velocity * @param qdot_out output joint velocities * * @return if < 0 something went wrong */ virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)=0; /** * Calculate inverse position and velocity kinematics, from *cartesian position and velocity to joint positions and velocities. * * @param q_init initial joint positions * @param v_in input cartesian position and velocity * @param q_out output joint position and velocity * * @return if < 0 something went wrong */ virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out)=0; virtual ~ChainIkSolverVel(){}; virtual void updateInternalDataStructures()=0; }; /** * \brief This abstract class encapsulates the inverse * acceleration solver for a KDL::Chain. * * @ingroup KinematicFamily */ class ChainIkSolverAcc : public KDL::SolverI { public: /** * Calculate inverse acceleration kinematics from joint * positions, joint velocities and cartesian acceleration to joint accelerations. * * @param q_in input joint positions * @param qdot_in input joint velocities * @param a_in input cartesian acceleration * @param qdotdot_out output joint accelerations * * @return if < 0 something went wrong */ virtual int CartToJnt(const JntArray& q_in, const JntArray& qdot_in, const Twist a_in, JntArray& qdotdot_out)=0; /** * Calculate inverse position, velocity and acceration *kinematics from cartesian coordinates to joint coordinates * * @param q_init initial guess for joint positions * @param a_in input cartesian position, velocity and acceleration * @param q_out output joint position, velocity and acceleration * * @return if < 0 something went wrong */ virtual int CartTojnt(const JntArray& q_init, const FrameAcc& a_in, JntArrayAcc& q_out)=0; /** * Calculate inverse velocity and acceleration kinematics from * joint positions and cartesian velocity and acceleration to * joint velocities and accelerations. * * @param q_in input joint positions * @param v_in input cartesian velocity * @param a_in input cartesian acceleration * @param qdot_out output joint velocities * @param qdotdot_out output joint accelerations * * @return if < 0 something went wrong */ virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, const Twist& a_in, JntArray& qdot_out, JntArray& qdotdot_out)=0; /** * Calculate inverse position and acceleration kinematics from *joint velocities and cartesian position and acceleration to *joint positions and accelerations * * @param q_init initial guess for joint positions * @param p_in input cartesian position * @param qdot_in input joint velocities * @param a_in input cartesian acceleration * @param q_out output joint positions * @param qdotdot_out output joint accelerations * * @return if < 0 something went wrong */ virtual int CartTojnt(const JntArray& q_init, const Frame& p_in, const JntArray& qdot_in, const Twist& a_in, JntArray& q_out, JntArray& qdotdot_out)=0; virtual void updateInternalDataStructures()=0; virtual ~ChainIkSolverAcc(){}; }; }//end of namespace KDL #endif orocos-kdl-1.4.0/orocos_kdl/src/chainiksolverpos_lma.cpp000066400000000000000000000241171326704774600234620ustar00rootroot00000000000000/** \file chainiksolverpos_lma.cpp \brief computing inverse position kinematics using Levenberg-Marquardt. */ /************************************************************************** begin : May 2012 copyright : (C) 2012 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 "chainiksolverpos_lma.hpp" #include namespace KDL { template inline void Twist_to_Eigen(const KDL::Twist& t,Eigen::MatrixBase& e) { e(0)=t.vel.data[0]; e(1)=t.vel.data[1]; e(2)=t.vel.data[2]; e(3)=t.rot.data[0]; e(4)=t.rot.data[1]; e(5)=t.rot.data[2]; } ChainIkSolverPos_LMA::ChainIkSolverPos_LMA( const KDL::Chain& _chain, const Eigen::Matrix& _L, double _eps, int _maxiter, double _eps_joints ) : chain(_chain), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), lastNrOfIter(0), lastDifference(0), lastTransDiff(0), lastRotDiff(0), lastSV(nj), jac(6, nj), grad(nj), display_information(false), maxiter(_maxiter), eps(_eps), eps_joints(_eps_joints), L(_L.cast()), T_base_jointroot(nj), T_base_jointtip(nj), q(nj), A(nj, nj), tmp(nj), ldlt(nj), svd(6, nj,Eigen::ComputeThinU | Eigen::ComputeThinV), diffq(nj), q_new(nj), original_Aii(nj) {} ChainIkSolverPos_LMA::ChainIkSolverPos_LMA( const KDL::Chain& _chain, double _eps, int _maxiter, double _eps_joints ) : chain(_chain), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), lastNrOfIter(0), lastDifference(0), lastTransDiff(0), lastRotDiff(0), lastSV(nj>6?6:nj), jac(6, nj), grad(nj), display_information(false), maxiter(_maxiter), eps(_eps), eps_joints(_eps_joints), T_base_jointroot(nj), T_base_jointtip(nj), q(nj), A(nj, nj), ldlt(nj), svd(6, nj,Eigen::ComputeThinU | Eigen::ComputeThinV), diffq(nj), q_new(nj), original_Aii(nj) { L(0)=1; L(1)=1; L(2)=1; L(3)=0.01; L(4)=0.01; L(5)=0.01; } void ChainIkSolverPos_LMA::updateInternalDataStructures() { nj = chain.getNrOfJoints(); ns = chain.getNrOfSegments(); lastSV.conservativeResize(nj>6?6:nj); jac.conservativeResize(Eigen::NoChange, nj); grad.conservativeResize(nj); T_base_jointroot.resize(nj); T_base_jointtip.resize(nj); q.conservativeResize(nj); A.conservativeResize(nj, nj); ldlt = Eigen::LDLT(nj); svd = Eigen::JacobiSVD(6, nj,Eigen::ComputeThinU | Eigen::ComputeThinV); diffq.conservativeResize(nj); q_new.conservativeResize(nj); original_Aii.conservativeResize(nj); } ChainIkSolverPos_LMA::~ChainIkSolverPos_LMA() {} void ChainIkSolverPos_LMA::compute_fwdpos(const VectorXq& q) { using namespace KDL; unsigned int jointndx=0; T_base_head = Frame::Identity(); // frame w.r.t. base of head for (unsigned int i=0;i(); compute_fwdpos(q); compute_jacobian(q); svd.compute(jac); std::cout << "Singular values : " << svd.singularValues().transpose()<<"\n"; } int ChainIkSolverPos_LMA::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out) { if (nj != chain.getNrOfJoints()) return (error = E_NOT_UP_TO_DATE); if (nj != q_init.rows() || nj != q_out.rows()) return (error = E_SIZE_MISMATCH); using namespace KDL; double v = 2; double tau = 10; double rho; double lambda; Twist t; double delta_pos_norm; Eigen::Matrix delta_pos; Eigen::Matrix delta_pos_new; q=q_init.data.cast(); compute_fwdpos(q); Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos ); delta_pos=L.asDiagonal()*delta_pos; delta_pos_norm = delta_pos.norm(); if (delta_pos_norm(); return (error = E_NOERROR); } compute_jacobian(q); jac = L.asDiagonal()*jac; lambda = tau; double dnorm = 1; for (unsigned int i=0;i(); if (dnorm < eps_joints) { lastDifference = delta_pos_norm; lastNrOfIter = i; lastSV = svd.singularValues(); q_out.data = q.cast(); compute_fwdpos(q); Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos ); lastTransDiff = delta_pos.topRows(3).norm(); lastRotDiff = delta_pos.bottomRows(3).norm(); return (error = E_INCREMENT_JOINTS_TOO_SMALL); } if (grad.transpose()*grad < eps_joints*eps_joints ) { compute_fwdpos(q); Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos ); lastDifference = delta_pos_norm; lastTransDiff = delta_pos.topRows(3).norm(); lastRotDiff = delta_pos.bottomRows(3).norm(); lastSV = svd.singularValues(); lastNrOfIter = i; q_out.data = q.cast(); return (error = E_GRADIENT_JOINTS_TOO_SMALL); } q_new = q+diffq; compute_fwdpos(q_new); Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos_new ); delta_pos_new = L.asDiagonal()*delta_pos_new; double delta_pos_new_norm = delta_pos_new.norm(); rho = delta_pos_norm*delta_pos_norm - delta_pos_new_norm*delta_pos_new_norm; rho /= diffq.transpose()*(lambda*diffq + grad); if (rho > 0) { q = q_new; delta_pos = delta_pos_new; delta_pos_norm = delta_pos_new_norm; if (delta_pos_norm(); return (error = E_NOERROR); } compute_jacobian(q_new); jac = L.asDiagonal()*jac; double tmp=2*rho-1; lambda = lambda*max(1/3.0, 1-tmp*tmp*tmp); v = 2; } else { lambda = lambda*v; v = 2*v; } } lastDifference = delta_pos_norm; lastTransDiff = delta_pos.topRows(3).norm(); lastRotDiff = delta_pos.bottomRows(3).norm(); lastSV = svd.singularValues(); lastNrOfIter = maxiter; q_out.data = q.cast(); return (error = E_MAX_ITERATIONS_EXCEEDED); } const char* ChainIkSolverPos_LMA::strError(const int error) const { if (E_GRADIENT_JOINTS_TOO_SMALL == error) return "The gradient of E towards the joints is to small"; else if (E_INCREMENT_JOINTS_TOO_SMALL == error) return "The joint position increments are to small"; else return SolverI::strError(error); } };//namespace KDL orocos-kdl-1.4.0/orocos_kdl/src/chainiksolverpos_lma.hpp000066400000000000000000000223061326704774600234650ustar00rootroot00000000000000#ifndef KDL_CHAINIKSOLVERPOS_GN_HPP #define KDL_CHAINIKSOLVERPOS_GN_HPP /** \file chainiksolverpos_lma.hpp \brief computing inverse position kinematics using Levenberg-Marquardt. */ /************************************************************************** begin : May 2012 copyright : (C) 2012 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 "chainiksolver.hpp" #include "chain.hpp" #include namespace KDL { /** * \brief Solver for the inverse position kinematics that uses Levenberg-Marquardt. * * The robustness and speed of this solver is improved in several ways: * - by using a Levenberg-Marquardt method that automatically adapts the damping when * computing the inverse damped least squares inverse velocity kinematics. * - by using an internal implementation of forward position kinematics and the * Jacobian kinematics. This implementation is more numerically robust, * is able to cache previous computations, and implements an \f$ \mathcal{O}(N) \f$ * algorithm for the computation of the Jacobian (with \f$N\f$, the number of joints, and for * a fixed size task space). * - by providing a way to specify the weights in task space, you can weigh rotations wrt translations. * This is important e.g. to specify that rotations do not matter for the problem at hand, or to * specify how important you judge rotations w.r.t. translations, typically in S.I.-units, ([m],[rad]), * the rotations are over-specified, this can be avoided using the weight matrix. Weights also * make the solver more robust . * - only the constructors call memory allocation. * * De general principles behind the optimisation is inspired on: * Jorge Nocedal, Stephen J. Wright, Numerical Optimization,Springer-Verlag New York, 1999. * \ingroup KinematicFamily */ class ChainIkSolverPos_LMA : public KDL::ChainIkSolverPos { private: typedef double ScalarType; typedef Eigen::Matrix MatrixXq; typedef Eigen::Matrix VectorXq; public: static const int E_GRADIENT_JOINTS_TOO_SMALL = -100; static const int E_INCREMENT_JOINTS_TOO_SMALL = -101; /** * \brief constructs an ChainIkSolverPos_LMA solver. * * The default parameters are chosen to be applicable to industrial-size robots * (e.g. 0.5 to 3 meters range in task space), with an accuracy that is more then * sufficient for typical industrial applications. * * Weights are applied in task space, i.e. the kinematic solver minimizes: * \f$ E = \Delta \mathbf{x}^T \mathbf{L} \mathbf{L}^T \Delta \mathbf{x} \f$, with \f$\mathbf{L}\f$ a diagonal matrix. * * \param _chain specifies the kinematic chain. * \param _L specifies the "square root" of the weight (diagonal) matrix in task space. This diagonal matrix is specified as a vector. * \param _eps specifies the desired accuracy in task space; after weighing with * the weight matrix, it is applied on \f$E\f$. * \param _maxiter specifies the maximum number of iterations. * \param _eps_joints specifies that the algorithm has to stop when the computed joint angle increments are * smaller then _eps_joints. This is to avoid unnecessary computations up to _maxiter when the joint angle * increments are so small that they effectively (in floating point) do not change the joint angles any more. The default * is a few digits above numerical accuracy. */ ChainIkSolverPos_LMA( const KDL::Chain& _chain, const Eigen::Matrix& _L, double _eps=1E-5, int _maxiter=500, double _eps_joints=1E-15 ); /** * \brief identical the full constructor for ChainIkSolverPos_LMA, but provides for a default weight matrix. * * \f$\mathbf{L} = \mathrm{diag}\left( \begin{bmatrix} 1 & 1 & 1 & 0.01 & 0.01 & 0.01 \end{bmatrix} \right) \f$. */ ChainIkSolverPos_LMA( const KDL::Chain& _chain, double _eps=1E-5, int _maxiter=500, double _eps_joints=1E-15 ); /** * \brief computes the inverse position kinematics. * * \param q_init initial joint position. * \param T_base_goal goal position expressed with respect to the robot base. * \param q_out joint position that achieves the specified goal position (if successful). * \return E_NOERROR if successful, * E_GRADIENT_JOINTS_TOO_SMALL the gradient of \f$ E \f$ towards the joints is to small, * E_INCREMENT_JOINTS_TOO_SMALL if joint position increments are to small, * E_MAX_ITER_EXCEEDED if number of iterations is exceeded. */ virtual int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out); /** * \brief destructor. */ virtual ~ChainIkSolverPos_LMA(); /** * \brief for internal use only. * * Only exposed for test and diagnostic purposes. */ void compute_fwdpos(const VectorXq& q); /** * \brief for internal use only. * Only exposed for test and diagnostic purposes. * compute_fwdpos(q) should always have been called before. */ void compute_jacobian(const VectorXq& q); /** * \brief for internal use only. * Only exposed for test and diagnostic purposes. */ void display_jac(const KDL::JntArray& jval); /// @copydoc KDL::SolverI::updateInternalDataStructures void updateInternalDataStructures(); /// @copydoc KDL::SolverI::strError() virtual const char* strError(const int error) const; private: const KDL::Chain& chain; unsigned int nj; unsigned int ns; public: /** * \brief contains the last number of iterations for an execution of CartToJnt. */ int lastNrOfIter; /** * \brief contains the last value for \f$ E \f$ after an execution of CartToJnt. */ double lastDifference; /** * \brief contains the last value for the (unweighted) translational difference after an execution of CartToJnt. */ double lastTransDiff; /** * \brief contains the last value for the (unweighted) rotational difference after an execution of CartToJnt. */ double lastRotDiff; /** * \brief contains the last values for the singular values of the weighted Jacobian after an execution of CartToJnt. */ VectorXq lastSV; /** * \brief for internal use only. * * contains the last value for the Jacobian after an execution of compute_jacobian. */ MatrixXq jac; /** * \brief for internal use only. * * contains the gradient of the error criterion after an execution of CartToJnt. */ VectorXq grad; /** * \brief for internal use only. * * contains the last value for the position of the tip of the robot (head) with respect to the base, after an execution of compute_jacobian. */ KDL::Frame T_base_head; /** * \brief display information on each iteration step to the console. */ bool display_information; private: // additional specification of the inverse position kinematics problem: unsigned int maxiter; double eps; double eps_joints; Eigen::Matrix L; // state of compute_fwdpos and compute_jacobian: std::vector T_base_jointroot; std::vector T_base_jointtip; // need 2 vectors because of the somewhat strange definition of segment.hpp // you could also recompute jointtip out of jointroot, // but then you'll need more expensive cos/sin functions. // the following are state of CartToJnt that is pre-allocated: VectorXq q; MatrixXq A; VectorXq tmp; Eigen::LDLT ldlt; Eigen::JacobiSVD svd; VectorXq diffq; VectorXq q_new; VectorXq original_Aii; }; }; // namespace KDL #endif orocos-kdl-1.4.0/orocos_kdl/src/chainiksolverpos_nr.cpp000066400000000000000000000062711326704774600233310ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "chainiksolverpos_nr.hpp" namespace KDL { ChainIkSolverPos_NR::ChainIkSolverPos_NR(const Chain& _chain,ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver, unsigned int _maxiter, double _eps): chain(_chain),nj (chain.getNrOfJoints()), iksolver(_iksolver),fksolver(_fksolver), delta_q(_chain.getNrOfJoints()), maxiter(_maxiter),eps(_eps) { } void ChainIkSolverPos_NR::updateInternalDataStructures() { nj = chain.getNrOfJoints(); iksolver.updateInternalDataStructures(); fksolver.updateInternalDataStructures(); delta_q.resize(nj); } int ChainIkSolverPos_NR::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out) { if (nj != chain.getNrOfJoints()) return (error = E_NOT_UP_TO_DATE); if(q_init.rows() != nj || q_out.rows() != nj) return (error = E_SIZE_MISMATCH); q_out = q_init; unsigned int i; for(i=0;i fksolver.JntToCart(q_out,f) ) return (error = E_FKSOLVERPOS_FAILED); delta_twist = diff(f,p_in); const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q); if (E_NOERROR > rc) return (error = E_IKSOLVER_FAILED); // we chose to continue if the child solver returned a positive // "error", which may simply indicate a degraded solution Add(q_out,delta_q,q_out); if(Equal(delta_twist,Twist::Zero(),eps)) // converged, but possibly with a degraded solution return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR); } return (error = E_MAX_ITERATIONS_EXCEEDED); // failed to converge } ChainIkSolverPos_NR::~ChainIkSolverPos_NR() { } const char* ChainIkSolverPos_NR::strError(const int error) const { if (E_IKSOLVER_FAILED == error) return "Child IK solver failed"; else if (E_FKSOLVERPOS_FAILED == error) return "Child FK solver failed"; else return SolverI::strError(error); } } orocos-kdl-1.4.0/orocos_kdl/src/chainiksolverpos_nr.hpp000066400000000000000000000071511326704774600233340ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDLCHAINIKSOLVERPOS_NR_HPP #define KDLCHAINIKSOLVERPOS_NR_HPP #include "chainiksolver.hpp" #include "chainfksolver.hpp" namespace KDL { /** * Implementation of a general inverse position kinematics * algorithm based on Newton-Raphson iterations to calculate the * position transformation from Cartesian to joint space of a general * KDL::Chain. * * @ingroup KinematicFamily */ class ChainIkSolverPos_NR : public ChainIkSolverPos { public: static const int E_IKSOLVER_FAILED = -100; //! Child IK solver vel failed static const int E_FKSOLVERPOS_FAILED = -101; //! Child FK solver failed /** * Constructor of the solver, it needs the chain, a forward * position kinematics solver and an inverse velocity * kinematics solver for that chain. * * @param chain the chain to calculate the inverse position for * @param fksolver a forward position kinematics solver * @param iksolver an inverse velocity kinematics solver * @param maxiter the maximum Newton-Raphson iterations, * default: 100 * @param eps the precision for the position, used to end the * iterations, default: epsilon (defined in kdl.hpp) * * @return */ ChainIkSolverPos_NR(const Chain& chain,ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver, unsigned int maxiter=100,double eps=1e-6); ~ChainIkSolverPos_NR(); /** * Find an output joint pose \a q_out, given a starting joint pose * \a q_init and a desired cartesian pose \a p_in * * @return: * E_NOERROR=solution converged to // Copyright (C) 2008 Mikael Mayer // Copyright (C) 2008 Julia Jesse // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "chainiksolverpos_nr_jl.hpp" #include namespace KDL { ChainIkSolverPos_NR_JL::ChainIkSolverPos_NR_JL(const Chain& _chain, const JntArray& _q_min, const JntArray& _q_max, ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver, unsigned int _maxiter, double _eps): chain(_chain), nj(chain.getNrOfJoints()), q_min(_q_min), q_max(_q_max), iksolver(_iksolver), fksolver(_fksolver), delta_q(_chain.getNrOfJoints()), maxiter(_maxiter),eps(_eps) { } ChainIkSolverPos_NR_JL::ChainIkSolverPos_NR_JL(const Chain& _chain, ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver, unsigned int _maxiter, double _eps): chain(_chain), nj(chain.getNrOfJoints()), q_min(nj), q_max(nj), iksolver(_iksolver), fksolver(_fksolver), delta_q(nj), maxiter(_maxiter),eps(_eps) { q_min.data.setConstant(std::numeric_limits::min()); q_max.data.setConstant(std::numeric_limits::max()); } void ChainIkSolverPos_NR_JL::updateInternalDataStructures() { nj = chain.getNrOfJoints(); q_min.data.conservativeResizeLike(Eigen::VectorXd::Constant(nj,std::numeric_limits::min())); q_max.data.conservativeResizeLike(Eigen::VectorXd::Constant(nj,std::numeric_limits::max())); iksolver.updateInternalDataStructures(); fksolver.updateInternalDataStructures(); delta_q.resize(nj); } int ChainIkSolverPos_NR_JL::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out) { if(nj != chain.getNrOfJoints()) return (error = E_NOT_UP_TO_DATE); if(nj != q_init.rows() || nj != q_out.rows() || nj != q_min.rows() || nj != q_max.rows()) return (error = E_SIZE_MISMATCH); q_out = q_init; unsigned int i; for(i=0;i q_max(j)) q_out(j) = q_max(j); } } if(i!=maxiter) return (error = E_NOERROR); else return (error = E_MAX_ITERATIONS_EXCEEDED); } int ChainIkSolverPos_NR_JL::setJointLimits(const JntArray& q_min_in, const JntArray& q_max_in) { if (q_min_in.rows() != nj || q_max_in.rows() != nj) return (error = E_SIZE_MISMATCH); q_min = q_min_in; q_max = q_max_in; return (error = E_NOERROR); } ChainIkSolverPos_NR_JL::~ChainIkSolverPos_NR_JL() { } const char* ChainIkSolverPos_NR_JL::strError(const int error) const { if (E_FKSOLVERPOS_FAILED == error) return "Internal forward position solver failed."; else if (E_IKSOLVERVEL_FAILED == error) return "Internal inverse velocity solver failed."; else return SolverI::strError(error); } } orocos-kdl-1.4.0/orocos_kdl/src/chainiksolverpos_nr_jl.hpp000066400000000000000000000120651326704774600240210ustar00rootroot00000000000000// Copyright (C) 2007-2008 Ruben Smits // Copyright (C) 2008 Mikael Mayer // Copyright (C) 2008 Julia Jesse // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDLCHAINIKSOLVERPOS_NR_JL_HPP #define KDLCHAINIKSOLVERPOS_NR_JL_HPP #include "chainiksolver.hpp" #include "chainfksolver.hpp" namespace KDL { /** * Implementation of a general inverse position kinematics * algorithm based on Newton-Raphson iterations to calculate the * position transformation from Cartesian to joint space of a general * KDL::Chain. Takes joint limits into account. * * @ingroup KinematicFamily */ class ChainIkSolverPos_NR_JL : public ChainIkSolverPos { public: static const int E_IKSOLVERVEL_FAILED = -100; //! Child IK solver vel failed static const int E_FKSOLVERPOS_FAILED = -101; //! Child FK solver failed /** * Constructor of the solver, it needs the chain, a forward * position kinematics solver and an inverse velocity * kinematics solver for that chain. * * @param chain the chain to calculate the inverse position for * @param q_min the minimum joint positions * @param q_max the maximum joint positions * @param fksolver a forward position kinematics solver * @param iksolver an inverse velocity kinematics solver * @param maxiter the maximum Newton-Raphson iterations, * default: 100 * @param eps the precision for the position, used to end the * iterations, default: epsilon (defined in kdl.hpp) * * @return */ ChainIkSolverPos_NR_JL(const Chain& chain,const JntArray& q_min, const JntArray& q_max, ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver,unsigned int maxiter=100,double eps=1e-6); /** * Constructor of the solver, it needs the chain, a forward * position kinematics solver and an inverse velocity * kinematics solver for that chain. * * @param chain the chain to calculate the inverse position for * @param fksolver a forward position kinematics solver * @param iksolver an inverse velocity kinematics solver * @param maxiter the maximum Newton-Raphson iterations, * default: 100 * @param eps the precision for the position, used to end the * iterations, default: epsilon (defined in kdl.hpp) * * @return */ ChainIkSolverPos_NR_JL(const Chain& chain, ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver,unsigned int maxiter=100,double eps=1e-6); ~ChainIkSolverPos_NR_JL(); /** * Calculates the joint values that correspond to the input pose given an initial guess. * @param q_init Initial guess for the joint values. * @param p_in The input pose of the chain tip. * @param q_out The resulting output joint values * @return E_MAX_ITERATIONS_EXCEEDED if the maximum number of iterations was exceeded before a result was found * E_NOT_UP_TO_DATE if the internal data is not up to date with the chain * E_SIZE_MISMATCH if the size of the input/output data does not match the chain. */ virtual int CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out); /** * Function to set the joint limits. * @param q_min minimum values for the joints * @param q_max maximum values for the joints * @return E_SIZE_MISMATCH if input sizes do not match the chain */ int setJointLimits(const JntArray& q_min, const JntArray& q_max); /// @copydoc KDL::SolverI::updateInternalDataStructures virtual void updateInternalDataStructures(); /// @copydoc KDL::SolverI::strError() const char* strError(const int error) const; private: const Chain& chain; unsigned int nj; JntArray q_min; JntArray q_max; ChainIkSolverVel& iksolver; ChainFkSolverPos& fksolver; JntArray delta_q; unsigned int maxiter; double eps; Frame f; Twist delta_twist; }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/chainiksolvervel_pinv.cpp000066400000000000000000000111321326704774600236430ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "chainiksolvervel_pinv.hpp" namespace KDL { ChainIkSolverVel_pinv::ChainIkSolverVel_pinv(const Chain& _chain,double _eps,int _maxiter): chain(_chain), nj(chain.getNrOfJoints()), jnt2jac(chain), jac(nj), svd(jac), U(6,JntArray(nj)), S(nj), V(nj,JntArray(nj)), tmp(nj), eps(_eps), maxiter(_maxiter), nrZeroSigmas(0), svdResult(0) { } void ChainIkSolverVel_pinv::updateInternalDataStructures() { jnt2jac.updateInternalDataStructures(); nj = chain.getNrOfJoints(); jac.resize(nj); svd = SVD_HH(jac); for(unsigned int i = 0 ; i < U.size(); i++) U[i].resize(nj); S.resize(nj); V.resize(nj); for(unsigned int i = 0 ; i < V.size(); i++) V[i].resize(nj); tmp.resize(nj); } ChainIkSolverVel_pinv::~ChainIkSolverVel_pinv() { } int ChainIkSolverVel_pinv::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) { if (nj != chain.getNrOfJoints()) return (error = E_NOT_UP_TO_DATE); if (nj != q_in.rows() || nj != qdot_out.rows()) return (error = E_SIZE_MISMATCH); //Let the ChainJntToJacSolver calculate the jacobian "jac" for //the current joint positions "q_in" error = jnt2jac.JntToJac(q_in,jac); if (error < E_NOERROR) return error; double sum; unsigned int i,j; // Initialize near zero singular value counter nrZeroSigmas = 0 ; //Do a singular value decomposition of "jac" with maximum //iterations "maxiter", put the results in "U", "S" and "V" //jac = U*S*Vt svdResult = svd.calculate(jac,U,S,V,maxiter); if (0 != svdResult) { qdot_out.data.setZero(); return (error = E_SVD_FAILED); } // We have to calculate qdot_out = jac_pinv*v_in // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut): // qdot_out = V*S_pinv*Ut*v_in //first we calculate Ut*v_in for (i=0;i (jac.columns()-jac.rows()) ) { return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular } else { return (error = E_NOERROR); // have converged } } const char* ChainIkSolverVel_pinv::strError(const int error) const { if (E_CONVERGE_PINV_SINGULAR == error) return "Converged put pseudo inverse of jacobian is singular."; else return SolverI::strError(error); } } orocos-kdl-1.4.0/orocos_kdl/src/chainiksolvervel_pinv.hpp000066400000000000000000000103541326704774600236550ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_CHAIN_IKSOLVERVEL_PINV_HPP #define KDL_CHAIN_IKSOLVERVEL_PINV_HPP #include "chainiksolver.hpp" #include "chainjnttojacsolver.hpp" #include "utilities/svd_HH.hpp" namespace KDL { /** * Implementation of a inverse velocity kinematics algorithm based * on the generalize pseudo inverse to calculate the velocity * transformation from Cartesian to joint space of a general * KDL::Chain. It uses a svd-calculation based on householders * rotations. * * @ingroup KinematicFamily */ class ChainIkSolverVel_pinv : public ChainIkSolverVel { public: /// solution converged but (pseudo)inverse is singular static const int E_CONVERGE_PINV_SINGULAR = +100; /** * Constructor of the solver * * @param chain the chain to calculate the inverse velocity * kinematics for * @param eps if a singular value is below this value, its * inverse is set to zero, default: 0.00001 * @param maxiter maximum iterations for the svd calculation, * default: 150 * */ explicit ChainIkSolverVel_pinv(const Chain& chain,double eps=0.00001,int maxiter=150); ~ChainIkSolverVel_pinv(); /** * Find an output joint velocity \a qdot_out, given a starting joint pose * \a q_init and a desired cartesian velocity \a v_in * * @return * E_NOERROR=solution converged to jac.col()-jac.row(), * then the jacobian pseudoinverse is singular */ unsigned int getNrZeroSigmas()const {return nrZeroSigmas;}; /** * Retrieve the latest return code from the SVD algorithm * @return 0 if CartToJnt() not yet called, otherwise latest SVD result code. */ int getSVDResult()const {return svdResult;}; /// @copydoc KDL::SolverI::strError() virtual const char* strError(const int error) const; /// @copydoc KDL::SolverI::updateInternalDataStructures virtual void updateInternalDataStructures(); private: const Chain& chain; ChainJntToJacSolver jnt2jac; unsigned int nj; Jacobian jac; SVD_HH svd; std::vector U; JntArray S; std::vector V; JntArray tmp; double eps; int maxiter; unsigned int nrZeroSigmas; int svdResult; }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp000066400000000000000000000075751326704774600252360ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "chainiksolvervel_pinv_givens.hpp" #include "utilities/svd_eigen_Macie.hpp" namespace KDL { ChainIkSolverVel_pinv_givens::ChainIkSolverVel_pinv_givens(const Chain& _chain): chain(_chain), nj(chain.getNrOfJoints()), jnt2jac(chain), jac(nj), transpose(nj>6),toggle(true), m(max(6,nj)), n(min(6,nj)), jac_eigen(m,n), U(MatrixXd::Identity(m,m)), V(MatrixXd::Identity(n,n)), B(m,n), S(n), tempi(m), tempj(m), UY(VectorXd::Zero(6)), SUY(VectorXd::Zero(nj)), qdot_eigen(nj), v_in_eigen(6) { } void ChainIkSolverVel_pinv_givens::updateInternalDataStructures() { nj = chain.getNrOfJoints(); jnt2jac.updateInternalDataStructures(); jac.resize(nj); transpose = (nj > 6); m = max(6,nj); n = min(6,nj); jac_eigen.conservativeResize(m,n); U.conservativeResizeLike(MatrixXd::Identity(m,m)); V.conservativeResizeLike(MatrixXd::Identity(n,n)); B.conservativeResize(m,n); S.conservativeResize(n); tempi.conservativeResize(m); tempj.conservativeResize(n); SUY.conservativeResizeLike(VectorXd::Zero(nj)); qdot_eigen.conservativeResize(nj); } ChainIkSolverVel_pinv_givens::~ChainIkSolverVel_pinv_givens() { } int ChainIkSolverVel_pinv_givens::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) { if (nj != chain.getNrOfJoints()) return (error = E_NOT_UP_TO_DATE); if (nj != q_in.rows() || nj != qdot_out.rows()) return (error = E_SIZE_MISMATCH); toggle=!toggle; error = jnt2jac.JntToJac(q_in,jac); if (E_NOERROR > error ) return error; for(unsigned int i=0;i<6;i++) v_in_eigen(i)=v_in(i); for(unsigned int i=0;i #ifndef KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP #define KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP #include "chainiksolver.hpp" #include "chainjnttojacsolver.hpp" #include using namespace Eigen; namespace KDL { /** * Implementation of a inverse velocity kinematics algorithm based * on the generalize pseudo inverse to calculate the velocity * transformation from Cartesian to joint space of a general * KDL::Chain. It uses a svd-calculation based on householders * rotations. * * @ingroup KinematicFamily */ class ChainIkSolverVel_pinv_givens : public ChainIkSolverVel { public: /** * Constructor of the solver * * @param chain the chain to calculate the inverse velocity * kinematics for * @param eps if a singular value is below this value, its * inverse is set to zero, default: 0.00001 * @param maxiter maximum iterations for the svd calculation, * default: 150 * */ explicit ChainIkSolverVel_pinv_givens(const Chain& chain); ~ChainIkSolverVel_pinv_givens(); virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); /** * not (yet) implemented. * */ virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return (error = E_NOT_IMPLEMENTED);}; /// @copydoc KDL::SolverI::updateInternalDataStructures virtual void updateInternalDataStructures(); private: const Chain& chain; unsigned int nj; ChainJntToJacSolver jnt2jac; Jacobian jac; bool transpose,toggle; unsigned int m,n; MatrixXd jac_eigen,U,V,B; VectorXd S,tempi,tempj,UY,SUY,qdot_eigen,v_in_eigen; }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/chainiksolvervel_pinv_nso.cpp000066400000000000000000000147561326704774600245410ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "chainiksolvervel_pinv_nso.hpp" #include "utilities/svd_eigen_HH.hpp" namespace KDL { ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, const JntArray& _opt_pos, const JntArray& _weights, double _eps, int _maxiter, double _alpha): chain(_chain), jnt2jac(chain), nj(chain.getNrOfJoints()), jac(nj), U(MatrixXd::Zero(6,nj)), S(VectorXd::Zero(nj)), Sinv(VectorXd::Zero(nj)), V(MatrixXd::Zero(nj,nj)), tmp(VectorXd::Zero(nj)), tmp2(VectorXd::Zero(nj)), eps(_eps), maxiter(_maxiter), svdResult(0), alpha(_alpha), weights(_weights), opt_pos(_opt_pos) { } ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, double _eps, int _maxiter, double _alpha): chain(_chain), jnt2jac(chain), nj(chain.getNrOfJoints()), jac(nj), U(MatrixXd::Zero(6,nj)), S(VectorXd::Zero(nj)), Sinv(VectorXd::Zero(nj)), V(MatrixXd::Zero(nj,nj)), tmp(VectorXd::Zero(nj)), tmp2(VectorXd::Zero(nj)), eps(_eps), maxiter(_maxiter), svdResult(0), alpha(_alpha) { } void ChainIkSolverVel_pinv_nso::updateInternalDataStructures() { jnt2jac.updateInternalDataStructures(); nj = chain.getNrOfJoints(); jac.resize(nj); U.conservativeResizeLike(MatrixXd::Zero(6,nj)); S.conservativeResizeLike(VectorXd::Zero(nj)); Sinv.conservativeResizeLike(VectorXd::Zero(nj)); V.conservativeResizeLike(MatrixXd::Zero(nj,nj)); tmp.conservativeResizeLike(VectorXd::Zero(nj)); tmp2.conservativeResizeLike(VectorXd::Zero(nj)); opt_pos.data.conservativeResizeLike(VectorXd::Zero(nj)); weights.data.conservativeResizeLike(VectorXd::Ones(nj)); } ChainIkSolverVel_pinv_nso::~ChainIkSolverVel_pinv_nso() { } int ChainIkSolverVel_pinv_nso::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) { if (nj != chain.getNrOfJoints()) return (error = E_NOT_UP_TO_DATE); if (nj != q_in.rows() || nj != qdot_out.rows() || nj != opt_pos.rows() || nj != weights.rows()) return (error = E_SIZE_MISMATCH); //Let the ChainJntToJacSolver calculate the jacobian "jac" for //the current joint positions "q_in" error = jnt2jac.JntToJac(q_in,jac); if (error < E_NOERROR) return error; //Do a singular value decomposition of "jac" with maximum //iterations "maxiter", put the results in "U", "S" and "V" //jac = U*S*Vt svdResult = svd_eigen_HH(jac.data,U,S,V,tmp,maxiter); if (0 != svdResult) { qdot_out.data.setZero() ; return error = E_SVD_FAILED; } unsigned int i; // We have to calculate qdot_out = jac_pinv*v_in // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut): // qdot_out = V*S_pinv*Ut*v_in // S^-1 for (i = 0; i < nj; ++i) { Sinv(i) = fabs(S(i)) 1e-9) { // Calculate inverse Jacobian Jc^-1 for (i = 0; i < nj; ++i) { tmp(i) = weights(i)*(q_in(i) - opt_pos(i)) / A; } // Calculate J^-1 * J * Jc^-1 = V*S^-1*U' * U*S*V' * tmp tmp2 = V * Sinv.asDiagonal() * U.transpose() * U * S.asDiagonal() * V.transpose() * tmp; for (i = 0; i < nj; ++i) { //std::cerr << i <<": "<< qdot_out(i) <<", "<< -2*alpha*g * (tmp(i) - tmp2(i)) << std::endl; qdot_out(i) += -2*alpha*g * (tmp(i) - tmp2(i)); } } //return the return value of the svd decomposition return (error = E_NOERROR); } int ChainIkSolverVel_pinv_nso::setWeights(const JntArray & _weights) { if (nj != _weights.rows()) return (error = E_SIZE_MISMATCH); weights = _weights; return (error = E_NOERROR); } int ChainIkSolverVel_pinv_nso::setOptPos(const JntArray & _opt_pos) { if (nj != _opt_pos.rows()) return (error = E_SIZE_MISMATCH); opt_pos = _opt_pos; return (error = E_NOERROR); } int ChainIkSolverVel_pinv_nso::setAlpha(const double _alpha) { alpha = _alpha; return 0; } } orocos-kdl-1.4.0/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp000066400000000000000000000123161326704774600245340ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_CHAIN_IKSOLVERVEL_PINV_NSO_HPP #define KDL_CHAIN_IKSOLVERVEL_PINV_NSO_HPP #include "chainiksolver.hpp" #include "chainjnttojacsolver.hpp" #include namespace KDL { /** * Implementation of a inverse velocity kinematics algorithm based * on the generalize pseudo inverse to calculate the velocity * transformation from Cartesian to joint space of a general * KDL::Chain. It uses a svd-calculation based on householders * rotations. * * In case of a redundant robot this solver optimizes the following criterium: * g=0.5*sum(weight*(Desired_joint_positions - actual_joint_positions))^2 as described in * A. Liegeois. Automatic supervisory control of the configuration and * behavior of multibody mechanisms. IEEE Transactions on Systems, Man, and * Cybernetics, 7(12):868–871, 1977 * * @ingroup KinematicFamily */ class ChainIkSolverVel_pinv_nso : public ChainIkSolverVel { public: /** * Constructor of the solver * * @param chain the chain to calculate the inverse velocity * kinematics for * @param opt_pos the desired positions of the chain used by to resolve the redundancy * @param weights the weights applied in the joint space * @param eps if a singular value is below this value, its * inverse is set to zero, default: 0.00001 * @param maxiter maximum iterations for the svd calculation, * default: 150 * @param alpha the null-space velocity gain * */ ChainIkSolverVel_pinv_nso(const Chain& chain, const JntArray& opt_pos, const JntArray& weights, double eps=0.00001,int maxiter=150, double alpha = 0.25); explicit ChainIkSolverVel_pinv_nso(const Chain& chain, double eps=0.00001,int maxiter=150, double alpha = 0.25); ~ChainIkSolverVel_pinv_nso(); virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); /** * not (yet) implemented. * */ virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;}; /** * Request the joint weights for optimization criterion * * * @return const reference to the joint weights */ const JntArray& getWeights()const { return weights; } /** * Request the optimal joint positions * * * @return const reference to the optimal joint positions */ const JntArray& getOptPos()const { return opt_pos; } /** * Request null space velocity gain * * * @return const reference to the null space velocity gain */ const double& getAlpha()const { return alpha; } /** *Set joint weights for optimization criterion * *@param weights the joint weights * */ virtual int setWeights(const JntArray &weights); /** *Set optimal joint positions * *@param opt_pos optimal joint positions * */ virtual int setOptPos(const JntArray &opt_pos); /** *Set null psace velocity gain * *@param alpha NUllspace velocity cgain * */ virtual int setAlpha(const double alpha); /** * Retrieve the latest return code from the SVD algorithm * @return 0 if CartToJnt() not yet called, otherwise latest SVD result code. */ int getSVDResult()const {return svdResult;}; /// @copydoc KDL::SolverI::updateInternalDataStructures virtual void updateInternalDataStructures(); private: const Chain& chain; ChainJntToJacSolver jnt2jac; unsigned int nj; Jacobian jac; Eigen::MatrixXd U; Eigen::VectorXd S; Eigen::VectorXd Sinv; Eigen::MatrixXd V; Eigen::VectorXd tmp; Eigen::VectorXd tmp2; double eps; int maxiter; int svdResult; double alpha; JntArray weights; JntArray opt_pos; }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/chainiksolvervel_wdls.cpp000066400000000000000000000162431326704774600236500ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "chainiksolvervel_wdls.hpp" #include "utilities/svd_eigen_HH.hpp" namespace KDL { ChainIkSolverVel_wdls::ChainIkSolverVel_wdls(const Chain& _chain,double _eps,int _maxiter): chain(_chain), jnt2jac(chain), nj(chain.getNrOfJoints()), jac(nj), U(MatrixXd::Zero(6,nj)), S(VectorXd::Zero(nj)), V(MatrixXd::Zero(nj,nj)), eps(_eps), maxiter(_maxiter), tmp(VectorXd::Zero(nj)), tmp_jac(MatrixXd::Zero(6,nj)), tmp_jac_weight1(MatrixXd::Zero(6,nj)), tmp_jac_weight2(MatrixXd::Zero(6,nj)), tmp_ts(MatrixXd::Zero(6,6)), tmp_js(MatrixXd::Zero(nj,nj)), weight_ts(MatrixXd::Identity(6,6)), weight_js(MatrixXd::Identity(nj,nj)), lambda(0.0), lambda_scaled(0.0), nrZeroSigmas(0), svdResult(0), sigmaMin(0) { } void ChainIkSolverVel_wdls::updateInternalDataStructures() { jnt2jac.updateInternalDataStructures(); nj = chain.getNrOfJoints(); jac.resize(nj); MatrixXd z6nj = MatrixXd::Zero(6,nj); VectorXd znj = VectorXd::Zero(nj); MatrixXd znjnj = MatrixXd::Zero(nj,nj); U.conservativeResizeLike(z6nj); S.conservativeResizeLike(znj); V.conservativeResizeLike(znjnj); tmp.conservativeResizeLike(znj); tmp_jac.conservativeResizeLike(z6nj); tmp_jac_weight1.conservativeResizeLike(z6nj); tmp_jac_weight2.conservativeResizeLike(z6nj); tmp_js.conservativeResizeLike(znjnj); weight_js.conservativeResizeLike(MatrixXd::Identity(nj,nj)); } ChainIkSolverVel_wdls::~ChainIkSolverVel_wdls() { } int ChainIkSolverVel_wdls::setWeightJS(const MatrixXd& Mq){ if(nj != chain.getNrOfJoints()) return (error = E_NOT_UP_TO_DATE); if (Mq.size() != weight_js.size()) return (error = E_SIZE_MISMATCH); weight_js = Mq; return (error = E_NOERROR); } int ChainIkSolverVel_wdls::setWeightTS(const MatrixXd& Mx){ if (Mx.size() != weight_ts.size()) return (error = E_SIZE_MISMATCH); weight_ts = Mx; return (error = E_NOERROR); } void ChainIkSolverVel_wdls::setLambda(const double lambda_in) { lambda=lambda_in; } void ChainIkSolverVel_wdls::setEps(const double eps_in) { eps=eps_in; } void ChainIkSolverVel_wdls::setMaxIter(const int maxiter_in) { maxiter=maxiter_in; } int ChainIkSolverVel_wdls::getSigma(Eigen::VectorXd& Sout) { if (Sout.size() != S.size()) return (error = E_SIZE_MISMATCH); Sout=S; return (error = E_NOERROR); } int ChainIkSolverVel_wdls::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) { if(nj != chain.getNrOfJoints()) return (error = E_NOT_UP_TO_DATE); if(nj != q_in.rows() || nj != qdot_out.rows()) return (error = E_SIZE_MISMATCH); error = jnt2jac.JntToJac(q_in,jac); if ( error < E_NOERROR) return error; double sum; unsigned int i,j; // Initialize (internal) return values nrZeroSigmas = 0 ; sigmaMin = 0.; lambda_scaled = 0.; /* for (i=0;i=6 and 0 for <6 if ( jac.columns() >= 6 ) { sigmaMin = S(5); } else { sigmaMin = 0.; } // tmp = (Si*U'*Ly*y), for (i=0;i eps, then wdls is not active and lambda_scaled = 0 (default value) // If sigmaMin < eps, then wdls is active and lambda_scaled is scaled from 0 to lambda // Note: singular values are always positive so sigmaMin >=0 if ( sigmaMin < eps ) { lambda_scaled = sqrt(1.0-(sigmaMin/eps)*(sigmaMin/eps))*lambda ; } if(fabs(S(i))=6 due to cols>rows } // Count number of singular values near zero ++nrZeroSigmas ; } else { tmp(i) = sum/S(i); } } /* // x = Lx^-1*V*tmp + x for (i=0;i (jac.columns()-jac.rows()) ) { return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular } else { return (error = E_NOERROR); // have converged } } const char* ChainIkSolverVel_wdls::strError(const int error) const { if (E_CONVERGE_PINV_SINGULAR == error) return "Converged put pseudo inverse of jacobian is singular."; else return SolverI::strError(error); } } orocos-kdl-1.4.0/orocos_kdl/src/chainiksolvervel_wdls.hpp000066400000000000000000000223231326704774600236510ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_CHAIN_IKSOLVERVEL_WDLS_HPP #define KDL_CHAIN_IKSOLVERVEL_WDLS_HPP #include "chainiksolver.hpp" #include "chainjnttojacsolver.hpp" #include namespace KDL { /** * Implementation of a inverse velocity kinematics algorithm based * on the weighted pseudo inverse with damped least-square to calculate the velocity * transformation from Cartesian to joint space of a general * KDL::Chain. It uses a svd-calculation based on householders * rotations. * * J# = M_q*Vb*pinv_dls(Db)*Ub'*M_x * * where B = Mx*J*Mq * * and B = Ub*Db*Vb' is the SVD decomposition of B * * Mq and Mx represent, respectively, the joint-space and task-space weighting * matrices. * Please refer to the documentation of setWeightJS(const Eigen::MatrixXd& Mq) * and setWeightTS(const Eigen::MatrixXd& Mx) for details on the effects of * these matrices. * * For more details on Weighted Pseudo Inverse, see : * 1) [Ben Israel 03] A. Ben Israel & T.N.E. Greville. * Generalized Inverses : Theory and Applications, * second edition. Springer, 2003. ISBN 0-387-00293-6. * * 2) [Doty 93] K. L. Doty, C. Melchiorri & C. Boniveto. * A theory of generalized inverses applied to Robotics. * The International Journal of Robotics Research, * vol. 12, no. 1, pages 1-19, february 1993. * * * @ingroup KinematicFamily */ class ChainIkSolverVel_wdls : public ChainIkSolverVel { public: /// solution converged but (pseudo)inverse is singular static const int E_CONVERGE_PINV_SINGULAR = +100; /** * Constructor of the solver * * @param chain the chain to calculate the inverse velocity * kinematics for * @param eps if a singular value is below this value, its * inverse is set to zero, default: 0.00001 * @param maxiter maximum iterations for the svd calculation, * default: 150 * */ explicit ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=150); //=ublas::identity_matrix ~ChainIkSolverVel_wdls(); /** * Find an output joint velocity \a qdot_out, given a starting joint pose * \a q_init and a desired cartesian velocity \a v_in * * @return * E_NOERROR=svd solution converged in maxiter * E_SVD_FAILED=svd solution failed * E_CONVERGE_PINV_SINGULAR=svd solution converged but (pseudo)inverse singular * * @note if E_CONVERGE_PINV_SINGULAR returned then converged and can * continue motion, but have degraded solution * * @note If E_SVD_FAILED returned, then getSvdResult() returns the error * code from the SVD algorithm. */ virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); /** * not (yet) implemented. * */ virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;}; /** * Set the joint space weighting matrix * * @param weight_js joint space weighting symmetric matrix, * default : identity. M_q : This matrix being used as a * weight for the norm of the joint space speed it HAS TO BE * symmetric and positive definite. We can actually deal with * matrices containing a symmetric and positive definite block * and 0s otherwise. Taking a diagonal matrix as an example, a * 0 on the diagonal means that the corresponding joints will * not contribute to the motion of the system. On the other * hand, the bigger the value, the most the corresponding * joint will contribute to the overall motion. The obtained * solution q_dot will actually minimize the weighted norm * sqrt(q_dot'*(M_q^-2)*q_dot). In the special case we deal * with, it does not make sense to invert M_q but what is * important is the physical meaning of all this : a joint * that has a zero weight in M_q will not contribute to the * motion of the system and this is equivalent to saying that * it gets an infinite weight in the norm computation. For * more detailed explanation : vincent.padois@upmc.fr * * @return success/error code */ int setWeightJS(const Eigen::MatrixXd& Mq); /** * Set the task space weighting matrix * * @param weight_ts task space weighting symmetric matrix, * default: identity M_x : This matrix being used as a weight * for the norm of the error (in terms of task space speed) it * HAS TO BE symmetric and positive definite. We can actually * deal with matrices containing a symmetric and positive * definite block and 0s otherwise. Taking a diagonal matrix * as an example, a 0 on the diagonal means that the * corresponding task coordinate will not be taken into * account (ie the corresponding error can be really big). If * the rank of the jacobian is equal to the number of task * space coordinates which do not have a 0 weight in M_x, the * weighting will actually not impact the results (ie there is * an exact solution to the velocity inverse kinematics * problem). In cases without an exact solution, the bigger * the value, the most the corresponding task coordinate will * be taken into account (ie the more the corresponding error * will be reduced). The obtained solution will minimize the * weighted norm sqrt(|x_dot-Jq_dot|'*(M_x^2)*|x_dot-Jq_dot|). * For more detailed explanation : vincent.padois@upmc.fr * * @return success/error code */ int setWeightTS(const Eigen::MatrixXd& Mx); /** * Set lambda */ void setLambda(const double lambda); /** * Set eps */ void setEps(const double eps_in); /** * Set maxIter */ void setMaxIter(const int maxiter_in); /** * Request the number of singular values of the jacobian that are < eps; * if the number of near zero singular values is > jac.col()-jac.row(), * then the jacobian pseudoinverse is singular */ unsigned int getNrZeroSigmas()const {return nrZeroSigmas;}; /** * Request the minimum of the first six singular values */ double getSigmaMin()const {return sigmaMin;}; /** * Request the six singular values of the Jacobian */ int getSigma(Eigen::VectorXd& Sout); /** * Request the value of eps */ double getEps()const {return eps;}; /** * Request the value of lambda for the minimum */ double getLambda()const {return lambda;}; /** * Request the scaled value of lambda for the minimum * singular value 1-6 */ double getLambdaScaled()const {return lambda_scaled;}; /** * Retrieve the latest return code from the SVD algorithm * @return 0 if CartToJnt() not yet called, otherwise latest SVD result code. */ int getSVDResult()const {return svdResult;}; /// @copydoc KDL::SolverI::strError() virtual const char* strError(const int error) const; /// @copydoc KDL::SolverI::updateInternalDataStructures() virtual void updateInternalDataStructures(); private: const Chain& chain; ChainJntToJacSolver jnt2jac; unsigned int nj; Jacobian jac; Eigen::MatrixXd U; Eigen::VectorXd S; Eigen::MatrixXd V; double eps; int maxiter; Eigen::VectorXd tmp; Eigen::MatrixXd tmp_jac; Eigen::MatrixXd tmp_jac_weight1; Eigen::MatrixXd tmp_jac_weight2; Eigen::MatrixXd tmp_ts; Eigen::MatrixXd tmp_js; Eigen::MatrixXd weight_ts; Eigen::MatrixXd weight_js; double lambda; double lambda_scaled; unsigned int nrZeroSigmas ; int svdResult; double sigmaMin; }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/chainjnttojacdotsolver.cpp000066400000000000000000000175671326704774600240420ustar00rootroot00000000000000/* Computes the Jacobian time derivative Copyright (C) 2015 Antoine Hoarau 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 Street, Fifth Floor, Boston, MA 02110-1301 USA */ #include "chainjnttojacdotsolver.hpp" namespace KDL { ChainJntToJacDotSolver::ChainJntToJacDotSolver(const Chain& _chain): chain(_chain), locked_joints_(chain.getNrOfJoints(),false), nr_of_unlocked_joints_(chain.getNrOfJoints()), jac_solver_(chain), jac_(chain.getNrOfJoints()), jac_dot_(chain.getNrOfJoints()), representation_(HYBRID), fk_solver_(chain) { } void ChainJntToJacDotSolver::updateInternalDataStructures() { locked_joints_.resize(chain.getNrOfJoints(),false); this->setLockedJoints(locked_joints_); jac_solver_.updateInternalDataStructures(); jac_.resize(chain.getNrOfJoints()); jac_dot_.resize(chain.getNrOfJoints()); fk_solver_.updateInternalDataStructures(); } int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Twist& jac_dot_q_dot, int seg_nr) { error = JntToJacDot(q_in,jac_dot_,seg_nr); if (error != E_NOERROR) { return error; } MultiplyJacobian(jac_dot_,q_in.qdot,jac_dot_q_dot); return (error = E_NOERROR); } int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot, int seg_nr) { if(locked_joints_.size() != chain.getNrOfJoints()) return (error = E_NOT_UP_TO_DATE); unsigned int segmentNr; if(seg_nr<0) segmentNr=chain.getNrOfSegments(); else segmentNr = seg_nr; //Initialize Jacobian to zero since only segmentNr columns are computed SetToZero(jdot) ; if(q_in.q.rows()!=chain.getNrOfJoints() || nr_of_unlocked_joints_!=jdot.columns()) return (error = E_SIZE_MISMATCH); else if(segmentNr>chain.getNrOfSegments()) return (error = E_OUT_OF_RANGE); // First compute the jacobian in the Hybrid representation if (jac_solver_.JntToJac(q_in.q,jac_,segmentNr) != E_NOERROR) return (error = E_JACSOLVER_FAILED); // Change the reference frame and/or the reference point if (representation_ != HYBRID) // If HYBRID do nothing is this is the default. { if (fk_solver_.JntToCart(q_in.q,F_bs_ee_,segmentNr) != E_NOERROR) return (error = E_FKSOLVERPOS_FAILED); if (representation_ == BODYFIXED) { // Ref Frame {ee}, Ref Point {ee} jac_.changeBase(F_bs_ee_.M.Inverse()); } else if (representation_ == INTERTIAL) { // Ref Frame {bs}, Ref Point {bs} jac_.changeRefPoint(-F_bs_ee_.p); } else { return (error = E_JAC_DOT_FAILED); } } // Let's compute Jdot in the corresponding representation int k=0; for(unsigned int i=0;it_djdq_); return t_djdq_; } } const Twist& ChainJntToJacDotSolver::getPartialDerivativeHybrid(const KDL::Jacobian& bs_J_ee, const unsigned int& joint_idx, const unsigned int& column_idx) { int j=joint_idx; int i=column_idx; jac_j_ = bs_J_ee.getColumn(j); jac_i_ = bs_J_ee.getColumn(i); SetToZero(t_djdq_); if(j < i) { // P_{\Delta}({}_{bs}J^{j}) ref (20) t_djdq_.vel = jac_j_.rot * jac_i_.vel; t_djdq_.rot = jac_j_.rot * jac_i_.rot; }else if(j > i) { // M_{\Delta}({}_{bs}J^{j}) ref (23) SetToZero(t_djdq_.rot); t_djdq_.vel = -jac_j_.vel * jac_i_.rot; }else if(j == i) { // ref (40) SetToZero(t_djdq_.rot); t_djdq_.vel = jac_i_.rot * jac_i_.vel; } return t_djdq_; } const Twist& ChainJntToJacDotSolver::getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee, const unsigned int& joint_idx, const unsigned int& column_idx) { int j=joint_idx; int i=column_idx; SetToZero(t_djdq_); if(j > i) { jac_j_ = ee_J_ee.getColumn(j); jac_i_ = ee_J_ee.getColumn(i); // - S_d_(ee_J^j) * ee_J^ee ref (23) t_djdq_.vel = jac_j_.rot * jac_i_.vel + jac_j_.vel * jac_i_.rot; t_djdq_.rot = jac_j_.rot * jac_i_.rot; t_djdq_ = -t_djdq_; } return t_djdq_; } const Twist& ChainJntToJacDotSolver::getPartialDerivativeInertial(const KDL::Jacobian& bs_J_bs, const unsigned int& joint_idx, const unsigned int& column_idx) { int j=joint_idx; int i=column_idx; SetToZero(t_djdq_); if(j < i) { jac_j_ = bs_J_bs.getColumn(j); jac_i_ = bs_J_bs.getColumn(i); // S_d_(bs_J^j) * bs_J^bs ref (23) t_djdq_.vel = jac_j_.rot * jac_i_.vel + jac_j_.vel * jac_i_.rot; t_djdq_.rot = jac_j_.rot * jac_i_.rot; } return t_djdq_; } void ChainJntToJacDotSolver::setRepresentation(const int& representation) { if(representation == HYBRID || representation == BODYFIXED || representation == INTERTIAL) this->representation_ = representation; } int ChainJntToJacDotSolver::setLockedJoints(const std::vector& locked_joints) { if(locked_joints.size()!=locked_joints_.size()) return E_SIZE_MISMATCH; locked_joints_=locked_joints; nr_of_unlocked_joints_=0; for(unsigned int i=0;i 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 Street, Fifth Floor, Boston, MA 02110-1301 USA */ #ifndef KDL_CHAINJNTTOJACDOTSOLVER_HPP #define KDL_CHAINJNTTOJACDOTSOLVER_HPP #include "solveri.hpp" #include "frames.hpp" #include "jntarrayvel.hpp" #include "jacobian.hpp" #include "chain.hpp" #include "framevel.hpp" #include "chainjnttojacsolver.hpp" #include "chainfksolverpos_recursive.hpp" namespace KDL { /** * @brief Computes the Jacobian time derivative (Jdot) by calculating the * partial derivatives regarding to a joint angle, in the Hybrid, Body-fixed * or Inertial representation. * * This work is based on : * Symbolic differentiation of the velocity mapping for a serial kinematic chain * H. Bruyninckx, J. De Schutter * doi:10.1016/0094-114X(95)00069-B * * url : http://www.sciencedirect.com/science/article/pii/0094114X9500069B */ class ChainJntToJacDotSolver : public SolverI { public: static const int E_JAC_DOT_FAILED = -100; static const int E_JACSOLVER_FAILED = -101; static const int E_FKSOLVERPOS_FAILED = -102; // Hybrid representation ref Frame: base, ref Point: end-effector static const int HYBRID = 0; // Body-fixed representation ref Frame: end-effector, ref Point: end-effector static const int BODYFIXED = 1; // Intertial representation ref Frame: base, ref Point: base static const int INTERTIAL = 2; explicit ChainJntToJacDotSolver(const Chain& chain); virtual ~ChainJntToJacDotSolver(); /** * @brief Computes \f$ {}_{bs}\dot{J}^{ee}.\dot{q} \f$ * * @param q_in Current joint positions and velocities * @param jac_dot_q_dot The twist representing Jdot*qdot * @param seg_nr The final segment to compute * @return int 0 if no errors happened */ virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Twist& jac_dot_q_dot, int seg_nr = -1); /** * @brief Computes \f$ {}_{bs}\dot{J}^{ee} \f$ * * @param q_in Current joint positions and velocities * @param jdot The jacobian time derivative in the configured representation * (HYBRID, BODYFIXED or INERTIAL) * @param seg_nr The final segment to compute * @return int 0 if no errors happened */ virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Jacobian& jdot, int seg_nr = -1); int setLockedJoints(const std::vector& locked_joints); /** * @brief JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector) * * * @return void */ void setHybridRepresentation(){setRepresentation(HYBRID);} /** * @brief JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector, ref Point: end-effector) * * @return void */ void setBodyFixedRepresentation(){setRepresentation(BODYFIXED);} /** * @brief JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base) * * @return void */ void setInternialRepresentation(){setRepresentation(INTERTIAL);} /** * @brief Sets the internal variable for the representation (with a check on the value) * * @param representation The representation for Jdot : HYBRID,BODYFIXED or INTERTIAL * @return void */ void setRepresentation(const int& representation); /// @copydoc KDL::SolverI::updateInternalDataStructures virtual void updateInternalDataStructures(); /// @copydoc KDL::SolverI::strError() virtual const char* strError(const int error) const; protected: /** * @brief Computes \f$ \frac{\partial {}_{bs}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$ * * @param bs_J_ee The Jacobian expressed in the base frame with the end effector as the reference point (default in KDL Jacobian Solver) * @param joint_idx The index of the current joint (j in the formula) * @param column_idx The index of the current column (i in the formula) * @return Twist The twist representing dJi/dqj .qdotj */ const Twist& getPartialDerivativeHybrid(const Jacobian& bs_J_ee, const unsigned int& joint_idx, const unsigned int& column_idx); /** * @brief Computes \f$ \frac{\partial {}_{ee}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$ * * @param bs_J_ee The Jacobian expressed in the end effector frame with the end effector as the reference point * @param joint_idx The indice of the current joint (j in the formula) * @param column_idx The indice of the current column (i in the formula) * @return Twist The twist representing dJi/dqj .qdotj */ const Twist& getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee, const unsigned int& joint_idx, const unsigned int& column_idx); /** * @brief Computes \f$ \frac{\partial {}_{bs}J^{i,bs}}{\partial q^{j}}.\dot{q}^{j} \f$ * * @param ee_J_ee The Jacobian expressed in the base frame with the base as the reference point * @param joint_idx The indice of the current joint (j in the formula) * @param column_idx The indice of the current column (i in the formula) * @return Twist The twist representing dJi/dqj .qdotj */ const Twist& getPartialDerivativeInertial(const Jacobian& bs_J_bs, const unsigned int& joint_idx, const unsigned int& column_idx); /** * @brief Computes \f$ \frac{\partial J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$ * * @param bs_J_bs The Jacobian expressed in the base frame with the end effector as the reference point * @param joint_idx The indice of the current joint (j in the formula) * @param column_idx The indice of the current column (i in the formula) * @param representation The representation (Hybrid,Body-fixed,Inertial) in which you want to get dJ/dqj .qdotj * @return Twist The twist representing dJi/dqj .qdotj */ const Twist& getPartialDerivative(const Jacobian& J, const unsigned int& joint_idx, const unsigned int& column_idx, const int& representation); private: const Chain& chain; std::vector locked_joints_; unsigned int nr_of_unlocked_joints_; ChainJntToJacSolver jac_solver_; Jacobian jac_; Jacobian jac_dot_; int representation_; ChainFkSolverPos_recursive fk_solver_; Frame F_bs_ee_; Twist jac_dot_k_; Twist jac_j_, jac_i_; Twist t_djdq_; }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/chainjnttojacsolver.cpp000066400000000000000000000072421326704774600233200ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "chainjnttojacsolver.hpp" namespace KDL { ChainJntToJacSolver::ChainJntToJacSolver(const Chain& _chain): chain(_chain),locked_joints_(chain.getNrOfJoints(),false) { } void ChainJntToJacSolver::updateInternalDataStructures() { locked_joints_.resize(chain.getNrOfJoints(),false); } ChainJntToJacSolver::~ChainJntToJacSolver() { } int ChainJntToJacSolver::setLockedJoints(const std::vector locked_joints) { if(locked_joints_.size() != chain.getNrOfJoints()) return (error = E_NOT_UP_TO_DATE); if(locked_joints.size()!=locked_joints_.size()) return (error = E_SIZE_MISMATCH); locked_joints_=locked_joints; return (error = E_NOERROR); } int ChainJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, int seg_nr) { if(locked_joints_.size() != chain.getNrOfJoints()) return (error = E_NOT_UP_TO_DATE); unsigned int segmentNr; if(seg_nr<0) segmentNr=chain.getNrOfSegments(); else segmentNr = seg_nr; //Initialize Jacobian to zero since only segmentNr colunns are computed SetToZero(jac) ; if( q_in.rows()!=chain.getNrOfJoints() || jac.columns() != chain.getNrOfJoints()) return (error = E_SIZE_MISMATCH); else if(segmentNr>chain.getNrOfSegments()) return (error = E_OUT_OF_RANGE); T_tmp = Frame::Identity(); SetToZero(t_tmp); int j=0; int k=0; Frame total; for (unsigned int i=0;i // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_CHAINJNTTOJACSOLVER_HPP #define KDL_CHAINJNTTOJACSOLVER_HPP #include "solveri.hpp" #include "frames.hpp" #include "jacobian.hpp" #include "jntarray.hpp" #include "chain.hpp" namespace KDL { /** * @brief Class to calculate the jacobian of a general * KDL::Chain, it is used by other solvers. It should not be used * outside of KDL. * * */ class ChainJntToJacSolver : public SolverI { public: explicit ChainJntToJacSolver(const Chain& chain); virtual ~ChainJntToJacSolver(); /** * Calculate the jacobian expressed in the base frame of the * chain, with reference point at the end effector of the * *chain. The alghoritm is similar to the one used in * KDL::ChainFkSolverVel_recursive * * @param q_in input joint positions * @param jac output jacobian * * @return success/error code */ virtual int JntToJac(const JntArray& q_in, Jacobian& jac, int segmentNR=-1); /** * * @param locked_joints new values for locked joints * @return success/error code */ int setLockedJoints(const std::vector locked_joints); /// @copydoc KDL::SolverI::updateInternalDataStructures virtual void updateInternalDataStructures(); private: const Chain& chain; Twist t_tmp; Frame T_tmp; std::vector locked_joints_; }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/config.h.in000066400000000000000000000027311326704774600205630ustar00rootroot00000000000000// Copyright (C) 2014 Ruben Smits // Version: 1.0 // Author: Brian Jensen // Maintainer: Ruben Smits // 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 KDL_CONFIG_H #define KDL_CONFIG_H #define KDL_VERSION_MAJOR @KDL_VERSION_MAJOR@ #define KDL_VERSION_MINOR @KDL_VERSION_MINOR@ #define KDL_VERSION_PATCH @KDL_VERSION_PATCH@ #define KDL_VERSION (KDL_VERSION_MAJOR << 16) | (KDL_VERSION_MINOR << 8) | KDL_VERSION_PATCH #define KDL_VERSION_STRING "@KDL_VERSION@" //Set which version of the Tree Interface to use #cmakedefine HAVE_STL_CONTAINER_INCOMPLETE_TYPES #cmakedefine KDL_USE_NEW_TREE_INTERFACE #endif //#define KDL_CONFIG_H orocos-kdl-1.4.0/orocos_kdl/src/frameacc.cpp000066400000000000000000000007701326704774600210060ustar00rootroot00000000000000/***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: rrframes.cpp,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ * $Name: $ ****************************************************************************/ #include "frameacc.hpp" namespace KDL { #ifndef KDL_INLINE #include "frameacc.inl" #endif } orocos-kdl-1.4.0/orocos_kdl/src/frameacc.hpp000066400000000000000000000257751326704774600210270ustar00rootroot00000000000000/***************************************************************************** * \file * This file contains the definition of classes for a * Rall Algebra of (subset of) the classes defined in frames, * i.e. classes that contain a set (value,derivative,2nd derivative) * and define operations on that set * this classes are useful for automatic differentiation ( <-> symbolic diff , * <-> numeric diff). * Defines VectorAcc, RotationAcc, FrameAcc, doubleAcc. * Look at the corresponding classes Vector Rotation Frame Twist and * Wrench for the semantics of the methods. * * It also contains the 2nd derivative <-> RFrames.h * * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: rrframes.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ * $Name: $ ****************************************************************************/ #ifndef RRFRAMES_H #define RRFRAMES_H #include "utilities/rall2d.h" #include "frames.hpp" namespace KDL { class TwistAcc; typedef Rall2d doubleAcc; // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) class FrameAcc; class RotationAcc; class VectorAcc; IMETHOD bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon); IMETHOD bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon); IMETHOD bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon); IMETHOD bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon); IMETHOD bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon); IMETHOD bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon); IMETHOD bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon); IMETHOD bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon); IMETHOD bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon); IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon); IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon); IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon); class VectorAcc { public: Vector p; //!< position vector Vector v; //!< velocity vector Vector dv; //!< acceleration vector public: VectorAcc():p(),v(),dv() {} explicit VectorAcc(const Vector& _p):p(_p),v(Vector::Zero()),dv(Vector::Zero()) {} VectorAcc(const Vector& _p,const Vector& _v):p(_p),v(_v),dv(Vector::Zero()) {} VectorAcc(const Vector& _p,const Vector& _v,const Vector& _dv): p(_p),v(_v),dv(_dv) {} IMETHOD VectorAcc& operator = (const VectorAcc& arg); IMETHOD VectorAcc& operator = (const Vector& arg); IMETHOD VectorAcc& operator += (const VectorAcc& arg); IMETHOD VectorAcc& operator -= (const VectorAcc& arg); IMETHOD static VectorAcc Zero(); IMETHOD void ReverseSign(); IMETHOD doubleAcc Norm(); IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2); IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2); IMETHOD friend VectorAcc operator + (const Vector& r1,const VectorAcc& r2); IMETHOD friend VectorAcc operator - (const Vector& r1,const VectorAcc& r2); IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const Vector& r2); IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const Vector& r2); IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2); IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const Vector& r2); IMETHOD friend VectorAcc operator * (const Vector& r1,const VectorAcc& r2); IMETHOD friend VectorAcc operator * (const VectorAcc& r1,double r2); IMETHOD friend VectorAcc operator * (double r1,const VectorAcc& r2); IMETHOD friend VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2); IMETHOD friend VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1); IMETHOD friend VectorAcc operator*(const Rotation& R,const VectorAcc& x); IMETHOD friend VectorAcc operator / (const VectorAcc& r1,double r2); IMETHOD friend VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1); IMETHOD friend bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps); IMETHOD friend bool Equal(const Vector& r1,const VectorAcc& r2,double eps); IMETHOD friend bool Equal(const VectorAcc& r1,const Vector& r2,double eps); IMETHOD friend VectorAcc operator - (const VectorAcc& r); IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs); IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const Vector& rhs); IMETHOD friend doubleAcc dot(const Vector& lhs,const VectorAcc& rhs); }; class RotationAcc { public: Rotation R; //!< rotation matrix Vector w; //!< angular velocity vector Vector dw; //!< angular acceration vector public: RotationAcc():R(),w() {} explicit RotationAcc(const Rotation& _R):R(_R),w(Vector::Zero()){} RotationAcc(const Rotation& _R,const Vector& _w,const Vector& _dw): R(_R),w(_w),dw(_dw) {} IMETHOD RotationAcc& operator = (const RotationAcc& arg); IMETHOD RotationAcc& operator = (const Rotation& arg); IMETHOD static RotationAcc Identity(); IMETHOD RotationAcc Inverse() const; IMETHOD VectorAcc Inverse(const VectorAcc& arg) const; IMETHOD VectorAcc Inverse(const Vector& arg) const; IMETHOD VectorAcc operator*(const VectorAcc& arg) const; IMETHOD VectorAcc operator*(const Vector& arg) const; // Rotations // The SetRot.. functions set the value of *this to the appropriate rotation matrix. // The Rot... static functions give the value of the appropriate rotation matrix back. // The DoRot... functions apply a rotation R to *this,such that *this = *this * R. // IMETHOD void DoRotX(const doubleAcc& angle); // IMETHOD void DoRotY(const doubleAcc& angle); // IMETHOD void DoRotZ(const doubleAcc& angle); // IMETHOD static RRotation RotX(const doubleAcc& angle); // IMETHOD static RRotation RotY(const doubleAcc& angle); // IMETHOD static RRotation RotZ(const doubleAcc& angle); // IMETHOD void SetRot(const Vector& rotaxis,const doubleAcc& angle); // Along an arbitrary axes. The norm of rotvec is neglected. // IMETHOD static RotationAcc Rot(const Vector& rotvec,const doubleAcc& angle); // rotvec has arbitrary norm // rotation around a constant vector ! // IMETHOD static RotationAcc Rot2(const Vector& rotvec,const doubleAcc& angle); // rotvec is normalized. // rotation around a constant vector ! IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2); IMETHOD friend RotationAcc operator* (const Rotation& r1,const RotationAcc& r2); IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const Rotation& r2); IMETHOD friend bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps); IMETHOD friend bool Equal(const Rotation& r1,const RotationAcc& r2,double eps); IMETHOD friend bool Equal(const RotationAcc& r1,const Rotation& r2,double eps); IMETHOD TwistAcc Inverse(const TwistAcc& arg) const; IMETHOD TwistAcc Inverse(const Twist& arg) const; IMETHOD TwistAcc operator * (const TwistAcc& arg) const; IMETHOD TwistAcc operator * (const Twist& arg) const; }; class FrameAcc { public: RotationAcc M; //!< Rotation,angular velocity, and angular acceleration of frame. VectorAcc p; //!< Translation, velocity and acceleration of origin. public: FrameAcc(){} explicit FrameAcc(const Frame& _T):M(_T.M),p(_T.p) {} FrameAcc(const Frame& _T,const Twist& _t,const Twist& _dt): M(_T.M,_t.rot,_dt.rot),p(_T.p,_t.vel,_dt.vel) {} FrameAcc(const RotationAcc& _M,const VectorAcc& _p):M(_M),p(_p) {} IMETHOD FrameAcc& operator = (const FrameAcc& arg); IMETHOD FrameAcc& operator = (const Frame& arg); IMETHOD static FrameAcc Identity(); IMETHOD FrameAcc Inverse() const; IMETHOD VectorAcc Inverse(const VectorAcc& arg) const; IMETHOD VectorAcc operator*(const VectorAcc& arg) const; IMETHOD VectorAcc operator*(const Vector& arg) const; IMETHOD VectorAcc Inverse(const Vector& arg) const; IMETHOD Frame GetFrame() const; IMETHOD Twist GetTwist() const; IMETHOD Twist GetAccTwist() const; IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const FrameAcc& f2); IMETHOD friend FrameAcc operator * (const Frame& f1,const FrameAcc& f2); IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const Frame& f2); IMETHOD friend bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps); IMETHOD friend bool Equal(const Frame& r1,const FrameAcc& r2,double eps); IMETHOD friend bool Equal(const FrameAcc& r1,const Frame& r2,double eps); IMETHOD TwistAcc Inverse(const TwistAcc& arg) const; IMETHOD TwistAcc Inverse(const Twist& arg) const; IMETHOD TwistAcc operator * (const TwistAcc& arg) const; IMETHOD TwistAcc operator * (const Twist& arg) const; }; //very similar to Wrench class. class TwistAcc { public: VectorAcc vel; //!< translational velocity and its 1st and 2nd derivative VectorAcc rot; //!< rotational velocity and its 1st and 2nd derivative public: TwistAcc():vel(),rot() {}; TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {}; IMETHOD TwistAcc& operator-=(const TwistAcc& arg); IMETHOD TwistAcc& operator+=(const TwistAcc& arg); IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,double rhs); IMETHOD friend TwistAcc operator*(double lhs,const TwistAcc& rhs); IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,double rhs); IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs); IMETHOD friend TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs); IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs); IMETHOD friend TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs); IMETHOD friend TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs); IMETHOD friend TwistAcc operator-(const TwistAcc& arg); IMETHOD friend void SetToZero(TwistAcc& v); static IMETHOD TwistAcc Zero(); IMETHOD void ReverseSign(); IMETHOD TwistAcc RefPoint(const VectorAcc& v_base_AB); // Changes the reference point of the RTwist. // The RVector v_base_AB is expressed in the same base as the RTwist // The RVector v_base_AB is a RVector from the old point to // the new point. // Complexity : 6M+6A IMETHOD friend bool Equal(const TwistAcc& a,const TwistAcc& b,double eps); IMETHOD friend bool Equal(const Twist& a,const TwistAcc& b,double eps); IMETHOD friend bool Equal(const TwistAcc& a,const Twist& b,double eps); IMETHOD Twist GetTwist() const; IMETHOD Twist GetTwistDot() const; friend class RotationAcc; friend class FrameAcc; }; #ifdef KDL_INLINE #include "frameacc.inl" #endif } #endif orocos-kdl-1.4.0/orocos_kdl/src/frameacc.inl000066400000000000000000000337401326704774600210110ustar00rootroot00000000000000/***************************************************************************** * \file * provides inline functions of rrframes.h * * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: rrframes.inl,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ * $Name: $ ****************************************************************************/ /////////////////// VectorAcc ///////////////////////////////////// VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2) { return VectorAcc(r1.p+r2.p,r1.v+r2.v,r1.dv+r2.dv); } VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2) { return VectorAcc(r1.p-r2.p, r1.v-r2.v, r1.dv-r2.dv); } VectorAcc operator + (const Vector& r1,const VectorAcc& r2) { return VectorAcc(r1+r2.p,r2.v,r2.dv); } VectorAcc operator - (const Vector& r1,const VectorAcc& r2) { return VectorAcc(r1-r2.p, -r2.v, -r2.dv); } VectorAcc operator + (const VectorAcc& r1,const Vector& r2) { return VectorAcc(r1.p+r2,r1.v,r1.dv); } VectorAcc operator - (const VectorAcc& r1,const Vector& r2) { return VectorAcc(r1.p-r2, r1.v, r1.dv); } // unary - VectorAcc operator - (const VectorAcc& r) { return VectorAcc(-r.p,-r.v,-r.dv); } // cross prod. VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2) { return VectorAcc(r1.p*r2.p, r1.p*r2.v+r1.v*r2.p, r1.dv*r2.p+2*r1.v*r2.v+r1.p*r2.dv ); } VectorAcc operator * (const VectorAcc& r1,const Vector& r2) { return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 ); } VectorAcc operator * (const Vector& r1,const VectorAcc& r2) { return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv ); } // scalar mult. VectorAcc operator * (double r1,const VectorAcc& r2) { return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv ); } VectorAcc operator * (const VectorAcc& r1,double r2) { return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 ); } VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2) { return VectorAcc(r1.t*r2.p, r1.t*r2.v + r1.d*r2.p, r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p ); } VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1) { return VectorAcc(r1.t*r2.p, r1.t*r2.v + r1.d*r2.p, r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p ); } VectorAcc& VectorAcc::operator = (const VectorAcc& arg) { p=arg.p; v=arg.v; dv=arg.dv; return *this; } VectorAcc& VectorAcc::operator = (const Vector& arg) { p=arg; v=Vector::Zero(); dv=Vector::Zero(); return *this; } VectorAcc& VectorAcc::operator += (const VectorAcc& arg) { p+=arg.p; v+=arg.v; dv+= arg.dv; return *this; } VectorAcc& VectorAcc::operator -= (const VectorAcc& arg) { p-=arg.p; v-=arg.v; dv-=arg.dv; return *this; } VectorAcc VectorAcc::Zero() { return VectorAcc(Vector::Zero(),Vector::Zero(),Vector::Zero()); } void VectorAcc::ReverseSign() { p.ReverseSign(); v.ReverseSign(); dv.ReverseSign(); } doubleAcc VectorAcc::Norm() { doubleAcc res; res.t = p.Norm(); res.d = dot(p,v)/res.t; res.dd = (dot(p,dv)+dot(v,v)-res.d*res.d)/res.t; return res; } doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs) { return doubleAcc( dot(lhs.p,rhs.p), dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p), dot(lhs.p,rhs.dv)+2*dot(lhs.v,rhs.v)+dot(lhs.dv,rhs.p) ); } doubleAcc dot(const VectorAcc& lhs,const Vector& rhs) { return doubleAcc( dot(lhs.p,rhs), dot(lhs.v,rhs), dot(lhs.dv,rhs) ); } doubleAcc dot(const Vector& lhs,const VectorAcc& rhs) { return doubleAcc( dot(lhs,rhs.p), dot(lhs,rhs.v), dot(lhs,rhs.dv) ); } bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps) { return (Equal(r1.p,r2.p,eps) && Equal(r1.v,r2.v,eps) && Equal(r1.dv,r2.dv,eps) ); } bool Equal(const Vector& r1,const VectorAcc& r2,double eps) { return (Equal(r1,r2.p,eps) && Equal(Vector::Zero(),r2.v,eps) && Equal(Vector::Zero(),r2.dv,eps) ); } bool Equal(const VectorAcc& r1,const Vector& r2,double eps) { return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps) && Equal(r1.dv,Vector::Zero(),eps) ); } VectorAcc operator / (const VectorAcc& r1,double r2) { return r1*(1.0/r2); } VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1) { return r2*(1.0/r1); } /////////////////// RotationAcc ///////////////////////////////////// RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2) { return RotationAcc( r1.R * r2.R, r1.w + r1.R*r2.w, r1.dw + r1.w*(r1.R*r2.w) + r1.R*r2.dw ); } RotationAcc operator* (const Rotation& r1,const RotationAcc& r2) { return RotationAcc( r1*r2.R, r1*r2.w, r1*r2.dw); } RotationAcc operator* (const RotationAcc& r1,const Rotation& r2) { return RotationAcc( r1.R*r2, r1.w, r1.dw ); } RotationAcc& RotationAcc::operator = (const RotationAcc& arg) { R=arg.R; w=arg.w; dw=arg.dw; return *this; } RotationAcc& RotationAcc::operator = (const Rotation& arg) { R = arg; w = Vector::Zero(); dw = Vector::Zero(); return *this; } RotationAcc RotationAcc::Identity() { return RotationAcc(Rotation::Identity(),Vector::Zero(),Vector::Zero()); } RotationAcc RotationAcc::Inverse() const { return RotationAcc(R.Inverse(),-R.Inverse(w),-R.Inverse(dw)); } VectorAcc RotationAcc::Inverse(const VectorAcc& arg) const { VectorAcc tmp; tmp.p = R.Inverse(arg.p); tmp.v = R.Inverse(arg.v - w * arg.p); tmp.dv = R.Inverse(arg.dv - dw*arg.p - w*(arg.v+R*tmp.v)); return tmp; } VectorAcc RotationAcc::Inverse(const Vector& arg) const { VectorAcc tmp; tmp.p = R.Inverse(arg); tmp.v = R.Inverse(-w*arg); tmp.dv = R.Inverse(-dw*arg - w*(R*tmp.v)); return tmp; } VectorAcc RotationAcc::operator*(const VectorAcc& arg) const { VectorAcc tmp; tmp.p = R*arg.p; tmp.dv = R*arg.v; tmp.v = w*tmp.p + tmp.dv; tmp.dv = dw*tmp.p + w*(tmp.v + tmp.dv) + R*arg.dv; return tmp; } VectorAcc operator*(const Rotation& R,const VectorAcc& x) { return VectorAcc(R*x.p,R*x.v,R*x.dv); } VectorAcc RotationAcc::operator*(const Vector& arg) const { VectorAcc tmp; tmp.p = R*arg; tmp.v = w*tmp.p; tmp.dv = dw*tmp.p + w*tmp.v; return tmp; } /* // = Rotations // The Rot... static functions give the value of the appropriate rotation matrix back. // The DoRot... functions apply a rotation R to *this,such that *this = *this * R. void RRotation::DoRotX(const RDouble& angle) { w+=R*Vector(angle.grad,0,0); R.DoRotX(angle.t); } RotationAcc RotationAcc::RotX(const doubleAcc& angle) { return RotationAcc(Rotation::RotX(angle.t), Vector(angle.d,0,0), Vector(angle.dd,0,0) ); } void RRotation::DoRotY(const RDouble& angle) { w+=R*Vector(0,angle.grad,0); R.DoRotY(angle.t); } RotationAcc RotationAcc::RotY(const doubleAcc& angle) { return RotationAcc( Rotation::RotX(angle.t), Vector(0,angle.d,0), Vector(0,angle.dd,0) ); } void RRotation::DoRotZ(const RDouble& angle) { w+=R*Vector(0,0,angle.grad); R.DoRotZ(angle.t); } RotationAcc RotationAcc::RotZ(const doubleAcc& angle) { return RotationAcc( Rotation::RotZ(angle.t), Vector(0,0,angle.d), Vector(0,0,angle.dd) ); } RRotation RRotation::Rot(const Vector& rotvec,const RDouble& angle) // rotvec has arbitrary norm // rotation around a constant vector ! { Vector v = rotvec.Normalize(); return RRotation(Rotation::Rot2(v,angle.t),v*angle.grad); } RRotation RRotation::Rot2(const Vector& rotvec,const RDouble& angle) // rotvec is normalized. { return RRotation(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad); } */ bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps) { return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps) && Equal(r1.dw,r2.dw,eps) ); } bool Equal(const Rotation& r1,const RotationAcc& r2,double eps) { return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps) && Equal(Vector::Zero(),r2.dw,eps) ); } bool Equal(const RotationAcc& r1,const Rotation& r2,double eps) { return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps) && Equal(r1.dw,Vector::Zero(),eps) ); } // Methods and operators related to FrameAcc // They all delegate most of the work to RotationAcc and VectorAcc FrameAcc& FrameAcc::operator = (const FrameAcc& arg) { M=arg.M; p=arg.p; return *this; } FrameAcc FrameAcc::Identity() { return FrameAcc(RotationAcc::Identity(),VectorAcc::Zero()); } FrameAcc operator *(const FrameAcc& lhs,const FrameAcc& rhs) { return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); } FrameAcc operator *(const FrameAcc& lhs,const Frame& rhs) { return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); } FrameAcc operator *(const Frame& lhs,const FrameAcc& rhs) { return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); } VectorAcc FrameAcc::operator *(const VectorAcc & arg) const { return M*arg+p; } VectorAcc FrameAcc::operator *(const Vector & arg) const { return M*arg+p; } VectorAcc FrameAcc::Inverse(const VectorAcc& arg) const { return M.Inverse(arg-p); } VectorAcc FrameAcc::Inverse(const Vector& arg) const { return M.Inverse(arg-p); } FrameAcc FrameAcc::Inverse() const { return FrameAcc(M.Inverse(),-M.Inverse(p)); } FrameAcc& FrameAcc::operator =(const Frame & arg) { M = arg.M; p = arg.p; return *this; } bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps) { return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); } bool Equal(const Frame& r1,const FrameAcc& r2,double eps) { return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); } bool Equal(const FrameAcc& r1,const Frame& r2,double eps) { return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); } Frame FrameAcc::GetFrame() const { return Frame(M.R,p.p); } Twist FrameAcc::GetTwist() const { return Twist(p.v,M.w); } Twist FrameAcc::GetAccTwist() const { return Twist(p.dv,M.dw); } TwistAcc TwistAcc::Zero() { return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero()); } void TwistAcc::ReverseSign() { vel.ReverseSign(); rot.ReverseSign(); } TwistAcc TwistAcc::RefPoint(const VectorAcc& v_base_AB) // Changes the reference point of the TwistAcc. // The RVector v_base_AB is expressed in the same base as the TwistAcc // The RVector v_base_AB is a RVector from the old point to // the new point. // Complexity : 6M+6A { return TwistAcc(this->vel+this->rot*v_base_AB,this->rot); } TwistAcc& TwistAcc::operator-=(const TwistAcc& arg) { vel-=arg.vel; rot -=arg.rot; return *this; } TwistAcc& TwistAcc::operator+=(const TwistAcc& arg) { vel+=arg.vel; rot +=arg.rot; return *this; } TwistAcc operator*(const TwistAcc& lhs,double rhs) { return TwistAcc(lhs.vel*rhs,lhs.rot*rhs); } TwistAcc operator*(double lhs,const TwistAcc& rhs) { return TwistAcc(lhs*rhs.vel,lhs*rhs.rot); } TwistAcc operator/(const TwistAcc& lhs,double rhs) { return TwistAcc(lhs.vel/rhs,lhs.rot/rhs); } TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs) { return TwistAcc(lhs.vel*rhs,lhs.rot*rhs); } TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs) { return TwistAcc(lhs*rhs.vel,lhs*rhs.rot); } TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs) { return TwistAcc(lhs.vel/rhs,lhs.rot/rhs); } // addition of TwistAcc's TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs) { return TwistAcc(lhs.vel+rhs.vel,lhs.rot+rhs.rot); } TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs) { return TwistAcc(lhs.vel-rhs.vel,lhs.rot-rhs.rot); } // unary - TwistAcc operator-(const TwistAcc& arg) { return TwistAcc(-arg.vel,-arg.rot); } TwistAcc RotationAcc::Inverse(const TwistAcc& arg) const { return TwistAcc(Inverse(arg.vel),Inverse(arg.rot)); } TwistAcc RotationAcc::operator * (const TwistAcc& arg) const { return TwistAcc((*this)*arg.vel,(*this)*arg.rot); } TwistAcc RotationAcc::Inverse(const Twist& arg) const { return TwistAcc(Inverse(arg.vel),Inverse(arg.rot)); } TwistAcc RotationAcc::operator * (const Twist& arg) const { return TwistAcc((*this)*arg.vel,(*this)*arg.rot); } TwistAcc FrameAcc::operator * (const TwistAcc& arg) const { TwistAcc tmp; tmp.rot = M*arg.rot; tmp.vel = M*arg.vel+p*tmp.rot; return tmp; } TwistAcc FrameAcc::operator * (const Twist& arg) const { TwistAcc tmp; tmp.rot = M*arg.rot; tmp.vel = M*arg.vel+p*tmp.rot; return tmp; } TwistAcc FrameAcc::Inverse(const TwistAcc& arg) const { TwistAcc tmp; tmp.rot = M.Inverse(arg.rot); tmp.vel = M.Inverse(arg.vel-p*arg.rot); return tmp; } TwistAcc FrameAcc::Inverse(const Twist& arg) const { TwistAcc tmp; tmp.rot = M.Inverse(arg.rot); tmp.vel = M.Inverse(arg.vel-p*arg.rot); return tmp; } Twist TwistAcc::GetTwist() const { return Twist(vel.p,rot.p); } Twist TwistAcc::GetTwistDot() const { return Twist(vel.v,rot.v); } bool Equal(const TwistAcc& a,const TwistAcc& b,double eps) { return (Equal(a.rot,b.rot,eps)&& Equal(a.vel,b.vel,eps) ); } bool Equal(const Twist& a,const TwistAcc& b,double eps) { return (Equal(a.rot,b.rot,eps)&& Equal(a.vel,b.vel,eps) ); } bool Equal(const TwistAcc& a,const Twist& b,double eps) { return (Equal(a.rot,b.rot,eps)&& Equal(a.vel,b.vel,eps) ); } orocos-kdl-1.4.0/orocos_kdl/src/frameacc_io.hpp000066400000000000000000000027341326704774600215040ustar00rootroot00000000000000/***************************************************************************** * \file * Defines I/O related routines to the FrameAccs classes defined in * FrameAccs.h * * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: rrframes_io.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ * $Name: $ ****************************************************************************/ #ifndef RRFRAMES_IO #define RRFRAMES_IO #include "utilities/utility_io.h" #include "utilities/rall2d_io.h" #include "frames_io.hpp" #include "frameacc.hpp" namespace KDL { // Output... inline std::ostream& operator << (std::ostream& os,const VectorAcc& r) { os << "{" << r.p << "," << r.v << "," << r.dv << "}" << std::endl; return os; } inline std::ostream& operator << (std::ostream& os,const RotationAcc& r) { os << "{" << std::endl << r.R << "," << std::endl << r.w << "," << std::endl << r.dw << std::endl << "}" << std::endl; return os; } inline std::ostream& operator << (std::ostream& os,const FrameAcc& r) { os << "{" << std::endl << r.M << "," << std::endl << r.p << "}" << std::endl; return os; } inline std::ostream& operator << (std::ostream& os,const TwistAcc& r) { os << "{" << std::endl << r.vel << "," << std::endl << r.rot << std::endl << "}" << std::endl; return os; } } // namespace Frame #endif orocos-kdl-1.4.0/orocos_kdl/src/frames.cpp000066400000000000000000000410161326704774600205200ustar00rootroot00000000000000/*************************************************************************** frames.cxx - description ------------------------- begin : June 2006 copyright : (C) 2006 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 "frames.hpp" #define _USE_MATH_DEFINES // For MSVC #include #include namespace KDL { #ifndef KDL_INLINE #include "frames.inl" #endif void Frame::Make4x4(double * d) { int i; int j; for (i=0;i<3;i++) { for (j=0;j<3;j++) d[i*4+j]=M(i,j); d[i*4+3] = p(i); } for (j=0;j<3;j++) d[12+j] = 0.; d[15] = 1; } Frame Frame::DH_Craig1989(double a,double alpha,double d,double theta) // returns Modified Denavit-Hartenberg parameters (According to Craig) { double ct,st,ca,sa; ct = cos(theta); st = sin(theta); sa = sin(alpha); ca = cos(alpha); return Frame(Rotation( ct, -st, 0, st*ca, ct*ca, -sa, st*sa, ct*sa, ca ), Vector( a, -sa*d, ca*d ) ); } Frame Frame::DH(double a,double alpha,double d,double theta) // returns Denavit-Hartenberg parameters (Non-Modified DH) { double ct,st,ca,sa; ct = cos(theta); st = sin(theta); sa = sin(alpha); ca = cos(alpha); return Frame(Rotation( ct, -st*ca, st*sa, st, ct*ca, -ct*sa, 0, sa, ca ), Vector( a*ct, a*st, d ) ); } double Vector2::Norm() const { double tmp1 = fabs(data[0]); double tmp2 = fabs(data[1]); if (tmp1 == 0.0 && tmp2 == 0.0) return 0.0; if (tmp1 > tmp2) { return tmp1*sqrt(1+sqr(data[1]/data[0])); } else { return tmp2*sqrt(1+sqr(data[0]/data[1])); } } // makes v a unitvector and returns the norm of v. // if v is smaller than eps, Vector(1,0,0) is returned with norm 0. // if this is not good, check the return value of this method. double Vector2::Normalize(double eps) { double v = this->Norm(); if (v < eps) { *this = Vector2(1,0); return v; } else { *this = (*this)/v; return v; } } // do some effort not to lose precision double Vector::Norm() const { double tmp1; double tmp2; tmp1 = fabs(data[0]); tmp2 = fabs(data[1]); if (tmp1 >= tmp2) { tmp2=fabs(data[2]); if (tmp1 >= tmp2) { if (tmp1 == 0) { // only to everything exactly zero case, all other are handled correctly return 0; } return tmp1*sqrt(1+sqr(data[1]/data[0])+sqr(data[2]/data[0])); } else { return tmp2*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2])); } } else { tmp1=fabs(data[2]); if (tmp2 > tmp1) { return tmp2*sqrt(1+sqr(data[0]/data[1])+sqr(data[2]/data[1])); } else { return tmp1*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2])); } } } // makes v a unitvector and returns the norm of v. // if v is smaller than eps, Vector(1,0,0) is returned with norm 0. // if this is not good, check the return value of this method. double Vector::Normalize(double eps) { double v = this->Norm(); if (v < eps) { *this = Vector(1,0,0); return v; } else { *this = (*this)/v; return v; } } bool Equal(const Rotation& a,const Rotation& b,double eps) { return (Equal(a.data[0],b.data[0],eps) && Equal(a.data[1],b.data[1],eps) && Equal(a.data[2],b.data[2],eps) && Equal(a.data[3],b.data[3],eps) && Equal(a.data[4],b.data[4],eps) && Equal(a.data[5],b.data[5],eps) && Equal(a.data[6],b.data[6],eps) && Equal(a.data[7],b.data[7],eps) && Equal(a.data[8],b.data[8],eps) ); } Rotation operator *(const Rotation& lhs,const Rotation& rhs) // Complexity : 27M+27A { return Rotation( lhs.data[0]*rhs.data[0]+lhs.data[1]*rhs.data[3]+lhs.data[2]*rhs.data[6], lhs.data[0]*rhs.data[1]+lhs.data[1]*rhs.data[4]+lhs.data[2]*rhs.data[7], lhs.data[0]*rhs.data[2]+lhs.data[1]*rhs.data[5]+lhs.data[2]*rhs.data[8], lhs.data[3]*rhs.data[0]+lhs.data[4]*rhs.data[3]+lhs.data[5]*rhs.data[6], lhs.data[3]*rhs.data[1]+lhs.data[4]*rhs.data[4]+lhs.data[5]*rhs.data[7], lhs.data[3]*rhs.data[2]+lhs.data[4]*rhs.data[5]+lhs.data[5]*rhs.data[8], lhs.data[6]*rhs.data[0]+lhs.data[7]*rhs.data[3]+lhs.data[8]*rhs.data[6], lhs.data[6]*rhs.data[1]+lhs.data[7]*rhs.data[4]+lhs.data[8]*rhs.data[7], lhs.data[6]*rhs.data[2]+lhs.data[7]*rhs.data[5]+lhs.data[8]*rhs.data[8] ); } Rotation Rotation::Quaternion(double x,double y,double z, double w) { double x2, y2, z2, w2; x2 = x*x; y2 = y*y; z2 = z*z; w2 = w*w; return Rotation(w2+x2-y2-z2, 2*x*y-2*w*z, 2*x*z+2*w*y, 2*x*y+2*w*z, w2-x2+y2-z2, 2*y*z-2*w*x, 2*x*z-2*w*y, 2*y*z+2*w*x, w2-x2-y2+z2); } /* From the following sources: http://web.archive.org/web/20041029003853/http:/www.j3d.org/matrix_faq/matrfaq_latest.html http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm RobOOP::quaternion.cpp */ void Rotation::GetQuaternion(double& x,double& y,double& z, double& w) const { double trace = (*this)(0,0) + (*this)(1,1) + (*this)(2,2); double epsilon=1E-12; if( trace > epsilon ){ double s = 0.5 / sqrt(trace + 1.0); w = 0.25 / s; x = ( (*this)(2,1) - (*this)(1,2) ) * s; y = ( (*this)(0,2) - (*this)(2,0) ) * s; z = ( (*this)(1,0) - (*this)(0,1) ) * s; }else{ if ( (*this)(0,0) > (*this)(1,1) && (*this)(0,0) > (*this)(2,2) ){ double s = 2.0 * sqrt( 1.0 + (*this)(0,0) - (*this)(1,1) - (*this)(2,2)); w = ((*this)(2,1) - (*this)(1,2) ) / s; x = 0.25 * s; y = ((*this)(0,1) + (*this)(1,0) ) / s; z = ((*this)(0,2) + (*this)(2,0) ) / s; } else if ((*this)(1,1) > (*this)(2,2)) { double s = 2.0 * sqrt( 1.0 + (*this)(1,1) - (*this)(0,0) - (*this)(2,2)); w = ((*this)(0,2) - (*this)(2,0) ) / s; x = ((*this)(0,1) + (*this)(1,0) ) / s; y = 0.25 * s; z = ((*this)(1,2) + (*this)(2,1) ) / s; }else { double s = 2.0 * sqrt( 1.0 + (*this)(2,2) - (*this)(0,0) - (*this)(1,1) ); w = ((*this)(1,0) - (*this)(0,1) ) / s; x = ((*this)(0,2) + (*this)(2,0) ) / s; y = ((*this)(1,2) + (*this)(2,1) ) / s; z = 0.25 * s; } } } Rotation Rotation::RPY(double roll,double pitch,double yaw) { double ca1,cb1,cc1,sa1,sb1,sc1; ca1 = cos(yaw); sa1 = sin(yaw); cb1 = cos(pitch);sb1 = sin(pitch); cc1 = cos(roll);sc1 = sin(roll); return Rotation(ca1*cb1,ca1*sb1*sc1 - sa1*cc1,ca1*sb1*cc1 + sa1*sc1, sa1*cb1,sa1*sb1*sc1 + ca1*cc1,sa1*sb1*cc1 - ca1*sc1, -sb1,cb1*sc1,cb1*cc1); } // Gives back a rotation matrix specified with RPY convention void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const { double epsilon=1E-12; pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) ); if ( fabs(pitch) > (M_PI/2.0-epsilon) ) { yaw = atan2( -data[1], data[4]); roll = 0.0 ; } else { roll = atan2(data[7], data[8]); yaw = atan2(data[3], data[0]); } } Rotation Rotation::EulerZYZ(double Alfa,double Beta,double Gamma) { double sa,ca,sb,cb,sg,cg; sa = sin(Alfa);ca = cos(Alfa); sb = sin(Beta);cb = cos(Beta); sg = sin(Gamma);cg = cos(Gamma); return Rotation( ca*cb*cg-sa*sg, -ca*cb*sg-sa*cg, ca*sb, sa*cb*cg+ca*sg, -sa*cb*sg+ca*cg, sa*sb, -sb*cg , sb*sg, cb ); } void Rotation::GetEulerZYZ(double& alpha,double& beta,double& gamma) const { double epsilon = 1E-12; if (fabs(data[8]) > 1-epsilon ) { gamma=0.0; if (data[8]>0) { beta = 0.0; alpha= atan2(data[3],data[0]); } else { beta = PI; alpha= atan2(-data[3],-data[0]); } } else { alpha=atan2(data[5], data[2]); beta=atan2(sqrt( sqr(data[6]) +sqr(data[7]) ),data[8]); gamma=atan2(data[7], -data[6]); } } Rotation Rotation::Rot(const Vector& rotaxis,double angle) { // The formula is // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr)) // can be found by multiplying it with an arbitrary vector p // and noting that this vector is rotated. Vector rotvec = rotaxis; rotvec.Normalize(); return Rotation::Rot2(rotvec,angle); } Rotation Rotation::Rot2(const Vector& rotvec,double angle) { // rotvec should be normalized ! // The formula is // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr)) // can be found by multiplying it with an arbitrary vector p // and noting that this vector is rotated. double ct = cos(angle); double st = sin(angle); double vt = 1-ct; double m_vt_0=vt*rotvec(0); double m_vt_1=vt*rotvec(1); double m_vt_2=vt*rotvec(2); double m_st_0=rotvec(0)*st; double m_st_1=rotvec(1)*st; double m_st_2=rotvec(2)*st; double m_vt_0_1=m_vt_0*rotvec(1); double m_vt_0_2=m_vt_0*rotvec(2); double m_vt_1_2=m_vt_1*rotvec(2); return Rotation( ct + m_vt_0*rotvec(0), -m_st_2 + m_vt_0_1, m_st_1 + m_vt_0_2, m_st_2 + m_vt_0_1, ct + m_vt_1*rotvec(1), -m_st_0 + m_vt_1_2, -m_st_1 + m_vt_0_2, m_st_0 + m_vt_1_2, ct + m_vt_2*rotvec(2) ); } Vector Rotation::GetRot() const // Returns a vector with the direction of the equiv. axis // and its norm is angle { Vector axis; double angle; angle = Rotation::GetRotAngle(axis,epsilon); return axis * angle; } /** Returns the rotation angle around the equiv. axis * @param axis the rotation axis is returned in this variable * @param eps : in the case of angle == 0 : rot axis is undefined and chosen * to be the Z-axis * in the case of angle == PI : 2 solutions, positive Z-component * of the axis is chosen. * @result returns the rotation angle (between [0..PI] ) * /todo : * Check corresponding routines in rframes and rrframes */ double Rotation::GetRotAngle(Vector& axis,double eps) const { double angle,x,y,z; // variables for result double epsilon = eps; // margin to allow for rounding errors double epsilon2 = eps*10; // margin to distinguish between 0 and 180 degrees // optional check that input is pure rotation, 'isRotationMatrix' is defined at: // http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/ if ((std::abs(data[1] - data[3]) < epsilon) && (std::abs(data[2] - data[6])< epsilon) && (std::abs(data[5] - data[7]) < epsilon)) { // singularity found // first check for identity matrix which must have +1 for all terms // in leading diagonal and zero in other terms if ((std::abs(data[1] + data[3]) < epsilon2) && (std::abs(data[2] + data[6]) < epsilon2) && (std::abs(data[5] + data[7]) < epsilon2) && (std::abs(data[0] + data[4] + data[8]-3) < epsilon2)) { // this singularity is identity matrix so angle = 0, axis is arbitrary // Choose 0, 0, 1 to pass orocos tests axis = KDL::Vector(0,0,1); angle = 0.0; return angle; } // otherwise this singularity is angle = 180 angle = M_PI; double xx = (data[0] + 1) / 2; double yy = (data[4] + 1) / 2; double zz = (data[8] + 1) / 2; double xy = (data[1] + data[3]) / 4; double xz = (data[2] + data[6]) / 4; double yz = (data[5] + data[7]) / 4; double half_sqrt_2 = 0.5 * sqrt(2.0); if ((xx > yy) && (xx > zz)) { // data[0] is the largest diagonal term if (xx < epsilon) { x = 0; y = half_sqrt_2; z = half_sqrt_2; } else { x = sqrt(xx); y = xy/x; z = xz/x; } } else if (yy > zz) { // data[4] is the largest diagonal term if (yy < epsilon) { x = half_sqrt_2; y = 0; z = half_sqrt_2; } else { y = sqrt(yy); x = xy/y; z = yz/y; } } else { // data[8] is the largest diagonal term so base result on this if (zz < epsilon) { x = half_sqrt_2; y = half_sqrt_2; z = 0; } else { z = sqrt(zz); x = xz/z; y = yz/z; } } axis = KDL::Vector(x, y, z); return angle; // return 180 deg rotation } // If the matrix is slightly non-orthogonal, `f` may be out of the (-1, +1) range. // Therefore, clamp it between those values to avoid NaNs. double f = (data[0] + data[4] + data[8] - 1) / 2; angle = acos(std::max(-1.0, std::min(1.0, f))); x = (data[7] - data[5]); y = (data[2] - data[6]); z = (data[3] - data[1]); axis = KDL::Vector(x, y, z); axis.Normalize(); return angle; } bool operator==(const Rotation& a,const Rotation& b) { #ifdef KDL_USE_EQUAL return Equal(a,b); #else return ( a.data[0]==b.data[0] && a.data[1]==b.data[1] && a.data[2]==b.data[2] && a.data[3]==b.data[3] && a.data[4]==b.data[4] && a.data[5]==b.data[5] && a.data[6]==b.data[6] && a.data[7]==b.data[7] && a.data[8]==b.data[8] ); #endif } } orocos-kdl-1.4.0/orocos_kdl/src/frames.hpp000066400000000000000000001357751326704774600205450ustar00rootroot00000000000000/*************************************************************************** frames.hpp `- description ------------------------- begin : June 2006 copyright : (C) 2006 Erwin Aertbelien email : firstname.lastname@mech.kuleuven.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 * * * ***************************************************************************/ /** * \file * \warning * Efficienty can be improved by writing p2 = A*(B*(C*p1))) instead of * p2=A*B*C*p1 * * \par PROPOSED NAMING CONVENTION FOR FRAME-like OBJECTS * * \verbatim * A naming convention of objects of the type defined in this file : * (1) Frame : F... * Rotation : R ... * (2) Twist : T ... * Wrench : W ... * Vector : V ... * This prefix is followed by : * for category (1) : * F_A_B : w.r.t. frame A, frame B expressed * ( each column of F_A_B corresponds to an axis of B, * expressed w.r.t. frame A ) * in mathematical convention : * A * F_A_B == F * B * * for category (2) : * V_B : a vector expressed w.r.t. frame B * * This can also be prepended by a name : * e.g. : temporaryV_B * * With this convention one can write : * * F_A_B = F_B_A.Inverse(); * F_A_C = F_A_B * F_B_C; * V_B = F_B_C * V_C; // both translation and rotation * V_B = R_B_C * V_C; // only rotation * \endverbatim * * \par CONVENTIONS FOR WHEN USED WITH ROBOTS : * * \verbatim * world : represents the frame ([1 0 0,0 1 0,0 0 1],[0 0 0]') * mp : represents mounting plate of a robot * (i.e. everything before MP is constructed by robot manufacturer * everything after MP is tool ) * tf : represents task frame of a robot * (i.e. frame in which motion and force control is expressed) * sf : represents sensor frame of a robot * (i.e. frame at which the forces measured by the force sensor * are expressed ) * * Frame F_world_mp=...; * Frame F_mp_sf(..) * Frame F_mp_tf(,.) * * Wrench are measured in sensor frame SF, so one could write : * Wrench_tf = F_mp_tf.Inverse()* ( F_mp_sf * Wrench_sf ); * \endverbatim * * \par CONVENTIONS REGARDING UNITS : * Any consistent series of units can be used, e.g. N,mm,Nmm,..mm/sec * * \par Twist and Wrench transformations * 3 different types of transformations do exist for the twists * and wrenches. * * \verbatim * 1) Frame * Twist or Frame * Wrench : * this transforms both the velocity/force reference point * and the basis to which the twist/wrench are expressed. * 2) Rotation * Twist or Rotation * Wrench : * this transforms the basis to which the twist/wrench are * expressed, but leaves the reference point intact. * 3) Twist.RefPoint(v_base_AB) or Wrench.RefPoint(v_base_AB) * this transforms only the reference point. v is expressed * in the same base as the twist/wrench and points from the * old reference point to the new reference point. * \endverbatim * *\par Spatial cross products * Let m be a 6D motion vector (Twist) and f be a 6D force vector (Wrench) * attached to a rigid body moving with a certain velocity v (Twist). Then *\verbatim * 1) m_dot = v cross m or Twist=Twist*Twist * 2) f_dot = v cross f or Wrench=Twist*Wrench *\endverbatim * * \par Complexity * Sometimes the amount of work is given in the documentation * e.g. 6M+3A means 6 multiplications and 3 additions. * * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * ****************************************************************************/ #ifndef KDL_FRAMES_H #define KDL_FRAMES_H #include "utilities/kdl-config.h" #include "utilities/utility.h" ///////////////////////////////////////////////////////////// namespace KDL { class Vector; class Rotation; class Frame; class Wrench; class Twist; class Vector2; class Rotation2; class Frame2; // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) inline bool Equal(const Vector& a,const Vector& b,double eps=epsilon); inline bool Equal(const Frame& a,const Frame& b,double eps=epsilon); inline bool Equal(const Twist& a,const Twist& b,double eps=epsilon); inline bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon); inline bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon); inline bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon); inline bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon); /** * \brief A concrete implementation of a 3 dimensional vector class */ class Vector { public: double data[3]; //! Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that inline Vector() {data[0]=data[1]=data[2] = 0.0;} //! Constructs a vector out of the three values x, y and z inline Vector(double x,double y, double z); //! Assignment operator. The normal copy by value semantics. inline Vector(const Vector& arg); //! Assignment operator. The normal copy by value semantics. inline Vector& operator = ( const Vector& arg); //! Access to elements, range checked when NDEBUG is not set, from 0..2 inline double operator()(int index) const; //! Access to elements, range checked when NDEBUG is not set, from 0..2 inline double& operator() (int index); //! Equivalent to double operator()(int index) const double operator[] ( int index ) const { return this->operator() ( index ); } //! Equivalent to double& operator()(int index) double& operator[] ( int index ) { return this->operator() ( index ); } inline double x() const; inline double y() const; inline double z() const; inline void x(double); inline void y(double); inline void z(double); //! Reverses the sign of the Vector object itself inline void ReverseSign(); //! subtracts a vector from the Vector object itself inline Vector& operator-=(const Vector& arg); //! Adds a vector from the Vector object itself inline Vector& operator +=(const Vector& arg); //! Scalar multiplication is defined inline friend Vector operator*(const Vector& lhs,double rhs); //! Scalar multiplication is defined inline friend Vector operator*(double lhs,const Vector& rhs); //! Scalar division is defined inline friend Vector operator/(const Vector& lhs,double rhs); inline friend Vector operator+(const Vector& lhs,const Vector& rhs); inline friend Vector operator-(const Vector& lhs,const Vector& rhs); inline friend Vector operator*(const Vector& lhs,const Vector& rhs); inline friend Vector operator-(const Vector& arg); inline friend double dot(const Vector& lhs,const Vector& rhs); //! To have a uniform operator to put an element to zero, for scalar values //! and for objects. inline friend void SetToZero(Vector& v); //! @return a zero vector inline static Vector Zero(); /** Normalizes this vector and returns it norm * makes v a unitvector and returns the norm of v. * if v is smaller than eps, Vector(1,0,0) is returned with norm 0. * if this is not good, check the return value of this method. */ double Normalize(double eps=epsilon); //! @return the norm of the vector double Norm() const; //! a 3D vector where the 2D vector v is put in the XY plane inline void Set2DXY(const Vector2& v); //! a 3D vector where the 2D vector v is put in the YZ plane inline void Set2DYZ(const Vector2& v); //! a 3D vector where the 2D vector v is put in the ZX plane inline void Set2DZX(const Vector2& v); //! a 3D vector where the 2D vector v_XY is put in the XY plane of the frame F_someframe_XY. inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY); //! do not use operator == because the definition of Equal(.,.) is slightly //! different. It compares whether the 2 arguments are equal in an eps-interval inline friend bool Equal(const Vector& a,const Vector& b,double eps); //! The literal equality operator==(), also identical. inline friend bool operator==(const Vector& a,const Vector& b); //! The literal inequality operator!=(). inline friend bool operator!=(const Vector& a,const Vector& b); friend class Rotation; friend class Frame; }; /** \brief represents rotations in 3 dimensional space. This class represents a rotation matrix with the following conventions : \verbatim Suppose V2 = R*V, (1) V is expressed in frame B V2 is expressed in frame A This matrix R consists of 3 columns [ X,Y,Z ], X,Y, and Z contain the axes of frame B, expressed in frame A Because of linearity expr(1) is valid. \endverbatim This class only represents rotational_interpolation, not translation Two interpretations are possible for rotation angles. * if you rotate with angle around X frame A to have frame B, then the result of SetRotX is equal to frame B expressed wrt A. In code: \verbatim Rotation R; F_A_B = R.SetRotX(angle); \endverbatim * Secondly, if you take the following code : \verbatim Vector p,p2; Rotation R; R.SetRotX(angle); p2 = R*p; \endverbatim then the frame p2 is rotated around X axis with (-angle). Analogue reasonings can be applyd to SetRotY,SetRotZ,SetRot \par type Concrete implementation */ class Rotation { public: double data[9]; inline Rotation() { *this = Rotation::Identity(); } inline Rotation(double Xx,double Yx,double Zx, double Xy,double Yy,double Zy, double Xz,double Yz,double Zz); inline Rotation(const Vector& x,const Vector& y,const Vector& z); // default copy constructor is sufficient inline Rotation& operator=(const Rotation& arg); //! Defines a multiplication R*V between a Rotation R and a Vector V. //! Complexity : 9M+6A inline Vector operator*(const Vector& v) const; //! Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set inline double& operator()(int i,int j); //! Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set inline double operator() (int i,int j) const; friend Rotation operator *(const Rotation& lhs,const Rotation& rhs); //! Sets the value of *this to its inverse. inline void SetInverse(); //! Gives back the inverse rotation matrix of *this. inline Rotation Inverse() const; //! The same as R.Inverse()*v but more efficient. inline Vector Inverse(const Vector& v) const; //! The same as R.Inverse()*arg but more efficient. inline Wrench Inverse(const Wrench& arg) const; //! The same as R.Inverse()*arg but more efficient. inline Twist Inverse(const Twist& arg) const; //! Gives back an identity rotaton matrix inline static Rotation Identity(); // = Rotations //! The Rot... static functions give the value of the appropriate rotation matrix back. inline static Rotation RotX(double angle); //! The Rot... static functions give the value of the appropriate rotation matrix back. inline static Rotation RotY(double angle); //! The Rot... static functions give the value of the appropriate rotation matrix back. inline static Rotation RotZ(double angle); //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.. //! DoRot... functions are only defined when they can be executed more efficiently inline void DoRotX(double angle); //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.. //! DoRot... functions are only defined when they can be executed more efficiently inline void DoRotY(double angle); //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.. //! DoRot... functions are only defined when they can be executed more efficiently inline void DoRotZ(double angle); //! Along an arbitrary axes. It is not necessary to normalize rotvec. //! returns identity rotation matrix in the case that the norm of rotvec //! is to small to be used. // @see Rot2 if you want to handle this error in another way. static Rotation Rot(const Vector& rotvec,double angle); //! Along an arbitrary axes. rotvec should be normalized. static Rotation Rot2(const Vector& rotvec,double angle); //! Returns a vector with the direction of the equiv. axis //! and its norm is angle Vector GetRot() const; /** Returns the rotation angle around the equiv. axis * @param axis the rotation axis is returned in this variable * @param eps : in the case of angle == 0 : rot axis is undefined and chosen * to be +/- Z-axis * in the case of angle == PI : 2 solutions, positive Z-component * of the axis is chosen. * @result returns the rotation angle (between [0..PI] ) */ double GetRotAngle(Vector& axis,double eps=epsilon) const; /** Gives back a rotation matrix specified with EulerZYZ convention : * - First rotate around Z with alfa, * - then around the new Y with beta, * - then around new Z with gamma. * Invariants: * - EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PHI, -beta, gamma +/- PI) * - (angle + 2*k*PI) **/ static Rotation EulerZYZ(double Alfa,double Beta,double Gamma); /** Gives back the EulerZYZ convention description of the rotation matrix : First rotate around Z with alpha, then around the new Y with beta, then around new Z with gamma. Variables are bound by: - (-PI < alpha <= PI), - (0 <= beta <= PI), - (-PI < gamma <= PI) if beta==0 or beta==PI, then alpha and gamma are not unique, in this case gamma is chosen to be zero. Invariants: - EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PI, -beta, gamma +/- PI) - angle + 2*k*PI */ void GetEulerZYZ(double& alpha,double& beta,double& gamma) const; //! Gives back a rotation matrix specified with Quaternion convention //! the norm of (x,y,z,w) should be equal to 1 static Rotation Quaternion(double x,double y,double z, double w); //! Get the quaternion of this matrix //! \post the norm of (x,y,z,w) is 1 void GetQuaternion(double& x,double& y,double& z, double& w) const; /** * * Gives back a rotation matrix specified with RPY convention: * first rotate around X with roll, then around the * old Y with pitch, then around old Z with yaw * * Invariants: * - RPY(roll,pitch,yaw) == RPY( roll +/- PI, PI-pitch, yaw +/- PI ) * - angles + 2*k*PI */ static Rotation RPY(double roll,double pitch,double yaw); /** Gives back a vector in RPY coordinates, variables are bound by - -PI <= roll <= PI - -PI <= Yaw <= PI - -PI/2 <= PITCH <= PI/2 convention : - first rotate around X with roll, - then around the old Y with pitch, - then around old Z with yaw if pitch == PI/2 or pitch == -PI/2, multiple solutions for gamma and alpha exist. The solution where roll==0 is chosen. Invariants: - RPY(roll,pitch,yaw) == RPY( roll +/- PI, PI-pitch, yaw +/- PI ) - angles + 2*k*PI **/ void GetRPY(double& roll,double& pitch,double& yaw) const; /** EulerZYX constructs a Rotation from the Euler ZYX parameters: * - First rotate around Z with alfa, * - then around the new Y with beta, * - then around new X with gamma. * * Closely related to RPY-convention. * * Invariants: * - EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PI, PI-beta, gamma +/- PI) * - (angle + 2*k*PI) **/ inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) { return RPY(Gamma,Beta,Alfa); } /** GetEulerZYX gets the euler ZYX parameters of a rotation : * First rotate around Z with alfa, * then around the new Y with beta, then around * new X with gamma. * * Range of the results of GetEulerZYX : * - -PI <= alfa <= PI * - -PI <= gamma <= PI * - -PI/2 <= beta <= PI/2 * * if beta == PI/2 or beta == -PI/2, multiple solutions for gamma and alpha exist. The solution where gamma==0 * is chosen. * * * Invariants: * - EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PI, PI-beta, gamma +/- PI) * - and also (angle + 2*k*PI) * * Closely related to RPY-convention. **/ inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const { GetRPY(Gamma,Beta,Alfa); } //! Transformation of the base to which the twist is expressed. //! Complexity : 18M+12A //! @see Frame*Twist for a transformation that also transforms //! the velocity reference point. inline Twist operator * (const Twist& arg) const; //! Transformation of the base to which the wrench is expressed. //! Complexity : 18M+12A //! @see Frame*Wrench for a transformation that also transforms //! the force reference point. inline Wrench operator * (const Wrench& arg) const; //! Access to the underlying unitvectors of the rotation matrix inline Vector UnitX() const { return Vector(data[0],data[3],data[6]); } //! Access to the underlying unitvectors of the rotation matrix inline void UnitX(const Vector& X) { data[0] = X(0); data[3] = X(1); data[6] = X(2); } //! Access to the underlying unitvectors of the rotation matrix inline Vector UnitY() const { return Vector(data[1],data[4],data[7]); } //! Access to the underlying unitvectors of the rotation matrix inline void UnitY(const Vector& X) { data[1] = X(0); data[4] = X(1); data[7] = X(2); } //! Access to the underlying unitvectors of the rotation matrix inline Vector UnitZ() const { return Vector(data[2],data[5],data[8]); } //! Access to the underlying unitvectors of the rotation matrix inline void UnitZ(const Vector& X) { data[2] = X(0); data[5] = X(1); data[8] = X(2); } //! do not use operator == because the definition of Equal(.,.) is slightly //! different. It compares whether the 2 arguments are equal in an eps-interval friend bool Equal(const Rotation& a,const Rotation& b,double eps); //! The literal equality operator==(), also identical. friend bool operator==(const Rotation& a,const Rotation& b); //! The literal inequality operator!=() friend bool operator!=(const Rotation& a,const Rotation& b); friend class Frame; }; bool operator==(const Rotation& a,const Rotation& b); bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon); /** \brief represents a frame transformation in 3D space (rotation + translation) if V2 = Frame*V1 (V2 expressed in frame A, V1 expressed in frame B) then V2 = Frame.M*V1+Frame.p Frame.M contains columns that represent the axes of frame B wrt frame A Frame.p contains the origin of frame B expressed in frame A. */ class Frame { public: Vector p; //!< origine of the Frame Rotation M; //!< Orientation of the Frame public: inline Frame(const Rotation& R,const Vector& V); //! The rotation matrix defaults to identity explicit inline Frame(const Vector& V); //! The position matrix defaults to zero explicit inline Frame(const Rotation& R); inline Frame() {} //! The copy constructor. Normal copy by value semantics. inline Frame(const Frame& arg); //! Reads data from an double array //\TODO should be formulated as a constructor void Make4x4(double* d); //! Treats a frame as a 4x4 matrix and returns element i,j //! Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set inline double operator()(int i,int j); //! Treats a frame as a 4x4 matrix and returns element i,j //! Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set inline double operator() (int i,int j) const; // = Inverse //! Gives back inverse transformation of a Frame inline Frame Inverse() const; //! The same as p2=R.Inverse()*p but more efficient. inline Vector Inverse(const Vector& arg) const; //! The same as p2=R.Inverse()*p but more efficient. inline Wrench Inverse(const Wrench& arg) const; //! The same as p2=R.Inverse()*p but more efficient. inline Twist Inverse(const Twist& arg) const; //! Normal copy-by-value semantics. inline Frame& operator = (const Frame& arg); //! Transformation of the base to which the vector //! is expressed. inline Vector operator * (const Vector& arg) const; //! Transformation of both the force reference point //! and of the base to which the wrench is expressed. //! look at Rotation*Wrench operator for a transformation //! of only the base to which the twist is expressed. //! //! Complexity : 24M+18A inline Wrench operator * (const Wrench& arg) const; //! Transformation of both the velocity reference point //! and of the base to which the twist is expressed. //! look at Rotation*Twist for a transformation of only the //! base to which the twist is expressed. //! //! Complexity : 24M+18A inline Twist operator * (const Twist& arg) const; //! Composition of two frames. inline friend Frame operator *(const Frame& lhs,const Frame& rhs); //! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()). inline static Frame Identity(); //! The twist is expressed wrt the current //! frame. This frame is integrated into an updated frame with //! . Very simple first order integration rule. inline void Integrate(const Twist& t_this,double frequency); /* // DH_Craig1989 : constructs a transformationmatrix // T_link(i-1)_link(i) with the Denavit-Hartenberg convention as // described in the Craigs book: Craig, J. J.,Introduction to // Robotics: Mechanics and Control, Addison-Wesley, // isbn:0-201-10326-5, 1986. // // Note that the frame is a redundant way to express the information // in the DH-convention. // \verbatim // Parameters in full : a(i-1),alpha(i-1),d(i),theta(i) // // axis i-1 is connected by link i-1 to axis i numbering axis 1 // to axis n link 0 (immobile base) to link n // // link length a(i-1) length of the mutual perpendicular line // (normal) between the 2 axes. This normal runs from (i-1) to // (i) axis. // // link twist alpha(i-1): construct plane perpendicular to the // normal project axis(i-1) and axis(i) into plane angle from // (i-1) to (i) measured in the direction of the normal // // link offset d(i) signed distance between normal (i-1) to (i) // and normal (i) to (i+1) along axis i joint angle theta(i) // signed angle between normal (i-1) to (i) and normal (i) to // (i+1) along axis i // // First and last joints : a(0)= a(n) = 0 // alpha(0) = alpha(n) = 0 // // PRISMATIC : theta(1) = 0 d(1) arbitrarily // // REVOLUTE : theta(1) arbitrarily d(1) = 0 // // Not unique : if intersecting joint axis 2 choices for normal // Frame assignment of the DH convention : Z(i-1) follows axis // (i-1) X(i-1) is the normal between axis(i-1) and axis(i) // Y(i-1) follows out of Z(i-1) and X(i-1) // // a(i-1) = distance from Z(i-1) to Z(i) along X(i-1) // alpha(i-1) = angle between Z(i-1) to Z(i) along X(i-1) // d(i) = distance from X(i-1) to X(i) along Z(i) // theta(i) = angle between X(i-1) to X(i) along X(i) // \endverbatim */ static Frame DH_Craig1989(double a,double alpha,double d,double theta); // DH : constructs a transformationmatrix T_link(i-1)_link(i) with // the Denavit-Hartenberg convention as described in the original // publictation: Denavit, J. and Hartenberg, R. S., A kinematic // notation for lower-pair mechanisms based on matrices, ASME // Journal of Applied Mechanics, 23:215-221, 1955. static Frame DH(double a,double alpha,double d,double theta); //! do not use operator == because the definition of Equal(.,.) is slightly //! different. It compares whether the 2 arguments are equal in an eps-interval inline friend bool Equal(const Frame& a,const Frame& b,double eps); //! The literal equality operator==(), also identical. inline friend bool operator==(const Frame& a,const Frame& b); //! The literal inequality operator!=(). inline friend bool operator!=(const Frame& a,const Frame& b); }; /** * \brief represents both translational and rotational velocities. * * This class represents a twist. A twist is the combination of translational * velocity and rotational velocity applied at one point. */ class Twist { public: Vector vel; //!< The velocity of that point Vector rot; //!< The rotational velocity of that point. public: //! The default constructor initialises to Zero via the constructor of Vector. Twist():vel(),rot() {}; Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {}; inline Twist& operator-=(const Twist& arg); inline Twist& operator+=(const Twist& arg); //! index-based access to components, first vel(0..2), then rot(3..5) inline double& operator()(int i); //! index-based access to components, first vel(0..2), then rot(3..5) //! For use with a const Twist inline double operator()(int i) const; double operator[] ( int index ) const { return this->operator() ( index ); } double& operator[] ( int index ) { return this->operator() ( index ); } inline friend Twist operator*(const Twist& lhs,double rhs); inline friend Twist operator*(double lhs,const Twist& rhs); inline friend Twist operator/(const Twist& lhs,double rhs); inline friend Twist operator+(const Twist& lhs,const Twist& rhs); inline friend Twist operator-(const Twist& lhs,const Twist& rhs); inline friend Twist operator-(const Twist& arg); inline friend double dot(const Twist& lhs,const Wrench& rhs); inline friend double dot(const Wrench& rhs,const Twist& lhs); inline friend void SetToZero(Twist& v); /// Spatial cross product for 6d motion vectors, beware all of them have to be expressed in the same reference frame/point inline friend Twist operator*(const Twist& lhs,const Twist& rhs); /// Spatial cross product for 6d force vectors, beware all of them have to be expressed in the same reference frame/point inline friend Wrench operator*(const Twist& lhs,const Wrench& rhs); //! @return a zero Twist : Twist(Vector::Zero(),Vector::Zero()) static inline Twist Zero(); //! Reverses the sign of the twist inline void ReverseSign(); //! Changes the reference point of the twist. //! The vector v_base_AB is expressed in the same base as the twist //! The vector v_base_AB is a vector from the old point to //! the new point. //! //! Complexity : 6M+6A inline Twist RefPoint(const Vector& v_base_AB) const; //! do not use operator == because the definition of Equal(.,.) is slightly //! different. It compares whether the 2 arguments are equal in an eps-interval inline friend bool Equal(const Twist& a,const Twist& b,double eps); //! The literal equality operator==(), also identical. inline friend bool operator==(const Twist& a,const Twist& b); //! The literal inequality operator!=(). inline friend bool operator!=(const Twist& a,const Twist& b); // = Friends friend class Rotation; friend class Frame; }; /** * \brief represents both translational and rotational acceleration. * * This class represents an acceleration twist. A acceleration twist is * the combination of translational * acceleration and rotational acceleration applied at one point. */ /* class AccelerationTwist { public: Vector trans; //!< The translational acceleration of that point Vector rot; //!< The rotational acceleration of that point. public: //! The default constructor initialises to Zero via the constructor of Vector. AccelerationTwist():trans(),rot() {}; AccelerationTwist(const Vector& _trans,const Vector& _rot):trans(_trans),rot(_rot) {}; inline AccelerationTwist& operator-=(const AccelerationTwist& arg); inline AccelerationTwist& operator+=(const AccelerationTwist& arg); //! index-based access to components, first vel(0..2), then rot(3..5) inline double& operator()(int i); //! index-based access to components, first vel(0..2), then rot(3..5) //! For use with a const AccelerationTwist inline double operator()(int i) const; double operator[] ( int index ) const { return this->operator() ( index ); } double& operator[] ( int index ) { return this->operator() ( index ); } inline friend AccelerationTwist operator*(const AccelerationTwist& lhs,double rhs); inline friend AccelerationTwist operator*(double lhs,const AccelerationTwist& rhs); inline friend AccelerationTwist operator/(const AccelerationTwist& lhs,double rhs); inline friend AccelerationTwist operator+(const AccelerationTwist& lhs,const AccelerationTwist& rhs); inline friend AccelerationTwist operator-(const AccelerationTwist& lhs,const AccelerationTwist& rhs); inline friend AccelerationTwist operator-(const AccelerationTwist& arg); //inline friend double dot(const AccelerationTwist& lhs,const Wrench& rhs); //inline friend double dot(const Wrench& rhs,const AccelerationTwist& lhs); inline friend void SetToZero(AccelerationTwist& v); //! @return a zero AccelerationTwist : AccelerationTwist(Vector::Zero(),Vector::Zero()) static inline AccelerationTwist Zero(); //! Reverses the sign of the AccelerationTwist inline void ReverseSign(); //! Changes the reference point of the AccelerationTwist. //! The vector v_base_AB is expressed in the same base as the AccelerationTwist //! The vector v_base_AB is a vector from the old point to //! the new point. //! //! Complexity : 6M+6A inline AccelerationTwist RefPoint(const Vector& v_base_AB) const; //! do not use operator == because the definition of Equal(.,.) is slightly //! different. It compares whether the 2 arguments are equal in an eps-interval inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double eps=epsilon); //! The literal equality operator==(), also identical. inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b); //! The literal inequality operator!=(). inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b); // = Friends friend class Rotation; friend class Frame; }; */ /** * \brief represents the combination of a force and a torque. * * This class represents a Wrench. A Wrench is the force and torque applied at a point */ class Wrench { public: Vector force; //!< Force that is applied at the origin of the current ref frame Vector torque; //!< Torque that is applied at the origin of the current ref frame public: //! Does initialise force and torque to zero via the underlying constructor of Vector Wrench():force(),torque() {}; Wrench(const Vector& _force,const Vector& _torque):force(_force),torque(_torque) {}; // = Operators inline Wrench& operator-=(const Wrench& arg); inline Wrench& operator+=(const Wrench& arg); //! index-based access to components, first force(0..2), then torque(3..5) inline double& operator()(int i); //! index-based access to components, first force(0..2), then torque(3..5) //! for use with a const Wrench inline double operator()(int i) const; double operator[] ( int index ) const { return this->operator() ( index ); } double& operator[] ( int index ) { return this->operator() ( index ); } //! Scalar multiplication inline friend Wrench operator*(const Wrench& lhs,double rhs); //! Scalar multiplication inline friend Wrench operator*(double lhs,const Wrench& rhs); //! Scalar division inline friend Wrench operator/(const Wrench& lhs,double rhs); inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs); inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs); //! An unary - operator inline friend Wrench operator-(const Wrench& arg); //! Sets the Wrench to Zero, to have a uniform function that sets an object or //! double to zero. inline friend void SetToZero(Wrench& v); //! @return a zero Wrench static inline Wrench Zero(); //! Reverses the sign of the current Wrench inline void ReverseSign(); //! Changes the reference point of the wrench. //! The vector v_base_AB is expressed in the same base as the twist //! The vector v_base_AB is a vector from the old point to //! the new point. //! //! Complexity : 6M+6A inline Wrench RefPoint(const Vector& v_base_AB) const; //! do not use operator == because the definition of Equal(.,.) is slightly //! different. It compares whether the 2 arguments are equal in an eps-interval inline friend bool Equal(const Wrench& a,const Wrench& b,double eps); //! The literal equality operator==(), also identical. inline friend bool operator==(const Wrench& a,const Wrench& b); //! The literal inequality operator!=(). inline friend bool operator!=(const Wrench& a,const Wrench& b); friend class Rotation; friend class Frame; }; //! 2D version of Vector class Vector2 { double data[2]; public: //! Does not initialise to Zero(). Vector2() {data[0]=data[1] = 0.0;} inline Vector2(double x,double y); inline Vector2(const Vector2& arg); inline Vector2& operator = ( const Vector2& arg); //! Access to elements, range checked when NDEBUG is not set, from 0..1 inline double operator()(int index) const; //! Access to elements, range checked when NDEBUG is not set, from 0..1 inline double& operator() (int index); //! Equivalent to double operator()(int index) const double operator[] ( int index ) const { return this->operator() ( index ); } //! Equivalent to double& operator()(int index) double& operator[] ( int index ) { return this->operator() ( index ); } inline double x() const; inline double y() const; inline void x(double); inline void y(double); inline void ReverseSign(); inline Vector2& operator-=(const Vector2& arg); inline Vector2& operator +=(const Vector2& arg); inline friend Vector2 operator*(const Vector2& lhs,double rhs); inline friend Vector2 operator*(double lhs,const Vector2& rhs); inline friend Vector2 operator/(const Vector2& lhs,double rhs); inline friend Vector2 operator+(const Vector2& lhs,const Vector2& rhs); inline friend Vector2 operator-(const Vector2& lhs,const Vector2& rhs); inline friend Vector2 operator*(const Vector2& lhs,const Vector2& rhs); inline friend Vector2 operator-(const Vector2& arg); inline friend void SetToZero(Vector2& v); //! @return a zero 2D vector. inline static Vector2 Zero(); /** Normalizes this vector and returns it norm * makes v a unitvector and returns the norm of v. * if v is smaller than eps, Vector(1,0,0) is returned with norm 0. * if this is not good, check the return value of this method. */ double Normalize(double eps=epsilon); //! @return the norm of the vector double Norm() const; //! projects v in its XY plane, and sets *this to these values inline void Set3DXY(const Vector& v); //! projects v in its YZ plane, and sets *this to these values inline void Set3DYZ(const Vector& v); //! projects v in its ZX plane, and sets *this to these values inline void Set3DZX(const Vector& v); //! projects v_someframe in the XY plane of F_someframe_XY, //! and sets *this to these values //! expressed wrt someframe. inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe); //! do not use operator == because the definition of Equal(.,.) is slightly //! different. It compares whether the 2 arguments are equal in an eps-interval inline friend bool Equal(const Vector2& a,const Vector2& b,double eps); //! The literal equality operator==(), also identical. inline friend bool operator==(const Vector2& a,const Vector2& b); //! The literal inequality operator!=(). inline friend bool operator!=(const Vector2& a,const Vector2& b); friend class Rotation2; }; //! A 2D Rotation class, for conventions see Rotation. For further documentation //! of the methods see Rotation class. class Rotation2 { double s,c; //! c,s represent cos(angle), sin(angle), this also represents first col. of rot matrix //! from outside, this class behaves as if it would store the complete 2x2 matrix. public: //! Default constructor does NOT initialise to Zero(). Rotation2() {c=1.0;s=0.0;} explicit Rotation2(double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {} Rotation2(double ca,double sa):s(sa),c(ca){} inline Rotation2& operator=(const Rotation2& arg); inline Vector2 operator*(const Vector2& v) const; //! Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set inline double operator() (int i,int j) const; inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs); inline void SetInverse(); inline Rotation2 Inverse() const; inline Vector2 Inverse(const Vector2& v) const; inline void SetIdentity(); inline static Rotation2 Identity(); //! The SetRot.. functions set the value of *this to the appropriate rotation matrix. inline void SetRot(double angle); //! The Rot... static functions give the value of the appropriate rotation matrix bac inline static Rotation2 Rot(double angle); //! Gets the angle (in radians) inline double GetRot() const; //! do not use operator == because the definition of Equal(.,.) is slightly //! different. It compares whether the 2 arguments are equal in an eps-interval inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps); }; //! A 2D frame class, for further documentation see the Frames class //! for methods with unchanged semantics. class Frame2 { public: Vector2 p; //!< origine of the Frame Rotation2 M; //!< Orientation of the Frame public: inline Frame2(const Rotation2& R,const Vector2& V); explicit inline Frame2(const Vector2& V); explicit inline Frame2(const Rotation2& R); inline Frame2(void); inline Frame2(const Frame2& arg); inline void Make4x4(double* d); //! Treats a frame as a 3x3 matrix and returns element i,j //! Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set inline double operator()(int i,int j); //! Treats a frame as a 4x4 matrix and returns element i,j //! Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set inline double operator() (int i,int j) const; inline void SetInverse(); inline Frame2 Inverse() const; inline Vector2 Inverse(const Vector2& arg) const; inline Frame2& operator = (const Frame2& arg); inline Vector2 operator * (const Vector2& arg) const; inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs); inline void SetIdentity(); inline void Integrate(const Twist& t_this,double frequency); inline static Frame2 Identity() { Frame2 tmp; tmp.SetIdentity(); return tmp; } inline friend bool Equal(const Frame2& a,const Frame2& b,double eps); }; /** * determines the difference of vector b with vector a. * * see diff for Rotation matrices for further background information. * * \param p_w_a start vector a expressed to some frame w * \param p_w_b end vector b expressed to some frame w . * \param dt [optional][obsolete] time interval over which the numerical differentiation takes place. * \return the difference (b-a) expressed to the frame w. */ IMETHOD Vector diff(const Vector& p_w_a,const Vector& p_w_b,double dt=1); /** * determines the (scaled) rotation axis necessary to rotate from b1 to b2. * * This rotation axis is expressed w.r.t. frame a. The rotation axis is scaled * by the necessary rotation angle. The rotation angle is always in the * (inclusive) interval \f$ [0 , \pi] \f$. * * This definition is chosen in this way to facilitate numerical differentiation. * With this definition diff(a,b) == -diff(b,a). * * The diff() function is overloaded for all classes in frames.hpp and framesvel.hpp, such that * numerical differentiation, equality checks with tolerances, etc. can be performed * without caring exactly on which type the operation is performed. * * \param R_a_b1: The rotation matrix \f$ _a^{b1} R \f$ of b1 with respect to frame a. * \param R_a_b2: The Rotation matrix \f$ _a^{b2} R \f$ of b2 with respect to frame a. * \param dt [optional][obsolete] time interval over which the numerical differentiation takes place. By default this is set to 1.0. * \return rotation axis to rotate from b1 to b2, scaled by the rotation angle, expressed in frame a. * \warning - The result is not a rotational vector, i.e. it is not a mathematical vector. * (no communitative addition). * * \warning - When used in the context of numerical differentiation, with the frames b1 and b2 very * close to each other, the semantics correspond to the twist, scaled by the time. * * \warning - For angles equal to \f$ \pi \f$, The negative of the * return value is equally valid. */ IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1); /** * determines the rotation axis necessary to rotate the frame b1 to the same orientation as frame b2 and the vector * necessary to translate the origin of b1 to the origin of b2, and stores the result in a Twist datastructure. * \param F_a_b1 frame b1 expressed with respect to some frame a. * \param F_a_b2 frame b2 expressed with respect to some frame a. * \warning The result is not a Twist! * see diff() for Rotation and Vector arguments for further detail on the semantics. */ IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1); /** * determines the difference between two twists i.e. the difference between * the underlying velocity vectors and rotational velocity vectors. */ IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1); /** * determines the difference between two wrenches i.e. the difference between * the underlying torque vectors and force vectors. */ IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1); /** * \brief adds vector da to vector a. * see also the corresponding diff() routine. * \param p_w_a vector a expressed to some frame w. * \param p_w_da vector da expressed to some frame w. * \returns the vector resulting from the displacement of vector a by vector da, expressed in the frame w. */ IMETHOD Vector addDelta(const Vector& p_w_a,const Vector& p_w_da,double dt=1); /** * returns the rotation matrix resulting from the rotation of frame a by the axis and angle * specified with da_w. * * see also the corresponding diff() routine. * * \param R_w_a Rotation matrix of frame a expressed to some frame w. * \param da_w axis and angle of the rotation expressed to some frame w. * \returns the rotation matrix resulting from the rotation of frame a by the axis and angle * specified with da. The resulting rotation matrix is expressed with respect to * frame w. */ IMETHOD Rotation addDelta(const Rotation& R_w_a,const Vector& da_w,double dt=1); /** * returns the frame resulting from the rotation of frame a by the axis and angle * specified in da_w and the translation of the origin (also specified in da_w). * * see also the corresponding diff() routine. * \param R_w_a Rotation matrix of frame a expressed to some frame w. * \param da_w axis and angle of the rotation (da_w.rot), together with a displacement vector for the origin (da_w.vel), expressed to some frame w. * \returns the frame resulting from the rotation of frame a by the axis and angle * specified with da.rot, and the translation of the origin da_w.vel . The resulting frame is expressed with respect to frame w. */ IMETHOD Frame addDelta(const Frame& F_w_a,const Twist& da_w,double dt=1); /** * \brief adds the twist da to the twist a. * see also the corresponding diff() routine. * \param a a twist wrt some frame * \param da a twist difference wrt some frame * \returns The twist (a+da) wrt the corresponding frame. */ IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1); /** * \brief adds the wrench da to the wrench w. * see also the corresponding diff() routine. * see also the corresponding diff() routine. * \param a a wrench wrt some frame * \param da a wrench difference wrt some frame * \returns the wrench (a+da) wrt the corresponding frame. */ IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1); #ifdef KDL_INLINE // #include "vector.inl" // #include "wrench.inl" //#include "rotation.inl" //#include "frame.inl" //#include "twist.inl" //#include "vector2.inl" //#include "rotation2.inl" //#include "frame2.inl" #include "frames.inl" #endif } #endif orocos-kdl-1.4.0/orocos_kdl/src/frames.inl000066400000000000000000000734401326704774600205260ustar00rootroot00000000000000/*************************************************************************** frames.inl - description ------------------------- begin : June 2006 copyright : (C) 2006 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 * * * ***************************************************************************/ /** * \file frames.inl * Inlined member functions and global functions that relate to the classes in frames.cpp * */ IMETHOD Vector::Vector(const Vector & arg) { data[0] = arg.data[0]; data[1] = arg.data[1]; data[2] = arg.data[2]; } IMETHOD Vector::Vector(double x,double y, double z) { data[0]=x;data[1]=y;data[2]=z; } IMETHOD Vector& Vector::operator =(const Vector & arg) { data[0] = arg.data[0]; data[1] = arg.data[1]; data[2] = arg.data[2]; return *this; } IMETHOD Vector operator +(const Vector & lhs,const Vector& rhs) { Vector tmp; tmp.data[0] = lhs.data[0]+rhs.data[0]; tmp.data[1] = lhs.data[1]+rhs.data[1]; tmp.data[2] = lhs.data[2]+rhs.data[2]; return tmp; } IMETHOD Vector operator -(const Vector & lhs,const Vector& rhs) { Vector tmp; tmp.data[0] = lhs.data[0]-rhs.data[0]; tmp.data[1] = lhs.data[1]-rhs.data[1]; tmp.data[2] = lhs.data[2]-rhs.data[2]; return tmp; } IMETHOD double Vector::x() const { return data[0]; } IMETHOD double Vector::y() const { return data[1]; } IMETHOD double Vector::z() const { return data[2]; } IMETHOD void Vector::x( double _x ) { data[0] = _x; } IMETHOD void Vector::y( double _y ) { data[1] = _y; } IMETHOD void Vector::z( double _z ) { data[2] = _z; } Vector operator *(const Vector& lhs,double rhs) { Vector tmp; tmp.data[0] = lhs.data[0]*rhs; tmp.data[1] = lhs.data[1]*rhs; tmp.data[2] = lhs.data[2]*rhs; return tmp; } Vector operator *(double lhs,const Vector& rhs) { Vector tmp; tmp.data[0] = lhs*rhs.data[0]; tmp.data[1] = lhs*rhs.data[1]; tmp.data[2] = lhs*rhs.data[2]; return tmp; } Vector operator /(const Vector& lhs,double rhs) { Vector tmp; tmp.data[0] = lhs.data[0]/rhs; tmp.data[1] = lhs.data[1]/rhs; tmp.data[2] = lhs.data[2]/rhs; return tmp; } Vector operator *(const Vector & lhs,const Vector& rhs) // Complexity : 6M+3A { Vector tmp; tmp.data[0] = lhs.data[1]*rhs.data[2]-lhs.data[2]*rhs.data[1]; tmp.data[1] = lhs.data[2]*rhs.data[0]-lhs.data[0]*rhs.data[2]; tmp.data[2] = lhs.data[0]*rhs.data[1]-lhs.data[1]*rhs.data[0]; return tmp; } Vector& Vector::operator +=(const Vector & arg) // Complexity : 3A { data[0]+=arg.data[0]; data[1]+=arg.data[1]; data[2]+=arg.data[2]; return *this; } Vector& Vector::operator -=(const Vector & arg) // Complexity : 3A { data[0]-=arg.data[0]; data[1]-=arg.data[1]; data[2]-=arg.data[2]; return *this; } Vector Vector::Zero() { return Vector(0,0,0); } double Vector::operator()(int index) const { FRAMES_CHECKI((0<=index)&&(index<=2)); return data[index]; } double& Vector::operator () (int index) { FRAMES_CHECKI((0<=index)&&(index<=2)); return data[index]; } Wrench Frame::operator * (const Wrench& arg) const // Complexity : 24M+18A { Wrench tmp; tmp.force = M*arg.force; tmp.torque = M*arg.torque + p*tmp.force; return tmp; } Wrench Frame::Inverse(const Wrench& arg) const { Wrench tmp; tmp.force = M.Inverse(arg.force); tmp.torque = M.Inverse(arg.torque-p*arg.force); return tmp; } Wrench Rotation::Inverse(const Wrench& arg) const { return Wrench(Inverse(arg.force),Inverse(arg.torque)); } Twist Rotation::Inverse(const Twist& arg) const { return Twist(Inverse(arg.vel),Inverse(arg.rot)); } Wrench Wrench::Zero() { return Wrench(Vector::Zero(),Vector::Zero()); } void Wrench::ReverseSign() { torque.ReverseSign(); force.ReverseSign(); } Wrench Wrench::RefPoint(const Vector& v_base_AB) const // Changes the reference point of the Wrench. // The vector v_base_AB is expressed in the same base as the twist // The vector v_base_AB is a vector from the old point to // the new point. { return Wrench(this->force, this->torque+this->force*v_base_AB ); } Wrench& Wrench::operator-=(const Wrench& arg) { torque-=arg.torque; force -=arg.force; return *this; } Wrench& Wrench::operator+=(const Wrench& arg) { torque+=arg.torque; force +=arg.force; return *this; } double& Wrench::operator()(int i) { // assert((0<=i)&&(i<6)); done by underlying routines if (i<3) return force(i); else return torque(i-3); } double Wrench::operator()(int i) const { // assert((0<=i)&&(i<6)); done by underlying routines if (i<3) return force(i); else return torque(i-3); } Wrench operator*(const Wrench& lhs,double rhs) { return Wrench(lhs.force*rhs,lhs.torque*rhs); } Wrench operator*(double lhs,const Wrench& rhs) { return Wrench(lhs*rhs.force,lhs*rhs.torque); } Wrench operator/(const Wrench& lhs,double rhs) { return Wrench(lhs.force/rhs,lhs.torque/rhs); } // addition of Wrench's Wrench operator+(const Wrench& lhs,const Wrench& rhs) { return Wrench(lhs.force+rhs.force,lhs.torque+rhs.torque); } Wrench operator-(const Wrench& lhs,const Wrench& rhs) { return Wrench(lhs.force-rhs.force,lhs.torque-rhs.torque); } // unary - Wrench operator-(const Wrench& arg) { return Wrench(-arg.force,-arg.torque); } Twist Frame::operator * (const Twist& arg) const // Complexity : 24M+18A { Twist tmp; tmp.rot = M*arg.rot; tmp.vel = M*arg.vel+p*tmp.rot; return tmp; } Twist Frame::Inverse(const Twist& arg) const { Twist tmp; tmp.rot = M.Inverse(arg.rot); tmp.vel = M.Inverse(arg.vel-p*arg.rot); return tmp; } Twist Twist::Zero() { return Twist(Vector::Zero(),Vector::Zero()); } void Twist::ReverseSign() { vel.ReverseSign(); rot.ReverseSign(); } Twist Twist::RefPoint(const Vector& v_base_AB) const // Changes the reference point of the twist. // The vector v_base_AB is expressed in the same base as the twist // The vector v_base_AB is a vector from the old point to // the new point. // Complexity : 6M+6A { return Twist(this->vel+this->rot*v_base_AB,this->rot); } Twist& Twist::operator-=(const Twist& arg) { vel-=arg.vel; rot -=arg.rot; return *this; } Twist& Twist::operator+=(const Twist& arg) { vel+=arg.vel; rot +=arg.rot; return *this; } double& Twist::operator()(int i) { // assert((0<=i)&&(i<6)); done by underlying routines if (i<3) return vel(i); else return rot(i-3); } double Twist::operator()(int i) const { // assert((0<=i)&&(i<6)); done by underlying routines if (i<3) return vel(i); else return rot(i-3); } Twist operator*(const Twist& lhs,double rhs) { return Twist(lhs.vel*rhs,lhs.rot*rhs); } Twist operator*(double lhs,const Twist& rhs) { return Twist(lhs*rhs.vel,lhs*rhs.rot); } Twist operator/(const Twist& lhs,double rhs) { return Twist(lhs.vel/rhs,lhs.rot/rhs); } // addition of Twist's Twist operator+(const Twist& lhs,const Twist& rhs) { return Twist(lhs.vel+rhs.vel,lhs.rot+rhs.rot); } Twist operator-(const Twist& lhs,const Twist& rhs) { return Twist(lhs.vel-rhs.vel,lhs.rot-rhs.rot); } // unary - Twist operator-(const Twist& arg) { return Twist(-arg.vel,-arg.rot); } //Spatial products for twists Twist operator*(const Twist& lhs,const Twist& rhs) { return Twist(lhs.rot*rhs.vel+lhs.vel*rhs.rot,lhs.rot*rhs.rot); } Wrench operator*(const Twist& lhs,const Wrench& rhs) { return Wrench(lhs.rot*rhs.force,lhs.rot*rhs.torque+lhs.vel*rhs.force); } Frame::Frame(const Rotation & R) { M=R; p=Vector::Zero(); } Frame::Frame(const Vector & V) { M = Rotation::Identity(); p = V; } Frame::Frame(const Rotation & R, const Vector & V) { M = R; p = V; } Frame operator *(const Frame& lhs,const Frame& rhs) // Complexity : 36M+36A { return Frame(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); } Vector Frame::operator *(const Vector & arg) const { return M*arg+p; } Vector Frame::Inverse(const Vector& arg) const { return M.Inverse(arg-p); } Frame Frame::Inverse() const { return Frame(M.Inverse(),-M.Inverse(p)); } Frame& Frame::operator =(const Frame & arg) { M = arg.M; p = arg.p; return *this; } Frame::Frame(const Frame & arg) : p(arg.p),M(arg.M) {} void Vector::ReverseSign() { data[0] = -data[0]; data[1] = -data[1]; data[2] = -data[2]; } Vector operator-(const Vector & arg) { Vector tmp; tmp.data[0]=-arg.data[0]; tmp.data[1]=-arg.data[1]; tmp.data[2]=-arg.data[2]; return tmp; } void Vector::Set2DXY(const Vector2& v) // a 3D vector where the 2D vector v is put in the XY plane { data[0]=v(0); data[1]=v(1); data[2]=0; } void Vector::Set2DYZ(const Vector2& v) // a 3D vector where the 2D vector v is put in the YZ plane { data[1]=v(0); data[2]=v(1); data[0]=0; } void Vector::Set2DZX(const Vector2& v) // a 3D vector where the 2D vector v is put in the ZX plane { data[2]=v(0); data[0]=v(1); data[1]=0; } double& Rotation::operator()(int i,int j) { FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2)); return data[i*3+j]; } double Rotation::operator()(int i,int j) const { FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2)); return data[i*3+j]; } Rotation::Rotation( double Xx,double Yx,double Zx, double Xy,double Yy,double Zy, double Xz,double Yz,double Zz) { data[0] = Xx;data[1]=Yx;data[2]=Zx; data[3] = Xy;data[4]=Yy;data[5]=Zy; data[6] = Xz;data[7]=Yz;data[8]=Zz; } Rotation::Rotation(const Vector& x,const Vector& y,const Vector& z) { data[0] = x.data[0];data[3] = x.data[1];data[6] = x.data[2]; data[1] = y.data[0];data[4] = y.data[1];data[7] = y.data[2]; data[2] = z.data[0];data[5] = z.data[1];data[8] = z.data[2]; } Rotation& Rotation::operator=(const Rotation& arg) { int count=9; while (count--) data[count] = arg.data[count]; return *this; } Vector Rotation::operator*(const Vector& v) const { // Complexity : 9M+6A return Vector( data[0]*v.data[0] + data[1]*v.data[1] + data[2]*v.data[2], data[3]*v.data[0] + data[4]*v.data[1] + data[5]*v.data[2], data[6]*v.data[0] + data[7]*v.data[1] + data[8]*v.data[2] ); } Twist Rotation::operator * (const Twist& arg) const // Transformation of the base to which the twist is expressed. // look at Frame*Twist for a transformation that also transforms // the velocity reference point. // Complexity : 18M+12A { return Twist((*this)*arg.vel,(*this)*arg.rot); } Wrench Rotation::operator * (const Wrench& arg) const // Transformation of the base to which the wrench is expressed. // look at Frame*Twist for a transformation that also transforms // the force reference point. { return Wrench((*this)*arg.force,(*this)*arg.torque); } Rotation Rotation::Identity() { return Rotation(1,0,0,0,1,0,0,0,1); } // *this = *this * ROT(X,angle) void Rotation::DoRotX(double angle) { double cs = cos(angle); double sn = sin(angle); double x1,x2,x3; x1 = cs* (*this)(0,1) + sn* (*this)(0,2); x2 = cs* (*this)(1,1) + sn* (*this)(1,2); x3 = cs* (*this)(2,1) + sn* (*this)(2,2); (*this)(0,2) = -sn* (*this)(0,1) + cs* (*this)(0,2); (*this)(1,2) = -sn* (*this)(1,1) + cs* (*this)(1,2); (*this)(2,2) = -sn* (*this)(2,1) + cs* (*this)(2,2); (*this)(0,1) = x1; (*this)(1,1) = x2; (*this)(2,1) = x3; } void Rotation::DoRotY(double angle) { double cs = cos(angle); double sn = sin(angle); double x1,x2,x3; x1 = cs* (*this)(0,0) - sn* (*this)(0,2); x2 = cs* (*this)(1,0) - sn* (*this)(1,2); x3 = cs* (*this)(2,0) - sn* (*this)(2,2); (*this)(0,2) = sn* (*this)(0,0) + cs* (*this)(0,2); (*this)(1,2) = sn* (*this)(1,0) + cs* (*this)(1,2); (*this)(2,2) = sn* (*this)(2,0) + cs* (*this)(2,2); (*this)(0,0) = x1; (*this)(1,0) = x2; (*this)(2,0) = x3; } void Rotation::DoRotZ(double angle) { double cs = cos(angle); double sn = sin(angle); double x1,x2,x3; x1 = cs* (*this)(0,0) + sn* (*this)(0,1); x2 = cs* (*this)(1,0) + sn* (*this)(1,1); x3 = cs* (*this)(2,0) + sn* (*this)(2,1); (*this)(0,1) = -sn* (*this)(0,0) + cs* (*this)(0,1); (*this)(1,1) = -sn* (*this)(1,0) + cs* (*this)(1,1); (*this)(2,1) = -sn* (*this)(2,0) + cs* (*this)(2,1); (*this)(0,0) = x1; (*this)(1,0) = x2; (*this)(2,0) = x3; } Rotation Rotation::RotX(double angle) { double cs=cos(angle); double sn=sin(angle); return Rotation(1,0,0,0,cs,-sn,0,sn,cs); } Rotation Rotation::RotY(double angle) { double cs=cos(angle); double sn=sin(angle); return Rotation(cs,0,sn,0,1,0,-sn,0,cs); } Rotation Rotation::RotZ(double angle) { double cs=cos(angle); double sn=sin(angle); return Rotation(cs,-sn,0,sn,cs,0,0,0,1); } void Frame::Integrate(const Twist& t_this,double samplefrequency) { double n = t_this.rot.Norm()/samplefrequency; if (n #include #include #include namespace KDL { std::ostream& operator << (std::ostream& os,const Vector& v) { os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<> operators. std::ostream& operator << (std::ostream& os,const Rotation2& R) { os << "[" << R.GetRot()*rad2deg << "]"; return os; } std::ostream& operator << (std::ostream& os, const Frame2& T) { os << T.M << T.p; return os; } std::istream& operator >> (std::istream& is,Vector& v) { IOTrace("Stream input Vector (vector or ZERO)"); char storage[10]; EatWord(is,"[]",storage,10); if (strlen(storage)==0) { Eat(is,'['); is >> v(0); Eat(is,','); is >> v(1); Eat(is,','); is >> v(2); EatEnd(is,']'); IOTracePop(); return is; } if (strcmp(storage,"ZERO")==0) { v = Vector::Zero(); IOTracePop(); return is; } throw Error_Frame_Vector_Unexpected_id(); } std::istream& operator >> (std::istream& is,Twist& v) { IOTrace("Stream input Twist"); Eat(is,'['); is >> v.vel(0); Eat(is,','); is >> v.vel(1); Eat(is,','); is >> v.vel(2); Eat(is,','); is >> v.rot(0); Eat(is,','); is >> v.rot(1); Eat(is,','); is >> v.rot(2); EatEnd(is,']'); IOTracePop(); return is; } std::istream& operator >> (std::istream& is,Wrench& v) { IOTrace("Stream input Wrench"); Eat(is,'['); is >> v.force(0); Eat(is,','); is >> v.force(1); Eat(is,','); is >> v.force(2); Eat(is,','); is >> v.torque(0); Eat(is,','); is >> v.torque(1); Eat(is,','); is >> v.torque(2); EatEnd(is,']'); IOTracePop(); return is; } std::istream& operator >> (std::istream& is,Rotation& r) { IOTrace("Stream input Rotation (Matrix or EULERZYX, EULERZYZ,RPY, ROT, IDENTITY)"); char storage[10]; EatWord(is,"[]",storage,10); if (strlen(storage)==0) { Eat(is,'['); for (int i=0;i<3;i++) { is >> r(i,0); Eat(is,',') ; is >> r(i,1); Eat(is,','); is >> r(i,2); if (i<2) Eat(is,';'); else EatEnd(is,']'); } IOTracePop(); return is; } Vector v; if (strcmp(storage,"EULERZYX")==0) { is >> v; v=v*deg2rad; r = Rotation::EulerZYX(v(0),v(1),v(2)); IOTracePop(); return is; } if (strcmp(storage,"EULERZYZ")==0) { is >> v; v=v*deg2rad; r = Rotation::EulerZYZ(v(0),v(1),v(2)); IOTracePop(); return is; } if (strcmp(storage,"RPY")==0) { is >> v; v=v*deg2rad; r = Rotation::RPY(v(0),v(1),v(2)); IOTracePop(); return is; } if (strcmp(storage,"ROT")==0) { is >> v; double angle; Eat(is,'['); is >> angle; EatEnd(is,']'); r = Rotation::Rot(v,angle*deg2rad); IOTracePop(); return is; } if (strcmp(storage,"IDENTITY")==0) { r = Rotation::Identity(); IOTracePop(); return is; } throw Error_Frame_Rotation_Unexpected_id(); return is; } std::istream& operator >> (std::istream& is,Frame& T) { IOTrace("Stream input Frame (Rotation,Vector) or DH[...]"); char storage[10]; EatWord(is,"[",storage,10); if (strlen(storage)==0) { Eat(is,'['); is >> T.M; is >> T.p; EatEnd(is,']'); IOTracePop(); return is; } if (strcmp(storage,"DH")==0) { double a,alpha,d,theta; Eat(is,'['); is >> a; Eat(is,','); is >> alpha; Eat(is,','); is >> d; Eat(is,','); is >> theta; EatEnd(is,']'); T = Frame::DH(a,alpha*deg2rad,d,theta*deg2rad); IOTracePop(); return is; } throw Error_Frame_Frame_Unexpected_id(); return is; } std::istream& operator >> (std::istream& is,Vector2& v) { IOTrace("Stream input Vector2"); Eat(is,'['); is >> v(0); Eat(is,','); is >> v(1); EatEnd(is,']'); IOTracePop(); return is; } std::istream& operator >> (std::istream& is,Rotation2& r) { IOTrace("Stream input Rotation2"); Eat(is,'['); double val; is >> val; r.Rot(val*deg2rad); EatEnd(is,']'); IOTracePop(); return is; } std::istream& operator >> (std::istream& is,Frame2& T) { IOTrace("Stream input Frame2"); is >> T.M; is >> T.p; IOTracePop(); return is; } } // namespace Frame orocos-kdl-1.4.0/orocos_kdl/src/frames_io.hpp000066400000000000000000000122411326704774600212120ustar00rootroot00000000000000/*************************************************************************** frames_io.h - description ------------------------- begin : June 2006 copyright : (C) 2006 Erwin Aertbelien email : firstname.lastname@mech.kuleuven.ac.be History (only major changes)( AUTHOR-Description ) : Ruben Smits - Added output for jacobian and jntarray 06/2007 *************************************************************************** * 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 * * * ***************************************************************************/ /** // // \file // Defines routines for I/O of Frame and related objects. // \verbatim // Spaces, tabs and newlines do not have any importance. // Comments are allowed C-style,C++-style, make/perl/csh -style // Description of the I/O : // Vector : OUTPUT : e.g. [10,20,30] // INPUT : // 1) [10,20,30] // 2) Zero // Twist : e.g. [1,2,3,4,5,6] // where [1,2,3] is velocity vector // where [4,5,6] is rotational velocity vector // Wrench : e.g. [1,2,3,4,5,6] // where [1,2,3] represents a force vector // where [4,5,6] represents a torque vector // Rotation : output : // [1,2,3; // 4,5,6; // 7,8,9] cfr definition of Rotation object. // input : // 1) like the output // 2) EulerZYX,EulerZYZ,RPY word followed by a vector, e.g. : // Eulerzyx[10,20,30] // (ANGLES are always expressed in DEGREES for I/O) // (ANGELS are always expressed in RADIANS for internal representation) // 3) Rot [1,2,3] [20] Rotates around axis [1,2,3] with an angle // of 20 degrees. // 4) Identity returns identity rotation matrix. // Frames : output : [ Rotationmatrix positionvector ] // e.g. [ [1,0,0;0,1,0;0,0,1] [1,2,3] ] // Input : // 1) [ Rotationmatrix positionvector ] // 2) DH [ 10,10,50,30] Denavit-Hartenberg representation // ( is in fact not the representation of a Frame, but more // limited, cfr. documentation of Frame object.) // \endverbatim // // \warning // You can use iostream.h or iostream header files for file I/O, // if one declares the define WANT_STD_IOSTREAM then the standard C++ // iostreams headers are included instead of the compiler-dependent version // * ****************************************************************************/ #ifndef FRAMES_IO_H #define FRAMES_IO_H #include "utilities/utility_io.h" #include "frames.hpp" #include "jntarray.hpp" #include "jacobian.hpp" namespace KDL { //! width to be used when printing variables out with frames_io.h //! global variable, can be changed. // I/O to C++ stream. std::ostream& operator << (std::ostream& os,const Vector& v); std::ostream& operator << (std::ostream& os,const Rotation& R); std::ostream& operator << (std::ostream& os,const Frame& T); std::ostream& operator << (std::ostream& os,const Twist& T); std::ostream& operator << (std::ostream& os,const Wrench& T); std::ostream& operator << (std::ostream& os,const Vector2& v); std::ostream& operator << (std::ostream& os,const Rotation2& R); std::ostream& operator << (std::ostream& os,const Frame2& T); std::istream& operator >> (std::istream& is,Vector& v); std::istream& operator >> (std::istream& is,Rotation& R); std::istream& operator >> (std::istream& is,Frame& T); std::istream& operator >> (std::istream& os,Twist& T); std::istream& operator >> (std::istream& os,Wrench& T); std::istream& operator >> (std::istream& is,Vector2& v); std::istream& operator >> (std::istream& is,Rotation2& R); std::istream& operator >> (std::istream& is,Frame2& T); } // namespace Frame #endif orocos-kdl-1.4.0/orocos_kdl/src/framevel.cpp000066400000000000000000000007701326704774600210460ustar00rootroot00000000000000/***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: rframes.cpp,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ * $Name: $ ****************************************************************************/ #include "framevel.hpp" namespace KDL { #ifndef KDL_INLINE #include "framevel.inl" #endif } orocos-kdl-1.4.0/orocos_kdl/src/framevel.hpp000066400000000000000000000325651326704774600210620ustar00rootroot00000000000000/***************************************************************************** * \file * This file contains the definition of classes for a * Rall Algebra of (subset of) the classes defined in frames, * i.e. classes that contain a pair (value,derivative) and define operations on that pair * this classes are useful for automatic differentiation ( <-> symbolic diff , <-> numeric diff) * Defines VectorVel, RotationVel, FrameVel. Look at Frames.h for details on how to work * with Frame objects. * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: rframes.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ * $Name: $ ****************************************************************************/ #ifndef KDL_FRAMEVEL_H #define KDL_FRAMEVEL_H #include "utilities/utility.h" #include "utilities/rall1d.h" #include "utilities/traits.h" #include "frames.hpp" namespace KDL { typedef Rall1d doubleVel; IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) { return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt); } IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) { return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt); } IMETHOD void random(doubleVel& F) { random(F.t); random(F.grad); } IMETHOD void posrandom(doubleVel& F) { posrandom(F.t); posrandom(F.grad); } } template <> struct Traits { typedef double valueType; typedef KDL::doubleVel derivType; }; namespace KDL { class TwistVel; class VectorVel; class FrameVel; class RotationVel; // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) IMETHOD bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon); IMETHOD bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon); IMETHOD bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon); IMETHOD bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon); IMETHOD bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon); IMETHOD bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon); IMETHOD bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon); IMETHOD bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon); IMETHOD bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon); IMETHOD bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon); IMETHOD bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon); IMETHOD bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon); class VectorVel // = TITLE // An VectorVel is a Vector and its first derivative // = CLASS TYPE // Concrete { public: Vector p; // position vector Vector v; // velocity vector public: VectorVel():p(),v(){} VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {} explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {} Vector value() const { return p;} Vector deriv() const { return v;} IMETHOD VectorVel& operator = (const VectorVel& arg); IMETHOD VectorVel& operator = (const Vector& arg); IMETHOD VectorVel& operator += (const VectorVel& arg); IMETHOD VectorVel& operator -= (const VectorVel& arg); IMETHOD static VectorVel Zero(); IMETHOD void ReverseSign(); IMETHOD doubleVel Norm() const; IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2); IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2); IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2); IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2); IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2); IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2); IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2); IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2); IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2); IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2); IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2); IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2); IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1); IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x); IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2); IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1); IMETHOD friend void SetToZero(VectorVel& v); IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps); IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps); IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps); IMETHOD friend VectorVel operator - (const VectorVel& r); IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs); IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs); IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs); }; class RotationVel // = TITLE // An RotationVel is a Rotation and its first derivative, a rotation vector // = CLASS TYPE // Concrete { public: Rotation R; // Rotation matrix Vector w; // rotation vector public: RotationVel():R(),w() {} explicit RotationVel(const Rotation& _R):R(_R),w(Vector::Zero()){} RotationVel(const Rotation& _R,const Vector& _w):R(_R),w(_w){} Rotation value() const { return R;} Vector deriv() const { return w;} IMETHOD RotationVel& operator = (const RotationVel& arg); IMETHOD RotationVel& operator = (const Rotation& arg); IMETHOD VectorVel UnitX() const; IMETHOD VectorVel UnitY() const; IMETHOD VectorVel UnitZ() const; IMETHOD static RotationVel Identity(); IMETHOD RotationVel Inverse() const; IMETHOD VectorVel Inverse(const VectorVel& arg) const; IMETHOD VectorVel Inverse(const Vector& arg) const; IMETHOD VectorVel operator*(const VectorVel& arg) const; IMETHOD VectorVel operator*(const Vector& arg) const; IMETHOD void DoRotX(const doubleVel& angle); IMETHOD void DoRotY(const doubleVel& angle); IMETHOD void DoRotZ(const doubleVel& angle); IMETHOD static RotationVel RotX(const doubleVel& angle); IMETHOD static RotationVel RotY(const doubleVel& angle); IMETHOD static RotationVel RotZ(const doubleVel& angle); IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle); // rotvec has arbitrary norm // rotation around a constant vector ! IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle); // rotvec is normalized. // rotation around a constant vector ! IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2); IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2); IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2); IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps); IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps); IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps); IMETHOD TwistVel Inverse(const TwistVel& arg) const; IMETHOD TwistVel Inverse(const Twist& arg) const; IMETHOD TwistVel operator * (const TwistVel& arg) const; IMETHOD TwistVel operator * (const Twist& arg) const; }; class FrameVel // = TITLE // An FrameVel is a Frame and its first derivative, a Twist vector // = CLASS TYPE // Concrete // = CAVEATS // { public: RotationVel M; VectorVel p; public: FrameVel(){} explicit FrameVel(const Frame& _T): M(_T.M),p(_T.p) {} FrameVel(const Frame& _T,const Twist& _t): M(_T.M,_t.rot),p(_T.p,_t.vel) {} FrameVel(const RotationVel& _M,const VectorVel& _p): M(_M),p(_p) {} Frame value() const { return Frame(M.value(),p.value());} Twist deriv() const { return Twist(p.deriv(),M.deriv());} IMETHOD FrameVel& operator = (const Frame& arg); IMETHOD FrameVel& operator = (const FrameVel& arg); IMETHOD static FrameVel Identity(); IMETHOD FrameVel Inverse() const; IMETHOD VectorVel Inverse(const VectorVel& arg) const; IMETHOD VectorVel operator*(const VectorVel& arg) const; IMETHOD VectorVel operator*(const Vector& arg) const; IMETHOD VectorVel Inverse(const Vector& arg) const; IMETHOD Frame GetFrame() const; IMETHOD Twist GetTwist() const; IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2); IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2); IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2); IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps); IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps); IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps); IMETHOD TwistVel Inverse(const TwistVel& arg) const; IMETHOD TwistVel Inverse(const Twist& arg) const; IMETHOD TwistVel operator * (const TwistVel& arg) const; IMETHOD TwistVel operator * (const Twist& arg) const; }; //very similar to Wrench class. class TwistVel // = TITLE // This class represents a TwistVel. This is a velocity and rotational velocity together { public: VectorVel vel; VectorVel rot; public: // = Constructors TwistVel():vel(),rot() {}; TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {}; TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {}; TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {}; Twist value() const { return Twist(vel.value(),rot.value()); } Twist deriv() const { return Twist(vel.deriv(),rot.deriv()); } // = Operators IMETHOD TwistVel& operator-=(const TwistVel& arg); IMETHOD TwistVel& operator+=(const TwistVel& arg); // = External operators IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs); IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs); IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs); IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs); IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs); IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs); IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs); IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs); IMETHOD friend TwistVel operator-(const TwistVel& arg); IMETHOD friend void SetToZero(TwistVel& v); // = Zero static IMETHOD TwistVel Zero(); // = Reverse Sign IMETHOD void ReverseSign(); // = Change Reference point IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB); // Changes the reference point of the TwistVel. // The VectorVel v_base_AB is expressed in the same base as the TwistVel // The VectorVel v_base_AB is a VectorVel from the old point to // the new point. // Complexity : 6M+6A // = Equality operators // do not use operator == because the definition of Equal(.,.) is slightly // different. It compares whether the 2 arguments are equal in an eps-interval IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps); IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps); IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps); // = Conversion to other entities IMETHOD Twist GetTwist() const; IMETHOD Twist GetTwistDot() const; // = Friends friend class RotationVel; friend class FrameVel; }; IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) { return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt)); } IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) { return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt)); } IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) { return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt)); } IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) { return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt)); } IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) { return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt)); } IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) { return FrameVel( addDelta(a.M,da.rot,dt), addDelta(a.p,da.vel,dt) ); } IMETHOD void random(VectorVel& a) { random(a.p); random(a.v); } IMETHOD void random(TwistVel& a) { random(a.vel); random(a.rot); } IMETHOD void random(RotationVel& R) { random(R.R); random(R.w); } IMETHOD void random(FrameVel& F) { random(F.M); random(F.p); } IMETHOD void posrandom(VectorVel& a) { posrandom(a.p); posrandom(a.v); } IMETHOD void posrandom(TwistVel& a) { posrandom(a.vel); posrandom(a.rot); } IMETHOD void posrandom(RotationVel& R) { posrandom(R.R); posrandom(R.w); } IMETHOD void posrandom(FrameVel& F) { posrandom(F.M); posrandom(F.p); } #ifdef KDL_INLINE #include "framevel.inl" #endif } // namespace #endif orocos-kdl-1.4.0/orocos_kdl/src/framevel.inl000066400000000000000000000304511326704774600210450ustar00rootroot00000000000000/***************************************************************************** * \file * provides inline functions of rframes.h * * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: rframes.inl,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ * $Name: $ ****************************************************************************/ // Methods and operators related to FrameVelVel // They all delegate most of the work to RotationVelVel and VectorVelVel FrameVel& FrameVel::operator = (const FrameVel& arg) { M=arg.M; p=arg.p; return *this; } FrameVel FrameVel::Identity() { return FrameVel(RotationVel::Identity(),VectorVel::Zero()); } FrameVel operator *(const FrameVel& lhs,const FrameVel& rhs) { return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); } FrameVel operator *(const FrameVel& lhs,const Frame& rhs) { return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); } FrameVel operator *(const Frame& lhs,const FrameVel& rhs) { return FrameVel(lhs.M*rhs.M , lhs.M*rhs.p+lhs.p ); } VectorVel FrameVel::operator *(const VectorVel & arg) const { return M*arg+p; } VectorVel FrameVel::operator *(const Vector & arg) const { return M*arg+p; } VectorVel FrameVel::Inverse(const VectorVel& arg) const { return M.Inverse(arg-p); } VectorVel FrameVel::Inverse(const Vector& arg) const { return M.Inverse(arg-p); } FrameVel FrameVel::Inverse() const { return FrameVel(M.Inverse(),-M.Inverse(p)); } FrameVel& FrameVel::operator = (const Frame& arg) { M = arg.M; p = arg.p; return *this; } bool Equal(const FrameVel& r1,const FrameVel& r2,double eps) { return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); } bool Equal(const Frame& r1,const FrameVel& r2,double eps) { return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); } bool Equal(const FrameVel& r1,const Frame& r2,double eps) { return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); } Frame FrameVel::GetFrame() const { return Frame(M.R,p.p); } Twist FrameVel::GetTwist() const { return Twist(p.v,M.w); } RotationVel operator* (const RotationVel& r1,const RotationVel& r2) { return RotationVel( r1.R*r2.R, r1.w + r1.R*r2.w ); } RotationVel operator* (const Rotation& r1,const RotationVel& r2) { return RotationVel( r1*r2.R, r1*r2.w ); } RotationVel operator* (const RotationVel& r1,const Rotation& r2) { return RotationVel( r1.R*r2, r1.w ); } RotationVel& RotationVel::operator = (const RotationVel& arg) { R=arg.R; w=arg.w; return *this; } RotationVel& RotationVel::operator = (const Rotation& arg) { R=arg; w=Vector::Zero(); return *this; } VectorVel RotationVel::UnitX() const { return VectorVel(R.UnitX(),w*R.UnitX()); } VectorVel RotationVel::UnitY() const { return VectorVel(R.UnitY(),w*R.UnitY()); } VectorVel RotationVel::UnitZ() const { return VectorVel(R.UnitZ(),w*R.UnitZ()); } RotationVel RotationVel::Identity() { return RotationVel(Rotation::Identity(),Vector::Zero()); } RotationVel RotationVel::Inverse() const { return RotationVel(R.Inverse(),-R.Inverse(w)); } VectorVel RotationVel::Inverse(const VectorVel& arg) const { Vector tmp=R.Inverse(arg.p); return VectorVel(tmp, R.Inverse(arg.v-w*arg.p) ); } VectorVel RotationVel::Inverse(const Vector& arg) const { Vector tmp=R.Inverse(arg); return VectorVel(tmp, R.Inverse(-w*arg) ); } VectorVel RotationVel::operator*(const VectorVel& arg) const { Vector tmp=R*arg.p; return VectorVel(tmp,w*tmp+R*arg.v); } VectorVel RotationVel::operator*(const Vector& arg) const { Vector tmp=R*arg; return VectorVel(tmp,w*tmp); } // = Rotations // The Rot... static functions give the value of the appropriate rotation matrix back. // The DoRot... functions apply a rotation R to *this,such that *this = *this * R. void RotationVel::DoRotX(const doubleVel& angle) { w+=R*Vector(angle.grad,0,0); R.DoRotX(angle.t); } RotationVel RotationVel::RotX(const doubleVel& angle) { return RotationVel(Rotation::RotX(angle.t),Vector(angle.grad,0,0)); } void RotationVel::DoRotY(const doubleVel& angle) { w+=R*Vector(0,angle.grad,0); R.DoRotY(angle.t); } RotationVel RotationVel::RotY(const doubleVel& angle) { return RotationVel(Rotation::RotX(angle.t),Vector(0,angle.grad,0)); } void RotationVel::DoRotZ(const doubleVel& angle) { w+=R*Vector(0,0,angle.grad); R.DoRotZ(angle.t); } RotationVel RotationVel::RotZ(const doubleVel& angle) { return RotationVel(Rotation::RotZ(angle.t),Vector(0,0,angle.grad)); } RotationVel RotationVel::Rot(const Vector& rotvec,const doubleVel& angle) // rotvec has arbitrary norm // rotation around a constant vector ! { Vector v(rotvec); v.Normalize(); return RotationVel(Rotation::Rot2(v,angle.t),v*angle.grad); } RotationVel RotationVel::Rot2(const Vector& rotvec,const doubleVel& angle) // rotvec is normalized. { return RotationVel(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad); } VectorVel operator + (const VectorVel& r1,const VectorVel& r2) { return VectorVel(r1.p+r2.p,r1.v+r2.v); } VectorVel operator - (const VectorVel& r1,const VectorVel& r2) { return VectorVel(r1.p-r2.p,r1.v-r2.v); } VectorVel operator + (const VectorVel& r1,const Vector& r2) { return VectorVel(r1.p+r2,r1.v); } VectorVel operator - (const VectorVel& r1,const Vector& r2) { return VectorVel(r1.p-r2,r1.v); } VectorVel operator + (const Vector& r1,const VectorVel& r2) { return VectorVel(r1+r2.p,r2.v); } VectorVel operator - (const Vector& r1,const VectorVel& r2) { return VectorVel(r1-r2.p,-r2.v); } // unary - VectorVel operator - (const VectorVel& r) { return VectorVel(-r.p,-r.v); } void SetToZero(VectorVel& v){ SetToZero(v.p); SetToZero(v.v); } // cross prod. VectorVel operator * (const VectorVel& r1,const VectorVel& r2) { return VectorVel(r1.p*r2.p, r1.p*r2.v+r1.v*r2.p); } VectorVel operator * (const VectorVel& r1,const Vector& r2) { return VectorVel(r1.p*r2, r1.v*r2); } VectorVel operator * (const Vector& r1,const VectorVel& r2) { return VectorVel(r1*r2.p, r1*r2.v); } // scalar mult. VectorVel operator * (double r1,const VectorVel& r2) { return VectorVel(r1*r2.p, r1*r2.v); } VectorVel operator * (const VectorVel& r1,double r2) { return VectorVel(r1.p*r2, r1.v*r2); } VectorVel operator * (const doubleVel& r1,const VectorVel& r2) { return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p); } VectorVel operator * (const VectorVel& r2,const doubleVel& r1) { return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p); } VectorVel operator / (const VectorVel& r1,double r2) { return VectorVel(r1.p/r2, r1.v/r2); } VectorVel operator / (const VectorVel& r2,const doubleVel& r1) { return VectorVel(r2.p/r1.t, r2.v/r1.t - r2.p*r1.grad/r1.t/r1.t); } VectorVel operator*(const Rotation& R,const VectorVel& x) { return VectorVel(R*x.p,R*x.v); } VectorVel& VectorVel::operator = (const VectorVel& arg) { p=arg.p; v=arg.v; return *this; } VectorVel& VectorVel::operator = (const Vector& arg) { p=arg; v=Vector::Zero(); return *this; } VectorVel& VectorVel::operator += (const VectorVel& arg) { p+=arg.p; v+=arg.v; return *this; } VectorVel& VectorVel::operator -= (const VectorVel& arg) { p-=arg.p; v-=arg.v; return *this; } VectorVel VectorVel::Zero() { return VectorVel(Vector::Zero(),Vector::Zero()); } void VectorVel::ReverseSign() { p.ReverseSign(); v.ReverseSign(); } doubleVel VectorVel::Norm() const { double n = p.Norm(); return doubleVel(n,dot(p,v)/n); } bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) { return (Equal(r1.p,r2.p,eps) && Equal(r1.v,r2.v,eps)); } bool Equal(const Vector& r1,const VectorVel& r2,double eps) { return (Equal(r1,r2.p,eps) && Equal(Vector::Zero(),r2.v,eps)); } bool Equal(const VectorVel& r1,const Vector& r2,double eps) { return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps)); } bool Equal(const RotationVel& r1,const RotationVel& r2,double eps) { return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps)); } bool Equal(const Rotation& r1,const RotationVel& r2,double eps) { return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps)); } bool Equal(const RotationVel& r1,const Rotation& r2,double eps) { return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps)); } bool Equal(const TwistVel& a,const TwistVel& b,double eps) { return (Equal(a.rot,b.rot,eps)&& Equal(a.vel,b.vel,eps) ); } bool Equal(const Twist& a,const TwistVel& b,double eps) { return (Equal(a.rot,b.rot,eps)&& Equal(a.vel,b.vel,eps) ); } bool Equal(const TwistVel& a,const Twist& b,double eps) { return (Equal(a.rot,b.rot,eps)&& Equal(a.vel,b.vel,eps) ); } IMETHOD doubleVel dot(const VectorVel& lhs,const VectorVel& rhs) { return doubleVel(dot(lhs.p,rhs.p),dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p)); } IMETHOD doubleVel dot(const VectorVel& lhs,const Vector& rhs) { return doubleVel(dot(lhs.p,rhs),dot(lhs.v,rhs)); } IMETHOD doubleVel dot(const Vector& lhs,const VectorVel& rhs) { return doubleVel(dot(lhs,rhs.p),dot(lhs,rhs.v)); } TwistVel TwistVel::Zero() { return TwistVel(VectorVel::Zero(),VectorVel::Zero()); } void TwistVel::ReverseSign() { vel.ReverseSign(); rot.ReverseSign(); } TwistVel TwistVel::RefPoint(const VectorVel& v_base_AB) // Changes the reference point of the TwistVel. // The VectorVel v_base_AB is expressed in the same base as the TwistVel // The VectorVel v_base_AB is a VectorVel from the old point to // the new point. // Complexity : 6M+6A { return TwistVel(this->vel+this->rot*v_base_AB,this->rot); } TwistVel& TwistVel::operator-=(const TwistVel& arg) { vel-=arg.vel; rot -=arg.rot; return *this; } TwistVel& TwistVel::operator+=(const TwistVel& arg) { vel+=arg.vel; rot +=arg.rot; return *this; } TwistVel operator*(const TwistVel& lhs,double rhs) { return TwistVel(lhs.vel*rhs,lhs.rot*rhs); } TwistVel operator*(double lhs,const TwistVel& rhs) { return TwistVel(lhs*rhs.vel,lhs*rhs.rot); } TwistVel operator/(const TwistVel& lhs,double rhs) { return TwistVel(lhs.vel/rhs,lhs.rot/rhs); } TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs) { return TwistVel(lhs.vel*rhs,lhs.rot*rhs); } TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs) { return TwistVel(lhs*rhs.vel,lhs*rhs.rot); } TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs) { return TwistVel(lhs.vel/rhs,lhs.rot/rhs); } // addition of TwistVel's TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs) { return TwistVel(lhs.vel+rhs.vel,lhs.rot+rhs.rot); } TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs) { return TwistVel(lhs.vel-rhs.vel,lhs.rot-rhs.rot); } // unary - TwistVel operator-(const TwistVel& arg) { return TwistVel(-arg.vel,-arg.rot); } void SetToZero(TwistVel& v) { SetToZero(v.vel); SetToZero(v.rot); } TwistVel RotationVel::Inverse(const TwistVel& arg) const { return TwistVel(Inverse(arg.vel),Inverse(arg.rot)); } TwistVel RotationVel::operator * (const TwistVel& arg) const { return TwistVel((*this)*arg.vel,(*this)*arg.rot); } TwistVel RotationVel::Inverse(const Twist& arg) const { return TwistVel(Inverse(arg.vel),Inverse(arg.rot)); } TwistVel RotationVel::operator * (const Twist& arg) const { return TwistVel((*this)*arg.vel,(*this)*arg.rot); } TwistVel FrameVel::operator * (const TwistVel& arg) const { TwistVel tmp; tmp.rot = M*arg.rot; tmp.vel = M*arg.vel+p*tmp.rot; return tmp; } TwistVel FrameVel::operator * (const Twist& arg) const { TwistVel tmp; tmp.rot = M*arg.rot; tmp.vel = M*arg.vel+p*tmp.rot; return tmp; } TwistVel FrameVel::Inverse(const TwistVel& arg) const { TwistVel tmp; tmp.rot = M.Inverse(arg.rot); tmp.vel = M.Inverse(arg.vel-p*arg.rot); return tmp; } TwistVel FrameVel::Inverse(const Twist& arg) const { TwistVel tmp; tmp.rot = M.Inverse(arg.rot); tmp.vel = M.Inverse(arg.vel-p*arg.rot); return tmp; } Twist TwistVel::GetTwist() const { return Twist(vel.p,rot.p); } Twist TwistVel::GetTwistDot() const { return Twist(vel.v,rot.v); } orocos-kdl-1.4.0/orocos_kdl/src/framevel_io.hpp000066400000000000000000000026251326704774600215430ustar00rootroot00000000000000/***************************************************************************** * \file * provides I/O operations on FrameVels classes * * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V2 * * \par History * - $log$ * * \par Release * $Id: rframes_io.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ * $Name: $ ****************************************************************************/ #ifndef KDL_FRAMESVEL_IO #define KDL_FRAMESVEL_IO #include "utilities/utility_io.h" #include "utilities/rall1d_io.h" #include "framevel_io.hpp" #include "frames_io.hpp" namespace KDL { // Output... inline std::ostream& operator << (std::ostream& os,const VectorVel& r) { os << "{" << r.p << "," << r.v << "}" << std::endl; return os; } inline std::ostream& operator << (std::ostream& os,const RotationVel& r) { os << "{" << std::endl << r.R << "," < // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "jacobian.hpp" namespace KDL { using namespace Eigen; Jacobian::Jacobian() { } Jacobian::Jacobian(unsigned int nr_of_columns): data(6,nr_of_columns) { } Jacobian::Jacobian(const Jacobian& arg): data(arg.data) { } Jacobian& Jacobian::operator = (const Jacobian& arg) { this->data=arg.data; return *this; } Jacobian::~Jacobian() { } void Jacobian::resize(unsigned int new_nr_of_columns) { data.conservativeResize(Eigen::NoChange,new_nr_of_columns); } double Jacobian::operator()(unsigned int i,unsigned int j)const { return data(i,j); } double& Jacobian::operator()(unsigned int i,unsigned int j) { return data(i,j); } unsigned int Jacobian::rows()const { return data.rows(); } unsigned int Jacobian::columns()const { return data.cols(); } void SetToZero(Jacobian& jac) { jac.data.setZero(); } void Jacobian::changeRefPoint(const Vector& base_AB){ for(unsigned int i=0;isetColumn(i,this->getColumn(i).RefPoint(base_AB)); } bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest) { if(src1.columns()!=dest.columns()) return false; for(unsigned int i=0;isetColumn(i,rot*this->getColumn(i));; } bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest) { if(src1.columns()!=dest.columns()) return false; for(unsigned int i=0;isetColumn(i,frame*this->getColumn(i)); } bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest) { if(src1.columns()!=dest.columns()) return false; for(unsigned int i=0;i()=Eigen::Map(t.vel.data); data.col(i).tail<3>()=Eigen::Map(t.rot.data); } } orocos-kdl-1.4.0/orocos_kdl/src/jacobian.hpp000066400000000000000000000060311326704774600210140ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_JACOBIAN_HPP #define KDL_JACOBIAN_HPP #include "frames.hpp" #include namespace KDL { // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) class Jacobian; bool Equal(const Jacobian& a,const Jacobian& b,double eps=epsilon); void SetToZero(Jacobian& jac); class Jacobian { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix data; Jacobian(); explicit Jacobian(unsigned int nr_of_columns); Jacobian(const Jacobian& arg); ///Allocates memory for new size (can break realtime behavior) void resize(unsigned int newNrOfColumns); ///Allocates memory if size of this and argument is different Jacobian& operator=(const Jacobian& arg); bool operator ==(const Jacobian& arg)const; bool operator !=(const Jacobian& arg)const; friend bool Equal(const Jacobian& a,const Jacobian& b,double eps); ~Jacobian(); double operator()(unsigned int i,unsigned int j)const; double& operator()(unsigned int i,unsigned int j); unsigned int rows()const; unsigned int columns()const; friend void SetToZero(Jacobian& jac); friend bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest); friend bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest); friend bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest); Twist getColumn(unsigned int i) const; void setColumn(unsigned int i,const Twist& t); void changeRefPoint(const Vector& base_AB); void changeBase(const Rotation& rot); void changeRefFrame(const Frame& frame); }; bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest); bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest); bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest); } #endif orocos-kdl-1.4.0/orocos_kdl/src/jntarray.cpp000066400000000000000000000061561326704774600211030ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "jntarray.hpp" namespace KDL { using namespace Eigen; JntArray::JntArray() { } JntArray::JntArray(unsigned int _size): data(_size) { data.setZero(); } JntArray::JntArray(const JntArray& arg): data(arg.data) { } JntArray& JntArray::operator = (const JntArray& arg) { data=arg.data; return *this; } JntArray::~JntArray() { } void JntArray::resize(unsigned int newSize) { data.conservativeResizeLike(VectorXd::Zero(newSize)); } double JntArray::operator()(unsigned int i,unsigned int j)const { assert(j==0); return data(i); } double& JntArray::operator()(unsigned int i,unsigned int j) { assert(j==0); return data(i); } unsigned int JntArray::rows()const { return data.rows(); } unsigned int JntArray::columns()const { return data.cols(); } void Add(const JntArray& src1,const JntArray& src2,JntArray& dest) { dest.data=src1.data+src2.data; } void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest) { dest.data=src1.data-src2.data; } void Multiply(const JntArray& src,const double& factor,JntArray& dest) { dest.data=factor*src.data; } void Divide(const JntArray& src,const double& factor,JntArray& dest) { dest.data=src.data/factor; } void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest) { Eigen::Matrix t=jac.data.lazyProduct(src.data); dest=Twist(Vector(t(0),t(1),t(2)),Vector(t(3),t(4),t(5))); } void SetToZero(JntArray& array) { array.data.setZero(); } bool Equal(const JntArray& src1, const JntArray& src2,double eps) { if(src1.rows()!=src2.rows()) return false; return src1.data.isApprox(src2.data,eps); } bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}; //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}; } orocos-kdl-1.4.0/orocos_kdl/src/jntarray.hpp000066400000000000000000000171021326704774600211010ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_JNTARRAY_HPP #define KDL_JNTARRAY_HPP #include "frames.hpp" #include "jacobian.hpp" #include namespace KDL { /** * @brief This class represents an fixed size array containing * joint values of a KDL::Chain. * * \warning An object constructed with the default constructor provides * a valid, but inert, object. Many of the member functions will do * the correct thing and have no affect on this object, but some * member functions can _NOT_ deal with an inert/empty object. These * functions will assert() and exit the program instead. The intended use * case for the default constructor (in an RTT/OCL setting) is outlined in * code below - the default constructor plus the resize() function allow * use of JntArray objects whose size is set within a configureHook() call * (typically based on a size determined from a property). \code class MyTask : public RTT::TaskContext { JntArray j; MyTask() {} // invokes j's default constructor bool configureHook() { unsigned int size = some_property.rvalue(); j.resize(size) ... } void updateHook() { ** use j here } }; \endcode */ class JntArray { public: Eigen::VectorXd data; /** Construct with _no_ data array * @post NULL == data * @post 0 == rows() * @warning use of an object constructed like this, without * a resize() first, may result in program exit! See class * documentation. */ JntArray(); /** * Constructor of the joint array * * @param size size of the array, this cannot be changed * afterwards. * @pre 0 < size * @post NULL != data * @post 0 < rows() * @post all elements in data have 0 value */ explicit JntArray(unsigned int size); /** Copy constructor * @note Will correctly copy an empty object */ JntArray(const JntArray& arg); ~JntArray(); /** Resize the array * @warning This causes a dynamic allocation (and potentially * also a dynamic deallocation). This _will_ negatively affect * real-time performance! * * @post newSize == rows() * @post NULL != data * @post all elements in data have 0 value */ void resize(unsigned int newSize); JntArray& operator = ( const JntArray& arg); /** * get_item operator for the joint array, if a second value is * given it should be zero, since a JntArray resembles a column. * * * @return the joint value at position i, starting from 0 * @pre 0 != size (ie non-default constructor or resize() called) */ double operator()(unsigned int i,unsigned int j=0)const; /** * set_item operator, again if a second value is given it *should be zero. * * @return reference to the joint value at position i,starting *from zero. * @pre 0 != size (ie non-default constructor or resize() called) */ double& operator()(unsigned int i,unsigned int j=0); /** * Returns the number of rows (size) of the array * */ unsigned int rows()const; /** * Returns the number of columns of the array, always 1. */ unsigned int columns()const; friend void Add(const JntArray& src1,const JntArray& src2,JntArray& dest); friend void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest); friend void Multiply(const JntArray& src,const double& factor,JntArray& dest); friend void Divide(const JntArray& src,const double& factor,JntArray& dest); friend void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest); friend void SetToZero(JntArray& array); friend bool Equal(const JntArray& src1,const JntArray& src2,double eps); friend bool operator==(const JntArray& src1,const JntArray& src2); //friend bool operator!=(const JntArray& src1,const JntArray& src2); }; bool operator==(const JntArray& src1,const JntArray& src2); //bool operator!=(const JntArray& src1,const JntArray& src2); /** * Function to add two joint arrays, all the arguments must * have the same size: A + B = C. This function is * aliasing-safe, A or B can be the same array as C. * * @param src1 A * @param src2 B * @param dest C */ void Add(const JntArray& src1,const JntArray& src2,JntArray& dest); /** * Function to subtract two joint arrays, all the arguments must * have the same size: A - B = C. This function is * aliasing-safe, A or B can be the same array as C. * * @param src1 A * @param src2 B * @param dest C */ void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest); /** * Function to multiply all the array values with a scalar * factor: A*b=C. This function is aliasing-safe, A can be the * same array as C. * * @param src A * @param factor b * @param dest C */ void Multiply(const JntArray& src,const double& factor,JntArray& dest); /** * Function to divide all the array values with a scalar * factor: A/b=C. This function is aliasing-safe, A can be the * same array as C. * * @param src A * @param factor b * @param dest C */ void Divide(const JntArray& src,const double& factor,JntArray& dest); /** * Function to multiply a KDL::Jacobian with a KDL::JntArray * to get a KDL::Twist, it should not be used to calculate the * forward velocity kinematics, the solver classes are built * for this purpose. * J*q = t * * @param jac J * @param src q * @param dest t * @post dest==Twist::Zero() if 0==src.rows() (ie src is empty) */ void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest); /** * Function to set all the values of the array to 0 * * @param array */ void SetToZero(JntArray& array); /** * Function to check if two arrays are the same with a *precision of eps * * @param src1 * @param src2 * @param eps default: epsilon * @return true if each element of src1 is within eps of the same * element in src2, or if both src1 and src2 have no data (ie 0==rows()) */ bool Equal(const JntArray& src1,const JntArray& src2,double eps=epsilon); } #endif orocos-kdl-1.4.0/orocos_kdl/src/jntarrayacc.cpp000066400000000000000000000141771326704774600215540ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "jntarrayacc.hpp" namespace KDL { JntArrayAcc::JntArrayAcc(unsigned int size): q(size),qdot(size),qdotdot(size) { } JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin,const JntArray& qdotdotin): q(qin),qdot(qdotin),qdotdot(qdotdotin) { assert(q.rows()==qdot.rows()&&qdot.rows()==qdotdot.rows()); } JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin): q(qin),qdot(qdotin),qdotdot(q.rows()) { assert(q.rows()==qdot.rows()); } JntArrayAcc::JntArrayAcc(const JntArray& qin): q(qin),qdot(q.rows()),qdotdot(q.rows()) { } void JntArrayAcc::resize(unsigned int newSize) { q.resize(newSize); qdot.resize(newSize); qdotdot.resize(newSize); } JntArray JntArrayAcc::value()const { return q; } JntArray JntArrayAcc::deriv()const { return qdot; } JntArray JntArrayAcc::dderiv()const { return qdotdot; } void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest) { Add(src1.q,src2.q,dest.q); Add(src1.qdot,src2.qdot,dest.qdot); Add(src1.qdotdot,src2.qdotdot,dest.qdotdot); } void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest) { Add(src1.q,src2.q,dest.q); Add(src1.qdot,src2.qdot,dest.qdot); dest.qdotdot=src1.qdotdot; } void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest) { Add(src1.q,src2,dest.q); dest.qdot=src1.qdot; dest.qdotdot=src1.qdotdot; } void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest) { Subtract(src1.q,src2.q,dest.q); Subtract(src1.qdot,src2.qdot,dest.qdot); Subtract(src1.qdotdot,src2.qdotdot,dest.qdotdot); } void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest) { Subtract(src1.q,src2.q,dest.q); Subtract(src1.qdot,src2.qdot,dest.qdot); dest.qdotdot=src1.qdotdot; } void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest) { Subtract(src1.q,src2,dest.q); dest.qdot=src1.qdot; dest.qdotdot=src1.qdotdot; } void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest) { Multiply(src.q,factor,dest.q); Multiply(src.qdot,factor,dest.qdot); Multiply(src.qdotdot,factor,dest.qdotdot); } void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest) { Multiply(src.qdot,factor.grad*2,dest.qdot); Multiply(src.qdotdot,factor.t,dest.qdotdot); Add(dest.qdot,dest.qdotdot,dest.qdotdot); Multiply(src.q,factor.grad,dest.q); Multiply(src.qdot,factor.t,dest.qdot); Add(dest.qdot,dest.q,dest.qdot); Multiply(src.q,factor.t,dest.q); } void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest) { Multiply(src.q,factor.dd,dest.q); Multiply(src.qdot,factor.d*2,dest.qdot); Multiply(src.qdotdot,factor.t,dest.qdotdot); Add(dest.qdotdot,dest.qdot,dest.qdotdot); Add(dest.qdotdot,dest.q,dest.qdotdot); Multiply(src.q,factor.d,dest.q); Multiply(src.qdot,factor.t,dest.qdot); Add(dest.qdot,dest.q,dest.qdot); Multiply(src.q,factor.t,dest.q); } void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest) { Divide(src.q,factor,dest.q); Divide(src.qdot,factor,dest.qdot); Divide(src.qdotdot,factor,dest.qdotdot); } void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest) { Multiply(src.q,(2*factor.grad*factor.grad)/(factor.t*factor.t*factor.t),dest.q); Multiply(src.qdot,(2*factor.grad)/(factor.t*factor.t),dest.qdot); Divide(src.qdotdot,factor.t,dest.qdotdot); Subtract(dest.qdotdot,dest.qdot,dest.qdotdot); Add(dest.qdotdot,dest.q,dest.qdotdot); Multiply(src.q,factor.grad/(factor.t*factor.t),dest.q); Divide(src.qdot,factor.t,dest.qdot); Subtract(dest.qdot,dest.q,dest.qdot); Divide(src.q,factor.t,dest.q); } void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest) { Multiply(src.q,(2*factor.d*factor.d)/(factor.t*factor.t*factor.t)-factor.dd/(factor.t*factor.t),dest.q); Multiply(src.qdot,(2*factor.d)/(factor.t*factor.t),dest.qdot); Divide(src.qdotdot,factor.t,dest.qdotdot); Subtract(dest.qdotdot,dest.qdot,dest.qdotdot); Add(dest.qdotdot,dest.q,dest.qdotdot); Multiply(src.q,factor.d/(factor.t*factor.t),dest.q); Divide(src.qdot,factor.t,dest.qdot); Subtract(dest.qdot,dest.q,dest.qdot); Divide(src.q,factor.t,dest.q); } void SetToZero(JntArrayAcc& array) { SetToZero(array.q); SetToZero(array.qdot); SetToZero(array.qdotdot); } bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps) { return (Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps)&&Equal(src1.qdotdot,src2.qdotdot,eps)); } } orocos-kdl-1.4.0/orocos_kdl/src/jntarrayacc.hpp000066400000000000000000000102131326704774600215440ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_JNTARRAYACC_HPP #define KDL_JNTARRAYACC_HPP #include "utilities/utility.h" #include "jntarray.hpp" #include "jntarrayvel.hpp" #include "frameacc.hpp" namespace KDL { // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) class JntArrayAcc; bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps=epsilon); void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest); void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest); void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest); void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest); void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest); void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest); void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest); void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest); void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest); void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest); void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest); void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest); void SetToZero(JntArrayAcc& array); class JntArrayAcc { public: JntArray q; JntArray qdot; JntArray qdotdot; public: JntArrayAcc(){}; explicit JntArrayAcc(unsigned int size); JntArrayAcc(const JntArray& q,const JntArray& qdot,const JntArray& qdotdot); JntArrayAcc(const JntArray& q,const JntArray& qdot); explicit JntArrayAcc(const JntArray& q); void resize(unsigned int newSize); JntArray value()const; JntArray deriv()const; JntArray dderiv()const; friend void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest); friend void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest); friend void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest); friend void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest); friend void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest); friend void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest); friend void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest); friend void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest); friend void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest); friend void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest); friend void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest); friend void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest); friend void SetToZero(JntArrayAcc& array); friend bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps); }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/jntarrayvel.cpp000066400000000000000000000066611326704774600216130ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "jntarrayacc.hpp" namespace KDL { JntArrayVel::JntArrayVel(unsigned int size): q(size),qdot(size) { } JntArrayVel::JntArrayVel(const JntArray& qin, const JntArray& qdotin): q(qin),qdot(qdotin) { assert(q.rows()==qdot.rows()); } JntArrayVel::JntArrayVel(const JntArray& qin): q(qin),qdot(q.rows()) { } void JntArrayVel::resize(unsigned int newSize) { q.resize(newSize); qdot.resize(newSize); } JntArray JntArrayVel::value()const { return q; } JntArray JntArrayVel::deriv()const { return qdot; } void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest) { Add(src1.q,src2.q,dest.q); Add(src1.qdot,src2.qdot,dest.qdot); } void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest) { Add(src1.q,src2,dest.q); dest.qdot=src1.qdot; } void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest) { Subtract(src1.q,src2.q,dest.q); Subtract(src1.qdot,src2.qdot,dest.qdot); } void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest) { Subtract(src1.q,src2,dest.q); dest.qdot=src1.qdot; } void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest) { Multiply(src.q,factor,dest.q); Multiply(src.qdot,factor,dest.qdot); } void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest) { Multiply(src.q,factor.grad,dest.q); Multiply(src.qdot,factor.t,dest.qdot); Add(dest.qdot,dest.q,dest.qdot); Multiply(src.q,factor.t,dest.q); } void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest) { Divide(src.q,factor,dest.q); Divide(src.qdot,factor,dest.qdot); } void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest) { Multiply(src.q,(factor.grad/factor.t/factor.t),dest.q); Divide(src.qdot,factor.t,dest.qdot); Subtract(dest.qdot,dest.q,dest.qdot); Divide(src.q,factor.t,dest.q); } void SetToZero(JntArrayVel& array) { SetToZero(array.q); SetToZero(array.qdot); } bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps) { return Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps); } } orocos-kdl-1.4.0/orocos_kdl/src/jntarrayvel.hpp000066400000000000000000000064321326704774600216140ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_JNTARRAYVEL_HPP #define KDL_JNTARRAYVEL_HPP #include "utilities/utility.h" #include "jntarray.hpp" #include "framevel.hpp" namespace KDL { // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) class JntArrayVel; bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps=epsilon); void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest); void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest); void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); void SetToZero(JntArrayVel& array); class JntArrayVel { public: JntArray q; JntArray qdot; public: JntArrayVel(){}; explicit JntArrayVel(unsigned int size); JntArrayVel(const JntArray& q,const JntArray& qdot); explicit JntArrayVel(const JntArray& q); void resize(unsigned int newSize); JntArray value()const; JntArray deriv()const; friend void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); friend void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); friend void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); friend void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); friend void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest); friend void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); friend void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest); friend void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); friend void SetToZero(JntArrayVel& array); friend bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps); }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/jntspaceinertiamatrix.cpp000066400000000000000000000067611326704774600236630ustar00rootroot00000000000000// Copyright (C) 2009 Dominick Vanthienen // Version: 1.0 // Author: Dominick Vanthienen // Maintainer: Dominick Vanthienen // 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 "jntspaceinertiamatrix.hpp" namespace KDL { using namespace Eigen; JntSpaceInertiaMatrix::JntSpaceInertiaMatrix() { } JntSpaceInertiaMatrix::JntSpaceInertiaMatrix(int _size): data(_size, _size) { data.setZero(); } JntSpaceInertiaMatrix::JntSpaceInertiaMatrix(const JntSpaceInertiaMatrix& arg): data(arg.data) { } JntSpaceInertiaMatrix& JntSpaceInertiaMatrix::operator = (const JntSpaceInertiaMatrix& arg) { data=arg.data; return *this; } JntSpaceInertiaMatrix::~JntSpaceInertiaMatrix() { } void JntSpaceInertiaMatrix::resize(unsigned int newSize) { data.resize(newSize,newSize); } double JntSpaceInertiaMatrix::operator()(unsigned int i,unsigned int j)const { return data(i, j); } double& JntSpaceInertiaMatrix::operator()(unsigned int i,unsigned int j) { return data(i, j); } unsigned int JntSpaceInertiaMatrix::rows()const { return data.rows(); } unsigned int JntSpaceInertiaMatrix::columns()const { return data.cols(); } void Add(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest) { dest.data=src1.data+src2.data; } void Subtract(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest) { dest.data=src1.data-src2.data; } void Multiply(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest) { dest.data=factor*src.data; } void Divide(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest) { dest.data=src.data/factor; } void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest) { dest.data=src.data.lazyProduct(vec.data); } void SetToZero(JntSpaceInertiaMatrix& mat) { mat.data.setZero(); } bool Equal(const JntSpaceInertiaMatrix& src1, const JntSpaceInertiaMatrix& src2,double eps) { if(src1.rows()!=src2.rows()||src1.columns()!=src2.columns()) return false; return src1.data.isApprox(src2.data,eps); } bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}; //bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}; } orocos-kdl-1.4.0/orocos_kdl/src/jntspaceinertiamatrix.hpp000066400000000000000000000203401326704774600236550ustar00rootroot00000000000000// Copyright (C) 2009 Dominick Vanthienen // Version: 1.0 // Author: Dominick Vanthienen // Maintainer: Dominick Vanthienen // 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 KDL_JNTSPACEINERTIAMATRIX_HPP #define KDL_JNTSPACEINERTIAMATRIX_HPP #include "frames.hpp" #include "jacobian.hpp" #include "jntarray.hpp" #include namespace KDL { /** * @brief This class represents an fixed size matrix containing * the Joint-Space Inertia Matrix of a KDL::Chain. * * \warning An object constructed with the default constructor provides * a valid, but inert, object. Many of the member functions will do * the correct thing and have no affect on this object, but some * member functions can _NOT_ deal with an inert/empty object. These * functions will assert() and exit the program instead. The intended use * case for the default constructor (in an RTT/OCL setting) is outlined in * code below - the default constructor plus the resize() function allow * use of JntSpaceInertiaMatrix objects whose size is set within a configureHook() call * (typically based on a size determined from a property). \code class MyTask : public RTT::TaskContext { JntSpaceInertiaMatrix j; MyTask() {} // invokes j's default constructor bool configureHook() { unsigned int size = some_property.rvalue(); j.resize(size) ... } void updateHook() { ** use j here } }; /endcode */ class JntSpaceInertiaMatrix { public: Eigen::MatrixXd data; /** Construct with _no_ data array * @post NULL == data * @post 0 == rows() * @warning use of an object constructed like this, without * a resize() first, may result in program exit! See class * documentation. */ JntSpaceInertiaMatrix(); /** * Constructor of the Joint-Space Inertia Matrix * * @param size of the matrix, this cannot be changed * afterwards. Size rows and size columns. * @pre 0 < size * @post NULL != data * @post 0 < rows() * @post all elements in data have 0 value */ explicit JntSpaceInertiaMatrix(int size); /** Copy constructor * @note Will correctly copy an empty object */ JntSpaceInertiaMatrix(const JntSpaceInertiaMatrix& arg); ~JntSpaceInertiaMatrix(); /** Resize the array * @warning This causes a dynamic allocation (and potentially * also a dynamic deallocation). This _will_ negatively affect * real-time performance! * * @post newSize == rows() * @post NULL != data * @post all elements in data have 0 value */ void resize(unsigned int newSize); JntSpaceInertiaMatrix& operator = ( const JntSpaceInertiaMatrix& arg); /** * get_item operator for the joint matrix * * * @return the joint value at position i, starting from 0 * @pre 0 != size (ie non-default constructor or resize() called) */ double operator()(unsigned int i,unsigned int j)const; /** * set_item operator * * @return reference to the joint value at position i,starting *from zero. * @pre 0 != size (ie non-default constructor or resize() called) */ double& operator()(unsigned int i,unsigned int j); /** * Returns the number of rows and columns of the matrix * */ unsigned int rows()const; /** * Returns the number of columns of the matrix. */ unsigned int columns()const; friend void Add(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest); friend void Subtract(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest); friend void Multiply(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest); friend void Divide(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest); friend void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest); friend void SetToZero(JntSpaceInertiaMatrix& matrix); friend bool Equal(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,double eps); friend bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); //friend bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); }; bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); //bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); /** * Function to add two joint matrix, all the arguments must * have the same size: A + B = C. This function is * aliasing-safe, A or B can be the same array as C. * * @param src1 A * @param src2 B * @param dest C */ void Add(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest); /** * Function to subtract two joint matrix, all the arguments must * have the same size: A - B = C. This function is * aliasing-safe, A or B can be the same array as C. * * @param src1 A * @param src2 B * @param dest C */ void Subtract(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest); /** * Function to multiply all the array values with a scalar * factor: A*b=C. This function is aliasing-safe, A can be the * same array as C. * * @param src A * @param factor b * @param dest C */ void Multiply(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest); /** * Function to divide all the array values with a scalar * factor: A/b=C. This function is aliasing-safe, A can be the * same array as C. * * @param src A * @param factor b * @param dest C */ void Divide(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest); /** * Function to multiply a KDL::Jacobian with a KDL::JntSpaceInertiaMatrix * to get a KDL::Twist, it should not be used to calculate the * forward velocity kinematics, the solver classes are built * for this purpose. * J*q = t * * @param jac J * @param src q * @param dest t * @post dest==Twist::Zero() if 0==src.rows() (ie src is empty) */ void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest); /** * Function to set all the values of the array to 0 * * @param array */ void SetToZero(JntSpaceInertiaMatrix& matrix); /** * Function to check if two matrices are the same with a *precision of eps * * @param src1 * @param src2 * @param eps default: epsilon * @return true if each element of src1 is within eps of the same * element in src2, or if both src1 and src2 have no data (ie 0==rows()) */ bool Equal(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,double eps=epsilon); bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); } #endif orocos-kdl-1.4.0/orocos_kdl/src/joint.cpp000066400000000000000000000135611326704774600203720ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "joint.hpp" namespace KDL { // constructor for joint along x,y or z axis, at origin of reference frame Joint::Joint(const std::string& _name, const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): name(_name),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) { if (type == RotAxis || type == TransAxis) throw joint_type_ex; q_previous = 0; } // constructor for joint along x,y or z axis, at origin of reference frame Joint::Joint(const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): name("NoName"),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) { if (type == RotAxis || type == TransAxis) throw joint_type_ex; q_previous = 0; } // constructor for joint along arbitrary axis, at arbitrary origin Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) , axis(_axis / _axis.Norm()), origin(_origin) { if (type != RotAxis && type != TransAxis) throw joint_type_ex; // initialize joint_pose.p = origin; joint_pose.M = Rotation::Rot2(axis, offset); q_previous = 0; } // constructor for joint along arbitrary axis, at arbitrary origin Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness), axis(_axis / _axis.Norm()),origin(_origin) { if (type != RotAxis && type != TransAxis) throw joint_type_ex; // initialize joint_pose.p = origin; joint_pose.M = Rotation::Rot2(axis, offset); q_previous = 0; } Joint::~Joint() { } Frame Joint::pose(const double& q)const { switch(type){ case RotAxis: // calculate the rotation matrix around the vector "axis" if (q != q_previous){ q_previous = q; joint_pose.M = Rotation::Rot2(axis, scale*q+offset); } return joint_pose; case RotX: return Frame(Rotation::RotX(scale*q+offset)); case RotY: return Frame(Rotation::RotY(scale*q+offset)); case RotZ: return Frame(Rotation::RotZ(scale*q+offset)); case TransAxis: return Frame(origin + (axis * (scale*q+offset))); case TransX: return Frame(Vector(scale*q+offset,0.0,0.0)); case TransY: return Frame(Vector(0.0,scale*q+offset,0.0)); case TransZ: return Frame(Vector(0.0,0.0,scale*q+offset)); case None: return Frame::Identity(); } return Frame::Identity(); } Twist Joint::twist(const double& qdot)const { switch(type){ case RotAxis: return Twist(Vector(0,0,0), axis * ( scale * qdot)); case RotX: return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0)); case RotY: return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0)); case RotZ: return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot)); case TransAxis: return Twist(axis * (scale * qdot), Vector(0,0,0)); case TransX: return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0)); case TransY: return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0)); case TransZ: return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0)); case None: return Twist::Zero(); } return Twist::Zero(); } Vector Joint::JointAxis() const { switch(type) { case RotAxis: return axis; case RotX: return Vector(1.,0.,0.); case RotY: return Vector(0.,1.,0.); case RotZ: return Vector(0.,0.,1.); case TransAxis: return axis; case TransX: return Vector(1.,0.,0.); case TransY: return Vector(0.,1.,0.); case TransZ: return Vector(0.,0.,1.); case None: return Vector::Zero(); } return Vector::Zero(); } Vector Joint::JointOrigin() const { return origin; } } // end of namespace KDL orocos-kdl-1.4.0/orocos_kdl/src/joint.hpp000066400000000000000000000177401326704774600204020ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_JOINT_HPP #define KDL_JOINT_HPP #include "frames.hpp" #include #include namespace KDL { /** * \brief This class encapsulates a simple joint, that is with one * parameterized degree of freedom and with scalar dynamic properties. * * A simple joint is described by the following properties : * - scale: ratio between motion input and motion output * - offset: between the "physical" and the "logical" zero position. * - type: revolute or translational, along one of the basic frame axes * - inertia, stiffness and damping: scalars representing the physical * effects along/about the joint axis only. * * @ingroup KinematicFamily */ class Joint { public: typedef enum { RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,None} JointType; /** * Constructor of a joint. * * @param name of the joint * @param type type of the joint, default: Joint::None * @param scale scale between joint input and actual geometric * movement, default: 1 * @param offset offset between joint input and actual * geometric input, default: 0 * @param inertia 1D inertia along the joint axis, default: 0 * @param damping 1D damping along the joint axis, default: 0 * @param stiffness 1D stiffness along the joint axis, * default: 0 */ explicit Joint(const std::string& name, const JointType& type=None,const double& scale=1,const double& offset=0, const double& inertia=0,const double& damping=0,const double& stiffness=0); /** * Constructor of a joint. * * @param type type of the joint, default: Joint::None * @param scale scale between joint input and actual geometric * movement, default: 1 * @param offset offset between joint input and actual * geometric input, default: 0 * @param inertia 1D inertia along the joint axis, default: 0 * @param damping 1D damping along the joint axis, default: 0 * @param stiffness 1D stiffness along the joint axis, * default: 0 */ explicit Joint(const JointType& type=None,const double& scale=1,const double& offset=0, const double& inertia=0,const double& damping=0,const double& stiffness=0); /** * Constructor of a joint. * * @param name of the joint * @param origin the origin of the joint * @param axis the axis of the joint * @param scale scale between joint input and actual geometric * movement, default: 1 * @param offset offset between joint input and actual * geometric input, default: 0 * @param inertia 1D inertia along the joint axis, default: 0 * @param damping 1D damping along the joint axis, default: 0 * @param stiffness 1D stiffness along the joint axis, * default: 0 */ Joint(const std::string& name, const Vector& _origin, const Vector& _axis, const JointType& type, const double& _scale=1, const double& _offset=0, const double& _inertia=0, const double& _damping=0, const double& _stiffness=0); /** * Constructor of a joint. * * @param origin the origin of the joint * @param axis the axis of the joint * @param scale scale between joint input and actual geometric * movement, default: 1 * @param offset offset between joint input and actual * geometric input, default: 0 * @param inertia 1D inertia along the joint axis, default: 0 * @param damping 1D damping along the joint axis, default: 0 * @param stiffness 1D stiffness along the joint axis, * default: 0 */ Joint(const Vector& _origin, const Vector& _axis, const JointType& type, const double& _scale=1, const double& _offset=0, const double& _inertia=0, const double& _damping=0, const double& _stiffness=0); /** * Request the 6D-pose between the beginning and the end of * the joint at joint position q * * @param q the 1D joint position * * @return the resulting 6D-pose */ Frame pose(const double& q)const; /** * Request the resulting 6D-velocity with a joint velocity qdot * * @param qdot the 1D joint velocity * * @return the resulting 6D-velocity */ Twist twist(const double& qdot)const; /** * Request the Vector corresponding to the axis of a revolute joint. * * @return Vector. e.g (1,0,0) for RotX etc. */ Vector JointAxis() const; /** * Request the Vector corresponding to the origin of a revolute joint. * * @return Vector */ Vector JointOrigin() const; /** * Request the name of the joint * * * @return const reference to the name of the joint */ const std::string& getName()const { return name; } /** * Request the type of the joint. * * @return const reference to the type */ const JointType& getType() const { return type; }; /** * Request the stringified type of the joint. * * @return const string */ const std::string getTypeName() const { switch (type) { case RotAxis: return "RotAxis"; case TransAxis: return "TransAxis"; case RotX: return "RotX"; case RotY: return "RotY"; case RotZ: return "RotZ"; case TransX: return "TransX"; case TransY: return "TransY"; case TransZ: return "TransZ"; case None: return "None"; default: return "None"; } }; virtual ~Joint(); private: std::string name; Joint::JointType type; double scale; double offset; double inertia; double damping; double stiffness; // variables for RotAxis joint Vector axis, origin; mutable Frame joint_pose; mutable double q_previous; class joint_type_exception: public std::exception{ virtual const char* what() const throw(){ return "Joint Type excption";} } joint_type_ex; }; } // end of namespace KDL #endif orocos-kdl-1.4.0/orocos_kdl/src/kdl.hpp000066400000000000000000000120051326704774600200160ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Copyright (C) 2011 Erwin Aertbelien // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 /** * \mainpage KDL * * This is the API reference of the * Kinematics and Dynamics * Library (KDL), a sub-project of Orocos, but that can be used * independently of Orocos. KDL offers different kinds of * functionality, grouped in the following Modules: * - \subpage geomprim * - \ref KinematicFamily : functionality to build kinematic chains and access their kinematic and dynamic properties, such as e.g. Forward and Inverse kinematics and dynamics. * - \ref Motion : functionality to specify motion trajectories of frames and kinematic chains, such as e.g. Trapezoidal Velocity profiles. * - \ref KDLTK : the interface code to integrate KDL into the Orocos Real-Time Toolkit (RTT). * * **/ /** * \page geomprim Geometric Primitives * \section Introduction * Geometric primitives are represented by the following classes. * - KDL::Vector * - KDL::Rotation * - KDL::Frame * - KDL::Twist * - KDL::Wrench * * \par Twist and Wrench transformations * 3 different types of transformations do exist for the twists * and wrenches. * * \verbatim * 1) Frame * Twist or Frame * Wrench : * this transforms both the velocity/force reference point * and the basis to which the twist/wrench are expressed. * 2) Rotation * Twist or Rotation * Wrench : * this transforms the basis to which the twist/wrench are * expressed, but leaves the reference point intact. * 3) Twist.RefPoint(v_base_AB) or Wrench.RefPoint(v_base_AB) * this transforms only the reference point. v is expressed * in the same base as the twist/wrench and points from the * old reference point to the new reference point. * \endverbatim * *\warning * Efficienty can be improved by writing p2 = A*(B*(C*p1))) instead of * p2=A*B*C*p1 * * \par PROPOSED NAMING CONVENTION FOR FRAME-like OBJECTS * * \verbatim * A naming convention of objects of the type defined in this file : * (1) Frame : F... * Rotation : R ... * (2) Twist : T ... * Wrench : W ... * Vector : V ... * This prefix is followed by : * for category (1) : * F_A_B : w.r.t. frame A, frame B expressed * ( each column of F_A_B corresponds to an axis of B, * expressed w.r.t. frame A ) * in mathematical convention : * A * F_A_B == F * B * * for category (2) : * V_B : a vector expressed w.r.t. frame B * * This can also be prepended by a name : * e.g. : temporaryV_B * * With this convention one can write : * * F_A_B = F_B_A.Inverse(); * F_A_C = F_A_B * F_B_C; * V_B = F_B_C * V_C; // both translation and rotation * V_B = R_B_C * V_C; // only rotation * \endverbatim * * \par CONVENTIONS FOR WHEN USED WITH ROBOTS : * * \verbatim * world : represents the frame ([1 0 0,0 1 0,0 0 1],[0 0 0]') * mp : represents mounting plate of a robot * (i.e. everything before MP is constructed by robot manufacturer * everything after MP is tool ) * tf : represents task frame of a robot * (i.e. frame in which motion and force control is expressed) * sf : represents sensor frame of a robot * (i.e. frame at which the forces measured by the force sensor * are expressed ) * * Frame F_world_mp=...; * Frame F_mp_sf(..) * Frame F_mp_tf(,.) * * Wrench are measured in sensor frame SF, so one could write : * Wrench_tf = F_mp_tf.Inverse()* ( F_mp_sf * Wrench_sf ); * \endverbatim * * \par CONVENTIONS REGARDING UNITS : * Typically we use the standard S.I. units: N, m, sec. * */ orocos-kdl-1.4.0/orocos_kdl/src/kdl.pc.in000066400000000000000000000004021326704774600202340ustar00rootroot00000000000000prefix=@CMAKE_INSTALL_PREFIX@ libdir=${prefix}/lib includedir=${prefix}/include Name: orocos-kdl Description: The Orocos Kinematics and Dynamics Library Requires: Version: @KDL_VERSION@ Libs: -L${libdir} -lorocos-kdl Cflags: -I${includedir} @KDL_CFLAGS@ orocos-kdl-1.4.0/orocos_kdl/src/kinfam.hpp000066400000000000000000000046261326704774600205230ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 /** * @defgroup KinematicFamily Kinematic Families * @brief All classes to support kinematic families. * * The Kinematic Families classes range from the basic building blocks * (KDL::Joint and KDL::Segment) and their interconnected kinematic * structures (KDL::Chain, KDL::Tree and KDL::Graph), to the solver * algorithms for the kinematics and dynamics of particular kinematic * families. * * A kinematic family is a set of kinematic structures that have * similar properties, such as the same interconnection topology, the same * numerical or analytical solver algorithms, etc. Different members of the * same kinematic family differ only by the concrete values of their * kinematic and dynamic properties (link lengths, mass, etc.). * * Each kinematic structure is built from one or more Segments * (KDL::Segment). A KDL::Chain is a serial connection of * these segments; a KDL:Tree is a tree-structured * interconnection; and a KDL:Graph is a kinematic structure with a * general graph topology. (The current implementation * supports only KDL::Chain.) * * A KDL::Segment contains a KDL::Joint and an offset frame ("link length", * defined by a KDL::Frame), that represents the geometric pose * between the KDL::Joint on the previous segment and its own KDL::Joint. * * A list of all the classes is available on the modules page: \ref KinFam * * */ orocos-kdl-1.4.0/orocos_kdl/src/kinfam_io.cpp000066400000000000000000000070631326704774600212030ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "kinfam_io.hpp" #include "frames_io.hpp" namespace KDL { std::ostream& operator <<(std::ostream& os, const Joint& joint) { return os << joint.getName()<<":["<>(std::istream& is, Joint& joint) { return is; } std::ostream& operator <<(std::ostream& os, const Segment& segment) { os << segment.getName()<<":[" << segment.getJoint() << ",\n tip: \n" << segment.getFrameToTip() << "]"; return os; } std::istream& operator >>(std::istream& is, Segment& segment) { return is; } std::ostream& operator <<(std::ostream& os, const Chain& chain) { os << "["; for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) os << chain.getSegment(i) << "\n"; os << "]"; return os; } std::istream& operator >>(std::istream& is, Chain& chain) { return is; } std::ostream& operator <<(std::ostream& os, const Tree& tree) { SegmentMap::const_iterator root = tree.getRootSegment(); return os << root; } std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator root) { //os<first<<": "<second.segment<<"\n"; os << root->first<<"(q_nr: "<< GetTreeElementQNr(root->second) << ")" << "\n \t"; for (unsigned int i = 0; i < GetTreeElementChildren(root->second).size(); i++) { os << ( GetTreeElementChildren(root->second)[i] ) << "\t"; } return os << "\n"; } std::istream& operator >>(std::istream& is, Tree& tree) { return is; } std::ostream& operator <<(std::ostream& os, const JntArray& array) { os << "["; for (unsigned int i = 0; i < array.rows(); i++) os << std::setw(KDL_FRAME_WIDTH) << array(i); os << "]"; return os; } std::istream& operator >>(std::istream& is, JntArray& array) { return is; } std::ostream& operator <<(std::ostream& os, const Jacobian& jac) { os << "["; for (unsigned int i = 0; i < jac.rows(); i++) { for (unsigned int j = 0; j < jac.columns(); j++) os << std::setw(KDL_FRAME_WIDTH) << jac(i, j); os << std::endl; } os << "]"; return os; } std::istream& operator >>(std::istream& is, Jacobian& jac) { return is; } std::ostream& operator <<(std::ostream& os, const JntSpaceInertiaMatrix& jntspaceinertiamatrix) { os << "["; for (unsigned int i = 0; i < jntspaceinertiamatrix.rows(); i++) { for (unsigned int j = 0; j < jntspaceinertiamatrix.columns(); j++) os << std::setw(KDL_FRAME_WIDTH) << jntspaceinertiamatrix(i, j); os << std::endl; } os << "]"; return os; } std::istream& operator >>(std::istream& is, JntSpaceInertiaMatrix& jntspaceinertiamatrix) { return is; } } orocos-kdl-1.4.0/orocos_kdl/src/kinfam_io.hpp000066400000000000000000000051271326704774600212070ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_KINFAM_IO_HPP #define KDL_KINFAM_IO_HPP #include #include #include "joint.hpp" #include "segment.hpp" #include "chain.hpp" #include "jntarray.hpp" #include "jacobian.hpp" #include "tree.hpp" #include "jntspaceinertiamatrix.hpp" namespace KDL { std::ostream& operator <<(std::ostream& os, const Joint& joint); std::istream& operator >>(std::istream& is, Joint& joint); std::ostream& operator <<(std::ostream& os, const Segment& segment); std::istream& operator >>(std::istream& is, Segment& segment); std::ostream& operator <<(std::ostream& os, const Chain& chain); std::istream& operator >>(std::istream& is, Chain& chain); std::ostream& operator <<(std::ostream& os, const Tree& tree); std::istream& operator >>(std::istream& is, Tree& tree); std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator it); std::ostream& operator <<(std::ostream& os, const JntArray& array); std::istream& operator >>(std::istream& is, JntArray& array); std::ostream& operator <<(std::ostream& os, const Jacobian& jac); std::istream& operator >>(std::istream& is, Jacobian& jac); std::ostream& operator <<(std::ostream& os, const JntSpaceInertiaMatrix& jntspaceinertiamatrix); std::istream& operator >>(std::istream& is, JntSpaceInertiaMatrix& jntspaceinertiamatrix); /* template std::ostream& operator<<(std::ostream& os, const std::vector& vec) { os << "["; for (unsigned int i = 0; i < vec.size(); i++) os << vec[i] << " "; os << "]"; return os; } ; template std::istream& operator >>(std::istream& is, std::vector& vec) { return is; } ; */ } #endif orocos-kdl-1.4.0/orocos_kdl/src/motion.hpp000066400000000000000000000023071326704774600205550ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 /** * @defgroup Motion Motion * @brief All classes related to the non-instantaneous motion of rigid * bodies and kinematic structures, e.g., path and trajecory definitions * and their building blocks. * */ orocos-kdl-1.4.0/orocos_kdl/src/path.cpp000066400000000000000000000115511326704774600202000ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path.cxx path.cxx - description ------------------- begin : Mon May 10 2004 copyright : (C) 2004 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path.cpp,v 1.1.1.1.2.4 2003/07/18 14:49:50 psoetens Exp $ * $Name: $ ****************************************************************************/ #include "utilities/error.h" #include "utilities/error_stack.h" #include "path.hpp" #include "path_line.hpp" #include "path_point.hpp" #include "path_circle.hpp" #include "path_composite.hpp" #include "path_roundedcomposite.hpp" #include "path_cyclic_closed.hpp" #include #include namespace KDL { using namespace std; Path* Path::Read(istream& is) { // auto_ptr because exception can be thrown ! IOTrace("Path::Read"); char storage[64]; EatWord(is,"[",storage,sizeof(storage)); Eat(is,'['); if (strcmp(storage,"POINT")==0) { IOTrace("POINT"); Frame startpos; is >> startpos; EatEnd(is,']'); IOTracePop(); IOTracePop(); return new Path_Point(startpos); } else if (strcmp(storage,"LINE")==0) { IOTrace("LINE"); Frame startpos; Frame endpos; is >> startpos; is >> endpos; auto_ptr orient( RotationalInterpolation::Read(is) ); double eqradius; is >> eqradius; EatEnd(is,']'); IOTracePop(); IOTracePop(); return new Path_Line(startpos,endpos,orient.release(),eqradius); } else if (strcmp(storage,"CIRCLE")==0) { IOTrace("CIRCLE"); Frame F_base_start; Vector V_base_center; Vector V_base_p; Rotation R_base_end; double alpha; double eqradius; is >> F_base_start; is >> V_base_center; is >> V_base_p; is >> R_base_end; is >> alpha; alpha *= deg2rad; auto_ptr orient( RotationalInterpolation::Read(is) ); is >> eqradius; EatEnd(is,']'); IOTracePop(); IOTracePop(); return new Path_Circle( F_base_start, V_base_center, V_base_p, R_base_end, alpha, orient.release() , eqradius ); } else if (strcmp(storage,"ROUNDEDCOMPOSITE")==0) { IOTrace("ROUNDEDCOMPOSITE"); double radius; is >> radius; double eqradius; is >> eqradius; auto_ptr orient( RotationalInterpolation::Read(is) ); auto_ptr tr( new Path_RoundedComposite(radius,eqradius,orient.release()) ); int size; is >> size; int i; for (i=0;i> f; tr->Add(f); } tr->Finish(); EatEnd(is,']'); IOTracePop(); IOTracePop(); return tr.release(); } else if (strcmp(storage,"COMPOSITE")==0) { IOTrace("COMPOSITE"); int size; auto_ptr tr( new Path_Composite() ); is >> size; int i; for (i=0;iAdd(Path::Read(is)); } EatEnd(is,']'); IOTracePop(); IOTracePop(); return tr.release(); } else if (strcmp(storage,"CYCLIC_CLOSED")==0) { IOTrace("CYCLIC_CLOSED"); int times; auto_ptr tr( Path::Read(is) ); is >> times; EatEnd(is,']'); IOTracePop(); IOTracePop(); return new Path_Cyclic_Closed(tr.release(),times); } else { throw Error_MotionIO_Unexpected_Traj(); } return NULL; // just to avoid the warning; } } orocos-kdl-1.4.0/orocos_kdl/src/path.hpp000066400000000000000000000103411326704774600202010ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 path.h path.h - description ------------------- begin : Mon January 10 2005 copyright : (C) 2005 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $ * $Name: $ ****************************************************************************/ #ifndef KDL_MOTION_PATH_H #define KDL_MOTION_PATH_H #include "frames.hpp" #include #include "frames_io.hpp" namespace KDL { /** * The specification of the path of a trajectory. */ class Path { public: enum IdentifierType { ID_LINE=1, ID_CIRCLE=2, ID_COMPOSITE=3, ID_ROUNDED_COMPOSITE=4, ID_POINT=5, ID_CYCLIC_CLOSED=6 }; /** * LengthToS() converts a physical length along the trajectory * to the parameter s used in Pos, Vel and Acc. This is used because * in cases with large rotations the parameter s does NOT correspond to * the lineair length along the trajectory. * User should be sure that the lineair distance travelled by this * path object is NOT zero, when using this method ! * (e.g. the case of only rotational change) * throws Error_MotionPlanning_Not_Applicable if used on composed * path objects. * @ingroup Motion */ virtual double LengthToS(double length) = 0; /** * Returns the total path length of the trajectory * (has dimension LENGTH) * This is not always a physical length , ie when dealing with rotations * that are dominant. */ virtual double PathLength() = 0; /** * Returns the Frame at the current path length s */ virtual Frame Pos(double s) const = 0; /** * Returns the velocity twist at path length s theta and with * derivative of s == sd */ virtual Twist Vel(double s,double sd) const = 0; /** * Returns the acceleration twist at path length s and with * derivative of s == sd, and 2nd derivative of s == sdd */ virtual Twist Acc(double s,double sd,double sdd) const = 0; /** * Writes one of the derived objects to the stream */ virtual void Write(std::ostream& os) = 0; /** * Reads one of the derived objects from the stream and returns a pointer * (factory method) */ static Path* Read(std::istream& is); /** * Virtual constructor, constructing by copying, * Returns a deep copy of this Path Object */ virtual Path* Clone() = 0; /** * gets an identifier indicating the type of this Path object */ virtual IdentifierType getIdentifier() const=0; virtual ~Path() {} }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/path_circle.cpp000066400000000000000000000123641326704774600215240ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path_circle.cxx path_circle.cxx - description ------------------- begin : Mon May 10 2004 copyright : (C) 2004 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path_circle.cpp,v 1.1.1.1.2.5 2003/07/24 13:26:15 psoetens Exp $ * $Name: $ ****************************************************************************/ #include "path_circle.hpp" #include "utilities/error.h" namespace KDL { Path_Circle::Path_Circle(const Frame& F_base_start, const Vector& _V_base_center, const Vector& V_base_p, const Rotation& R_base_end, double alpha, RotationalInterpolation* _orient, double _eqradius, bool _aggregate) : orient(_orient) , eqradius(_eqradius), aggregate(_aggregate) { F_base_center.p = _V_base_center; orient->SetStartEnd(F_base_start.M,R_base_end); double oalpha = orient->Angle(); Vector x(F_base_start.p - F_base_center.p); radius = x.Normalize(); if (radius < epsilon) throw Error_MotionPlanning_Circle_ToSmall(); Vector tmpv(V_base_p-F_base_center.p); tmpv.Normalize(); Vector z( x * tmpv); double n = z.Normalize(); if (n < epsilon) throw Error_MotionPlanning_Circle_No_Plane(); F_base_center.M = Rotation(x,z*x,z); double dist = alpha*radius; // See what has the slowest eq. motion, and adapt // the other to this slower motion // use eqradius to transform between rot and transl. // the same as for lineair motion if (oalpha*eqradius > dist) { // rotational_interpolation is the limitation pathlength = oalpha*eqradius; scalerot = 1/eqradius; scalelin = dist/pathlength; } else { // translation is the limitation pathlength = dist; scalerot = oalpha/pathlength; scalelin = 1; } } double Path_Circle::LengthToS(double length) { return length/scalelin; } double Path_Circle::PathLength() { return pathlength; } Frame Path_Circle::Pos(double s) const { double p = s*scalelin / radius; return Frame(orient->Pos(s*scalerot), F_base_center*Vector(radius*cos(p),radius*sin(p),0) ); } Twist Path_Circle::Vel(double s,double sd) const { double p = s*scalelin / radius; double v = sd*scalelin / radius; return Twist( F_base_center.M*Vector(-radius*sin(p)*v,radius*cos(p)*v,0), orient->Vel(s*scalerot,sd*scalerot) ); } Twist Path_Circle::Acc(double s,double sd,double sdd) const { double p = s*scalelin / radius; double cp = cos(p); double sp = sin(p); double v = sd*scalelin / radius; double a = sdd*scalelin / radius; return Twist( F_base_center.M*Vector( -radius*cp*v*v - radius*sp*a, -radius*sp*v*v + radius*cp*a, 0 ), orient->Acc(s*scalerot,sd*scalerot,sdd*scalerot) ); } Path* Path_Circle::Clone() { return new Path_Circle( Pos(0), F_base_center.p, F_base_center.M.UnitY(), orient->Pos(pathlength*scalerot), pathlength*scalelin/radius/deg2rad, orient->Clone(), eqradius, aggregate ); } Path_Circle::~Path_Circle() { if (aggregate) delete orient; } void Path_Circle::Write(std::ostream& os) { os << "CIRCLE[ "; os << " " << Pos(0) << std::endl; os << " " << F_base_center.p << std::endl; os << " " << F_base_center.M.UnitY() << std::endl; os << " " << orient->Pos(pathlength*scalerot) << std::endl; os << " " << pathlength*scalelin/radius/deg2rad << std::endl; os << " ";orient->Write(os); os << " " << eqradius; os << "]"<< std::endl; } } orocos-kdl-1.4.0/orocos_kdl/src/path_circle.hpp000066400000000000000000000070051326704774600215250ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 path_circle.h path_circle.h - description ------------------- begin : Mon January 10 2005 copyright : (C) 2005 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * ALTERNATIVE FOR trajectory_circle.h/cpp * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path_circle.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $ * $Name: $ ****************************************************************************/ #ifndef KDL_MOTION_PATHCIRCLE_H #define KDL_MOTION_PATHCIRCLE_H #include "path.hpp" #include "rotational_interpolation.hpp" namespace KDL { /** * A circular Path with 'open ends'. Path_Arc would * have been a better name though. * @ingroup Motion */ class Path_Circle : public Path { // Orientatie gedeelte RotationalInterpolation* orient; // Circular gedeelte double radius; Frame F_base_center; // equivalent radius double eqradius; // verdeling baanlengte over pos/rot double pathlength; double scalelin; double scalerot; bool aggregate; public: /** * * CAN THROW Error_MotionPlanning_Circle_ToSmall * CAN THROW Error_MotionPlanning_Circle_No_Plane */ Path_Circle(const Frame& F_base_start,const Vector& V_base_center, const Vector& V_base_p, const Rotation& R_base_end, double alpha, RotationalInterpolation* otraj, double eqradius, bool _aggregate=true); double LengthToS(double length); virtual double PathLength(); virtual Frame Pos(double s) const; virtual Twist Vel(double s,double sd) const; virtual Twist Acc(double s,double sd,double sdd) const; virtual Path* Clone(); virtual void Write(std::ostream& os); /** * gets an identifier indicating the type of this Path object */ virtual IdentifierType getIdentifier() const { return ID_CIRCLE; } virtual ~Path_Circle(); }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/path_composite.cpp000066400000000000000000000112761326704774600222660ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path_composite.cxx path_composite.cxx - description ------------------- begin : Mon May 10 2004 copyright : (C) 2004 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path_composite.cpp,v 1.1.1.1.2.7 2003/07/24 13:49:16 rwaarsin Exp $ * $Name: $ ****************************************************************************/ #include "path_composite.hpp" #include "utilities/error.h" #include namespace KDL { // s should be in allowable limits, this is not checked // simple linear search : TODO : make it binary search // uses cached_... variables // returns the relative path length within the segment // you propably want to use the cached_index variable double Path_Composite::Lookup(double s) const { assert(s>=-1e-12); assert(s<=pathlength+1e-12); if ( (cached_starts <=s) && ( s <= cached_ends) ) { return s - cached_starts; } double previous_s=0; for (unsigned int i=0;iPathLength(); dv.insert(dv.end(),pathlength); gv.insert( gv.end(),std::make_pair(geom,aggregate) ); } double Path_Composite::LengthToS(double length) { throw Error_MotionPlanning_Not_Applicable(); return 0; } double Path_Composite::PathLength() { return pathlength; } Frame Path_Composite::Pos(double s) const { s = Lookup(s); return gv[cached_index].first->Pos(s); } Twist Path_Composite::Vel(double s,double sd) const { s = Lookup(s); return gv[cached_index].first->Vel(s,sd); } Twist Path_Composite::Acc(double s,double sd,double sdd) const { s = Lookup(s); return gv[cached_index].first->Acc(s,sd,sdd); } Path* Path_Composite::Clone() { std::auto_ptr comp( new Path_Composite() ); for (unsigned int i = 0; i < dv.size(); ++i) { comp->Add(gv[i].first->Clone(), gv[i].second); } return comp.release(); } void Path_Composite::Write(std::ostream& os) { os << "COMPOSITE[ " << std::endl; os << " " << dv.size() << std::endl; for (unsigned int i=0;iWrite(os); } os << "]" << std::endl; } int Path_Composite::GetNrOfSegments() { return dv.size(); } Path* Path_Composite::GetSegment(int i) { assert(i>=0); assert(i=0); assert(isecond) delete it->first; } } } // namespace KDL orocos-kdl-1.4.0/orocos_kdl/src/path_composite.hpp000066400000000000000000000126421326704774600222710ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 path_composite.h path_composite.h - description ------------------- begin : Mon January 10 2005 copyright : (C) 2005 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path_composite.h,v 1.1.1.1.2.5 2003/07/24 13:49:16 rwaarsin Exp $ * $Name: $ ****************************************************************************/ #ifndef KDL_PATHCOMPOSITE_H #define KDL_PATHCOMPOSITE_H #include "frames.hpp" #include "frames_io.hpp" #include "path.hpp" #include namespace KDL { /** * A Path being the composition of other Path objects. * * For several of its methods, this class needs to lookup the segment corresponding to a value * of the path variable s. To increase efficiency, this value is cached. * * \TODO Currently a linear search is used to look up the segment. A binary search is more efficient. Can STL be used for this ? * \TODO Increase the efficiency for caching for the common case of a fine grained monotonously increasing path variable s. * * \TODO For all Path.., VelocityProfile.., Trajectory... check the bounds on the inputs with asserts. * * \TODO explain this routine in the wiki. * * @ingroup Motion */ class Path_Composite : public Path { typedef std::vector< std::pair > PathVector; typedef std::vector DoubleVector; PathVector gv; DoubleVector dv; double pathlength; // lookup mechanism : mutable double cached_starts; mutable double cached_ends; mutable int cached_index; double Lookup(double s) const; public: Path_Composite(); /** * Adds a Path* to this composite */ void Add(Path* geom, bool aggregate=true); virtual double LengthToS(double length); /** * Returns the total path length of the trajectory * (has dimension LENGTH) * This is not always a physical length , ie when dealing with rotations * that are dominant. */ virtual double PathLength(); /** * Returns the Frame at the current path length s */ virtual Frame Pos(double s) const; /** * Returns the velocity twist at path length s theta and with * derivative of s == sd */ virtual Twist Vel(double s,double sd) const; /** * Returns the acceleration twist at path length s and with * derivative of s == sd, and 2nd derivative of s == sdd */ virtual Twist Acc(double s,double sd,double sdd) const; virtual Path* Clone(); /** * Writes one of the derived objects to the stream */ virtual void Write(std::ostream& os); /** * returns the number of underlying segments. */ virtual int GetNrOfSegments(); /** * returns a pointer to the underlying Path of the given segment number i. * \param i segment number * \return pointer to the underlying Path * \warning The pointer is still owned by this class and is lifetime depends on the lifetime * of this class. */ virtual Path* GetSegment(int i); /** * gets the length to the end of the given segment. * \param i segment number * \return length to the end of the segment, i.e. the value for s corresponding to the end of * this segment. */ virtual double GetLengthToEndOfSegment(int i); /** * \param s [INPUT] path length variable for the composite. * \param segment_number [OUTPUT] segments that corresponds to the path length variable s. * \param inner_s [OUTPUT] path length to use within the segment. */ virtual void GetCurrentSegmentLocation(double s, int &segment_number, double& inner_s); /** * gets an identifier indicating the type of this Path object */ virtual IdentifierType getIdentifier() const { return ID_COMPOSITE; } virtual ~Path_Composite(); }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/path_cyclic_closed.cpp000066400000000000000000000063301326704774600230560ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path_cyclic_closed.cxx path_cyclic_closed.cxx - description ------------------- begin : Mon May 10 2004 copyright : (C) 2004 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path_cyclic_closed.cpp,v 1.1.1.1.2.5 2003/07/24 13:26:15 psoetens Exp $ * $Name: $ ****************************************************************************/ #include "path_cyclic_closed.hpp" #include "utilities/error.h" namespace KDL { Path_Cyclic_Closed::Path_Cyclic_Closed(Path* _geom,int _times, bool _aggregate): times(_times),geom(_geom), aggregate(_aggregate) {} double Path_Cyclic_Closed::LengthToS(double length) { throw Error_MotionPlanning_Not_Applicable(); return 0; } double Path_Cyclic_Closed::PathLength(){ return geom->PathLength()*times; } Frame Path_Cyclic_Closed::Pos(double s) const { return geom->Pos( fmod(s,geom->PathLength()) ); } Twist Path_Cyclic_Closed::Vel(double s,double sd) const { return geom->Vel( fmod(s,geom->PathLength()),sd ); } Twist Path_Cyclic_Closed::Acc(double s,double sd,double sdd) const { return geom->Acc( fmod(s,geom->PathLength()),sd,sdd ); } Path_Cyclic_Closed::~Path_Cyclic_Closed() { if (aggregate) delete geom; } Path* Path_Cyclic_Closed::Clone() { return new Path_Cyclic_Closed(geom->Clone(),times, aggregate); } void Path_Cyclic_Closed::Write(std::ostream& os) { os << "CYCLIC_CLOSED[ "; os << " ";geom->Write(os);os << std::endl; os << " " << times << std::endl; os << "]" << std::endl; } } orocos-kdl-1.4.0/orocos_kdl/src/path_cyclic_closed.hpp000066400000000000000000000061321326704774600230630ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 path_cyclic_closed.h path_cyclic_closed.h - description ------------------- begin : Mon January 10 2005 copyright : (C) 2005 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V2 * * \par History * - $log$ * * \par Release * $Id: path_cyclic_closed.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $ * $Name: $ ****************************************************************************/ #ifndef KDL_MOTION_PATH_CYCLIC_CLOSED_H #define KDL_MOTION_PATH_CYCLIC_CLOSED_H #include "frames.hpp" #include "frames_io.hpp" #include "path.hpp" #include namespace KDL { /** * A Path representing a closed circular movement, * which is traversed a number of times. * @ingroup Motion */ class Path_Cyclic_Closed : public Path { int times; Path* geom; bool aggregate; public: Path_Cyclic_Closed(Path* _geom,int _times, bool _aggregate=true); virtual double LengthToS(double length); virtual double PathLength(); virtual Frame Pos(double s) const; virtual Twist Vel(double s,double sd) const; virtual Twist Acc(double s,double sd,double sdd) const; virtual void Write(std::ostream& os); static Path* Read(std::istream& is); virtual Path* Clone(); /** * gets an identifier indicating the type of this Path object */ virtual IdentifierType getIdentifier() const { return ID_CYCLIC_CLOSED; } virtual ~Path_Cyclic_Closed(); }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/path_line.cpp000066400000000000000000000146651326704774600212200ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path_line.cxx path_line.cxx - description ------------------- begin : Mon May 10 2004 copyright : (C) 2004 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path_line.cpp,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $ * $Name: $ ****************************************************************************/ #include "path_line.hpp" namespace KDL { Path_Line::Path_Line(const Frame& startpos, const Frame& endpos, RotationalInterpolation* _orient, double _eqradius, bool _aggregate ): orient(_orient), V_base_start(startpos.p), V_base_end(endpos.p), eqradius(_eqradius), aggregate(_aggregate) { V_start_end = V_base_end - V_base_start; double dist = V_start_end.Normalize(); orient->SetStartEnd(startpos.M,endpos.M); double alpha = orient->Angle(); // See what has the slowest eq. motion, and adapt // the other to this slower motion // use eqradius to transform between rot and transl. // Only modify if non zero (prevent division by zero) if ( alpha != 0 && alpha*eqradius > dist) { // rotational_interpolation is the limitation pathlength = alpha*eqradius; scalerot = 1/eqradius; scalelin = dist/pathlength; } else if ( dist != 0 ) { // translation is the limitation pathlength = dist; scalerot = alpha/pathlength; scalelin = 1; } else { // both were zero pathlength = 0; scalerot = 1; scalelin = 1; } } Path_Line::Path_Line(const Frame& startpos, const Twist& starttwist, RotationalInterpolation* _orient, double _eqradius, bool _aggregate ): orient(_orient), V_base_start(startpos.p), V_base_end(startpos.p + starttwist.vel), eqradius(_eqradius), aggregate(_aggregate) { // startframe and starttwist are expressed in Wo. // after 1 time unit, startframe has translated over starttwist.vel // and rotated over starttwist.rot.Norm() (both vectors can be zero) // Thus the frame on the path after 1 time unit is defined by // startframe.Integrate(starttwist, 1); V_start_end = V_base_end - V_base_start; double dist = V_start_end.Normalize(); // distance traveled during 1 time unit orient->SetStartEnd(startpos.M, (startpos*Frame( Rotation::Rot(starttwist.rot, starttwist.rot.Norm() ), starttwist.vel )).M); double alpha = orient->Angle(); // rotation during 1 time unit // See what has the slowest eq. motion, and adapt // the other to this slower motion // use eqradius to transform between rot and transl. // Only modify if non zero (prevent division by zero) if ( alpha != 0 && alpha*eqradius > dist) { // rotational_interpolation is the limitation pathlength = alpha*eqradius; scalerot = 1/eqradius; scalelin = dist/pathlength; } else if ( dist != 0 ) { // translation is the limitation pathlength = dist; scalerot = alpha/pathlength; scalelin = 1; } else { // both were zero pathlength = 0; scalerot = 1; scalelin = 1; } } double Path_Line::LengthToS(double length) { return length/scalelin; } double Path_Line::PathLength(){ return pathlength; } Frame Path_Line::Pos(double s) const { return Frame(orient->Pos(s*scalerot),V_base_start + V_start_end*s*scalelin ); } Twist Path_Line::Vel(double s,double sd) const { return Twist( V_start_end*sd*scalelin, orient->Vel(s*scalerot,sd*scalerot) ); } Twist Path_Line::Acc(double s,double sd,double sdd) const { return Twist( V_start_end*sdd*scalelin, orient->Acc(s*scalerot,sd*scalerot,sdd*scalerot) ); } Path_Line::~Path_Line() { if (aggregate) delete orient; } Path* Path_Line::Clone() { if (aggregate ) return new Path_Line( Frame(orient->Pos(0),V_base_start), Frame(orient->Pos(pathlength*scalerot),V_base_end), orient->Clone(), eqradius, true ); // else : return new Path_Line( Frame(orient->Pos(0),V_base_start), Frame(orient->Pos(pathlength*scalerot),V_base_end), orient, eqradius, false ); } void Path_Line::Write(std::ostream& os) { os << "LINE[ "; os << " " << Frame(orient->Pos(0),V_base_start) << std::endl; os << " " << Frame(orient->Pos(pathlength*scalerot),V_base_end) << std::endl; os << " ";orient->Write(os); os << " " << eqradius; os << "]" << std::endl; } } orocos-kdl-1.4.0/orocos_kdl/src/path_line.hpp000066400000000000000000000122741326704774600212170ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 path_line.h path_line.h - description ------------------- begin : Mon January 10 2005 copyright : (C) 2005 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * ALTERNATIVE FOR trajectory_line.h/cpp * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path_line.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $ * $Name: $ ****************************************************************************/ #ifndef KDL_MOTION_PATH_LINE_H #define KDL_MOTION_PATH_LINE_H #include "path.hpp" #include "rotational_interpolation.hpp" namespace KDL { /** * A path representing a line from A to B. * @ingroup Motion */ class Path_Line : public Path { // Orientatie gedeelte RotationalInterpolation* orient; // Lineair gedeelte Vector V_base_start; Vector V_base_end; Vector V_start_end; double eqradius; // equivalent radius // verdeling baanlengte over pos/rot double pathlength; double scalelin; double scalerot; bool aggregate; public: /** * Constructs a Line Path * F_base_start and F_base_end give the begin and end frame wrt the base * orient gives the method of rotation interpolation * eqradius : equivalent radius : * serves to compare rotations and translations. * the "amount of motion"(pos,vel,acc) of the rotation is taken * to be the amount motion of a point at distance eqradius from the * rotation axis. * * Eqradius is introduced because it is unavoidable that you have to compare rotations and translations : * e.g. : You can have motions that only contain rotation, and motions that only contain translations. * The motion planning goes as follows : * - translation is planned with the given parameters * - rotation is planned planned with the parameters calculated with eqradius. * - The longest of the previous two remains unchanged, * the shortest in duration is scaled to take as long as the longest. * This guarantees that the geometric path in 6D space remains independent of the motion profile parameters. * * RotationalInterpolation_SingleAxis() has the advantage that it is independent * of the frame in which you express your path. * Other implementations for RotationalInterpolations COULD be * (not implemented) (yet) : * 1) quaternion interpolation : but this is more difficult for the human to interprete * 2) 3-axis interpolation : express the orientation of the frame in e.g. * euler zyx angles alfa,beta, gamma and interpolate these parameters. * But this is dependent of the frame you choose as a reference and * their can occur representation singularities. */ Path_Line(const Frame& F_base_start, const Frame& F_base_end, RotationalInterpolation* orient, double eqradius, bool _aggregate=true); Path_Line(const Frame& F_base_start, const Twist& twist_in_base, RotationalInterpolation* orient, double eqradius, bool _aggregate=true); double LengthToS(double length); virtual double PathLength(); virtual Frame Pos(double s) const; virtual Twist Vel(double s,double sd) const ; virtual Twist Acc(double s,double sd,double sdd) const; virtual void Write(std::ostream& os); virtual Path* Clone(); /** * gets an identifier indicating the type of this Path object */ virtual IdentifierType getIdentifier() const { return ID_LINE; } virtual ~Path_Line(); }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/path_point.cpp000066400000000000000000000053661326704774600214200ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path_point.cxx path_point.cxx - description ------------------- begin : Mon May 10 2004 copyright : (C) 2004 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path_point.cpp,v 1.1.2.3 2003/07/24 13:40:49 psoetens Exp $ * $Name: $ ****************************************************************************/ #include "path_point.hpp" namespace KDL { Path_Point::Path_Point(const Frame& startpos) :F_base_start(startpos) { } double Path_Point::LengthToS(double length) { return length; } double Path_Point::PathLength(){ return 0; } Frame Path_Point::Pos(double s) const { return F_base_start; } Twist Path_Point::Vel(double s,double sd) const { return Twist::Zero(); } Twist Path_Point::Acc(double s,double sd,double sdd) const { return Twist::Zero(); } Path_Point::~Path_Point() { } Path* Path_Point::Clone() { return new Path_Point( F_base_start ); } void Path_Point::Write(std::ostream& os) { os << "POINT[ "<< F_base_start << "]" << std::endl; } } orocos-kdl-1.4.0/orocos_kdl/src/path_point.hpp000066400000000000000000000056771326704774600214320ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 path_point.h path_point.h - description ------------------- begin : Mon January 10 2005 copyright : (C) 2005 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * ALTERNATIVE FOR trajectory_stationary.h/cpp * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path_point.h,v 1.1.2.3 2003/07/24 13:40:49 psoetens Exp $ * $Name: $ ****************************************************************************/ #ifndef KDL_MOTION_PATH_POINT_H #define KDL_MOTION_PATH_POINT_H #include "path.hpp" #include "rotational_interpolation.hpp" namespace KDL { /** * A Path consisting only of a point in space. * @ingroup Motion */ class Path_Point : public Path { Frame F_base_start; public: /** * Constructs a Point Path */ Path_Point(const Frame& F_base_start); double LengthToS(double length); virtual double PathLength(); virtual Frame Pos(double s) const; virtual Twist Vel(double s,double sd) const ; virtual Twist Acc(double s,double sd,double sdd) const; virtual void Write(std::ostream& os); virtual Path* Clone(); /** * gets an identifier indicating the type of this Path object */ virtual IdentifierType getIdentifier() const { return ID_POINT; } virtual ~Path_Point(); }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/path_roundedcomposite.cpp000066400000000000000000000147671326704774600236570ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path_roundedcomposite.cxx path_roundedcomposite.cxx - description ------------------- begin : Mon May 10 2004 copyright : (C) 2004 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path_roundedcomposite.cpp,v 1.1.1.1.2.5 2003/07/24 13:26:15 psoetens Exp $ * $Name: $ ****************************************************************************/ #include "path_roundedcomposite.hpp" #include "path_line.hpp" #include "path_circle.hpp" #include "utilities/error.h" #include namespace KDL { // private constructor, to keep the type when cloning a Path_RoundedComposite, such that getIdentifier keeps on returning // the correct value: Path_RoundedComposite::Path_RoundedComposite(Path_Composite* _comp, double _radius, double _eqradius, RotationalInterpolation* _orient, bool _aggregate,int _nrofpoints): comp(_comp), radius(_radius), eqradius(_eqradius), orient(_orient), nrofpoints(_nrofpoints), aggregate(_aggregate) { } Path_RoundedComposite::Path_RoundedComposite(double _radius,double _eqradius,RotationalInterpolation* _orient, bool _aggregate) : comp( new Path_Composite()), radius(_radius),eqradius(_eqradius), orient(_orient), aggregate(_aggregate) { nrofpoints = 0; if (eqradius<=0) { throw Error_MotionPlanning_Not_Feasible(1); } } void Path_RoundedComposite::Add(const Frame& F_base_point) { double eps = 1E-7; if (nrofpoints == 0) { F_base_start = F_base_point; } else if (nrofpoints == 1) { F_base_via = F_base_point; } else { // calculate rounded segment : line + circle, // determine the angle between the line segments : Vector ab = F_base_via.p - F_base_start.p; Vector bc = F_base_point.p - F_base_via.p; double abdist = ab.Norm(); double bcdist = bc.Norm(); if (abdist < eps) { throw Error_MotionPlanning_Not_Feasible(2); } if (bcdist < eps) { throw Error_MotionPlanning_Not_Feasible(3); } double alpha = acos(dot(ab, bc) / abdist / bcdist); if ((PI - alpha) < eps) { throw Error_MotionPlanning_Not_Feasible(4); } if (alpha < eps) { // no rounding is done in the case of parallel line segments comp->Add( new Path_Line(F_base_start, F_base_via, orient->Clone(), eqradius)); F_base_start = F_base_via; F_base_via = F_base_point; } else { double d = radius / tan((PI - alpha) / 2); // tan. is guaranteed not to return zero. if (d >= abdist) throw Error_MotionPlanning_Not_Feasible(5); if (d >= bcdist) throw Error_MotionPlanning_Not_Feasible(6); std::auto_ptr < Path > line1( new Path_Line(F_base_start, F_base_via, orient->Clone(), eqradius)); std::auto_ptr < Path > line2( new Path_Line(F_base_via, F_base_point, orient->Clone(), eqradius)); Frame F_base_circlestart = line1->Pos(line1->LengthToS(abdist - d)); Frame F_base_circleend = line2->Pos(line2->LengthToS(d)); // end of circle segment, beginning of next line Vector V_base_t = ab * (ab * bc); V_base_t.Normalize(); comp->Add( new Path_Line(F_base_start, F_base_circlestart, orient->Clone(), eqradius)); comp->Add( new Path_Circle(F_base_circlestart, F_base_circlestart.p - V_base_t * radius, F_base_circleend.p, F_base_circleend.M, alpha, orient->Clone(), eqradius)); // shift for next line F_base_start = F_base_circleend; // end of the circle segment F_base_via = F_base_point; } } nrofpoints++; } void Path_RoundedComposite::Finish() { if (nrofpoints >= 1) { comp->Add( new Path_Line(F_base_start, F_base_via, orient->Clone(), eqradius)); } } double Path_RoundedComposite::LengthToS(double length) { return comp->LengthToS(length); } double Path_RoundedComposite::PathLength() { return comp->PathLength(); } Frame Path_RoundedComposite::Pos(double s) const { return comp->Pos(s); } Twist Path_RoundedComposite::Vel(double s, double sd) const { return comp->Vel(s, sd); } Twist Path_RoundedComposite::Acc(double s, double sd, double sdd) const { return comp->Acc(s, sd, sdd); } void Path_RoundedComposite::Write(std::ostream& os) { comp->Write(os); } int Path_RoundedComposite::GetNrOfSegments() { return comp->GetNrOfSegments(); } Path* Path_RoundedComposite::GetSegment(int i) { return comp->GetSegment(i); } double Path_RoundedComposite::GetLengthToEndOfSegment(int i) { return comp->GetLengthToEndOfSegment(i); } void Path_RoundedComposite::GetCurrentSegmentLocation(double s, int& segment_number, double& inner_s) { comp->GetCurrentSegmentLocation(s,segment_number,inner_s); } Path_RoundedComposite::~Path_RoundedComposite() { if (aggregate) delete orient; delete comp; } Path* Path_RoundedComposite::Clone() { return new Path_RoundedComposite(static_cast(comp->Clone()),radius,eqradius,orient->Clone(), true, nrofpoints); } } orocos-kdl-1.4.0/orocos_kdl/src/path_roundedcomposite.hpp000066400000000000000000000155321326704774600236530ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 path_roundedcomposite.h path_roundedcomposite.h - description ------------------- begin : Mon January 10 2005 copyright : (C) 2005 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path_roundedcomposite.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $ * $Name: $ ****************************************************************************/ #ifndef KDL_MOTION_ROUNDEDCOMPOSITE_H #define KDL_MOTION_ROUNDEDCOMPOSITE_H #include "path.hpp" #include "path_composite.hpp" #include "rotational_interpolation.hpp" namespace KDL { /** * The specification of a path, composed of way-points with rounded corners. * * @ingroup Motion */ class Path_RoundedComposite : public Path { /** a Path_Composite is aggregated to hold the rounded trajectory * with circles and lines */ Path_Composite* comp; double radius; double eqradius; RotationalInterpolation* orient; // cached from underlying path objects for generating the rounding : Frame F_base_start; Frame F_base_via; //Frame F_base_end; int nrofpoints; bool aggregate; Path_RoundedComposite(Path_Composite* comp,double radius,double eqradius,RotationalInterpolation* orient, bool aggregate, int nrofpoints); public: /** * @param radius : radius of the rounding circles * @param eqradius : equivalent radius to compare rotations/velocities * @param orient : method of rotational_interpolation interpolation * @param aggregate : if true, this object will own the _orient pointer, i.e. it will delete the _orient pointer * when the destructor of this object is called. */ Path_RoundedComposite(double radius,double eqradius,RotationalInterpolation* orient, bool aggregate=true); /** * Adds a point to this rounded composite, between to adjecent points * a Path_Line will be created, between two lines there will be * rounding with the given radius with a Path_Circle * * The Error_MotionPlanning_Not_Feasible has a type (obtained by GetType) of: * - 3101 if the eq. radius <= 0 * - 3102 if the first segment in a rounding has zero length. * - 3103 if the second segment in a rounding has zero length. * - 3104 if the angle between the first and the second segment is close to M_PI. * (meaning that the segments are on top of each other) * - 3105 if the distance needed for the rounding is larger then the first segment. * - 3106 if the distance needed for the rounding is larger then the second segment. * * @param F_base_point the pose of a new via point. * @warning Can throw Error_MotionPlanning_Not_Feasible object * @TODO handle the case of error type 3105 and 3106 by skipping segments, such that the class could be applied * with points that are very close to each other. */ void Add(const Frame& F_base_point); /** * to be called after the last line is added to finish up * the work */ void Finish(); virtual double LengthToS(double length); /** * Returns the total path length of the trajectory * (has dimension LENGTH) * This is not always a physical length , ie when dealing with rotations * that are dominant. */ virtual double PathLength(); /** * Returns the Frame at the current path length s */ virtual Frame Pos(double s) const; /** * Returns the velocity twist at path length s theta and with * derivative of s == sd */ virtual Twist Vel(double s,double sd) const; /** * Returns the acceleration twist at path length s and with * derivative of s == sd, and 2nd derivative of s == sdd */ virtual Twist Acc(double s,double sd,double sdd) const; /** * virtual constructor, constructing by copying. * In this case it returns the Clone() of the aggregated Path_Composite * because this is all one ever will need. */ virtual Path* Clone(); /** * Writes one of the derived objects to the stream */ virtual void Write(std::ostream& os); /** * returns the number of underlying segments. */ virtual int GetNrOfSegments(); /** * returns a pointer to the underlying Path of the given segment number i. * \param i segment number * \return pointer to the underlying Path * \warning The pointer is still owned by this class and is lifetime depends on the lifetime * of this class. */ virtual Path* GetSegment(int i); /** * gets the length to the end of the given segment. * \param i segment number * \return length to the end of the segment, i.e. the value for s corresponding to the end of * this segment. */ virtual double GetLengthToEndOfSegment(int i); /** * \param s [INPUT] path length variable for the composite. * \param segment_number [OUTPUT] segments that corresponds to the path length variable s. * \param inner_s [OUTPUT] path length to use within the segment. */ virtual void GetCurrentSegmentLocation(double s, int &segment_number, double& inner_s); /** * gets an identifier indicating the type of this Path object */ virtual IdentifierType getIdentifier() const { return ID_ROUNDED_COMPOSITE; } virtual ~Path_RoundedComposite(); }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/rigidbodyinertia.cpp000066400000000000000000000076641326704774600226060ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "rigidbodyinertia.hpp" #include using namespace Eigen; namespace KDL{ const static bool mhi=true; RigidBodyInertia::RigidBodyInertia(double m_,const Vector& h_,const RotationalInertia& I_,bool mhi): m(m_),h(h_),I(I_) { } RigidBodyInertia::RigidBodyInertia(double m_, const Vector& c_, const RotationalInertia& Ic): m(m_),h(m*c_){ //I=Ic-c x c x Vector3d c_eig=Map(c_.data); Map(I.data)=Map(Ic.data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Matrix3d::Identity()); } RigidBodyInertia operator*(double a,const RigidBodyInertia& I){ return RigidBodyInertia(a*I.m,a*I.h,a*I.I,mhi); } RigidBodyInertia operator+(const RigidBodyInertia& Ia, const RigidBodyInertia& Ib){ return RigidBodyInertia(Ia.m+Ib.m,Ia.h+Ib.h,Ia.I+Ib.I,mhi); } Wrench operator*(const RigidBodyInertia& I,const Twist& t){ return Wrench(I.m*t.vel-I.h*t.rot,I.I*t.rot+I.h*t.vel); } RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& 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' Vector hmr = (I.h-I.m*X.p); Vector3d r_eig = Map(X.p.data); Vector3d h_eig = Map(I.h.data); Vector3d hmr_eig = Map(hmr.data); Matrix3d rcrosshcross = h_eig *r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity(); Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity(); Matrix3d R = Map(X.M.data); RotationalInertia Ib; Map(Ib.data) = R*((Map(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose()); return RigidBodyInertia(I.m,T.M*hmr,Ib,mhi); } RigidBodyInertia operator*(const Rotation& M,const RigidBodyInertia& I){ //mb=ma //hb=R*h //Ib = R(Ia)R' with r=0 Matrix3d R = Map(M.data); RotationalInertia Ib; Map(Ib.data) = R.transpose()*(Map(I.I.data)*R); return RigidBodyInertia(I.m,M*I.h,Ib,mhi); } RigidBodyInertia RigidBodyInertia::RefPoint(const Vector& p){ //mb=ma //hb=(h-m*r) //Ib = (Ia+r x h x + (h-m*r) x r x) Vector hmr = (this->h-this->m*p); Vector3d r_eig = Map(p.data); Vector3d h_eig = Map(this->h.data); Vector3d hmr_eig = Map(hmr.data); Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity(); Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity(); RotationalInertia Ib; Map(Ib.data) = Map(this->I.data)+rcrosshcross+hmrcrossrcross; return RigidBodyInertia(this->m,hmr,Ib,mhi); } }//namespace orocos-kdl-1.4.0/orocos_kdl/src/rigidbodyinertia.hpp000066400000000000000000000106741326704774600226060ustar00rootroot00000000000000// Copyright (C) 2009 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_RIGIDBODYINERTIA_HPP #define KDL_RIGIDBODYINERTIA_HPP #include "frames.hpp" #include "rotationalinertia.hpp" namespace KDL { /** * \brief 6D Inertia of a rigid body * * The inertia is defined in a certain reference point and a certain reference base. * The reference point does not have to coincide with the origin of the reference frame. */ class RigidBodyInertia{ public: /** * This constructor creates a cartesian space inertia matrix, * the arguments are the mass, the vector from the reference point to cog and the rotational inertia in the cog. */ explicit RigidBodyInertia(double m=0, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero()); /** * Creates an inertia with zero mass, and zero RotationalInertia */ static inline RigidBodyInertia Zero(){ return RigidBodyInertia(0.0,Vector::Zero(),RotationalInertia::Zero()); }; ~RigidBodyInertia(){}; friend RigidBodyInertia operator*(double a,const RigidBodyInertia& I); friend RigidBodyInertia operator+(const RigidBodyInertia& Ia,const RigidBodyInertia& Ib); friend Wrench operator*(const RigidBodyInertia& I,const Twist& t); friend RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I); friend RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I); /** * Reference point change with v the vector from the old to * the new point expressed in the current reference frame */ RigidBodyInertia RefPoint(const Vector& p); /** * Get the mass of the rigid body */ double getMass() const{ return m; }; /** * Get the center of gravity of the rigid body */ Vector getCOG() const{ if(m==0) return Vector::Zero(); else return h/m; }; /** * Get the rotational inertia expressed in the reference frame (not the cog) */ RotationalInertia getRotationalInertia() const{ return I; }; private: RigidBodyInertia(double m,const Vector& h,const RotationalInertia& I,bool mhi); double m; Vector h; RotationalInertia I; friend class ArticulatedBodyInertia; }; /** * Scalar product: I_new = double * I_old */ RigidBodyInertia operator*(double a,const RigidBodyInertia& I); /** * addition I: I_new = I_old1 + I_old2, make sure that I_old1 * and I_old2 are expressed in the same reference frame/point, * otherwise the result is worth nothing */ RigidBodyInertia operator+(const RigidBodyInertia& Ia,const RigidBodyInertia& Ib); /** * calculate spatial momentum: h = I*v * make sure that the twist v and the inertia are expressed in the same reference frame/point */ Wrench operator*(const RigidBodyInertia& I,const Twist& t); /** * Coordinate system transform Ia = T_a_b*Ib with T_a_b the frame from a to b. */ RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I); /** * Reference frame orientation change Ia = R_a_b*Ib with R_a_b * the rotation of b expressed in a */ RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I); }//namespace #endif orocos-kdl-1.4.0/orocos_kdl/src/rotational_interpolation.cpp000066400000000000000000000062431326704774600243710ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 rotational_interpolation.cxx rotational_interpolation.cxx - description ------------------- begin : Mon May 10 2004 copyright : (C) 2004 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: rotational_interpolation.cpp,v 1.1.1.1.2.3 2003/02/24 13:13:06 psoetens Exp $ * $Name: $ ****************************************************************************/ #include "utilities/error.h" #include "utilities/error_stack.h" #include "rotational_interpolation.hpp" #include "rotational_interpolation_sa.hpp" #include #include namespace KDL { using namespace std; RotationalInterpolation* RotationalInterpolation::Read(istream& is) { // auto_ptr because exception can be thrown ! IOTrace("RotationalInterpolation::Read"); char storage[64]; EatWord(is,"[",storage,sizeof(storage)); Eat(is,'['); if (strcmp(storage,"SINGLEAXIS")==0) { IOTrace("SINGLEAXIS"); EatEnd(is,']'); IOTracePop(); IOTracePop(); return new RotationalInterpolation_SingleAxis(); } else if (strcmp(storage,"THREEAXIS")==0) { IOTrace("THREEAXIS"); throw Error_Not_Implemented(); EatEnd(is,']'); IOTracePop(); IOTracePop(); return NULL; } else if (strcmp(storage,"TWOAXIS")==0) { IOTrace("TWOAXIS"); throw Error_Not_Implemented(); EatEnd(is,']'); IOTracePop(); IOTracePop(); return NULL; } else { throw Error_MotionIO_Unexpected_Traj(); } return NULL; // just to avoid the warning; } } orocos-kdl-1.4.0/orocos_kdl/src/rotational_interpolation.hpp000066400000000000000000000101011326704774600243620ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 rotational_interpolation.h rotational_interpolation.h - description ------------------- begin : Mon January 10 2005 copyright : (C) 2005 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: rotational_interpolation.h,v 1.1.1.1.2.2 2003/02/24 13:13:06 psoetens Exp $ * $Name: $ ****************************************************************************/ #ifndef KDL_ROTATIONALINTERPOLATION_H #define KDL_ROTATIONALINTERPOLATION_H #include "frames.hpp" #include "frames_io.hpp" namespace KDL { /** * RotationalInterpolation specifies the rotational part of a geometric trajectory * - The different derived objects specify different methods for interpolating * rotations. * - SetStartEnd should be called before * using the other methods * - The start and end position do NOT belong to the persistent state ! The owner of this * object is responsible for setting these each time * @ingroup Motion */ class RotationalInterpolation { public: /** * Set the start and end rotational_interpolation */ virtual void SetStartEnd(Rotation start,Rotation end) = 0; /** * - Returns the angle value to move from start to end. * This should have units radians, * - With Single Axis interp corresponds to the angle rotation * - With Three Axis interp corresponds to the slowest of the three * rotations. */ virtual double Angle() = 0; /** * Returns the rotation matrix at angle theta */ virtual Rotation Pos(double theta) const = 0; /** * Returns the rotational velocity at angle theta and with * derivative of theta == thetad */ virtual Vector Vel(double theta,double thetad) const = 0; /** * Returns the rotational acceleration at angle theta and with * derivative of theta == thetad, and 2nd derivative of theta == thdd */ virtual Vector Acc(double theta,double thetad,double thetadd) const = 0; /** * Writes one of the derived objects to the stream */ virtual void Write(std::ostream& os) const = 0; /** * Reads one of the derived objects from the stream and returns a pointer * (factory method) */ static RotationalInterpolation* Read(std::istream& is); /** * virtual constructor, construction by copying .. */ virtual RotationalInterpolation* Clone() const = 0; virtual ~RotationalInterpolation() {} }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/rotational_interpolation_sa.cpp000066400000000000000000000065251326704774600250570ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 rotational_interpolation_sa.cxx rotational_interpolation_sa.cxx - description ------------------- begin : Mon May 10 2004 copyright : (C) 2004 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: rotational_interpolation_singleaxis.cpp,v 1.1.1.1.2.2 2003/02/24 13:13:06 psoetens Exp $ * $Name: $ ****************************************************************************/ #include "rotational_interpolation_sa.hpp" #include "trajectory.hpp" namespace KDL { RotationalInterpolation_SingleAxis::RotationalInterpolation_SingleAxis() {}; void RotationalInterpolation_SingleAxis::SetStartEnd(Rotation start,Rotation end) { R_base_start = start; R_base_end = end; Rotation R_start_end = R_base_start.Inverse()*R_base_end; angle = R_start_end.GetRotAngle(rot_start_end); } Rotation RotationalInterpolation_SingleAxis::Pos(double theta) const { return R_base_start* Rotation::Rot2(rot_start_end,theta); } Vector RotationalInterpolation_SingleAxis::Vel(double theta,double thetad) const { return R_base_start * ( rot_start_end*thetad ); } Vector RotationalInterpolation_SingleAxis::Acc(double theta,double thetad,double thetadd) const { return R_base_start * ( rot_start_end* thetadd); } double RotationalInterpolation_SingleAxis::Angle() { return angle; } void RotationalInterpolation_SingleAxis::Write(std::ostream& os) const { os << "SingleAxis[] " << std::endl; } RotationalInterpolation_SingleAxis::~RotationalInterpolation_SingleAxis() { } RotationalInterpolation* RotationalInterpolation_SingleAxis::Clone() const { return new RotationalInterpolation_SingleAxis(); } } orocos-kdl-1.4.0/orocos_kdl/src/rotational_interpolation_sa.hpp000066400000000000000000000064141326704774600250610ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 rotational_interpolation_sa.h rotational_interpolation_sa.h - description ------------------- begin : Mon January 10 2005 copyright : (C) 2005 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: rotational_interpolation_singleaxis.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $ * $Name: $ ****************************************************************************/ #ifndef KDL_ROTATIONALINTERPOLATION_SINGLEAXIS_H #define KDL_ROTATIONALINTERPOLATION_SINGLEAXIS_H #include "frames.hpp" #include "frames_io.hpp" #include "rotational_interpolation.hpp" namespace KDL { /** * An interpolation algorithm which rotates a frame over the existing * single rotation axis * formed by start and end rotation. If more than one rotational axis * exist, an arbitrary one will be chosen, therefore it is not recommended * to try to interpolate a 180 degrees rotation. * @ingroup Motion */ class RotationalInterpolation_SingleAxis: public RotationalInterpolation { Rotation R_base_start; Rotation R_base_end; Vector rot_start_end; double angle; public: RotationalInterpolation_SingleAxis(); virtual void SetStartEnd(Rotation start,Rotation end); virtual double Angle(); virtual Rotation Pos(double th) const; virtual Vector Vel(double th,double thd) const; virtual Vector Acc(double th,double thd,double thdd) const; virtual void Write(std::ostream& os) const; virtual RotationalInterpolation* Clone() const; virtual ~RotationalInterpolation_SingleAxis(); }; } #endif orocos-kdl-1.4.0/orocos_kdl/src/rotationalinertia.cpp000066400000000000000000000041121326704774600227670ustar00rootroot00000000000000// Copyright (C) 2009 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "rotationalinertia.hpp" #include using namespace Eigen; namespace KDL { RotationalInertia::RotationalInertia(double Ixx,double Iyy,double Izz,double Ixy,double Ixz,double Iyz) { data[0]=Ixx; data[1]=data[3]=Ixy; data[2]=data[6]=Ixz; data[4]=Iyy; data[5]=data[7]=Iyz; data[8]=Izz; } RotationalInertia::~RotationalInertia() { } Vector RotationalInertia::operator*(const Vector& omega) const { // Complexity : 9M+6A Vector result; Map(result.data)=Map(this->data)*Map(omega.data); return result; } RotationalInertia operator*(double a, const RotationalInertia& I){ RotationalInertia result; Map(result.data)=a*Map(I.data); return result; } RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib){ RotationalInertia result; Map(result.data)=Map(Ia.data)+Map(Ib.data); return result; } } orocos-kdl-1.4.0/orocos_kdl/src/rotationalinertia.hpp000066400000000000000000000054671326704774600230120ustar00rootroot00000000000000// Copyright (C) 2009 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_ROTATIONALINERTIA_HPP #define KDL_ROTATIONALINERTIA_HPP #include "frames.hpp" //------- class for only the Rotational Inertia -------- namespace KDL { //Forward declaration class RigidBodyInertia; class RotationalInertia{ public: explicit RotationalInertia(double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0); static inline RotationalInertia Zero(){ return RotationalInertia(0,0,0,0,0,0); }; friend RotationalInertia operator*(double a, const RotationalInertia& I); friend RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib); /** * This function calculates the angular momentum resulting from a rotational velocity omega */ KDL::Vector operator*(const KDL::Vector& omega) const; ~RotationalInertia(); friend class RigidBodyInertia; ///Scalar product friend RigidBodyInertia operator*(double a,const RigidBodyInertia& I); ///addition friend RigidBodyInertia operator+(const RigidBodyInertia& Ia,const RigidBodyInertia& Ib); ///calculate spatial momentum friend Wrench operator*(const RigidBodyInertia& I,const Twist& t); ///coordinate system transform Ia = T_a_b*Ib with T_a_b the frame from a to b friend RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I); ///base frame orientation change Ia = R_a_b*Ib with R_a_b the rotation for frame from a to b friend RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I); double data[9]; }; RotationalInertia operator*(double a, const RotationalInertia& I); RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib); } #endif orocos-kdl-1.4.0/orocos_kdl/src/segment.cpp000066400000000000000000000041271326704774600207070ustar00rootroot00000000000000// Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 "segment.hpp" namespace KDL { Segment::Segment(const std::string& _name, const Joint& _joint, const Frame& _f_tip, const RigidBodyInertia& _I): name(_name), joint(_joint),I(_I), f_tip(_joint.pose(0).Inverse() * _f_tip) { } Segment::Segment(const Joint& _joint, const Frame& _f_tip, const RigidBodyInertia& _I): name("NoName"), joint(_joint),I(_I), f_tip(_joint.pose(0).Inverse() * _f_tip) { } Segment::Segment(const Segment& in): name(in.name),joint(in.joint),I(in.I), f_tip(in.f_tip) { } Segment& Segment::operator=(const Segment& arg) { name=arg.name; joint=arg.joint; I=arg.I; f_tip=arg.f_tip; return *this; } Segment::~Segment() { } Frame Segment::pose(const double& q)const { return joint.pose(q)*f_tip; } Twist Segment::twist(const double& q, const double& qdot)const { return joint.twist(qdot).RefPoint(joint.pose(q).M * f_tip.p); } void Segment::setFrameToTip(const Frame& f_tip_new) { f_tip = joint.pose(0).Inverse() * f_tip_new; } }//end of namespace KDL orocos-kdl-1.4.0/orocos_kdl/src/segment.hpp000066400000000000000000000125121326704774600207110ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_SEGMENT_HPP #define KDL_SEGMENT_HPP #include "frames.hpp" #include "rigidbodyinertia.hpp" #include "joint.hpp" #include namespace KDL { /** * \brief This class encapsulates a simple segment, that is a "rigid * body" (i.e., a frame and a rigid body inertia) with a joint and with * "handles", root and tip to connect to other segments. * * A simple segment is described by the following properties : * - Joint * - Rigid Body Inertia: of the rigid body part of the Segment * - Offset from the end of the joint to the tip of the segment: * the joint is located at the root of the segment. * * @ingroup KinematicFamily */ class Segment { friend class Chain; private: std::string name; Joint joint; RigidBodyInertia I; Frame f_tip; public: /** * Constructor of the segment * * @param name name of the segment * @param joint joint of the segment, default: * Joint(Joint::None) * @param f_tip frame from the end of the joint to the tip of * the segment, default: Frame::Identity() * @param M rigid body inertia of the segment, default: Inertia::Zero() */ explicit Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); /** * Constructor of the segment * * @param joint joint of the segment, default: * Joint(Joint::None) * @param f_tip frame from the end of the joint to the tip of * the segment, default: Frame::Identity() * @param M rigid body inertia of the segment, default: Inertia::Zero() */ explicit Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); Segment(const Segment& in); Segment& operator=(const Segment& arg); virtual ~Segment(); /** * Request the pose of the segment, given the joint position q. * * @param q 1D position of the joint * * @return pose from the root to the tip of the segment */ Frame pose(const double& q)const; /** * Request the 6D-velocity of the tip of the segment, given * the joint position q and the joint velocity qdot. * * @param q 1D position of the joint * @param qdot 1D velocity of the joint * * @return 6D-velocity of the tip of the segment, expressed *in the base-frame of the segment(root) and with the tip of *the segment as reference point. */ Twist twist(const double& q,const double& qdot)const; /** * Request the name of the segment * * * @return const reference to the name of the segment */ const std::string& getName()const { return name; } /** * Request the joint of the segment * * * @return const reference to the joint of the segment */ const Joint& getJoint()const { return joint; } /** * Request the inertia of the segment * * * @return const reference to the inertia of the segment */ const RigidBodyInertia& getInertia()const { return I; } /** * Request the inertia of the segment * * * @return const reference to the inertia of the segment */ void setInertia(const RigidBodyInertia& Iin) { this->I=Iin; } /** * Request the pose from the joint end to the tip of the *segment. * * @return the original parent end - segment end pose. */ Frame getFrameToTip()const { return joint.pose(0)*f_tip; } /** * Set the pose from the joint end to the tip of the * segment. * * @param f_tip_new pose from the joint end to the tip of the segment */ void setFrameToTip(const Frame& f_tip_new); }; }//end of namespace KDL #endif orocos-kdl-1.4.0/orocos_kdl/src/solveri.hpp000066400000000000000000000113351326704774600207340ustar00rootroot00000000000000// Copyright (C) 2013 Stephen Roderick // 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 __SOLVERI_HPP #define __SOLVERI_HPP namespace KDL { /** * Solver interface supporting storage and description of the latest error. * * Error codes: Zero (0) indicates no error, positive error codes indicate more * of a warning (e.g. a degraded solution, but motion can continue), and * negative error codes indicate failure (e.g. a singularity, and motion * can not continue). * * Error codes between -99 and +99 (inclusive) are reserved for system-wide * error codes. Derived classes should use values > +100, and < -100. * * Example use * * \code * class MySolver : public SolverI * { * public: * static const int E_CHILDFAILD = xxx; * * MySolver(SomeOtherSolver& other); * virtual ~MySolver(); * int CartToJnt(...); * virtual const char* strError(const int error) const; * protected: * SomeOtherSolver& child; * }; * * ... * * int MySolver::CartToJnt(...) * { * error = child->SomeCall(); * if (E_NOERROR != error) { * error = E_CHILDFAILED; * } else { * ... * } * return error; * } * * const char* MySolver::strError(const int error) const * { * if (E_CHILDFAILED == error) return "Child solver failed"; * else return SolverI::strError(error); * } * * void someFunc() * { * SomeOtherSolver child = new SomeOtherSolver(...); * MySolver parent = new MySolver(child); * ... * int rc = parent->CartToJnt(...); * if (E_NOERROR != rc) { * if (MySolver::E_CHILDFAILED == rc) { * rc = child->getError(); * // cope with child failure 'rc' * } * } * ... * } * \endcode */ class SolverI { public: enum { /// Converged but degraded solution (e.g. WDLS with psuedo-inverse singular) E_DEGRADED = +1, //! No error E_NOERROR = 0, //! Failed to converge E_NO_CONVERGE = -1, //! Undefined value (e.g. computed a NAN, or tan(90 degrees) ) E_UNDEFINED = -2, //! Chain size changed E_NOT_UP_TO_DATE = -3, //! Input size does not match internal state E_SIZE_MISMATCH = -4, //! Maximum number of iterations exceeded E_MAX_ITERATIONS_EXCEEDED = -5, //! Requested index out of range E_OUT_OF_RANGE = -6, //! Not yet implemented E_NOT_IMPLEMENTED = -7, //! Internal svd calculation failed E_SVD_FAILED = -8 }; /// Initialize latest error to E_NOERROR SolverI() : error(E_NOERROR) {} virtual ~SolverI() {} /// Return the latest error virtual int getError() const { return error; } /** Return a description of the latest error \return if \a error is known then a description of \a error, otherwise "UNKNOWN ERROR" */ virtual const char* strError(const int error) const { if (E_NOERROR == error) return "No error"; else if (E_NO_CONVERGE == error) return "Failed to converge"; else if (E_UNDEFINED == error) return "Undefined value"; else if (E_DEGRADED == error) return "Converged but degraded solution"; else if (E_NOT_UP_TO_DATE == error) return "Internal data structures not up to date with Chain"; else if (E_SIZE_MISMATCH == error) return "The size of the input does not match the internal state"; else if (E_MAX_ITERATIONS_EXCEEDED == error) return "The maximum number of iterations is exceeded"; else if (E_OUT_OF_RANGE == error) return "The requested index is out of range"; else if (E_NOT_IMPLEMENTED == error) return "The requested function is not yet implemented"; else if (E_SVD_FAILED == error) return "SVD failed"; else return "UNKNOWN ERROR"; } /** * Update the internal data structures. This is required if the number * of segments or number of joints of a chain/tree have changed. * This provides a single point of contact for solver memory allocations. */ virtual void updateInternalDataStructures() = 0; protected: /// Latest error, initialized to E_NOERROR in constructor int error; }; } // namespaces #endif orocos-kdl-1.4.0/orocos_kdl/src/stiffness.hpp000066400000000000000000000056231326704774600212600ustar00rootroot00000000000000// Copyright (C) 2007 Ruben Smits // Version: 1.0 // Author: Ruben Smits // Maintainer: Ruben Smits // 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 KDL_STIFFNESS_H #define KDL_STIFFNESS_H #include "frames.hpp" namespace KDL { /** * Preliminary class to implement Stiffness, only diagonal stiffness is implemented * no transformations provided... * * Implements a diagonal stiffness matrix. * first 3 elements are stiffness for translations * last 3 elements are stiffness for rotations. */ class Stiffness { double data[6]; public: Stiffness() { data[0]=0; data[1]=0; data[2]=0; data[3]=0; data[4]=0; data[5]=0; } Stiffness(double* d) { data[0]=d[0]; data[1]=d[1]; data[2]=d[2]; data[3]=d[3]; data[4]=d[4]; data[5]=d[5]; } Stiffness(double x,double y,double z,double rx,double ry,double rz) { data[0]=x; data[1]=y; data[2]=z; data[3]=rx; data[4]=ry; data[5]=rz; } double& operator[](int i) { return data[i]; } double operator[](int i) const { return data[i]; } Twist Inverse(const Wrench& w) const{ Twist t; t[0]=w[0]/data[0]; t[1]=w[1]/data[1]; t[2]=w[2]/data[2]; t[3]=w[3]/data[3]; t[4]=w[4]/data[4]; t[5]=w[5]/data[5]; return t; } }; inline Wrench operator * (const Stiffness& s, const Twist& t) { Wrench w; w[0]=s[0]*t[0]; w[1]=s[1]*t[1]; w[2]=s[2]*t[2]; w[3]=s[3]*t[3]; w[4]=s[4]*t[4]; w[5]=s[5]*t[5]; return w; } inline Stiffness operator+(const Stiffness& s1, const Stiffness& s2) { Stiffness s; s[0]=s1[0]+s2[0]; s[1]=s1[1]+s2[1]; s[2]=s1[2]+s2[2]; s[3]=s1[3]+s2[3]; s[4]=s1[4]+s2[4]; s[5]=s1[5]+s2[5]; return s; } inline void posrandom(Stiffness& F) { posrandom(F[0]); posrandom(F[1]); posrandom(F[2]); posrandom(F[3]); posrandom(F[4]); posrandom(F[5]); } inline void random(Stiffness& F) { posrandom(F); } } #endif orocos-kdl-1.4.0/orocos_kdl/src/trajectory.cpp000066400000000000000000000056221326704774600214340ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 trajectory.cxx trajectory.cxx - description ------------------- begin : Mon May 10 2004 copyright : (C) 2004 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: trajectory.cpp,v 1.1.1.1.2.4 2003/07/18 14:58:36 psoetens Exp $ * $Name: $ ****************************************************************************/ #include "utilities/error.h" #include "utilities/error_stack.h" #include "trajectory.hpp" #include "path.hpp" #include "trajectory_segment.hpp" #include #include namespace KDL { using namespace std; Trajectory* Trajectory::Read(std::istream& is) { // auto_ptr because exception can be thrown ! IOTrace("Trajectory::Read"); char storage[64]; EatWord(is,"[",storage,sizeof(storage)); Eat(is,'['); if (strcmp(storage,"SEGMENT")==0) { IOTrace("SEGMENT"); auto_ptr geom( Path::Read(is) ); auto_ptr motprof( VelocityProfile::Read(is) ); EatEnd(is,']'); IOTracePop(); IOTracePop(); return new Trajectory_Segment(geom.release(),motprof.release()); } else { throw Error_MotionIO_Unexpected_Traj(); } return NULL; // just to avoid the warning; } } orocos-kdl-1.4.0/orocos_kdl/src/trajectory.hpp000066400000000000000000000075241326704774600214440ustar00rootroot00000000000000/*************************************************************************** tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 trajectory.h trajectory.h - description ------------------- begin : Mon January 10 2005 copyright : (C) 2005 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * 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 * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: trajectory.h,v 1.1.1.1.2.5 2003/07/23 16:44:25 psoetens Exp $ * $Name: $ * \todo * Peter's remark : should separate I/O from other routines in the * motion/chain directories * The problem is that the I/O uses virtual inheritance to write * the trajectories/geometries/velocityprofiles/... * Have no good solution for this, perhaps * * #ifdef's * * declaring dummy ostream/istream and change implementation file .cpp * * declaring some sort of VISITOR object (containing an ostream) , * the classes contain code to pass this object around along its children * a subroutine can then be called with overloading. * PROBLEM : if you declare a friend you have to fully declare it ==> exposing I/O with ostream/istream decl * CONSEQUENCE : everything has to be declared public. ****************************************************************************/ #ifndef TRAJECTORY_H #define TRAJECTORY_H #include "frames.hpp" #include "frames_io.hpp" #include "path.hpp" #include "velocityprofile.hpp" namespace KDL { /** * An abstract class that implements * a trajectory contains a cartesian space trajectory and an underlying * velocity profile. * @ingroup Motion */ class Trajectory { public: virtual double Duration() const = 0; // The duration of the trajectory virtual Frame Pos(double time) const = 0; // Position of the trajectory at